4、姿态解算进阶：互补滤波、Mahony滤波、EKF（扩展卡尔曼滤波）原理与实现

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

上一章我们聊了最基础的姿态解算，用加速度计和磁力计直接算欧拉角。说实话，那种方法在静态或者慢速运动时还行，一旦无人机开始剧烈机动，数据就完全没法看了。为什么？因为加速度计测的是「比力」，它分不清重力加速度和运动加速度。你想想看，飞机一个急转弯，加速度计输出的数据里既有重力又有向心力，你拿这个去算俯仰和横滚，不翻车才怪。

所以，这一章我们来点真格的。我会带你手撕三种主流的姿态解算算法：互补滤波、Mahony滤波，还有EKF。这三种算法，说白了都是在做同一件事——把多个传感器的优点结合起来，互相弥补对方的短板。

4.1 为什么需要融合？——传感器的「性格」差异

在深入算法之前，我们先聊聊传感器的脾气。

  • 加速度计 + 磁力计：低频特性好，长期稳定，但高频噪声大，动态响应慢。你拿着手机快速晃动，加速度计的输出会乱跳。
  • 陀螺仪：高频特性好，动态响应快，但有零偏漂移。你把它放桌上一动不动，几分钟后它输出的角度可能已经偏了好几度。

你看，这两个传感器正好互补。陀螺仪擅长短时间内的快速变化，加速度计和磁力计擅长长时间内的稳定参考。融合算法的核心，就是用陀螺仪做「主角」来推算姿态，用加速度计和磁力计做「裁判」来修正陀螺仪的漂移。

核心思想：高频段信任陀螺仪，低频段信任加速度计/磁力计。这就是互补滤波名字的由来——在频域上互补。

4.2 互补滤波——最简单实用的入门算法

互补滤波的原理非常直观。我直接给你公式：

角度 = α × (角度 + 陀螺仪积分) + (1 - α) × 加速度计角度

这里的 α 是一个介于0到1之间的系数。α 越大，越信任陀螺仪；α 越小，越信任加速度计。α 通常取0.98左右，具体值取决于你的采样频率和运动剧烈程度。

我在项目中遇到过一个问题：α 取0.98时，地面静态测试效果很好，但一飞起来，姿态就开始滞后。后来我意识到，飞行时的振动会让加速度计噪声变大，应该适当提高 α 值。嗯，这里要注意，α 不是一成不变的，要根据实际工况动态调整。

我的经验：α 的计算公式可以写成 α = τ / (τ + dt)，其中 τ 是时间常数（通常0.1~0.5秒），dt 是采样周期。这样你改采样频率时，只需要调 τ 就行了。

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

// 互补滤波单轴实现（以横滚角为例）
float complementary_filter(float gyro_rate, float acc_angle, float dt) {
    static float angle = 0.0f;
    float alpha = 0.98f;  // 可调参数
    
    // 陀螺仪积分
    float gyro_angle = angle + gyro_rate * dt;
    // 融合
    angle = alpha * gyro_angle + (1.0f - alpha) * acc_angle;
    
    return angle;
}

但是，互补滤波有个明显的缺点：它只能处理单轴或者简单的欧拉角融合。当飞机做大角度机动时，欧拉角会出现万向锁问题，这时候互补滤波就力不从心了。

4.3 Mahony滤波——四元数版本的互补滤波

Mahony滤波，说白了就是互补滤波的四元数版本。它用四元数来表示姿态，完美避开了万向锁问题。

Mahony滤波的核心思想是：用加速度计和磁力计的测量值来修正陀螺仪的偏差。具体来说，它通过一个PI控制器来估计陀螺仪的零偏，然后补偿到陀螺仪的输出上。

我给你们画个流程图（用文字描述）：

  1. 用陀螺仪数据更新四元数（预测步骤）
  2. 用当前四元数计算理论上的重力向量和磁场向量
  3. 用加速度计和磁力计的实际测量值与理论值做叉积，得到误差
  4. 误差经过PI控制器，得到陀螺仪零偏的估计值
  5. 用这个估计值修正陀螺仪数据，然后回到第1步

你看，这个流程里最关键的就是那个PI控制器。比例项（Kp）决定了修正的速度，积分项（Ki）负责消除稳态误差。Kp越大，修正越快，但容易引入高频噪声；Ki越大，对陀螺仪零偏的估计越准确，但响应会变慢。

我曾经踩过的坑：有一次我把Ki设得太大，结果飞机悬停时姿态一直在低频振荡。后来查了半天才发现，是积分项饱和了。解决办法很简单——给积分项加一个限幅。

Mahony滤波的代码实现比互补滤波复杂一些，但网上有很多成熟的开源实现。我建议你直接拿PX4或者ArduPilot里的代码来改，别自己从头写。为什么？因为四元数更新、叉积计算这些细节，稍不注意就会出错。

// 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 vx = 2*(q1*q3 - q0*q2);
    float vy = 2*(q0*q1 + q2*q3);
    float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
    // 2. 计算误差（叉积）
    float ex = ay*vz - az*vy;
    float ey = az*vx - ax*vz;
    float ez = ax*vy - ay*vx;
    
    // 3. PI控制器
    integral_x += Ki * ex * dt;
    integral_y += Ki * ey * dt;
    integral_z += Ki * ez * dt;
    
    // 4. 修正陀螺仪
    gx += Kp*ex + integral_x;
    gy += Kp*ey + integral_y;
    gz += Kp*ez + integral_z;
    
    // 5. 更新四元数（一阶龙格库塔）
    q0 += 0.5f * (-q1*gx - q2*gy - q3*gz) * dt;
    q1 += 0.5f * ( q0*gx + q2*gz - q3*gy) * dt;
    q2 += 0.5f * ( q0*gy - q1*gz + q3*gx) * dt;
    q3 += 0.5f * ( q0*gz + q1*gy - q2*gx) * dt;
    
    // 归一化
    float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}

Mahony滤波的优点是计算量小，适合在STM32这类MCU上跑。我做过测试，在STM32F405上跑Mahony滤波，只需要几十微秒，比EKF快了一个数量级。

4.4 EKF——精度最高的选择

如果你追求极致的精度，那就得上EKF了。EKF（扩展卡尔曼滤波）是卡尔曼滤波在非线性系统上的推广。它把姿态解算建模成一个状态估计问题，用概率论的方法来融合多传感器数据。

EKF的流程分为两步：预测和更新。

  • 预测：用陀螺仪数据预测下一时刻的状态（四元数）和协方差矩阵。
  • 更新：用加速度计和磁力计的测量值来修正预测结果。

EKF的数学推导比较复杂，涉及到雅可比矩阵、协方差传播等概念。我不打算在这里展开讲公式，因为那会劝退很多人。我直接给你们看一个关键结论：

EKF vs Mahony：在剧烈机动下，EKF的精度比Mahony高20%~30%。但代价是计算量大了10倍以上，而且调参非常痛苦。

我记得第一次调EKF时，光协方差矩阵的初值就折腾了两天。Q矩阵（过程噪声协方差）和R矩阵（测量噪声协方差）的取值，直接决定了滤波器的收敛速度和稳定性。Q设大了，滤波器响应快但噪声大；Q设小了，响应慢但平滑。R也是类似的道理。

这里我给你们一个调参的参考值（以四元数状态为例）：

参数 典型值 说明
Q (过程噪声) diag(0.001, 0.001, 0.001, 0.001) 四元数过程噪声，越大越信任测量值
R_acc (加速度计噪声) diag(0.1, 0.1, 0.1) 加速度计测量噪声，越大越信任预测值
R_mag (磁力计噪声) diag(0.5, 0.5, 0.5) 磁力计测量噪声，通常比加速度计大

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

// EKF预测步骤（简化）
void ekf_predict(float gx, float gy, float gz, float dt) {
    // 状态转移矩阵 F
    // 计算雅可比矩阵
    // 更新状态：x = f(x, u)
    // 更新协方差：P = F * P * F^T + Q
}

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

说实话，EKF的代码我建议你直接用现成的库，比如PX4的ecl库或者ROS的robot_localization包。自己手写EKF，调试起来太痛苦了。我当年为了验证一个雅可比矩阵的正确性，花了整整一周时间。

4.5 三种算法的对比与选型建议

最后，我给你们一个选型指南：

算法 计算量 精度 调参难度 适用场景
互补滤波 极低 低 简单 玩具级无人机、平衡车
Mahony滤波 低 中 中等 大多数消费级无人机
EKF 高 高 困难 工业级、测绘级无人机

我个人习惯是：原型验证用Mahony，产品量产用EKF。Mahony滤波代码简单，跑得快，足够应付大多数场景。只有当你的无人机需要做高精度悬停、自主导航时，才值得上EKF。

一个小建议：如果你用的是STM32F4或F7系列，跑Mahony滤波绰绰有余。如果要用EKF，建议上F7/H7或者带FPU的芯片，否则CPU占用率会很高。

好了，这一章的内容就到这里。下一章我们会讲传感器校准——这是所有算法能正常工作的前提。你想想看，如果加速度计和磁力计的数据本身就不准，再好的融合算法也是白搭。我们下章见。

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