3、坐标系与位姿表示：世界坐标系、机器人坐标系、欧拉角、四元数、齐次变换矩阵

各位同学，咱们今天聊点硬核的——坐标系与位姿表示。

说实话，我见过太多做轨迹规划的工程师，算法写得飞起，结果一上真机就翻车。翻来覆去查，最后发现是坐标系搞混了。嗯，这种坑我年轻时也踩过，而且不止一次。

所以这一章，咱们把坐标系和位姿表示彻底捋清楚。你想想看，机器人要纠偏，首先得知道“我在哪”、“我要去哪”、“我现在的姿态对不对”。这三个问题，全得靠坐标系和位姿来描述。

3.1 世界坐标系 vs 机器人坐标系

先讲最基础的。世界坐标系，说白了就是“参考系”。它固定在地面上，通常我们取机器人基座的中心作为原点，X轴朝前，Y轴朝左，Z轴朝上。这个坐标系一旦定下来，整个工作空间里的所有物体位置就都有了“地址”。

机器人坐标系呢？它是跟着机器人走的。比如机械臂的末端执行器，它自己就有一个坐标系，原点在夹爪中心，Z轴指向夹爪张开方向。这个坐标系会随着机械臂运动而不断变化。

为什么这两个坐标系这么重要？

我举个例子。有一次我在调试一个移动抓取项目，视觉系统检测到工件在世界坐标系下的位置是 (0.5m, 0.3m, 0.2m)。但机器人当时正站在另一个位置，它需要先把自己在世界坐标系下的位姿算出来，然后才能规划出一条路径去抓那个工件。如果直接把世界坐标系的坐标扔给机器人控制器，它根本不知道该怎么走——因为它只认识自己的机器人坐标系。

所以，轨迹纠偏的第一步，就是搞清楚当前数据是在哪个坐标系下表达的。我个人习惯，在代码里给每个坐标变量加一个后缀，比如 pos_world、pos_robot，一眼就能看出来。

3.2 欧拉角：直观但容易踩坑

欧拉角，大家应该不陌生。就是用三个角度来描述旋转：绕X轴转的叫横滚角（Roll），绕Y轴转的叫俯仰角（Pitch），绕Z轴转的叫偏航角（Yaw）。

听起来很直观对吧？但我要告诉你，欧拉角有个致命问题——万向锁。

什么叫万向锁？当俯仰角达到 ±90° 时，横滚和偏航的旋转轴会重合，导致丢失一个自由度。这时候你无论怎么调横滚和偏航，机器人末端都只能在一个平面内转，完全失控。

我曾经在一个六轴机械臂的项目里遇到过这个问题。当时做姿态插补，用的就是欧拉角。结果机械臂运动到某个奇异点附近，末端突然剧烈抖动，差点把工件甩飞出去。从那以后，我只要做连续轨迹规划，就坚决不用欧拉角做内部运算。

欧拉角的另一个坑是旋转顺序。同样的三个角度，按 XYZ 顺序旋转和按 ZYX 顺序旋转，得到的结果完全不同。所以你在看任何文档或代码时，一定要确认旋转顺序。我个人习惯用 ZYX 顺序（先绕Z转，再绕Y，最后绕X），这也是工业机器人最常用的约定。

3.3 四元数：稳定但抽象

四元数，听起来很数学，其实没那么可怕。它用四个数来表示旋转：一个实部 + 三个虚部。写成 q = w + xi + yj + zk，其中 w 是实部，x, y, z 是虚部。

为什么用四元数？因为它完美解决了欧拉角的两个问题：无万向锁、插值平滑。

你想想看，如果机器人要从姿态A平滑过渡到姿态B，用欧拉角做线性插值，中间姿态可能会乱跳。但用四元数的球面线性插值（SLERP），轨迹就非常平滑自然。

我记得有一次做无人机视觉伺服，需要根据图像特征实时调整相机姿态。如果用欧拉角做反馈控制，每帧都要处理万向锁的边界条件，代码写得想哭。换成四元数之后，控制律直接写成 q_dot = 0.5 * q * ω，简洁又稳定。

当然，四元数也有缺点——不直观。你看到 q = [0.707, 0, 0.707, 0]，很难想象它对应什么姿态。所以实际工程中，我通常这样分工：

• 内部运算：用四元数（滤波、插值、控制）
• 外部显示：转成欧拉角给人看
• 数据存储：用四元数（避免歧义）

3.4 齐次变换矩阵：把位置和姿态打包

齐次变换矩阵，说白了就是一个 4×4 的矩阵，把旋转和位移打包在一起。它的形式是：

| R   t |
| 0   1 |

其中 R 是 3×3 的旋转矩阵，t 是 3×1 的平移向量。最后一行是 [0, 0, 0, 1]。

为什么用齐次矩阵？因为你可以把多个变换“串”起来。比如，从世界坐标系到机器人基座的变换是 T_world_to_base，从基座到末端执行器的变换是 T_base_to_ee，那么从世界到末端的变换就是：

T_world_to_ee = T_world_to_base * T_base_to_ee

矩阵乘法，一步到位。这在轨迹规划里太方便了。

我举个例子。假设你要让机械臂末端沿着世界坐标系的X轴走直线。你只需要在世界坐标系下生成一系列目标点，然后用 T_world_to_base 的逆矩阵，把这些点转换到机器人基座坐标系下，再发给关节控制器。整个过程，就是齐次矩阵的乘法和求逆。

3.5 知识体系总览

说了这么多，咱们用一张图把整个知识体系串起来。下面这张 SVG 图展示了坐标系与位姿表示的核心逻辑：

从这张图可以看得很清楚：世界坐标系和机器人坐标系之间通过齐次变换矩阵关联；而位姿中的姿态部分，可以用欧拉角、四元数或旋转矩阵来表示。在实际工程中，我建议你记住一个原则：存储用四元数，显示用欧拉角，运算用齐次矩阵。

3.6 实战中的坐标系管理

最后，分享一个我个人的坐标系管理习惯。在写代码时，我会维护一个坐标系树：

world
  ├── robot_base
  │   ├── joint_1
  │   │   ├── joint_2
  │   │   │   └── ... (直到末端执行器)
  │   └── gripper
  ├── camera_1
  ├── camera_2
  └── workpiece_table

每个节点都存储一个相对于父节点的齐次变换矩阵。这样，任意两个坐标系之间的变换，只需要沿着树向上走到公共祖先，再向下走到目标节点，矩阵连乘即可。

嗯，这套方法我用了快十年，从来没出过坐标系混乱的问题。你也不妨试试。

好了，坐标系与位姿表示就讲到这里。下一节咱们聊传感器数据融合，到时候会大量用到今天讲的这些概念。记得把齐次变换矩阵的乘法练熟，后面会频繁用到。