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

各位同学，咱们今天聊点硬核的。坐标系和空间描述，说白了就是回答三个问题：我在哪？我要去哪？我怎么去？ 搞机器人运动规划，这玩意儿就是地基。地基没打好，后面避障算法再花哨，也是空中楼阁。

我个人习惯，拿到一个新项目，第一件事不是调参数，而是先把坐标系理清楚。你想想看，机械臂末端抓个杯子，你告诉它“往右移10厘米”。问题是，这个“右”是相对于谁的右？是机器人基座的右，还是你站在它面前看到的右？这就是坐标系要解决的事。

  核心观点： 没有坐标系，就没有空间。没有空间描述，就没有运动规划。

全局坐标系，也叫世界坐标系。你可以把它想象成整个场景的“参考系”。比如工厂里，我们把坐标系原点定在车间角落，X轴指向东，Y轴指向北，Z轴指向上。那么车间里所有东西——机器人、工件、传送带——都有一个固定的全局坐标。

局部坐标系，也叫物体坐标系。它是附着在某个物体上的。比如机械臂的每个关节，都有自己的局部坐标系。我建议你把它理解成“我自己的小世界”。

举个例子。我在项目中遇到过一台移动机器人，它要导航到某个工位。全局坐标系告诉它“工位在 (5, 3) 处”。但机器人自己并不知道自己面朝哪。它需要把自己的局部坐标系（车头方向为X轴）和全局坐标系对齐，才能算出“我该往左转30度还是右转30度”。

我的小技巧： 画图时，全局坐标系用大写字母 {W} 表示，局部坐标系用 {B} 表示。这样看公式时一眼就能分清。

好了，现在我们有多个坐标系了。怎么把一个点从局部坐标系转换到全局坐标系？齐次变换矩阵就是干这个的。

说白了，它就是一张“地图”，告诉你：从坐标系A到坐标系B，需要旋转多少、平移多少。

齐次变换矩阵长这样：

| R   t |
| 0   1 |

其中 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。最后一行是 [0 0 0 1]，这是为了保持矩阵形式统一，方便连续变换。

我记得刚入行时，总觉得齐次变换矩阵很抽象。后来我把它当成“坐标转换的说明书”。你有一个点 p 在局部坐标系 {B} 中，想把它转到全局坐标系 {W} 中，那就直接左乘变换矩阵：

p_W = T_WB * p_B

就这么简单。一个矩阵，搞定旋转和平移。

我曾经踩过的坑： 齐次变换矩阵的乘法顺序不能乱！先旋转后平移，和先平移后旋转，结果完全不同。我有个项目，就是因为搞反了顺序，机械臂差点撞到旁边的设备。从那以后，我每次写变换矩阵，都会在注释里标清楚“先转后移”。

旋转矩阵虽然好用，但9个参数太冗余了。欧拉角用三个角度来描述旋转：绕X轴转（滚转 Roll）、绕Y轴转（俯仰 Pitch）、绕Z轴转（偏航 Yaw）。

你想想看，飞机飞行员就用这三个角度。抬头、低头是俯仰，左右转弯是偏航，机身侧倾是滚转。很直观，对吧？

但欧拉角有个大问题——万向锁。当俯仰角达到 ±90° 时，滚转和偏航会变得无法区分。我在做无人机仿真时就遇到过这问题，飞控突然“卡住”了，怎么调都不对。后来查了半天，才发现是欧拉角在作怪。

  我的建议： 欧拉角适合给人看，不适合给机器算。如果你要做插值、平滑旋转，别用欧拉角。

四元数，听起来很玄乎，其实就是一种“不会卡住”的旋转表示方法。它用四个数来表示旋转：一个实部加三个虚部。

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。约束条件是 w² + x² + y² + z² = 1。

为什么用四元数？因为它没有万向锁，而且做旋转插值非常平滑。你想想看，机械臂要从姿态A平滑过渡到姿态B，用欧拉角插值可能会绕远路，但四元数插值（球面线性插值，SLERP）走的是最短路径。

我个人习惯，所有内部计算都用四元数。只有在最后显示给用户看时，才转成欧拉角。这样既保证了计算稳定性，又保留了直观性。

快速记忆： 四元数 = 绕某个轴转某个角度。轴是 (x, y, z)，角度是 arccos(w) 的两倍。

下面这张图，是我自己总结的坐标系与空间描述的知识结构。你把它存下来，以后遇到相关问题时，先看这张图，思路就清晰了。

说了这么多，到底该用哪种？我根据项目经验，给你一个快速参考表：

重要提醒： 无论你选哪种表示方法，一定要在代码里统一。我见过最惨的案例，是团队里有人用欧拉角、有人用四元数，结果对接时角度差了180度，机器人直接撞墙。从那以后，我们团队定了个规矩：所有接口统一用四元数，谁用欧拉数谁请客。

嗯，关于坐标系和空间描述，今天就聊这么多。这些东西看着基础，但真到了项目里，每一个细节都可能让你加班到深夜。记住一句话：坐标系理不清，后面全是坑。