3、姿态表示方法：欧拉角（Yaw/Pitch/Roll）、方向余弦矩阵（DCM）、四元数、旋转向量，各自优缺点与转换关系

姿态解算是惯性导航、机器人控制、无人机飞控、AR/VR 头显追踪等领域的核心基础。无论是读取 IMU（惯性测量单元）的原始数据，还是设计卡尔曼滤波器进行数据融合，第一步都必须明确：我们用什么数学工具来描述刚体在三维空间中的朝向？ 欧拉角、方向余弦矩阵（DCM）、四元数和旋转向量是四种最主流的表示方法。它们各有优劣，适用于不同的计算阶段和工程场景。本章将深入剖析这四种方法的数学本质、工程适用性、致命缺陷以及它们之间的转换逻辑，帮助你建立一套“选型—转换—校验”的完整知识框架。

姿态表示方法选型与转换关系图 三维刚体姿态 欧拉角 Yaw / Pitch / Roll ✅ 直观，符合人类直觉 ✅ 三个参数，存储量小 ❌ 万向节死锁 ❌ 插值不平滑 ⚠ 适合：人机交互显示 方向余弦矩阵 DCM (3×3) ✅ 无奇异值问题 ✅ 向量旋转直接矩阵乘法 ❌ 9个参数，冗余大 ❌ 需频繁正交化修正 ⚠ 适合：坐标变换计算 四元数 q = (w, x, y, z) ✅ 无奇异值，紧凑 ✅ 插值平滑 (Slerp) ❌ 不直观，调试困难 ❌ 双覆盖 (q=-q) ⚠ 适合：滤波/融合/插值 旋转向量 轴角 (θ·n̂) ✅ 最少的3参数表示 ✅ 物理意义明确 ❌ 小角度近似误差 ❌ 大角度插值困难 ⚠ 适合：陀螺积分/优化 转换关系速查 欧拉角 → DCM: R = Rz(ψ)·Ry(θ)·Rx(φ) 欧拉角 → 四元数: q = qz(ψ) ⊗ qy(θ) ⊗ qx(φ) DCM → 四元数: 由矩阵迹与主对角元素求解 四元数 → 旋转向量: θ = 2·acos(w), n̂ = (x,y,z)/sin(θ/2) 旋转向量 → DCM: 罗德里格斯公式 四元数 → 欧拉角: atan2 与 asin 组合，注意死锁 ⚠ 转换时需注意象限与归一化

上图展示了四种姿态表示方法的核心特征与转换路径。在工程实践中，我们通常不会只使用一种表示方法，而是根据计算阶段的需求进行切换：传感器原始数据用旋转向量积分，滤波融合用四元数，坐标变换用DCM，人机交互显示用欧拉角。 下面逐一深入剖析。

3.1 欧拉角：直观但危险的“万向节死锁”

3.1.1 核心原理

欧拉角通过三次绕不同坐标轴的旋转来描述姿态，最常见的航空航天约定为 Z-Y-X 顺序：先绕 Z 轴偏航（Yaw, ψ），再绕新 Y 轴俯仰（Pitch, θ），最后绕新 X 轴横滚（Roll, φ）。其数学本质是三个基本旋转矩阵的乘积：

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

其中每个基本旋转矩阵都是 3×3 的正交矩阵。欧拉角的优势在于参数少（仅3个）、物理意义明确，飞行员或操作员可以直观理解“机头偏了多少度”。

3.1.2 判断方法与操作流程

在工程中判断是否适合使用欧拉角，可遵循以下流程：

  1. 检查姿态范围：如果俯仰角接近 ±90°，则存在死锁风险，应避免使用欧拉角。
  2. 检查应用场景：如果仅用于显示或人工控制，欧拉角是首选；如果用于自动控制或滤波，则需谨慎。
  3. 检查插值需求：如果需要对姿态进行平滑插值（如动画），欧拉角会产生不自然的旋转路径。

3.1.3 案例拆解：无人机俯仰90°时的航向突变

某固定翼无人机在垂直爬升时，俯仰角 θ 接近 90°。此时，偏航角 ψ 和横滚角 φ 的旋转轴重合，导致自由度丢失。在飞控日志中观察到：当 θ 从 89.9° 变为 90.1° 时，输出的 ψ 值从 120° 瞬间跳变到 -60°，尽管飞机实际航向并未改变。这就是典型的万向节死锁现象。解决方法是：在飞控内部状态估计中完全禁用欧拉角，改用四元数；仅在数传链路下行时，将四元数转换为欧拉角供地面站显示，并限制俯仰角显示范围在 ±85° 以内。

风险提醒： 欧拉角的死锁问题不仅是数学上的奇异点，在物理上意味着两个控制通道耦合。如果飞控内部使用欧拉角进行 PID 控制，在死锁附近会导致控制量发散，甚至坠机。任何涉及自动控制的姿态解算，内部表示都应避免使用欧拉角。

3.1.4 常见误区与总结

  • 误区1：认为欧拉角旋转顺序无关紧要。实际上，Z-Y-X 与 Z-X-Y 得到的最终姿态完全不同。
  • 误区2：直接对欧拉角做差分或积分。欧拉角的微分方程包含三角函数耦合，不能简单累加。
  • 总结：欧拉角适合“人读”和“显示”，不适合“机算”和“融合”。

3.2 方向余弦矩阵（DCM）：稳健但冗余的“全能选手”

3.2.1 核心原理

DCM 是一个 3×3 的正交矩阵，其每一列表示体坐标系（b系）在参考坐标系（n系）中的方向余弦。任意向量在两个坐标系间的变换只需一次矩阵乘法：v_n = C_b^n · v_b。DCM 的显著优点是不存在奇异值，可以描述任意姿态，且组合旋转只需矩阵乘法，计算效率高。

3.2.2 判断方法与操作流程

DCM 的维护需要正交化修正。由于数值积分误差，矩阵会逐渐失去正交性，因此需要定期修正：

// 正交化修正（Gram-Schmidt 过程）
// 假设 C 为当前 DCM，误差为 E = C^T·C - I
// 修正：C = C · (I - 0.5·E)

操作流程：每次更新后计算误差矩阵，若误差超过阈值（如 1e-6），则执行修正。

3.2.3 案例拆解：捷联惯导中的 DCM 更新

在捷联惯导系统中，陀螺仪输出角速度，通过 C_dot = C · Ω（Ω 为角速度反对称矩阵）更新 DCM。某水下机器人使用 DCM 进行姿态解算，在连续工作 2 小时后，发现 DCM 的行列式从 1.0 漂移到 0.98，导致加速度计重力补偿出现明显偏差。通过加入正交化修正，行列式误差控制在 1e-5 以内，姿态精度提升了 40%。

经验提示： 虽然 DCM 有 9 个参数，但在嵌入式平台上，3×3 矩阵乘法通常比四元数乘法更高效（因为无特殊乘法规则）。如果 MCU 没有硬件浮点单元，DCM 可能是更好的选择。

3.2.4 常见误区与总结

  • 误区：认为 DCM 一定比四元数差。实际上，DCM 的冗余性带来了数值稳定性，且无需处理双覆盖问题。
  • 总结：DCM 适合需要频繁坐标变换、且对奇异值零容忍的场合，但需付出正交化维护的代价。

3.3 四元数：现代姿态解算的“事实标准”

3.3.1 核心原理

四元数是一个四维超复数 q = w + xi + yj + zk，满足 i² = j² = k² = ijk = -1。单位四元数（模长为1）可以表示三维旋转。其核心优势在于：无奇异值、参数紧凑（4个）、插值平滑（Slerp）、组合旋转只需一次四元数乘法。四元数乘法定义为：

q1 ⊗ q2 = (w1w2 - v1·v2, w1v2 + w2v1 + v1×v2)

3.3.2 判断方法与操作流程

在卡尔曼滤波或互补滤波中，四元数是状态量的首选。操作流程：

  1. 初始化四元数为单位四元数 (1,0,0,0)。
  2. 陀螺仪积分：q_dot = 0.5 · q ⊗ ω（ω 为角速度四元数，实部为0）。
  3. 归一化：每次更新后强制 q = q / ||q||。
  4. 加速度计/磁力计修正：通过梯度下降或卡尔曼增益修正四元数。

3.3.3 案例拆解：四元数双覆盖问题导致的控制反转

某四轴飞行器在飞控代码中，将四元数直接作为 PID 控制器的误差输入。当飞机翻转 360° 时，四元数从 (1,0,0,0) 变为 (-1,0,0,0)，虽然表示同一姿态，但误差计算出现 2π 的跳变，导致电机输出瞬间反向，飞机剧烈抖动。解决方案：在误差计算前，检查 q 与目标四元数的点积，若为负则取反，确保最短路径插值。

关键要点： 四元数的双覆盖性（q 和 -q 表示同一旋转）是工程中极易忽略的坑。在比较两个四元数时，应先判断点积符号，再计算差值。

3.3.4 常见误区与总结

  • 误区1：忘记归一化。四元数模长漂移会导致旋转缩放，必须每次更新后归一化。
  • 误区2：混淆 Hamilton 约定与 JPL 约定。不同约定下乘法顺序不同，跨团队协作时极易出错。
  • 总结：四元数是姿态融合、插值、优化的最佳选择，但需要开发者具备较强的数学抽象能力。

3.4 旋转向量：小角度积分的“利器”

3.4.1 核心原理

旋转向量（轴角）用一个三维向量 θ·n̂ 表示旋转，其中 n̂ 是旋转轴单位向量，θ 是旋转角度。其物理意义最直观：绕某个轴转多少度。旋转向量与 DCM 的转换由罗德里格斯公式给出：

R = I + sin(θ)·K + (1-cos(θ))·K²

其中 K 是 n̂ 的反对称矩阵。旋转向量的核心优势在于：它是陀螺仪积分的最自然形式——角速度乘以时间即为小角度旋转向量。

3.4.2 判断方法与操作流程

旋转向量适合以下场景：

  • 陀螺仪短时间积分（如 1ms 内的角度增量）。
  • 优化算法中的姿态增量表示（如图优化中的相对位姿）。
  • 需要最小参数表示且无死锁的场合。

操作流程：将陀螺仪角速度乘以采样周期 Δt，得到旋转向量 Δθ，然后转换为四元数增量并累乘。

3.4.3 案例拆解：IMU 预积分中的旋转向量应用

在视觉惯性里程计（VIO）中，IMU 预积分需要计算两帧图像之间的相对旋转。直接对四元数积分会产生非线性误差，而使用旋转向量进行预积分，可以线性化处理。某 VIO 系统将旋转向量作为预积分残差，相比直接四元数积分，在快速旋转场景下（角速度 > 200°/s），轨迹漂移减少了 35%。

风险提醒： 旋转向量在角度接近 2π 时，存在周期性跳变（θ 与 θ-2π 表示同一旋转）。在优化中需注意角度归一化，否则会导致雅可比矩阵奇异。

3.4.4 常见误区与总结

  • 误区：将旋转向量直接累加。大角度旋转不满足向量加法交换律，必须通过指数映射转换。
  • 总结：旋转向量是“微分”层面的最佳表示，适合积分和优化，但不适合作为全局状态量。

3.5 四种表示方法的转换关系与工程选型

3.5.1 转换关系总表

转换方向核心公式/方法注意事项
欧拉角 → DCMR = Rz(ψ)·Ry(θ)·Rx(φ)旋转顺序必须明确
欧拉角 → 四元数q = qz(ψ) ⊗ qy(θ) ⊗ qx(φ)注意四元数乘法顺序
DCM → 四元数由矩阵迹和主对角元素求解需处理数值稳定性，选择最大分量法
四元数 → 欧拉角ψ = atan2(2(wz+xy), 1-2(y²+z²)) 等θ = ±90° 时需特殊处理
旋转向量 → DCM罗德里格斯公式θ 接近 0 时用泰勒展开
四元数 → 旋转向量θ = 2·acos(w), n̂ = (x,y,z)/sin(θ/2)θ 接近 0 时 n̂ 任意

3.5.2 工程选型决策树

在实际项目中，可参考以下决策逻辑：

  1. 是否需要人机交互显示？ 是 → 最终输出用欧拉角，内部计算用四元数。
  2. 是否涉及滤波/融合？ 是 → 状态量用四元数，观测量用旋转向量或 DCM。
  3. 是否做坐标变换？ 是 → 用 DCM 或四元数，避免欧拉角。
  4. 是否做优化/图优化？ 是 → 用旋转向量作为增量，四元数作为全局量。

3.5.3 案例拆解：从传感器到控制的完整链路

以某自动驾驶车辆姿态估计模块为例：

  • 输入：IMU 输出角速度 ω 和加速度 a。
  • 积分：将 ω·Δt 作为旋转向量，转换为四元数增量，累乘得到当前姿态四元数 q。
  • 融合：在扩展卡尔曼滤波（EKF）中，状态量为四元数，观测量为加速度计/磁力计推导的 DCM。
  • 输出：将四元数转换为欧拉角，用于仪表盘显示；将四元数转换为 DCM，用于将车身坐标系下的加速度转换到导航坐标系。

该链路中，每种表示方法都发挥了其最大优势，同时避免了各自的缺陷。

经验总结： 没有“最好”的姿态表示方法，只有“最合适”的。建议在代码中封装统一的转换库，并编写单元测试验证转换的互逆性（如四元数→DCM→四元数，误差应小于 1e-6）。

3.6 总结复盘：姿态表示方法的本质

四种表示方法本质上是 SO(3) 李群的不同参数化方式。欧拉角是局部坐标，存在奇异；DCM 是全局坐标，冗余但稳定；四元数是双覆盖的全局坐标，紧凑且适合插值；旋转向量是李代数，适合微分和优化。在工程实践中，建议遵循以下原则：

  • 内部计算统一用四元数，避免频繁切换带来的转换误差。
  • 所有转换函数必须处理边界条件（如 θ 接近 0、θ 接近 ±90°）。
  • 定期校验：在每次启动时，用已知姿态验证转换链路的正确性。
  • 文档化旋转顺序：在代码注释中明确标注欧拉角的旋转顺序（如 Z-Y-X 内旋），防止团队协作歧义。

掌握这四种表示方法及其转换，是姿态解算从“会用”到“精通”的关键分水岭。下一阶段，我们将基于这些数学工具，深入 IMU 的误差模型与标定方法。

← 上一章 📖 返回目录 下一章 →