2. 坐标系与位姿表示：全局坐标系、局部坐标系、齐次变换矩阵、欧拉角与四元数

做路径规划这些年，我踩过最大的坑，就是坐标系搞混了。你想想看，机器人明明按照指令走，结果一头撞墙上——查了半天，原来是全局坐标和局部坐标没对齐。说白了，坐标系就是机器人的“世界观”，搞不懂它，后面所有算法都是空中楼阁。

全局坐标系，也叫世界坐标系。它是固定不动的，就像我们教室里的东南西北。机器人一开始放在哪里，原点就在哪里。我习惯用 W 表示它。

局部坐标系呢，是绑在机器人身上的。机器人动，它就跟着动。比如机械臂的每个关节，都有自己的局部坐标系。你想想看，机械臂末端抓杯子，它只关心“杯子在我手心里什么位置”，才不管杯子在房间哪个角落。

核心区别一句话：全局坐标系是“地图”，局部坐标系是“我”。

我在项目中遇到过一件事：AGV小车明明路径规划好了，但转弯时总偏。查了半天，原来是里程计给的局部坐标，和激光雷达的全局坐标没对齐。嗯，从那以后，我每次调试第一件事就是打印坐标变换矩阵。

齐次变换矩阵，说白了就是“坐标翻译官”。它能把局部坐标系下的点，翻译到全局坐标系下。一个4x4的矩阵，长这样：

| R11 R12 R13 Tx |
| R21 R22 R23 Ty |
| R31 R32 R33 Tz |
|  0   0   0   1  |

左上角3x3是旋转矩阵 R，右上角3x1是平移向量 T。最后一行永远是 [0 0 0 1]，这是齐次坐标的规矩。

为什么要用4x4？因为3x3只能做旋转，做不了平移。加上齐次坐标后，旋转和平移就能用一个矩阵搞定。我刚开始学的时候也觉得多此一举，直到自己手写代码——嗯，真香。

我的小技巧：调试时，先检查平移向量 T 对不对。旋转错了顶多歪着走，平移错了直接撞墙。

欧拉角是描述旋转最直观的方式。绕X轴转叫横滚角（Roll），绕Y轴转叫俯仰角（Pitch），绕Z轴转叫偏航角（Yaw）。

但欧拉角有个大坑——万向锁。当Pitch接近90度时，Roll和Yaw就分不清了。我曾经在无人机项目里吃过这个亏：飞控突然乱转，查了三天，原来是欧拉角在奇异点附近数值爆炸了。

避坑指南：如果你做的是飞行器或机械臂，尽量别用欧拉角做插值。我曾经用欧拉角做路径平滑，结果机器人转了个大圈才到目标位置——因为角度绕了360度。

四元数，听起来玄乎，其实就是一个复数升级版。它用四个数表示旋转：q = w + xi + yj + zk。其中 w 是实部，x, y, z 是虚部。

为什么用四元数？三个原因：

我建议你记住这个公式：

// 四元数乘法（组合旋转）
q_result = q1 * q2

// 用四元数旋转一个向量
v_rotated = q * v * q_conjugate

注意顺序：先旋转的放右边，后旋转的放左边。这个顺序我当年搞反过，结果机器人转得跟陀螺似的。

实战建议：ROS里默认用四元数。如果你拿到欧拉角，记得先转成四元数再处理。转换公式网上都有，但别手写——用现成库，比如Eigen或tf2。

下面这张图，是我自己总结的坐标系与位姿表示的知识结构。你把它存下来，以后遇到相关问题，先看这张图定位问题。

我的经验：如果你刚开始做路径规划，先用四元数存姿态，需要给人看时再转欧拉角。这样既保证了算法稳定性，又方便调试。我曾经在项目里全程用欧拉角，结果万向锁导致路径规划直接崩了——从那以后，我再也不敢偷懒了。

好了，坐标系和位姿表示就聊到这儿。记住：全局坐标是地图，局部坐标是我自己，齐次变换是翻译官，四元数是保命符。下次你写路径规划代码时，先花5分钟确认坐标系对不对——这5分钟，能省你后面5小时的调试时间。