4、姿态表示与转换：欧拉角、四元数、旋转矩阵的相互转换，万向锁问题，四元数运算与插值

姿态描述是飞控算法的基础，无论是姿态解算、控制器设计还是导航解算，都离不开对刚体旋转的数学表达。欧拉角、旋转矩阵和四元数是三种最常用的姿态表示方法，它们各有优劣，适用于不同场景。本章将深入剖析三者的数学本质、相互转换关系、万向锁的成因与规避策略，以及四元数在工程中的核心运算与插值方法，帮助你在实际飞控开发中做出正确的数学工具选型。

上图展示了三种姿态表示方法之间的转换关系与核心应用场景。在飞控系统中，我们通常使用四元数作为姿态解算和控制律的内部状态，而将欧拉角仅用于地面站显示和用户交互。下面我们将逐一深入每个知识模块。

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

欧拉角通过三个依次绕固定轴（通常是机体坐标系的Z-Y-X轴）的旋转角度来描述姿态，即偏航角（Yaw, ψ）、俯仰角（Pitch, θ）和横滚角（Roll, φ）。其物理意义直观，但旋转顺序敏感，且存在万向锁问题。旋转矩阵是一个3×3的正交矩阵（满足 R^TR = I，det(R)=1），它完整描述了从一个坐标系到另一个坐标系的线性变换，无奇异点，但9个参数存在冗余，且连续旋转会导致数值误差累积，需要定期正交化。四元数是一个四维超复数 q = w + xi + yj + zk，其中 w 为标量部分，(x,y,z) 为矢量部分，满足约束 w²+x²+y²+z²=1。它用4个参数无奇异地表示三维旋转，且运算效率高，非常适合嵌入式实时计算。

4.2 判断方法与转换操作流程

在实际工程中，我们经常需要在三种表示之间切换。以下是标准转换流程：

1. 欧拉角 → 旋转矩阵：按 Z-Y-X 顺序，R = Rz(ψ)·Ry(θ)·Rx(φ)。其中 Rx(φ) = [[1,0,0],[0,cosφ,-sinφ],[0,sinφ,cosφ]]，Ry(θ) = [[cosθ,0,sinθ],[0,1,0],[-sinθ,0,cosθ]]，Rz(ψ) = [[cosψ,-sinψ,0],[sinψ,cosψ,0],[0,0,1]]。
2. 旋转矩阵 → 欧拉角：从矩阵元素提取，θ = asin(-R₃₁)，φ = atan2(R₃₂, R₃₃)，ψ = atan2(R₂₁, R₁₁)。注意当 θ = ±90° 时，atan2 的参数退化，需要特殊处理。
3. 欧拉角 → 四元数：将三个单轴四元数相乘，q = qz(ψ) ⊗ qy(θ) ⊗ qx(φ)。其中 qx(φ) = [cos(φ/2), sin(φ/2), 0, 0]，qy(θ) = [cos(θ/2), 0, sin(θ/2), 0]，qz(ψ) = [cos(ψ/2), 0, 0, sin(ψ/2)]。
4. 四元数 → 旋转矩阵：利用 Rodrigues 公式，R = I + 2s·[v]_× + 2[v]_ײ，其中 s 为标量，v 为矢量部分，[v]_× 为叉乘矩阵。
5. 旋转矩阵 → 四元数：通过矩阵迹和主对角元素求解，或使用 Shepperd 方法避免数值不稳定。

4.3 万向锁问题深度解析

万向锁发生在欧拉角表示中，当中间旋转轴（Pitch）旋转 ±90° 时，第一次旋转（Yaw）和第三次旋转（Roll）的旋转轴在空间上重合，导致系统丢失一个旋转自由度。此时，任何 Roll 和 Yaw 的组合都只能产生绕同一轴的旋转，无法独立控制三个轴向。在飞控中，如果姿态解算输出欧拉角，当飞行器垂直向上或向下（Pitch接近±90°）时，控制器会变得不稳定，甚至出现奇异值。

规避万向锁的根本方法是采用四元数或旋转矩阵。四元数通过四维单位球面上的点表示姿态，不存在奇异性。在飞控中，我们通常使用四元数进行姿态解算（如互补滤波、EKF），并将控制误差定义为四元数误差，再转换为旋转矢量用于角速度环控制。

4.4 四元数运算与插值实战

四元数运算包括乘法、共轭、归一化和旋转操作。乘法 q₁ ⊗ q₂ 表示先进行 q₂ 旋转再进行 q₁ 旋转，其公式为：

q1 ⊗ q2 = (w1w2 - x1x2 - y1y2 - z1z2,
            w1x2 + x1w2 + y1z2 - z1y2,
            w1y2 - x1z2 + y1w2 + z1x2,
            w1z2 + x1y2 - y1x2 + z1w2)

旋转一个三维向量 v 时，先将 v 构造为纯四元数 (0, v)，然后计算 v' = q ⊗ v ⊗ q*，其中 q* 为共轭四元数 (w, -x, -y, -z)。

四元数插值（Slerp）用于在两个姿态之间平滑过渡，公式为：

q(t) = q0 · (q0⁻¹ ⊗ q1)^t,  t ∈ [0,1]

其中 q0⁻¹ 为 q0 的逆（对于单位四元数，逆等于共轭）。实际实现时，通常使用以下等价形式：

Ω = acos(q0·q1)  // 点积
q(t) = (sin((1-t)Ω)/sinΩ) · q0 + (sin(tΩ)/sinΩ) · q1

注意：当 q0·q1 < 0 时，应取 q1 = -q1 以保证插值路径为最短弧线。

4.5 案例拆解：四元数在姿态解算中的完整闭环

背景与约束：某多旋翼飞控使用 BMI088 IMU，需要以 1kHz 频率进行姿态解算，MCU 为 STM32F405（168MHz）。要求姿态解算延迟小于 0.5ms，且在大机动（Pitch 达到 ±90°）时保持稳定。

初始现象：早期版本使用欧拉角 + 互补滤波，在普通飞行中表现正常。但在测试筋斗机动时，飞控输出异常，飞行器失控。日志显示，当 Pitch 接近 90° 时，Roll 和 Yaw 角度发生剧烈跳变，控制量饱和。

分析过程：通过回放飞行日志，发现欧拉角在奇异点附近出现数值振荡。进一步检查代码，确认互补滤波内部使用欧拉角进行积分和误差修正，导致在奇异点附近梯度计算错误。

实际操作步骤：

1. 将姿态状态量从欧拉角改为四元数，使用 Mahony 互补滤波算法，通过加速度计和磁力计修正四元数误差。
2. 将控制器输入从欧拉角误差改为四元数误差，并转换为旋转矢量：e = 2·sign(qe_w)·qe_vec，其中 qe = q_desired ⊗ q_current*。
3. 将地面站显示部分保留欧拉角转换，但仅在数据发送时进行，不参与控制计算。
4. 在代码中加入四元数归一化步骤，每 100 次解算循环执行一次，防止数值漂移。

结果对比：修改后，在相同筋斗机动测试中，姿态解算输出平滑，控制量无饱和，飞行器顺利完成机动。解算耗时从 0.35ms 降至 0.28ms，因为四元数运算避免了三角函数调用。

风险与代价：四元数调试难度略高，需要理解其数学含义。此外，四元数误差到旋转矢量的转换需要正确处理符号，否则会导致控制方向错误。

复盘经验：飞控内部状态应始终使用四元数，欧拉角仅作为人机交互接口。在姿态解算中，应避免使用欧拉角进行积分或微分运算。对于新手，建议直接使用成熟的四元数姿态解算库（如 Madgwick、Mahony），并理解其内部原理。

4.6 常见误区与总结复盘

• 误区一：认为欧拉角更直观，就在控制回路中使用。实际上，控制律需要的是姿态误差的线性化表示，四元数误差转换为旋转矢量后，可以直接映射到角速度环，物理意义清晰且无奇异。
• 误区二：忽略四元数的归一化。四元数必须保持单位模长，否则旋转矩阵不正交，导致姿态漂移。应在每次更新后或定期进行归一化。
• 误区三：在四元数插值时未处理双覆盖问题。q 和 -q 表示同一旋转，插值前应检查点积符号，确保插值路径为最短弧线。
• 误区四：旋转矩阵与四元数转换时未考虑数值稳定性。当旋转角度接近 180° 时，某些转换公式会出现除零问题，应使用 Shepperd 方法或基于迹的鲁棒算法。