4、传感器数据融合：互补滤波原理与代码实现

飞控里有个经典问题：陀螺仪短时间准，但长时间飘；加速度计短时间吵，但长时间稳。你想想看，一个怕积分，一个怕振动。那怎么办？把它们俩“互补”一下。

这就是互补滤波的核心思路。说白了，就是让陀螺仪负责高频姿态变化，加速度计负责低频修正。我最早接触这个算法是在大学做四轴飞行器的时候，当时觉得卡尔曼滤波太复杂，互补滤波刚好够用，而且调起来很直观。

4.1 为什么需要互补滤波？

先看两个传感器的“性格”：

传感器	优点	缺点
陀螺仪	动态响应快，高频噪声小	有零偏漂移，积分误差随时间累积
加速度计	长期稳定，能测重力方向	对振动敏感，高频噪声大

陀螺仪输出角速度，积分得到角度。但积分会累积误差——哪怕零点几度的零偏，几秒后就能偏出好几度。加速度计呢？它直接测重力在三个轴上的分量，能算出俯仰和横滚角，但一有振动，数据就跳得厉害。

互补滤波的思路就是：用陀螺仪做短期的姿态预测，用加速度计做长期的姿态修正。两者通过一个权重系数融合，各取所长。

核心公式（一维简化版）：

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

其中 α 是互补系数，通常取 0.98 ~ 0.995。

α 越接近 1，陀螺仪权重越大，系统响应快但长期漂移大；α 越小，加速度计修正越强，系统更稳定但动态响应变慢。这个平衡点，就是调参的关键。

4.2 互补滤波的完整流程

实际工程中，我们处理的是三维姿态。下面这张图展示了完整的融合流程：

4.3 代码实现（C语言，STM32平台）

下面是我在Pixhawk原型机上验证过的互补滤波代码。注意，这里用了四元数来避免万向锁问题。

// 互补滤波结构体
typedef struct {
    float q0, q1, q2, q3;  // 姿态四元数
    float ex, ey, ez;      // 误差积分项
    float kp, ki;          // PI 系数
    float dt;              // 采样周期
} ComplementaryFilter_t;

// 初始化
void CF_Init(ComplementaryFilter_t *cf, float kp, float ki, float dt) {
    cf->q0 = 1.0f;
    cf->q1 = 0.0f;
    cf->q2 = 0.0f;
    cf->q3 = 0.0f;
    cf->ex = cf->ey = cf->ez = 0.0f;
    cf->kp = kp;
    cf->ki = ki;
    cf->dt = dt;
}

// 互补滤波更新（核心函数）
void CF_Update(ComplementaryFilter_t *cf,
               float gx, float gy, float gz,  // 陀螺仪，单位 rad/s
               float ax, float ay, float az)  // 加速度计，单位 m/s²
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    // 1. 加速度计归一化
    norm = sqrt(ax*ax + ay*ay + az*az);
    if (norm < 0.001f) return;  // 防止除零
    ax /= norm;
    ay /= norm;
    az /= norm;

    // 2. 从四元数估计重力方向
    vx = 2.0f * (cf->q1*cf->q3 - cf->q0*cf->q2);
    vy = 2.0f * (cf->q0*cf->q1 + cf->q2*cf->q3);
    vz = cf->q0*cf->q0 - cf->q1*cf->q1 - cf->q2*cf->q2 + cf->q3*cf->q3;

    // 3. 计算误差（叉积）
    ex = ay*vz - az*vy;
    ey = az*vx - ax*vz;
    ez = ax*vy - ay*vx;

    // 4. 误差积分（消除稳态误差）
    cf->ex += ex * cf->ki * cf->dt;
    cf->ey += ey * cf->ki * cf->dt;
    cf->ez += ez * cf->ki * cf->dt;

    // 5. 修正陀螺仪角速度
    gx += cf->kp * ex + cf->ex;
    gy += cf->kp * ey + cf->ey;
    gz += cf->kp * ez + cf->ez;

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

    // 7. 归一化四元数
    norm = sqrt(cf->q0*cf->q0 + cf->q1*cf->q1 + cf->q2*cf->q2 + cf->q3*cf->q3);
    cf->q0 /= norm;
    cf->q1 /= norm;
    cf->q2 /= norm;
    cf->q3 /= norm;
}

调参经验：

kp（比例系数）：控制加速度计修正的强度。我一般从 0.5 开始试，如果姿态响应太慢就加大，如果高频抖动就减小。
ki（积分系数）：消除陀螺仪零偏漂移。通常设 kp 的 1/10 ~ 1/100，太大容易引起低频振荡。
dt：采样周期必须准确。我踩过坑——用定时器中断时忘了考虑中断执行时间，结果 dt 偏大，姿态发散。

4.4 避坑指南

我在实际项目中遇到过不少问题，挑几个典型的说说：

我曾经踩过的坑：

加速度计未归一化直接参与叉积运算——导致修正量随飞行器倾斜角度变化，姿态发散。一定要先归一化。
四元数更新后忘记归一化——四元数会逐渐偏离单位长度，导致姿态畸变。每次更新后必须归一化。
陀螺仪零偏未校准——静止时陀螺仪输出不为零，积分后姿态一直漂。上电后先采集 100 组数据取平均，减去零偏。
dt 不准确——用延时函数或阻塞式循环会导致 dt 抖动，姿态忽快忽慢。建议用硬件定时器或 RTOS 的精确延时。

4.5 互补系数 α 的选取

前面提到 α 是关键参数。怎么选？我一般用这个公式：

α = τ / (τ + dt)

其中 τ 是时间常数，单位秒。τ 越大，陀螺仪信任时间越长，系统越“相信”陀螺仪；τ 越小，加速度计修正越强。

应用场景	τ 推荐值	α（dt=0.004s 时）	特点
竞速穿越机	0.5 ~ 1.0	0.992 ~ 0.996	响应快，适合剧烈机动
航拍无人机	1.0 ~ 2.0	0.996 ~ 0.998	平滑稳定，抗振动
自稳模式	0.3 ~ 0.5	0.987 ~ 0.992	加速度计修正强，悬停稳

嗯，这里要注意：α 不是越大越好。我见过有人设 α=0.999，结果飞行器悬停时姿态缓慢漂移，因为加速度计几乎不起作用了。反过来，α 太小，飞控会跟着振动乱跳。

4.6 验证方法

代码写完了，怎么验证对不对？我习惯做两个测试：

静态测试：把飞控放在水平桌面上，观察姿态角输出。理想情况是俯仰和横滚在 ±0.5° 以内波动。如果漂移超过 1°，检查陀螺仪零偏或 ki 是否太小。
动态测试：用手快速翻转飞控 90°，看姿态角能否快速跟上。如果滞后明显，加大 kp 或减小 τ。如果超调振荡，减小 kp。

我的个人习惯：先用串口把原始陀螺仪和加速度计数据打印出来，确认传感器本身没问题，再开融合算法。这样能快速定位问题是出在传感器还是算法上。

互补滤波虽然简单，但调好了非常可靠。我做过对比测试，在同样的硬件上，调好的互补滤波和卡尔曼滤波在姿态精度上差距不到 0.5°，但互补滤波的计算量只有卡尔曼的 1/5。对于大多数飞控应用，互补滤波完全够用。