3. 姿态解算算法(一)：Mahony互补滤波原理、代码实现与参数调优

各位飞手、调参老铁们，大家好。

今天咱们来啃一块硬骨头——Mahony互补滤波。说实话，我在竞赛圈混了这么多年，见过太多人一上来就上EKF（扩展卡尔曼滤波），结果调得死去活来。其实对于大多数竞速场景，Mahony滤波完全够用，而且计算量小、调参直观。我个人习惯，在飞控原型验证阶段，首选就是Mahony。

3.1 为什么需要姿态解算？

说白了，飞控得知道自己“脑袋朝哪”。加速度计告诉你重力方向，陀螺仪告诉你转得多快，但两者都有毛病：加速度计怕震动，陀螺仪会漂移。Mahony互补滤波就是干这个的——把两个传感器的优点结合起来，输出一个稳定、低延迟的姿态角。

3.2 Mahony滤波原理（一句话讲透）

嗯，这里要注意，Mahony本质上是一个基于误差的PI反馈系统。它不直接融合角度，而是融合四元数。

流程是这样的：

1. 用陀螺仪数据积分，得到一个预测姿态（四元数）。
2. 用加速度计测量值，算出当前“真实”的重力方向。
3. 计算预测方向与真实方向之间的误差。
4. 用PI控制器把这个误差反馈到陀螺仪积分上，修正漂移。

为什么会这样设计？因为陀螺仪积分是“开环”的，误差会累积。加速度计虽然不准，但长期均值是可信的。用PI控制器，就是让加速度计“慢慢拉”着陀螺仪走，既不会让姿态乱跳，又能消除漂移。

3.3 代码实现（核心部分）

我直接上干货。这是我在多个竞赛飞控上验证过的精简版Mahony实现。注意，这里只展示核心的更新函数。

// 定义全局变量
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 姿态四元数
float Kp = 2.0f;  // 比例增益
float Ki = 0.001f; // 积分增益
float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // 积分误差

void MahonyAHRSupdate(float gx, float gy, float gz,   // 陀螺仪，单位rad/s
                      float ax, float ay, float az) { // 加速度计，单位g

    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;

    // 1. 归一化加速度计数据
    recipNorm = invSqrt(ax*ax + ay*ay + az*az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // 2. 用当前四元数估计重力方向
    halfvx = q1*q3 - q0*q2;
    halfvy = q0*q1 + q2*q3;
    halfvz = q0*q0 - 0.5f + q3*q3;

    // 3. 计算误差 = 测量值 叉乘 估计值
    halfex = ay * halfvz - az * halfvy;
    halfey = az * halfvx - ax * halfvz;
    halfez = ax * halfvy - ay * halfvx;

    // 4. 积分误差（消除静态偏差）
    integralFBx += Ki * halfex * (1.0f / 500.0f); // 假设采样率500Hz
    integralFBy += Ki * halfey * (1.0f / 500.0f);
    integralFBz += Ki * halfez * (1.0f / 500.0f);

    // 5. 用PI控制器修正陀螺仪
    gx += Kp * halfex + integralFBx;
    gy += Kp * halfey + integralFBy;
    gz += Kp * halfez + integralFBz;

    // 6. 四元数更新（一阶龙格-库塔）
    q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * (1.0f / 500.0f);
    q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * (1.0f / 500.0f);
    q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * (1.0f / 500.0f);
    q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * (1.0f / 500.0f);

    // 7. 归一化四元数
    recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

3.4 参数调优（Kp和Ki的玄学）

调参是门手艺活。Kp和Ki决定了滤波器的“性格”。

参数	作用	调大后果	调小后果
Kp（比例）	加速度计校正的强度	姿态响应快，但易受震动干扰	姿态平滑，但跟踪慢，有滞后
Ki（积分）	消除陀螺仪零偏漂移	能消除稳态误差，但可能引起低频振荡	漂移无法完全消除，长时间飞行会偏

我个人习惯的调参步骤：

1. 先调Kp： 把Ki设为0，只调Kp。从0.5开始，慢慢往上加，直到姿态能快速响应，但不要出现高频抖动。我一般控制在1.0~3.0之间。
2. 再加Ki： 从0.0001开始，每次翻倍。观察长时间悬停时，姿态是否慢慢漂移。Ki太大时，你会看到姿态像“呼吸”一样缓慢摆动，那就是振荡了。
3. 联合微调： 在快速横滚、俯仰机动时，观察姿态是否“过冲”或“跟不上”。过冲就降Kp，跟不上就升Kp。

3.5 知识体系与核心逻辑

下面这张图，是我自己总结的Mahony滤波的完整数据流。你把它看懂了，调参就有方向了。

你看，整个流程就是一个闭环。陀螺仪积分是“主角”，加速度计是“裁判”，PI控制器是“教练”。教练根据裁判的反馈，不断调整主角的动作。

3.6 实战中的几个坑

• 采样率不一致： 陀螺仪和加速度计的采样率必须同步，否则误差计算会乱。我习惯用DMA + 定时器中断来保证。
• 归一化不能省： 加速度计数据如果不归一化，误差的幅值会随飞行姿态变化，导致Kp失效。
• 积分限幅： 给积分项加一个限幅，比如±0.5，防止积分饱和。这在剧烈机动时特别重要。

好了，Mahony互补滤波的核心就这些。你把它吃透了，后面学EKF会轻松很多。毕竟，滤波器的本质都是“预测+校正”，只是数学工具不同而已。