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

三角函数实现

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

#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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

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})")

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

矩阵实现：

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

#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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

Eigen::Vector2d 用于存储坐标点和原点。
Eigen::Matrix3d 用于表示3x3矩阵，进行平移和旋转操作。
worldToLocal 方法使用上述的数学公式和矩阵进行坐标变换。
1
2
3

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})")

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

Tips：