机器人运动——全局转车辆坐标下-坐标变换

问题叙述：

已经知道车辆全局坐标x,y,theta.令有一个全局坐标点（x1，y1），使用c++将全局坐标点（x1，y1），转换到相对于车辆坐标下的坐标：

解决方案

要将全局坐标点 (x1, y1) 转换为相对于车辆坐标系下的坐标，需要进行以下步骤：
1.计算全局坐标点 (x1, y1) 相对于车辆的偏移量；
2.根据车辆的朝向 theta ，对偏移量进行旋转，以得到相对于车辆坐标系下的坐标。

下面是一个简单的 C++ 示例代码：

#include <iostream>
#include <cmath>

struct Point {
    double x;
    double y;
};

// 转换全局坐标到车辆坐标系下的函数
Point globalToLocal(const Point& globalPoint, const Point& vehicleGlobalPose) {
    Point localPoint;

    // 计算相对车辆的偏移量
    double dx = globalPoint.x - vehicleGlobalPose.x;
    double dy = globalPoint.y - vehicleGlobalPose.y;

    // 计算相对车辆的偏移量在车辆坐标系下的坐标
    localPoint.x = dx * cos(vehicleGlobalPose.theta) + dy * sin(vehicleGlobalPose.theta);
    localPoint.y = -dx * sin(vehicleGlobalPose.theta) + dy * cos(vehicleGlobalPose.theta);

    return localPoint;
}

int main() {
    // 定义车辆全局坐标和朝向
    Point vehicleGlobalPose = {0.0, 0.0, 0.0}; // x, y, theta

    // 定义全局坐标点
    Point globalPoint = {3.0, 4.0}; // x1, y1

    // 转换为车辆坐标系下的坐标
    Point localPoint = globalToLocal(globalPoint, vehicleGlobalPose);

    // 输出结果
    std::cout << "Local coordinates relative to vehicle: (" << localPoint.x << ", " << localPoint.y << ")" << std::endl;

    return 0;
}

在这个示例中，globalToLocal 函数接受全局坐标点和车辆的全局坐标以及朝向作为输入，并返回相对于车辆坐标系下的坐标。