4、姿态解算：互补滤波、Mahony滤波、EKF原理与实现

各位同学，欢迎来到第四讲。

上一章我们聊了传感器数据怎么读、怎么预处理。但说实话，拿到加速度计、陀螺仪、磁力计的原始数据，离真正能用还差着十万八千里。为什么？因为每个传感器都有毛病——陀螺仪有零漂，加速度计怕震动，磁力计容易受干扰。

姿态解算，说白了就是把这些「各有缺陷」的传感器数据揉在一起，取长补短，算出一个靠谱的无人机姿态角。我个人习惯把姿态解算比作「三个臭皮匠顶个诸葛亮」——每个传感器都是臭皮匠，解算算法就是那个诸葛亮。

4.1 为什么需要姿态解算？

先想一个问题：你拿着一个IMU模块，静止放在桌上，加速度计读出来的是不是就是重力方向？理论上是的。但如果你轻轻敲一下桌子，加速度计数值立马就乱了。陀螺仪呢？短时间内积分出来的角度很准，但时间一长，零漂累积，角度就飘到天上去了。

所以，我们需要一个算法，把陀螺仪的短期精度和加速度计、磁力计的长期稳定性结合起来。这就是姿态解算的核心思想——传感器融合。

核心矛盾：

  • 陀螺仪：动态响应快，但有累积误差
  • 加速度计：无累积误差，但易受振动干扰
  • 磁力计：提供航向参考，但易受磁场干扰

我在项目里遇到过最典型的情况：一架无人直升机在悬停时，因为机臂振动传递到IMU，加速度计数据全是毛刺。如果只用加速度计解算，姿态角会跟着振动一起抖，飞控根本稳不住。后来换了互补滤波，把陀螺仪的高频响应和加速度计的低频校正结合起来，问题才解决。

4.2 互补滤波原理与实现

互补滤波是最简单、最直观的融合方法。它的思路很朴素：陀螺仪算出来的角度，我信它的高频部分；加速度计算出来的角度，我信它的低频部分。然后加起来。

数学上就是一句话：

angle = α * (angle + gyro * dt) + (1 - α) * acc_angle

其中α是一个0到1之间的系数，通常取0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。

嗯，这里要注意：α的取值很关键。取大了，陀螺仪零漂会慢慢累积；取小了，加速度计的噪声会直接串进来。我一般先在仿真里调，然后上真机试飞，看悬停时的角度波动幅度。

我的经验：互补滤波的α系数，建议根据陀螺仪和加速度计的噪声特性来定。如果陀螺仪质量好（比如ADIS16470），α可以取到0.995；如果陀螺仪一般（比如MPU6050），α取0.98左右比较稳妥。

互补滤波的代码实现非常简单，几行就能搞定：

// 互补滤波单轴实现（俯仰角为例）
float complementary_filter(float acc_pitch, float gyro_pitch_rate, float dt) {
    static float pitch = 0.0f;
    const float alpha = 0.98f;
    
    // 陀螺仪积分
    float gyro_pitch = pitch + gyro_pitch_rate * dt;
    // 融合加速度计
    pitch = alpha * gyro_pitch + (1.0f - alpha) * acc_pitch;
    
    return pitch;
}

你看，就这么简单。但互补滤波有个致命弱点——它只能处理单轴角度，而且对加速度计的噪声很敏感。说白了，它只适合用在振动小、运动平缓的场景。如果你要做特技飞行或者抗振要求高的机型，互补滤波就不太够用了。

4.3 Mahony滤波：从互补到互补+PI

Mahony滤波是互补滤波的升级版。它引入了一个PI控制器，用加速度计和磁力计的误差来修正陀螺仪的零漂。我当年第一次看到Mahony的论文时，觉得这哥们儿真聪明——把姿态解算问题转化成了伺服跟踪问题。

Mahony的核心思想是：

  1. 用当前姿态预测出重力方向（和磁北方向）
  2. 用加速度计和磁力计的实际测量值算出误差
  3. 用PI控制器把这个误差反馈到陀螺仪角速度上
  4. 用修正后的角速度更新四元数

说白了，就是让陀螺仪「跟着」加速度计和磁力计走，但又不会跟得太紧，以免被噪声带偏。

代码实现稍微复杂一点，但核心也就几十行：

// Mahony滤波核心步骤（简化版）
void mahony_update(float gx, float gy, float gz, 
                   float ax, float ay, float az,
                   float mx, float my, float mz, float dt) {
    float q0, q1, q2, q3;  // 四元数
    
    // 1. 用当前四元数计算重力方向
    float norm = inv_sqrt(ax*ax + ay*ay + az*az);
    ax *= norm; ay *= norm; az *= norm;
    
    // 2. 计算误差（叉积）
    float vx = 2*(q1*q3 - q0*q2);
    float vy = 2*(q0*q1 + q2*q3);
    float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
    float ex = ay*vz - az*vy;
    float ey = az*vx - ax*vz;
    float ez = ax*vy - ay*vx;
    
    // 3. PI控制器修正陀螺仪
    ex_int += ex * Ki * dt;
    ey_int += ey * Ki * dt;
    ez_int += ez * Ki * dt;
    
    gx += Kp*ex + ex_int;
    gy += Kp*ey + ey_int;
    gz += Kp*ez + ez_int;
    
    // 4. 用修正后的角速度更新四元数
    // ...（四元数微分方程）
}

我曾经踩过的坑：Mahony滤波的PI参数（Kp和Ki）调起来很玄学。Kp太大，姿态会跟着加速度计噪声一起抖；Kp太小，陀螺仪零漂修正不过来。我的建议是：先设Ki=0，只调Kp，等姿态能跟上快速运动了，再慢慢加Ki来消除稳态误差。

Mahony滤波在实际工程中用得非常广。PX4、ArduPilot这些开源飞控里，默认的姿态解算就是Mahony。为什么？因为它计算量小、效果好、参数好调。对于大多数无人直升机应用场景，Mahony完全够用。

4.4 EKF：当精度要求更高时

EKF（扩展卡尔曼滤波）是姿态解算里的「重武器」。它把姿态、陀螺仪零漂、甚至加速度计偏置都作为状态变量，用完整的非线性模型来估计。

说实话，EKF的原理比互补滤波和Mahony复杂得多。它涉及状态预测、协方差更新、卡尔曼增益计算、观测更新等一系列步骤。但它的优势也很明显：

  • 能同时估计多个状态（姿态、零漂、偏置）
  • 能处理非线性系统（四元数运动学就是非线性的）
  • 能融合多种传感器（GPS、气压计、视觉都可以加进来）

EKF的状态向量通常长这样：

状态向量 x = [q0, q1, q2, q3, bgx, bgy, bgz, bax, bay, baz]^T
其中：
  q0~q3: 姿态四元数
  bg: 陀螺仪零漂
  ba: 加速度计偏置

观测方程则用加速度计和磁力计的测量值来更新：

观测向量 z = [ax, ay, az, mx, my, mz]^T

EKF的代码实现比较长，这里只展示核心的预测和更新步骤：

// EKF预测步骤（简化）
void ekf_predict(float gx, float gy, float gz, float dt) {
    // 状态预测：四元数运动学
    // 协方差预测：P = F * P * F^T + Q
}

// EKF更新步骤（简化）
void ekf_update(float ax, float ay, float az, 
                float mx, float my, float mz) {
    // 计算卡尔曼增益：K = P * H^T * (H * P * H^T + R)^(-1)
    // 状态更新：x = x + K * (z - h(x))
    // 协方差更新：P = (I - K * H) * P
}

什么时候该用EKF？

  • 你的传感器噪声模型比较清楚（知道协方差矩阵Q和R）
  • 你需要高精度的姿态估计（比如自主降落、编队飞行）
  • 你的计算资源足够（STM32F4以上级别）
  • 你愿意花时间调参和调试

我在做一款工业级无人直升机时，用过EKF。那款飞机要求悬停精度在10厘米以内，Mahony滤波在强风干扰下会有几度的姿态误差，导致位置漂移。换成EKF后，姿态误差降到了0.5度以内，悬停精度明显提升。但代价是——调参调了整整两周。

4.5 三种算法的对比与选型

为了让你更直观地理解，我做了一张对比表：

特性 互补滤波 Mahony滤波 EKF
计算量 极低 低 高
精度 一般 良好 优秀
抗振能力 弱 中等 强
参数调优难度 简单 中等 复杂
适用场景 玩具、教学 大多数飞控 高精度、工业级

我的建议是：初学者先从互补滤波入手，理解传感器融合的基本思想。然后切换到Mahony，这是工程中最实用的方案。等你对姿态解算有了深入理解，再考虑EKF——它虽然复杂，但能解决很多Mahony解决不了的问题。

4.6 本章知识体系

下面这张图展示了本章的核心逻辑：

姿态解算知识体系 传感器输入 陀螺仪 加速度计 磁力计 姿态解算算法 互补滤波 Mahony滤波 EKF 姿态输出 横滚角 俯仰角 偏航角 选型建议 • 玩具/教学：互补滤波 • 大多数飞控：Mahony滤波 • 高精度/工业级：EKF

从这张图可以看得很清楚：传感器数据进来后，经过不同的算法处理，最终输出三个姿态角。选哪种算法，取决于你的应用场景和计算资源。

4.7 实战建议

最后，给你几条实战建议：

  1. 先从互补滤波开始——写代码、跑仿真、理解原理。这一步花不了多少时间，但能帮你建立直觉。
  2. 然后切换到Mahony——把互补滤波的代码改成Mahony，调一下Kp和Ki，看看效果。你会发现Mahony的抗干扰能力明显强于互补滤波。
  3. 如果还不够，再上EKF——EKF的代码量大概是Mahony的5倍，调试时间可能是10倍。但如果你需要厘米级的悬停精度，EKF是绕不开的。

一个小技巧：不管用哪种算法，一定要先做传感器校准。加速度计和磁力计的零偏、比例因子、非正交误差，这些不校准好，再好的算法也白搭。我见过太多人花几周调EKF参数，结果发现是加速度计没校准——白白浪费时间。

好了，这一讲就到这里。姿态解算是飞控的核心，也是很多问题的根源。把这一章吃透了，后面的控制、导航、航线规划，你都会觉得轻松很多。

公众号:蓝海资料掘金营，微信deep3321

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