4、姿态表示方法：欧拉角、旋转矩阵、四元数、轴角之间的转换关系与优缺点对比

在嵌入式飞控算法中，姿态解算是一切控制与导航决策的基础。飞控系统需要实时获取机体相对于导航坐标系（通常为NED系，即北东地系）的角位置关系，并将该关系用于加速度分解、速度积分、控制量分配以及任务规划。当前工程实践中，姿态的数学表示主要有四种：欧拉角、旋转矩阵（方向余弦矩阵，DCM）、四元数和轴角（旋转向量）。这四种表示方法在数学上等价，但在计算效率、奇异性、内存占用、插值特性以及工程可读性上存在显著差异。本章将深入剖析四种表示方法的数学本质、相互转换公式、工程选型依据，并结合飞控代码中的典型应用场景进行拆解。

上图展示了四种姿态表示方法在飞控系统中的核心地位与相互转换关系。在后续内容中，我们将逐一剖析每种方法的数学定义、转换公式、工程代价，并通过一个完整的姿态融合与指令转换案例，展示如何在实际代码中高效切换这四种表示。

4.1 核心原理：四种姿态表示的数学本质

欧拉角是三个独立的角度，通常定义为绕Z轴（偏航ψ）、Y轴（俯仰θ）、X轴（横滚φ）的依次旋转。其物理意义极其直观，飞手遥控器给出的油门、俯仰、横滚指令直接对应欧拉角。然而，欧拉角的核心缺陷在于万向锁（Gimbal Lock）现象：当俯仰角接近±90°时，横滚与偏航的旋转轴重合，导致一个自由度丢失，姿态解算出现奇异。

旋转矩阵（DCM）是一个3×3的正交矩阵，其列向量表示机体坐标系的三个轴在导航坐标系中的投影。DCM无奇异问题，且可以直接用于向量坐标变换，例如将机体加速度计读数转换到NED系。但其包含9个元素，存在6个约束条件（正交性），在迭代更新中需要频繁正交化修正，计算量较大。

四元数是一个四维超复数，定义为 q = q0 + q1·i + q2·j + q3·k，其中q0为标量部分，表示旋转角的一半余弦，向量部分表示旋转轴方向与半角正弦的乘积。四元数仅需4个参数，无奇异，且通过简单的乘法即可实现旋转合成，是卡尔曼滤波、互补滤波中最常用的姿态表示。

轴角（旋转向量）用一个单位向量e表示旋转轴，一个标量θ表示旋转角度。它是李代数so(3)的物理映射，在姿态误差计算中非常有用。轴角与四元数的转换关系最为直接，常用于飞控中姿态误差的提取。

4.2 转换关系与操作流程

四种表示方法之间的转换是飞控代码中的高频操作。以下给出最核心的转换公式与工程实现要点。

4.2.1 欧拉角 → 四元数

设横滚、俯仰、偏航角分别为φ、θ、ψ，采用Z-Y-X旋转顺序（内旋），转换公式为：

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

在嵌入式实现时，应使用半角公式的泰勒展开近似以降低三角函数开销，或直接查表。

4.2.2 四元数 → 旋转矩阵

给定归一化四元数 q = (q0, q1, q2, q3)，对应的DCM为：

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²)]]

该矩阵可直接用于向量变换，例如将机体坐标系下的加速度 a_b 转换到导航系 a_n = R · a_b。

4.2.3 四元数 → 欧拉角

从四元数提取欧拉角时，需注意万向锁判断。标准公式为：

φ = atan2( 2(q0q1 + q2q3), 1 - 2(q1² + q2²) )
θ = asin( 2(q0q2 - q3q1) )
ψ = atan2( 2(q0q3 + q1q2), 1 - 2(q2² + q3²) )

当 θ 接近 ±90° 时，atan2 的参数趋于零，此时应直接根据 q0 与 q2 的符号设定 φ 与 ψ 的差值。

4.2.4 轴角与四元数互转

轴角 (e, θ) 转四元数：

q0 = cos(θ/2)
q1 = e_x · sin(θ/2)
q2 = e_y · sin(θ/2)
q3 = e_z · sin(θ/2)

四元数转轴角时，θ = 2·acos(q0)，e = (q1, q2, q3) / sin(θ/2)。当 θ 接近 0 时，应使用线性近似避免除零。

转换方向	核心公式	计算量	工程注意点
欧拉角→四元数	半角余弦/正弦乘积	3次cos+3次sin	注意旋转顺序需一致
四元数→DCM	9元素二次型	约20次乘法	需先归一化四元数
DCM→欧拉角	atan2/asin	2次atan2+1次asin	俯仰角±90°时奇异
轴角→四元数	半角公式	1次cos+1次sin	轴向量需单位化

4.3 案例拆解：飞控中姿态融合与指令转换的工程实现

以某四旋翼飞控为例，其姿态控制周期为 1kHz，使用互补滤波进行姿态融合。初始状态时，飞控通过加速度计和磁力计计算初始欧拉角，然后转换为四元数作为滤波器初值。在运行过程中，陀螺仪积分更新四元数，加速度计与磁力计数据用于修正漂移。

关键场景一：当飞手通过遥控器给出横滚角 30°、俯仰角 -20°、偏航角 45° 的目标姿态时，飞控需要将欧拉角指令转换为四元数目标值，并与当前四元数做误差计算。该误差通过轴角形式提取，即计算 q_error = q_target ⊗ q_current⁻¹，然后取 q_error 的向量部分乘以 2 得到旋转向量，作为PID控制器的输入。

关键场景二：在降落阶段，飞控需要将机体加速度转换到NED系以判断垂直速度。此时使用四元数生成的DCM矩阵，将加速度计读数 a_b 左乘 R 得到 a_n。若直接使用欧拉角进行坐标变换，在俯仰角较大时会出现明显的非线性误差。

实际操作步骤：

1. 初始化：读取加速度计与磁力计，计算初始欧拉角，调用 euler_to_quat() 生成 q_init。
2. 姿态更新：每 1ms 读取陀螺仪角速度，使用一阶龙格库塔积分更新四元数：q_new = q_old ⊗ Δq，其中 Δq 由角速度与周期计算。
3. 误差修正：将加速度计测量的重力方向与四元数预测的重力方向做叉积，得到修正向量，以比例系数叠加到陀螺仪角速度上。
4. 指令转换：接收遥控器欧拉角指令，转换为四元数 q_cmd，计算误差旋转向量 e_err = 2 · (q_cmd ⊗ q_cur⁻¹).vec。
5. 控制输出：将 e_err 送入角度环PID，输出期望角速度，再经角速度环PID得到电机指令。

结果对比：若全程使用欧拉角进行姿态更新，在俯仰角超过 60° 时，解算结果发散；而使用四元数后，在 90° 俯仰下仍能保持稳定。同时，四元数融合的代码量比DCM减少约 30%，且无需频繁正交化。

4.4 常见误区与总结复盘

误区一：认为欧拉角旋转顺序无关紧要。实际上，Z-Y-X与Z-X-Y顺序得到的姿态完全不同，转换公式必须严格匹配飞控定义的旋转顺序。

误区二：忽略四元数的归一化。四元数在连续乘法后，由于浮点误差会逐渐失去单位模长，导致旋转矩阵非正交。工程上应每 100 次迭代或每 10ms 进行一次归一化。

误区三：在万向锁附近直接使用欧拉角做控制。当俯仰角接近 ±90° 时，横滚与偏航通道耦合严重，应切换至四元数或DCM进行控制律设计。

误区四：轴角与旋转向量混淆。轴角是旋转轴与角度的组合，而旋转向量是轴角乘以角度后的三维向量，两者在数值上相差一个角度因子，使用时需明确区分。

总结：四种姿态表示各有优劣，飞控工程师应掌握其数学本质与转换细节。在实时性要求高的控制内核中，使用四元数；在任务规划与地面站显示中，使用欧拉角；在坐标变换与多传感器融合中，使用DCM；在误差提取与优化求解中，使用轴角。理解这些表示方法的适用边界，是编写鲁棒飞控代码的前提。