机器人坐标系转换之从世界坐标系到局部坐标系

三角函数实现

下面是代码c++和python实现:

cpp 复制代码
#include <iostream>
#include <cmath>

struct Point {
    double x;
    double y;
};

class RobotCoordinateTransform {
private:
    Point origin; // 局部坐标系的原点在世界坐标系中的坐标

public:
    RobotCoordinateTransform(double originX, double originY) : origin({originX, originY}) {}

    // 将世界坐标转换到局部坐标
    Point worldToLocal(double x_w, double y_w, double theta_w) {
        Point local;

        // 平移坐标系
        local.x = x_w - origin.x;
        local.y = y_w - origin.y;

        // 旋转坐标系
        double x_local = local.x * cos(theta_w) - local.y * sin(theta_w);
        double y_local = local.x * sin(theta_w) + local.y * cos(theta_w);

        local.x = x_local;
        local.y = y_local;

        return local;
    }
};

int main() {
    RobotCoordinateTransform transform(0, 0); // 假设局部坐标系原点在世界坐标系的(0, 0)点

    double x_w, y_w, theta_w;

    std::cout << "Enter world coordinates (x_w, y_w) and orientation theta_w (in radians): ";
    std::cin >> x_w >> y_w >> theta_w;

    Point local = transform.worldToLocal(x_w, y_w, theta_w);

    std::cout << "Local coordinates (x_local, y_local): (" << local.x << ", " << local.y << ")" << std::endl;

    return 0;
}
python 复制代码
import math

class RobotCoordinateTransform:
    def __init__(self, origin_x, origin_y):
        self.origin = (origin_x, origin_y)  # 局部坐标系的原点在世界坐标系中的坐标

    # 将世界坐标转换到局部坐标
    def world_to_local(self, x_w, y_w, theta_w):
        # 平移坐标系
        x_local = x_w - self.origin[0]
        y_local = y_w - self.origin[1]

        # 旋转坐标系
        x_local_rotated = x_local * math.cos(theta_w) - y_local * math.sin(theta_w)
        y_local_rotated = x_local * math.sin(theta_w) + y_local * math.cos(theta_w)

        return x_local_rotated, y_local_rotated

if __name__ == "__main__":
    origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))
    origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))

    transform = RobotCoordinateTransform(origin_x, origin_y)

    x_w = float(input("Enter the x-coordinate in the world coordinate system: "))
    y_w = float(input("Enter the y-coordinate in the world coordinate system: "))
    theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))

    x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)

    print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

矩阵实现:

下面是代码c++和python实现:

cpp 复制代码
#include <iostream>
#include <cmath>
#include <Eigen/Dense>  // Eigen库用于矩阵运算

class RobotCoordinateTransform {
private:
    Eigen::Vector2d origin;  // 局部坐标系的原点在世界坐标系中的坐标

public:
    RobotCoordinateTransform(double originX, double originY) : origin(originX, originY) {}

    // 将世界坐标转换到局部坐标
    std::pair<double, double> worldToLocal(double x_w, double y_w, double theta_w) {
        // 平移坐标系的矩阵
        Eigen::Matrix3d translationMatrix;
        translationMatrix << 1, 0, -origin[0],
                             0, 1, -origin[1],
                             0, 0, 1;

        // 旋转坐标系的矩阵
        Eigen::Matrix3d rotationMatrix;
        rotationMatrix << cos(theta_w), -sin(theta_w), 0,
                          sin(theta_w),  cos(theta_w), 0,
                          0,             0,            1;

        // 世界坐标的齐次坐标
        Eigen::Vector3d worldCoords(x_w, y_w, 1);

        // 应用平移和旋转变换
        Eigen::Vector3d localCoords = rotationMatrix * translationMatrix * worldCoords;

        return std::make_pair(localCoords[0], localCoords[1]);
    }
};

int main() {
    double originX, originY;
    std::cout << "Enter the x-coordinate of the origin of the local coordinate system: ";
    std::cin >> originX;
    std::cout << "Enter the y-coordinate of the origin of the local coordinate system: ";
    std::cin >> originY;

    RobotCoordinateTransform transform(originX, originY);

    double x_w, y_w, theta_w;
    std::cout << "Enter the x-coordinate in the world coordinate system: ";
    std::cin >> x_w;
    std::cout << "Enter the y-coordinate in the world coordinate system: ";
    std::cin >> y_w;
    std::cout << "Enter the orientation (in radians) in the world coordinate system: ";
    std::cin >> theta_w;

    auto [x_local, y_local] = transform.worldToLocal(x_w, y_w, theta_w);

    std::cout << "Local coordinates (x_local, y_local): (" << x_local << ", " << y_local << ")" << std::endl;

    return 0;
}
Eigen::Vector2d 用于存储坐标点和原点。
Eigen::Matrix3d 用于表示3x3矩阵,进行平移和旋转操作。
worldToLocal 方法使用上述的数学公式和矩阵进行坐标变换。
python 复制代码
import numpy as np
import math

class RobotCoordinateTransform:
    def __init__(self, origin_x, origin_y):
        self.origin = np.array([[origin_x], [origin_y]])  # 局部坐标系的原点在世界坐标系中的坐标

    def world_to_local(self, x_w, y_w, theta_w):
        # 平移坐标系的矩阵
        translation_matrix = np.array([
            [1, 0, -self.origin[0][0]],
            [0, 1, -self.origin[1][0]],
            [0, 0, 1]
        ])

        # 旋转坐标系的矩阵
        rotation_matrix = np.array([
            [math.cos(theta_w), -math.sin(theta_w), 0],
            [math.sin(theta_w), math.cos(theta_w), 0],
            [0, 0, 1]
        ])

        # 世界坐标的齐次坐标
        world_coords = np.array([[x_w], [y_w], [1]])

        # 应用平移和旋转变换
        local_coords = np.dot(rotation_matrix, np.dot(translation_matrix, world_coords))

        return local_coords[0][0], local_coords[1][0]

if __name__ == "__main__":
    origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))
    origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))

    transform = RobotCoordinateTransform(origin_x, origin_y)

    x_w = float(input("Enter the x-coordinate in the world coordinate system: "))
    y_w = float(input("Enter the y-coordinate in the world coordinate system: "))
    theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))

    x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)

    print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

Tips:

相关推荐
云卓SKYDROID3 小时前
除草机器人算法以及技术详解!
算法·机器人·科普·高科技·云卓科技·算法技术
袁牛逼15 小时前
电话语音机器人,是由哪些功能构成?
人工智能·自然语言处理·机器人·语音识别
TsingtaoAI16 小时前
2024.10|AI/大模型在机器人/自动驾驶/智能驾舱领域的最新应用和深度洞察
机器人·自动驾驶·ai大模型·具身智能·智能驾舱
不是AI18 小时前
【持续更新】【NLP项目】【自然语言处理】智能聊天机器人——“有问必答”【Chatbot】第2章、《模式一:问候模式》
人工智能·自然语言处理·机器人
鱼会上树cy19 小时前
【机器人学】2-2.六自由度机器人运动学逆解-奇异位形分析【附MATLAB代码】
机器人
北京搜维尔科技有限公司21 小时前
搜维尔科技:【煤矿虚拟仿真】煤矿企业、高校、科研单位-多语言支持、数字孪生、交互式学习体验
科技·机器人·vr
Matlab程序猿小助手21 小时前
【MATLAB源码-第208期】基于matlab的改进A*算法和传统A*算法对比仿真;改进点:1.无斜穿障碍物顶点2.删除中间多余节点,减少转折。
开发语言·嵌入式硬件·算法·matlab·机器人
高登先生1 天前
京津冀自动驾驶技术行业盛会|2025北京自动驾驶技术展会
大数据·人工智能·科技·机器人·自动驾驶
不是AI2 天前
【持续更新】【NLP项目】【自然语言处理】智能聊天机器人——“有问必答”【Chatbot】第1章、《系统、环境》
人工智能·自然语言处理·机器人
北京搜维尔科技有限公司2 天前
搜维尔科技:Manus VR数据手套-人形机器人的远程操作和机器学习
科技·机器人·vr