3. 坐标系与坐标变换：全局坐标系、局部坐标系、齐次变换矩阵

各位同学，欢迎来到第三章。这一章，我们聊聊坐标系。

说实话，我刚入行做机器人路径规划那会儿，觉得坐标系这东西太简单了。不就是X、Y、Z三个轴嘛，谁不会啊？结果第一次做AGV（自动导引车）项目，小车死活走不对路线，折腾了两天才发现——我把全局坐标和局部坐标搞混了。嗯，从那以后，我再也不敢小看坐标系了。

3.1 为什么需要坐标系？

你想想看，机器人要完成从A点到B点的任务，它得知道两件事：

• 我在哪儿？——这是定位问题
• 目标在哪儿？——这是目标描述问题

这两个问题，都离不开坐标系。说白了，坐标系就是给空间中的点一个「身份证号」，让机器人能精确地知道每个位置。

3.2 全局坐标系（World Coordinate System）

全局坐标系，也叫世界坐标系。它是固定不变的，是整个地图的「绝对参考系」。

我习惯把全局坐标系想象成一张地图的经纬度。不管你在北京的哪个角落，天安门广场的经纬度是固定的。同样，在机器人世界里，全局坐标系的原点和方向一旦定义好，就不再改变。

全局坐标系的特点：

• 固定不变，不随机器人移动而改变
• 所有物体（障碍物、目标点、机器人自身）都可以用全局坐标描述
• 通常以地图的某个角落或中心为原点

举个例子，假设我们的仓库地图是20米×30米，我一般会把原点设在左下角：

// 全局坐标系下的点
Point robotPosition = {x: 5.0, y: 3.0};      // 机器人位置
Point targetPosition = {x: 15.0, y: 25.0};   // 目标位置
Point obstaclePosition = {x: 10.0, y: 12.0}; // 障碍物位置

3.3 局部坐标系（Local Coordinate System）

局部坐标系就不一样了。它是「跟着机器人走的」。机器人往左转，它的局部坐标系也跟着往左转。

为什么要搞个局部坐标系？因为机器人身上的传感器（激光雷达、摄像头）都是装在它自己身上的。传感器看到的东西，天然就是局部坐标。

局部坐标系的特点：

• 原点在机器人中心（或传感器安装位置）
• 方向随机器人旋转而旋转
• 通常X轴指向机器人前进方向，Y轴指向左侧

来看个具体的例子。假设机器人前方2米处有个障碍物：

// 局部坐标系下（机器人自身视角）
Point obstacleInLocal = {x: 2.0, y: 0.0};  // 正前方2米

// 如果机器人全局位置是(5,3)，朝向是90度（朝北）
// 那么障碍物的全局坐标是多少？
// 这就需要坐标变换了！

3.4 齐次变换矩阵（Homogeneous Transformation Matrix）

好，关键来了。怎么把局部坐标变成全局坐标？或者反过来？

答案就是——齐次变换矩阵。

我第一次看到这个名词时，觉得好高大上。其实说白了，它就是一张「翻译表」，能把一个坐标系下的坐标，翻译到另一个坐标系下。

齐次变换矩阵的形式：

| R(3x3)   t(3x1) |
| 0 0 0     1     |

其中：
R —— 旋转矩阵（描述两个坐标系之间的旋转关系）
t —— 平移向量（描述两个坐标系原点之间的偏移）

举个例子，假设机器人全局位置是(5, 3)，朝向是0度（朝东）。那么从局部到全局的变换矩阵就是：

// 2D情况下的齐次变换矩阵
| cos(0)  -sin(0)   5 |
| sin(0)   cos(0)   3 |
|   0        0      1 |

// 简化后：
| 1  0  5 |
| 0  1  3 |
| 0  0  1 |

用这个矩阵，就可以把局部坐标(2, 0)变成全局坐标：

| 1  0  5 |   | 2 |   | 7 |
| 0  1  3 | × | 0 | = | 3 |
| 0  0  1 |   | 1 |   | 1 |

// 所以局部坐标(2,0) → 全局坐标(7,3)

3.5 坐标变换的链式法则

实际项目中，坐标变换往往不是一步到位的。比如：

• 激光雷达看到障碍物 → 得到传感器局部坐标
• 传感器装在机器人上 → 需要转到机器人坐标系
• 机器人在全局地图中 → 需要转到全局坐标系

这就形成了变换链：

T_global = T_robot_to_global × T_sensor_to_robot × P_sensor

其中：
P_sensor —— 传感器坐标系下的点
T_sensor_to_robot —— 传感器到机器人的变换
T_robot_to_global —— 机器人到全局的变换

3.6 实践中的坐标系管理

做实际项目时，我建议你养成几个好习惯：

1. 统一命名规范——比如用_w表示全局坐标，_l表示局部坐标
2. 画坐标系图——每次做变换前，先在纸上画清楚各个坐标系的关系
3. 写单元测试——用已知点验证变换矩阵是否正确

下面是我常用的一个坐标系管理类模板：

class CoordinateManager {
public:
    // 局部坐标转全局坐标
    Point localToGlobal(const Point& localPoint) {
        // 使用齐次变换矩阵
        return transformMatrix * localPoint;
    }
    
    // 全局坐标转局部坐标
    Point globalToLocal(const Point& globalPoint) {
        // 使用逆矩阵
        return inverseTransformMatrix * globalPoint;
    }
    
private:
    Matrix4x4 transformMatrix;   // 变换矩阵
    Matrix4x4 inverseTransformMatrix;  // 逆变换矩阵
};

3.7 小结

这一章的内容，说白了就是三件事：

• 全局坐标系——固定不变，地图的绝对参考
• 局部坐标系——跟着机器人走，传感器的天然视角
• 齐次变换矩阵——连接两个坐标系的桥梁

我个人觉得，坐标系和坐标变换是路径规划中最基础也最容易出错的地方。你想想看，如果连「我在哪儿」都搞不清楚，后面的路径规划再漂亮也是白搭。

下一章，我们会把这些知识用起来，开始真正规划路径。到时候你会发现，今天学的这些变换，就是整个路径规划大厦的地基。