坐标系与变换：全局坐标系、局部坐标系、齐次变换矩阵、欧拉角与四元数

做机器人路径规划，说白了就是回答三个问题：我在哪？我要去哪？怎么去？ 而第一个问题「我在哪」，就离不开坐标系。

我记得刚入行那会儿，带我的老工程师丢给我一句话：「坐标系搞不明白，你写的路径规划代码就是废纸。」当时我不以为然，直到第一次把机器人跑飞了……嗯，从那以后，我再也不敢轻视坐标系变换了。

1. 全局坐标系 vs 局部坐标系

先说说最基础的概念。全局坐标系，也叫世界坐标系，是固定不动的参考系。你可以把它想象成房间的墙角——原点定在那里，X、Y、Z轴方向不变。机器人在地图上的位置，就是用全局坐标表示的。

局部坐标系呢？它是跟着机器人走的。机器人转个身，它的局部坐标系也跟着转。我习惯把局部坐标系叫做「机器人自己的小世界」——在它眼里，前方永远是X轴正方向。

举个例子。你在全局地图上规划了一条直线路径，但机器人走到半路遇到障碍物。这时候，传感器检测到的障碍物位置是相对于机器人自身的——也就是局部坐标。你需要把障碍物的局部坐标转换到全局坐标，才能更新路径。

为什么会这样？因为传感器装在机器人身上，它只能告诉你「前方1米有障碍」，而不是「全局坐标(2.5, 3.8)处有障碍」。这个转换，就是坐标系变换的核心工作。

2. 齐次变换矩阵

说到变换，就绕不开齐次变换矩阵。我个人觉得，这是机器人学里最优雅的数学工具之一。

一个4x4的矩阵，同时包含了旋转和平移信息：

| R11  R12  R13  tx |
| R21  R22  R23  ty |
| R31  R32  R33  tz |
|  0    0    0    1  |

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

我在项目中遇到过一个问题：用3x3旋转矩阵加3x1平移向量做变换，代码写起来特别啰嗦。每次都要先旋转，再平移，还得单独处理特殊情况。后来换成齐次矩阵，一行代码搞定：

# 齐次变换：从局部到全局
global_pose = T_local_to_global @ local_pose

你想想看，一个矩阵乘法就把旋转和平移都包含了，多清爽。

3. 欧拉角

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

但这里有个大坑。我曾经在这个坑里摔得鼻青脸肿。

另外，欧拉角的旋转顺序也很重要。常见的顺序有ZYX、ZYZ等。同样的角度值，不同顺序得到的结果完全不同。所以，千万别在代码里只用三个数字表示欧拉角，一定要注明旋转顺序。

4. 四元数

为了解决欧拉角的缺陷，四元数登场了。说实话，我第一次看到四元数时也是一脸懵——四个数字，还有虚部，什么鬼？

但用多了你会发现，四元数真香。它没有万向锁，插值平滑，计算效率高。在路径规划中，我几乎只用四元数做姿态插值。

四元数的形式：

q = w + xi + yj + zk

其中w是实部，x、y、z是虚部。而且满足 w² + x² + y² + z² = 1，这叫单位四元数。

实际项目中，你不需要手算四元数。主流库都提供了转换工具：

# Python示例：欧拉角转四元数
from scipy.spatial.transform import Rotation as R

# 欧拉角 (roll, pitch, yaw)，单位弧度
euler = [0.1, 0.2, 0.3]
quat = R.from_euler('zyx', euler).as_quat()
print(quat)  # [x, y, z, w]

5. 知识体系一览

下面这张图，是我梳理的坐标系与变换的核心逻辑。你可以把它当作本章的「地图」：

6. 实战避坑指南

最后，分享几个我踩过的坑，希望能帮你省点时间：

• 坐标系方向要统一：我曾经在一个项目里，激光雷达用右手系，IMU用左手系，结果融合出来的姿态乱七八糟。后来强制所有传感器统一到右手系，问题解决。
• 四元数归一化：每次做四元数乘法后，记得归一化。浮点误差累积会让四元数偏离单位长度，导致旋转失真。
• 欧拉角转四元数时指定顺序：不同库的默认顺序可能不同。我习惯显式指定，比如 from_euler('zyx', angles)，避免歧义。
• 齐次矩阵的逆：求逆矩阵时，别直接用4x4矩阵求逆。利用旋转矩阵正交性，可以快速计算：旋转部分转置，平移部分取反再旋转。

---