3. 姿态解算入门：欧拉角与四元数、互补滤波原理、Mahony滤波算法

姿态解算是飞控的“感知层”。说白了，就是回答三个问题：飞机现在头朝哪？机身歪了多少？是在爬升还是俯冲？

我刚开始做飞控时，觉得姿态解算不就是算几个角度嘛，有什么难的？结果第一次试飞，飞机在天上疯狂自旋，差点炸机。后来才发现，是姿态解算的算法选错了，角度漂移得一塌糊涂。

这一章，我们就来把姿态解算的底子打扎实。我会从最直观的欧拉角讲起，再引出四元数，最后带你手撕互补滤波和Mahony滤波算法。

3.1 欧拉角：最直观的姿态表示

欧拉角用三个角度来描述刚体在三维空间中的朝向：横滚角(Roll, φ)、俯仰角(Pitch, θ)、偏航角(Yaw, ψ)。你想想看，这其实就是把一次旋转拆成了绕三个轴依次旋转。

我个人习惯用Z-Y-X顺序（先偏航、再俯仰、最后横滚），这也是飞控领域最常用的约定。对应的旋转矩阵长这样：

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

但欧拉角有个致命问题——万向锁(Gimbal Lock)。当俯仰角接近±90°时，横滚和偏航会失去一个自由度。我在项目中遇到过一架固定翼在做筋斗动作时，姿态解算突然跳变，就是因为触发了万向锁。

3.2 四元数：无奇点的优雅方案

四元数是什么？说白了就是一个超复数：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。它用四个数来表示三维空间中的旋转，没有万向锁问题。

四元数最大的好处是：可以平滑插值。你想想看，如果无人机要从姿态A转到姿态B，用欧拉角插值可能会绕远路，但四元数的球面线性插值(SLERP)能走最短路径。

四元数和欧拉角的转换公式，我建议你直接背下来：

// 四元数 → 欧拉角
roll  = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw   = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))

3.3 互补滤波原理：为什么需要融合？

陀螺仪测角速度，积分得到角度。优点是高频响应好，但缺点是有零漂，长时间积分会飘走。

加速度计和磁力计可以直接测出绝对角度。优点是低频稳定，但缺点是噪声大，受振动影响严重。

互补滤波的思路很简单：陀螺仪负责短期，加速度计/磁力计负责长期。用高通滤波器处理陀螺仪信号，用低通滤波器处理加速度计信号，然后加起来。

公式长这样：

角度 = α * (角度 + 陀螺仪角速度 * dt) + (1 - α) * 加速度计角度

其中α是滤波系数，一般取0.98左右。α越大，陀螺仪权重越高，响应快但可能漂移；α越小，加速度计权重越高，抗振性好但响应慢。

3.4 Mahony滤波算法：实战中的首选

Mahony滤波是互补滤波的进阶版。它不直接融合角度，而是通过误差修正的方式来调整陀螺仪的零偏。

核心思想是：用加速度计和磁力计的测量值，计算出一个“期望的”重力方向/地磁方向，然后和当前四元数推算出的方向做叉积，得到误差。这个误差经过PI控制器后，用来修正陀螺仪的角速度。

代码实现大概长这样：

// Mahony滤波核心步骤
// 1. 从四元数计算重力方向
gx = 2*(q1*q3 - q0*q2);
gy = 2*(q0*q1 + q2*q3);
gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// 2. 计算误差（加速度计测量值与重力方向的叉积）
ex = ay*gz - az*gy;
ey = az*gx - ax*gz;
ez = ax*gy - ay*gx;

// 3. PI控制器修正陀螺仪
gx_corrected = gx_gyro + Kp*ex + Ki*ex_int;
gy_corrected = gy_gyro + Kp*ey + Ki*ey_int;
gz_corrected = gz_gyro + Kp*ez + Ki*ez_int;

// 4. 用修正后的角速度更新四元数
// （四元数微分方程）

为什么我推荐Mahony？因为它在计算量小的情况下，能达到接近卡尔曼滤波的效果。在STM32F4这类MCU上，跑Mahony只需要几十微秒，而卡尔曼滤波可能要几百微秒。

3.5 三种方法的对比与选择

我个人在大多数项目中都用Mahony。只有在做高精度测绘无人机时，才会上卡尔曼滤波。因为Mahony的代码量少、调试简单，而且效果已经足够好。