互补滤波：融合加速度计与陀螺仪数据

引言

你有一个平均而言正确、但当下是错的加速度计，和一个当下正确、但长期是错的陀螺仪。互补滤波的观察就是：这两个误差特性正好相反，因此你可以各取一半好的。

它只是一行算术。对一台自平衡机器人来说，它也几乎总是正确答案——卡尔曼滤波换来的是一点点精度提升，代价是在 8 位单片机上大幅增加复杂度和 CPU 时间。

思想

每个传感器在不同的频带里可靠。

• 加速度计在低频上可靠。按一秒平均，它正确地报告“下”是哪个方向；按 10 ms 采样，它报告的主要是机器人自身的运动。
• 陀螺仪在高频上可靠。在 10 ms 上它的积分角率基本精确；在一分钟上累积的偏置已经把它毁掉了。

所以：对加速度计低通，对陀螺仪高通，然后相加。两个滤波器被设计成在每个频率上响应之和恰好为一——它们互补彼此，名字就是这么来的。没有任何频带被算两次，也没有任何频带被漏掉。

实现

const float ALPHA = 0.98;
float angle = 0;

void updateAngle(float accelAngle, float gyroRate, float dt) {
  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
}

这就是整个滤波器。读法是：取陀螺仪说的“自上次以来我们移到了哪里”，再往加速度计当前声称的方向修正 2%。

陀螺项提供响应性。小小的加速度计项提供一个缓慢而永久的修正，阻止陀螺漂移不断累积——它持续地把估计拉回物理真相，力度之轻，以至于瞬时的加速度尖峰几乎撼动不了它。

选择时间常数

ALPHA 不是一个随意的旋钮。它对应一个时间常数：

τ = (ALPHA · dt) / (1 − ALPHA)

τ 就是两个频带的分界。比 τ 快的变化来自陀螺仪，比 τ 慢的变化由加速度计说了算。在 5 ms 的循环和 ALPHA = 0.98 下，τ = 0.245 s——大约四分之一秒的陀螺仪权限，之后加速度计开始重新占上风。

注意这个公式蕴含的东西：**ALPHA 取决于你的循环频率。**把一个跑在 100 Hz 上的项目里的数值抄进你 500 Hz 的循环，你就悄悄地把滤波器变慢了五倍。如果循环时序变了，就从你真正想要的 τ 反算 ALPHA，而不是保留原来那个数。

时序必须是真实的

dt 必须是实际流逝的时间，而不是你本来打算让它流逝的时间：

unsigned long now = micros();
float dt = (now - lastUpdate) / 1000000.0;
lastUpdate = now;

因为你调用了 delay(5) 就把 dt = 0.005 写死，是一个会悄悄污染结果的常见捷径——单是 I²C 读就接近一毫秒，所以你真实的循环是 6 ms，每次积分都短了 17%。于是角度的漂移看起来和陀螺偏置一模一样，而且标定不掉。

更好的做法是让循环跑在固定频率的硬件定时器上，而不是 delay()。平衡环对时序一致性的需求，远大于对某个特定频率的需求。

把符号弄对

最常见的单一故障，是两个源的符号不一致。如果加速度计对前倾报 +3°，而陀螺仪对同一个动作报一个负的角率，这两项就互相打架：滤波输出会滞后、抖动，并稳定在错误的数值上，而无论怎么调 ALPHA 都修不好。

直接检查。把 accelAngle 和 gyroRate 的运行积分并排打印出来，然后用手慢慢把机器人前倾：

• 两者应当同方向变化。
• 两者应当大致相同的量级。

如果两者反向变化，就把陀螺角率取反。如果两者相差 90°，说明你读的加速度计轴和你机器人倾倒所在的那个平面不对应。

为什么不用卡尔曼滤波？

如果你知道传感器的噪声特性，卡尔曼滤波是统计上最优的答案——而且它确实表现略好，在持续加速时尤其明显。

对平衡机器人来说，这个差别很少值得。互补滤波的代价是一次乘加，有一个物理意义清晰的可调参数，而且失效方式是你打印两个数就能诊断的。卡尔曼滤波的代价是每个周期都要做矩阵运算，多一个要调的协方差矩阵，而且它行为异常时原因真的很难看出来。

先用互补滤波把机器人调得能站。如果它站得稳，答案就有了。关于这些取舍更完整的讨论见传感器噪声、滤波与融合。

闭环之前先验证

两项检查能抓住几乎所有问题，而且各只要一分钟：

**静止保持两分钟。**滤波后的角度应当稳在原地。如果它在慢慢爬，说明你的陀螺偏置标定不对——加速度计项本来就应该正好防止这件事，所以会爬意味着 ALPHA 对你的偏置来说太高了。

**晃动但不要旋转。**把机器人整体左右平移，保持它竖直。单独的加速度计会剧烈摆动；滤波后的角度应当几乎不动。如果滤波后的角度也摆得厉害，说明 ALPHA 太低，你在加速过程中信任了加速度计——而那恰恰是它最不可靠的场景。

一旦角度静止时保持不动、被晃时依然诚实，就好到可以用来平衡了。在级联 PID 平衡控制里闭合这个环，或者在平衡模拟器里看一个调好的环怎么工作。同样的滤波形状——一个会漂移但平滑的信号，由一个嘈杂但有界的信号来修正——也是户外机器人把 1 Hz 的 GPS 定位变成可用位置的方式。