2、坐标系与刚体运动基础：世界坐标系、机体坐标系、欧拉角、四元数、旋转矩阵、齐次变换

各位同学，欢迎来到这门课的第一节硬核内容。

说实话，做SLAM这么多年，我见过太多新手一上来就怼算法，结果坐标系都搞混了，最后定位数据飘到天上去。坐标系和刚体运动，说白了就是整个导航系统的「语言」。语言不通，后面全是白搭。

今天我们就把这套「语言」彻底捋清楚。

2.1 世界坐标系与机体坐标系

先问个问题：你站在操场上，说「前面10米有棵树」。这个「前面」是谁的前面？是你的前面，还是操场的北边？

这就是坐标系的核心矛盾——参考系不同，描述就不同。

2.1.1 世界坐标系（World Frame）

世界坐标系，也叫全局坐标系。它是一个固定不动的参考系。在GPS拒止环境下，我们通常用第一帧图像或者起飞点来定义它。

我个人习惯用 W 表示世界坐标系。它的三个轴一般这样定义：

• X轴：指向东（或者起飞时的正前方）
• Y轴：指向北（或者起飞时的左侧）
• Z轴：指向天（右手系规则）

注意，这里有个坑。我在做无人机项目时，发现有些团队用「东北天」，有些用「北东地」。一旦混用，融合出来的位置直接差一个旋转。嗯，这里要统一约定，我建议团队内部用「东北天」，跟大多数惯性导航库保持一致。

2.1.2 机体坐标系（Body Frame）

机体坐标系是贴在机器人身上的。它跟着机器人一起动。我们用 B 表示。

对于四旋翼或者地面机器人，机体坐标系通常这样定义：

• X轴：指向机头方向（前进方向）
• Y轴：指向机身右侧
• Z轴：指向机身下方（右手系）

你想想看，IMU（惯性测量单元）输出的加速度和角速度，就是在机体坐标系下测量的。而我们要做导航，必须把这些数据转换到世界坐标系下。怎么转？这就是接下来要讲的旋转。

2.2 旋转的三种数学表达

描述旋转，有三大主流工具：欧拉角、旋转矩阵、四元数。我当年刚入行时，觉得它们就是三种不同的数学游戏。后来踩了坑才发现，选错工具，调试能调到你怀疑人生。

2.2.1 欧拉角（Euler Angles）

欧拉角最直观。它用三个角度来描述旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。

• Yaw（ψ）：绕Z轴旋转，像你原地转圈
• Pitch（θ）：绕Y轴旋转，像点头
• Roll（φ）：绕X轴旋转，像歪头

顺序很重要。我见过一个项目，团队用了不同的旋转顺序（比如ZYX和XYZ），结果融合出来的姿态差了90度。找bug找了三天。

在机器人领域，最常用的是 ZYX顺序：先偏航，再俯仰，最后横滚。

2.2.2 旋转矩阵（Rotation Matrix）

旋转矩阵是一个3x3的正交矩阵。它把机体坐标系下的向量，映射到世界坐标系下。

公式长这样：

R = Rz(ψ) * Ry(θ) * Rx(φ)

其中每个矩阵都是绕单轴旋转的标准形式。比如绕Z轴旋转ψ角度：

Rz(ψ) = [cosψ  -sinψ  0]
        [sinψ   cosψ  0]
        [0      0     1]

旋转矩阵的好处是：没有奇点，可以连续旋转。坏处是：9个参数，冗余。而且每次更新都要做矩阵乘法，计算量稍大。

在实际工程中，我一般用旋转矩阵做坐标变换，但不用它做姿态的递推更新。为什么？因为数值误差会让它慢慢不再是正交矩阵，需要频繁做正交化修正，很烦。

2.2.3 四元数（Quaternion）

四元数是我个人最喜欢的旋转表达方式。它用4个参数，没有奇点，计算效率高。

一个四元数长这样：

q = w + xi + yj + zk

或者写成向量形式：q = [w, x, y, z]。其中w是实部，x、y、z是虚部。

单位四元数（模长为1）可以用来表示旋转。给定一个旋转轴 u = [ux, uy, uz] 和旋转角度 θ：

q = [cos(θ/2), ux*sin(θ/2), uy*sin(θ/2), uz*sin(θ/2)]

用四元数旋转一个向量 v 的公式：

v' = q * v * q_conjugate

这里 q_conjugate 是四元数的共轭。注意，乘法是四元数乘法，不是普通乘法。

2.3 齐次变换（Homogeneous Transformation）

旋转搞定了，那位置呢？在三维空间中，一个刚体的完整状态包括：位置（3个自由度） + 姿态（3个自由度），一共6个自由度。

齐次变换矩阵把旋转和平移统一到一个4x4的矩阵里：

T = [R   t]
    [0   1]

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

用齐次坐标表示一个三维点 p = [x, y, z, 1]^T，变换后的点就是：

p' = T * p

为什么用齐次坐标？说白了，就是为了把旋转和平移写成一个矩阵乘法。否则你得先旋转再平移，写两行代码，容易出错。

举个例子。假设世界坐标系下有一个点 p_w = [1, 2, 3]。你的机器人当前在世界坐标系下的位置是 t = [10, 0, 5]，姿态用旋转矩阵 R 表示。那么该点在机体坐标系下的坐标就是：

p_b = R^T * (p_w - t)

用齐次变换就简洁多了：

T_wb = [R  t]
       [0  1]
p_b = inv(T_wb) * p_w

这里 inv(T_wb) 是齐次变换矩阵的逆，它等于：

inv(T_wb) = [R^T  -R^T * t]
            [0     1]

2.4 总结与对比

好了，我们来个快速对比，帮你记住什么时候用哪个：

表达方式	参数数量	优点	缺点	推荐场景
欧拉角	3	直观，容易理解	万向锁，不适合大角度	人机交互、可视化
旋转矩阵	9	无奇点，适合坐标变换	冗余，需正交化	坐标变换、标定
四元数	4	无奇点，计算快，易插值	不够直观	姿态估计、滤波、优化
齐次变换	16	统一旋转和平移	冗余，占用内存大	多坐标系管理、传感器融合

最后说一句心里话。坐标系和刚体运动，看起来是基础，但它是整个导航系统的地基。我见过太多项目，算法写得花里胡哨，结果坐标系定义错了，整个系统推倒重来。所以，花时间把这块吃透，绝对值得。

下一节，我们会把这些数学工具用到IMU的测量模型里。到时候你会发现，今天学的这些，全是IMU预积分和状态估计的「原材料」。