3、姿态表示方法：欧拉角、方向余弦矩阵、四元数的优缺点及相互转换关系

捷联惯导系统的核心任务之一是实时、准确地解算载体相对于导航坐标系的姿态。姿态的数学表示方法直接决定了算法复杂度、计算精度和工程实现的可行性。在工程实践中，欧拉角、方向余弦矩阵（DCM）和四元数是三种最常用的姿态描述工具。它们各有优劣，适用于不同的算法阶段和硬件约束。本章将深入剖析这三种表示方法的数学本质、工程适用场景、相互转换关系，并结合实际代码实现中的数值问题给出可操作的建议。

姿态表示方法对比与转换关系 欧拉角 (Roll, Pitch, Yaw) 优点：直观、物理意义明确 缺点：万向节锁、三角函数计算 适用：飞行控制显示、初始对准 3个参数 方向余弦矩阵 (DCM) 优点：无奇异性、向量旋转直接 缺点：9个参数冗余、需正交化 适用：姿态矩阵运算、坐标变换 9个参数 四元数 (Quaternion) 优点：无奇异性、计算量小 缺点：不直观、需归一化 适用：姿态更新、姿态解算核心 4个参数 公式(1) 公式(3) 公式(4) 关键特性对比 特性 欧拉角 DCM 四元数 奇异性 有（万向节锁） 无 无 计算量 小（但含三角） 大（9元素） 小（4元素） 直观性 最直观 一般 不直观 工程应用 显示、指令 坐标变换 姿态更新

3.1 欧拉角：直观但存在奇异性

核心原理

欧拉角通过三次绕不同坐标轴的旋转来描述载体姿态，通常采用Z-Y-X（偏航-俯仰-横滚）顺序。设载体坐标系为b系，导航坐标系为n系，则姿态角定义为：偏航角ψ（绕Z轴）、俯仰角θ（绕Y轴）、横滚角φ（绕X轴）。旋转矩阵可表示为三个基本旋转矩阵的乘积：

C_n^b = R_x(φ) · R_y(θ) · R_z(ψ)

其中R_x、R_y、R_z分别为绕X、Y、Z轴旋转的基本旋转矩阵。欧拉角的物理意义极其明确，飞行员和地面站操作员可以直接从数值判断载体的当前朝向。

判断方法与操作流程

在实际工程中，选择欧拉角作为姿态表示方式时，需要重点评估以下三点：

  • 姿态角范围：若载体俯仰角接近±90°，则必须避免使用欧拉角，因为此时会发生万向节锁（Gimbal Lock），导致偏航和横滚无法区分。
  • 计算效率：欧拉角微分方程包含大量三角函数运算，在嵌入式平台上会消耗较多CPU周期。
  • 插值需求：若需要在两个姿态之间进行平滑插值（如云台控制），欧拉角插值会产生不自然的旋转路径。

案例拆解：无人机初始对准中的欧拉角应用

某四旋翼无人机在起飞前需要进行初始对准，此时载体处于近似水平状态（θ≈0°，φ≈0°）。工程师选择欧拉角作为初始姿态表示，利用加速度计测量值直接计算初始横滚角和俯仰角：

φ_0 = atan2(accel_y, accel_z)
θ_0 = atan2(-accel_x, sqrt(accel_y² + accel_z²))

初始偏航角则通过磁力计或外部参考确定。这一过程利用欧拉角的直观性快速完成了初始对准。但工程师同时注意到，当无人机进行大机动飞行时，俯仰角可能接近90°，此时欧拉角微分方程中的tan(θ)项趋于无穷大，导致数值不稳定。因此，在姿态解算核心算法中，他们改用四元数进行姿态更新，仅在输出显示和指令生成时转换为欧拉角。

风险提示：欧拉角微分方程在θ=±90°时存在奇异点，此时偏航角速率和横滚角速率无法分离。任何使用欧拉角进行姿态更新的算法都必须设置角度保护机制，否则会导致解算结果发散。

常见误区与总结

  • 误区一：认为欧拉角旋转顺序无关紧要。实际上，不同的旋转顺序会产生不同的姿态矩阵，必须明确约定并保持一致。
  • 误区二：直接对欧拉角进行数值积分。欧拉角微分方程是耦合的，不能简单地对每个角独立积分。
  • 总结：欧拉角适合作为人机交互接口和初始对准工具，但不适合作为姿态解算的核心表示。

3.2 方向余弦矩阵：稳健但计算冗余

核心原理

方向余弦矩阵是一个3×3的正交矩阵，其元素表示两个坐标系各轴之间的夹角余弦。设姿态矩阵为C，则任意向量在n系和b系之间的变换为：

v_b = C · v_n

DCM的9个元素并非独立，它们满足6个约束条件（3个归一化条件+3个正交条件）。DCM的最大优势在于：它不存在奇异性问题，可以描述任意姿态；同时，向量旋转和坐标变换可以直接通过矩阵乘法完成，无需额外计算。

判断方法与操作流程

在以下场景中，DCM是更优选择：

  • 需要频繁进行坐标变换：如将加速度计测量值从b系转换到n系。
  • 需要与其他矩阵运算结合：如组合导航中的卡尔曼滤波状态转移。
  • 对数值稳定性要求高：DCM的正交化修正相对简单。

但DCM的缺点是参数冗余，每次更新后都需要进行正交化处理，否则误差会逐渐累积。常用的正交化方法是：

C_corrected = (3/2)·C - (1/2)·C·C^T·C

案例拆解：捷联惯导中的DCM应用

某捷联惯导系统在姿态解算中采用DCM作为中间表示。系统以100Hz的频率读取陀螺仪数据，每次读取后执行以下流程：

  1. 读取角增量Δθ = [Δθ_x, Δθ_y, Δθ_z]^T
  2. 构造反对称矩阵Ω = [[0, -Δθ_z, Δθ_y], [Δθ_z, 0, -Δθ_x], [-Δθ_y, Δθ_x, 0]]
  3. 更新DCM：C_new = C_old · (I + Ω)
  4. 执行正交化修正

该方案在实验室测试中表现出良好的稳定性，但工程师发现，在嵌入式平台上，9个元素的矩阵乘法每次需要27次乘法和18次加法，计算量较大。同时，正交化修正虽然保证了矩阵的正交性，但引入了额外的计算开销。经过性能分析，他们决定在姿态更新核心算法中改用四元数，仅在需要输出姿态矩阵时临时转换为DCM。

经验提示：DCM的正交化修正频率可以低于姿态更新频率。例如，每10次更新执行一次正交化，可以在保证精度的同时减少计算量。

常见误区与总结

  • 误区一：忽略DCM的正交化。长时间运行后，矩阵会逐渐偏离正交性，导致姿态误差累积。
  • 误区二：认为DCM比四元数更精确。实际上，在相同字长下，四元数的归一化约束比DCM的正交化约束更容易满足。
  • 总结：DCM适合作为姿态矩阵的输出形式，但作为核心更新算法时，其计算冗余和正交化负担使其不如四元数高效。

3.3 四元数：高效且无奇异

核心原理

四元数是一个超复数，由一个实部和三个虚部组成：q = q0 + q1·i + q2·j + q3·k，其中q0为标量部分，[q1, q2, q3]为矢量部分。单位四元数满足q0² + q1² + q2² + q3² = 1，可以表示三维空间中的旋转。四元数乘法定义为：

q ⊗ p = [q0·p0 - q·p, q0·p + p0·q + q×p]

四元数的主要优势在于：仅需4个参数，无奇异性，且乘法运算比矩阵乘法更高效。姿态更新方程简洁：

q_new = q_old ⊗ Δq

其中Δq由角增量构造：Δq = [cos(Δθ/2), (Δθ_x/Δθ)·sin(Δθ/2), ...]

判断方法与操作流程

四元数是捷联惯导姿态解算的首选表示方法，原因如下：

  • 计算效率：四元数乘法仅需16次乘法和12次加法，远少于DCM的27次乘法。
  • 无奇异性：四元数可以描述任意姿态，不存在万向节锁问题。
  • 易于归一化：只需将四元数除以模长即可完成归一化，计算简单。

但四元数的缺点是物理意义不直观，且不能直接用于向量旋转（需要转换为矩阵或使用特殊公式）。

案例拆解：四元数姿态解算的完整实现

某高精度捷联惯导系统采用四元数作为核心姿态表示，运行频率为200Hz。系统初始化时，通过初始对准获得欧拉角，然后转换为四元数：

q0 = cos(φ/2)·cos(θ/2)·cos(ψ/2) + sin(φ/2)·sin(θ/2)·sin(ψ/2)
q1 = sin(φ/2)·cos(θ/2)·cos(ψ/2) - cos(φ/2)·sin(θ/2)·sin(ψ/2)
q2 = cos(φ/2)·sin(θ/2)·cos(ψ/2) + sin(φ/2)·cos(θ/2)·sin(ψ/2)
q3 = cos(φ/2)·cos(θ/2)·sin(ψ/2) - sin(φ/2)·sin(θ/2)·cos(ψ/2)

每次采样后，系统执行以下步骤：

  1. 读取陀螺仪角增量Δθ
  2. 构造增量四元数Δq
  3. 更新四元数：q_new = q_old ⊗ Δq
  4. 归一化：q_new = q_new / ||q_new||

在持续2小时的跑车试验中，四元数算法保持了良好的数值稳定性，姿态误差小于0.01°。工程师还实现了四元数到欧拉角的转换函数，用于实时显示：

φ = atan2(2·(q0·q1 + q2·q3), 1 - 2·(q1² + q2²))
θ = asin(2·(q0·q2 - q3·q1))
ψ = atan2(2·(q0·q3 + q1·q2), 1 - 2·(q2² + q3²))
工程要点：四元数归一化是防止误差累积的关键。建议在每次更新后立即执行归一化，而不是每隔几次执行一次。虽然归一化会增加少量计算量，但可以显著提高长期运行的稳定性。

常见误区与总结

  • 误区一：忽略四元数的单位约束。非单位四元数会导致姿态缩放误差，必须严格归一化。
  • 误区二：混淆四元数乘法的顺序。四元数乘法不满足交换律，q1⊗q2 ≠ q2⊗q1，必须按照约定的旋转顺序进行。
  • 总结：四元数是捷联惯导姿态解算的最佳选择，兼顾了计算效率、数值稳定性和无奇异性。

3.4 三种表示方法的相互转换

核心原理与转换公式

三种表示方法在数学上是等价的，可以相互转换。转换关系总结如下表：

转换方向 公式/方法 注意事项
欧拉角 → DCM C = R_x(φ)·R_y(θ)·R_z(ψ) 旋转顺序必须一致
DCM → 欧拉角 φ = atan2(C[2,1], C[2,2])
θ = asin(-C[2,0])
ψ = atan2(C[1,0], C[0,0])		 θ接近±90°时需特殊处理
欧拉角 → 四元数 见3.3节公式 注意半角计算
四元数 → 欧拉角 见3.3节公式 atan2和asin的象限判断
DCM → 四元数 q0 = 0.5·sqrt(1+C[0,0]+C[1,1]+C[2,2])
q1 = (C[2,1]-C[1,2])/(4·q0)
q2 = (C[0,2]-C[2,0])/(4·q0)
q3 = (C[1,0]-C[0,1])/(4·q0)		 当q0接近0时需改用其他公式
四元数 → DCM C[0,0]=1-2(q2²+q3²)
C[0,1]=2(q1q2-q0q3)
...		 确保四元数已归一化

转换中的数值稳定性问题

在DCM到四元数的转换中，当q0接近0时（即旋转角接近180°），上述公式会出现除零问题。此时应改用以下方法：

if (C[0,0] > C[1,1] && C[0,0] > C[2,2]) {
    q1 = 0.5·sqrt(1 + C[0,0] - C[1,1] - C[2,2]);
    q0 = (C[2,1] - C[1,2]) / (4·q1);
    q2 = (C[0,1] + C[1,0]) / (4·q1);
    q3 = (C[0,2] + C[2,0]) / (4·q1);
} else if (C[1,1] > C[2,2]) {
    // 类似处理
}

案例拆解：多源姿态数据融合中的转换

某组合导航系统需要融合陀螺仪、加速度计和磁力计数据。系统架构如下：

  • 陀螺仪输出用于四元数姿态预测
  • 加速度计和磁力计输出用于计算欧拉角形式的观测值
  • 卡尔曼滤波器在四元数域进行状态估计

在滤波器的观测更新步骤中，需要将欧拉角观测值转换为四元数，并与预测四元数进行比较。工程师发现，直接使用欧拉角观测值会导致滤波器性能下降，因为欧拉角的方差在不同姿态下变化很大。解决方案是：先将欧拉角转换为DCM，再转换为四元数，并在转换过程中考虑协方差矩阵的传播。这一设计使得滤波器在无人机大机动飞行时仍能保持稳定。

风险提示：在四元数到欧拉角的转换中，当θ接近±90°时，atan2的参数会同时趋近于0，导致数值不稳定。工程上应设置角度阈值（如±89.9°），超出范围时使用上一时刻的偏航角值。

常见误区与总结

  • 误区一：在转换过程中忽略四元数的归一化。未归一化的四元数会导致转换后的DCM不正交。
  • 误区二：在DCM到四元数转换中只使用单一公式。应根据矩阵元素的大小选择最稳定的计算路径。
  • 总结：三种表示方法的转换是姿态解算中的基础操作，必须熟练掌握并注意数值稳定性。在实际工程中，建议以四元数作为核心表示，仅在需要时转换为其他形式。

综上所述，欧拉角、DCM和四元数各有其适用场景。欧拉角直观但存在奇异性，适合人机交互和初始对准；DCM稳健但计算冗余，适合坐标变换和矩阵运算；四元数高效且无奇异，是捷联惯导姿态解算的核心表示。在实际系统中，通常采用四元数进行姿态更新，在需要时转换为欧拉角或DCM用于显示和外部接口。掌握三者的优缺点和转换关系，是构建高性能捷联惯导系统的关键基础。

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