3. 坐标系与姿态表示

惯性导航与姿态解算的核心任务，是实时确定载体（无人机、机器人、手机、车辆等）相对于某个参考基准的角位置关系。要完成这一任务，首先必须建立一套严谨的数学语言来描述“姿态”。本章将系统梳理姿态描述的两大坐标系——世界坐标系与机体坐标系，并深入对比三种主流姿态表示方法：欧拉角、旋转矩阵与四元数。理解这些基础概念，是后续学习加速度计与陀螺仪数据融合、卡尔曼滤波设计的必要前提。

3.1 两大坐标系的定义与物理意义

世界坐标系（World Frame，记为 n 系或 g 系）是描述载体姿态的参考基准。在工程实践中，最常用的是 NED（北-东-地）坐标系：X 轴指向地理北极，Y 轴指向正东，Z 轴垂直指向地心。另一种常见形式是 ENU（东-北-天）坐标系，多用于航空与机器人领域。世界坐标系的关键特征在于：其原点固定于地球表面某一点，且各轴方向不随载体运动而改变。重力向量在此坐标系中具有恒定表达（NED 下为 [0, 0, g]），这为加速度计的姿态解算提供了天然基准。

机体坐标系（Body Frame，记为 b 系）则与载体固连，原点通常取 IMU 的测量中心或载体质心。其 X 轴沿载体纵轴指向机头/前进方向，Y 轴指向载体右侧，Z 轴垂直于载体平面向下（与 NED 习惯一致）。陀螺仪输出的角速度、加速度计输出的比力，均是在机体坐标系下测量的。当载体姿态变化时，机体坐标系相对于世界坐标系发生旋转，而这一旋转关系正是我们要求解的姿态。

3.2 欧拉角：直观的物理含义与万向锁问题

欧拉角通过三次绕不同坐标轴的旋转来描述姿态，最常用的顺序为 Z-Y-X（先偏航、再俯仰、最后横滚）。设世界坐标系为 n 系，机体坐标系为 b 系，则从 n 系到 b 系的旋转可分解为：

1. 绕 n 系 Z 轴旋转偏航角 ψ（Yaw）
2. 绕新 Y 轴旋转俯仰角 θ（Pitch）
3. 绕新 X 轴旋转横滚角 φ（Roll）

这种表示方法的物理意义极其直观：无人机飞行员可以直接通过这三个角度理解飞行姿态。然而，欧拉角存在一个致命缺陷——万向锁（Gimbal Lock）。当俯仰角 θ = ±90° 时，第一次旋转与第三次旋转的旋转轴重合，导致系统丢失一个自由度，此时偏航角与横滚角无法独立区分。在卡尔曼滤波中，若状态向量直接使用欧拉角，万向锁附近会出现协方差矩阵奇异，导致滤波器发散。

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
物理直观性	★★★★★	★★	★★★
万向锁问题	存在	不存在	不存在
计算效率	高（三角函数）	低（矩阵乘法）	最高（四元数乘法）
插值友好性	差	差	好（球面线性插值）
典型应用	显示、遥控指令	坐标变换、视觉	滤波、融合、导航

3.3 旋转矩阵：精确的坐标变换工具

旋转矩阵 R ∈ SO(3) 是一个 3×3 的正交矩阵，满足 R^TR = I 且 det(R) = +1。它完整描述了从机体坐标系到世界坐标系的旋转关系：

v_n = R · v_b

其中 v_b 是向量在机体坐标系中的坐标，v_n 是同一向量在世界坐标系中的坐标。旋转矩阵的列向量分别表示机体坐标系的三个轴在世界坐标系中的方向余弦。例如，R 的第一列就是机体 X 轴在世界坐标系中的单位向量。这种表示方法没有奇异问题，且可以直接用于向量变换，因此在视觉SLAM和机械臂控制中广泛使用。

然而，旋转矩阵的 9 个参数并非相互独立，它们受到 6 个正交约束的限制。在滤波迭代过程中，数值误差会逐渐破坏正交性，因此需要定期进行正交化修正（如 SVD 分解或 Gram-Schmidt 正交化）。此外，9 个参数的存储和运算开销也相对较大，在资源受限的嵌入式系统中不够经济。

3.4 四元数：姿态解算的工程首选

四元数是一种超复数，形式为 q = q₀ + q₁i + q₂j + q₃k，其中 i² = j² = k² = ijk = -1。单位四元数（模长为 1）可以表示三维空间中的任意旋转。若旋转轴为单位向量 u = [uₓ, u_y, u_z]，旋转角度为 θ，则对应的四元数为：

q = [cos(θ/2), uₓ·sin(θ/2), u_y·sin(θ/2), u_z·sin(θ/2)]

四元数的主要优势体现在三个方面：第一，它仅用 4 个参数描述姿态，无冗余且无奇异；第二，四元数乘法可以高效地组合连续旋转，计算量远小于矩阵乘法；第三，它天然适合插值（如 Slerp），便于平滑过渡。在卡尔曼滤波中，四元数常被选为状态向量的一部分，其更新方程简洁且数值稳定性好。

3.5 三种表示方法的转换与实战选择

在实际工程中，三种表示方法并非互斥，而是经常需要相互转换。例如，IMU 原始数据在机体坐标系下，滤波内部使用四元数递推，最终输出给飞控的指令则需转换为欧拉角。以下给出核心转换公式：

// 四元数 → 旋转矩阵
R = [
  1-2(q2²+q3²),   2(q1q2-q0q3),   2(q1q3+q0q2);
  2(q1q2+q0q3),   1-2(q1²+q3²),   2(q2q3-q0q1);
  2(q1q3-q0q2),   2(q2q3+q0q1),   1-2(q1²+q2²)
]

// 四元数 → 欧拉角（Z-Y-X顺序）
φ = atan2(2(q0q1+q2q3), 1-2(q1²+q2²))
θ = asin(2(q0q2-q3q1))
ψ = atan2(2(q0q3+q1q2), 1-2(q2²+q3²))

案例拆解：某四旋翼无人机项目，采用 MPU6050 IMU，主控为 STM32F405（168MHz）。设计姿态解算模块时，我们做了如下选型：

• 滤波状态量：使用四元数 q = [q₀,q₁,q₂,q₃] 作为状态向量，避免欧拉角的万向锁问题，同时减少状态维度。
• 陀螺仪积分：采用四元数微分方程 q̇ = 0.5 · q ⊗ ω，其中 ω 为陀螺仪角速度四元数形式。
• 加速度计修正：将重力向量从世界坐标系变换到机体坐标系（使用旋转矩阵），与加速度计实测值比较，得到误差后通过 PI 控制器修正陀螺仪零偏。
• 输出接口：仅在需要向地面站发送姿态时，才将四元数转换为欧拉角，避免在控制回路中频繁使用三角函数。

实测结果表明：在 1kHz 更新率下，四元数解算耗时约 12μs，而等效的旋转矩阵法耗时 28μs，欧拉角法在俯仰角接近 90° 时出现明显抖动。最终系统在剧烈机动下仍能保持稳定，验证了四元数方案的优越性。

3.6 常见误区与工程复盘

误区一：混淆坐标系定义。不同厂商的 IMU 可能采用不同的机体轴定义（如 DJI 的 NED 与部分手机传感器的 ENU），若未仔细核对数据手册，会导致姿态解算结果完全错误。建议在代码中明确注释坐标系定义，并在上电自检时通过静态放置验证重力方向。

误区二：忽视四元数归一化。部分初学者在滤波更新后忘记归一化，导致姿态模长漂移，最终表现为“姿态缓慢旋转”或“尺度缩放”。应在每次更新后立即执行归一化，并可在调试时打印模长偏差。

误区三：欧拉角直接用于积分。欧拉角微分方程包含三角函数且存在奇异，直接对欧拉角做数值积分在俯仰角较大时会迅速发散。正确做法是使用四元数或旋转矢量积分。

本章建立的坐标系与姿态表示体系，是后续所有融合算法的数学基石。无论是互补滤波中的向量叉积修正，还是卡尔曼滤波中的状态转移矩阵设计，都依赖于对这三种表示方法深刻而准确的理解。在实际项目中，建议先在仿真环境中验证坐标变换的正确性，再进行硬件联调，可大幅减少排障时间。