4、姿态解算入门：互补滤波原理与实现
各位同学，今天我们来聊聊姿态解算里最经典、也最实用的一个方法——互补滤波。
说实话，我刚入行那会儿，面对一堆加速度计、陀螺仪的数据，头都是大的。加速度计噪声大，陀螺仪有漂移，怎么把它们揉到一起？后来师傅丢给我一句话：「互补滤波，就是取长补短。」嗯，这句话我记到现在。
4.1 为什么需要互补滤波？
先想一个问题：
你手里有个陀螺仪，它能测角速度。积分一下，就能得到角度。但陀螺仪有零偏，时间一长，积分出来的角度就飘到姥姥家去了。
你手里还有个加速度计，它能测重力方向。静态时算角度很准，但一有运动，振动、加速度一上来，角度就抖得像筛糠。
你看，一个高频准但低频漂，一个低频稳但高频噪。互补滤波，就是把这两个信号「互补」起来。
核心思想：陀螺仪负责短期动态响应（高通），加速度计负责长期稳态校正（低通）。两者一结合，就得到了一个既响应快、又不漂移的姿态角。
4.2 互补滤波的数学原理
说白了，互补滤波就是一个一阶滤波器组合。公式很简单：
angle = α * (angle + gyro * dt) + (1 - α) * acc_angle其中：
angle：融合后的角度gyro：陀螺仪角速度dt：采样时间间隔acc_angle：加速度计计算出的角度α：权重系数，通常取 0.9~0.99
为什么会这样？
你想想看，α 越接近 1，陀螺仪的权重越大，系统响应快，但长期漂移也大。α 越接近 0，加速度计的权重越大，系统稳定，但动态响应慢。这个 α，就是你要调的「平衡点」。
个人经验：我一般从 α = 0.98 开始调。如果发现角度飘得厉害，就减小一点；如果发现角度跟不上动作，就增大一点。说白了，就是个手感活。
4.3 互补滤波的代码实现
下面是我在实际项目中用过的互补滤波代码。麻雀虽小，五脏俱全。
// 互补滤波结构体
typedef struct {
float angle; // 融合后的角度
float gyro; // 陀螺仪角速度
float acc_angle; // 加速度计角度
float alpha; // 权重系数
float dt; // 采样时间
} ComplementaryFilter_t;
// 初始化
void CF_Init(ComplementaryFilter_t *cf, float alpha, float dt) {
cf->angle = 0.0f;
cf->gyro = 0.0f;
cf->acc_angle = 0.0f;
cf->alpha = alpha;
cf->dt = dt;
}
// 更新姿态
void CF_Update(ComplementaryFilter_t *cf, float gyro, float acc_angle) {
// 陀螺仪积分
float gyro_angle = cf->angle + gyro * cf->dt;
// 互补融合
cf->angle = cf->alpha * gyro_angle + (1.0f - cf->alpha) * acc_angle;
// 保存当前值
cf->gyro = gyro;
cf->acc_angle = acc_angle;
}这段代码我用了好几年，从四轴飞行器到平衡小车，基本没出过问题。你直接拿去用就行。
4.4 加速度计角度计算
刚才代码里有个 acc_angle，它是怎么来的？
对于俯仰角（Pitch）和横滚角（Roll），可以用加速度计的三轴数据算出来：
// 从加速度计计算俯仰角
float pitch = atan2(-acc_x, sqrt(acc_y * acc_y + acc_z * acc_z)) * 180.0f / M_PI;
// 从加速度计计算横滚角
float roll = atan2(acc_y, acc_z) * 180.0f / M_PI;注意：加速度计算不出偏航角（Yaw）！因为偏航角绕着重力方向旋转，加速度计感知不到。所以偏航角通常要靠磁力计来校正。
4.5 互补滤波的优缺点
| 优点 | 缺点 |
|---|---|
| 计算量极小，适合低性能MCU | 精度有限，不如卡尔曼滤波 |
| 实现简单，调参直观 | 对加速度计噪声敏感 |
| 响应速度快，适合实时控制 | 无法处理非线性运动 |
| 不需要系统模型 | 权重系数需要手动调整 |
我个人觉得，互补滤波最大的优势就是「简单粗暴」。你不需要懂什么协方差矩阵、状态空间，一个公式就能跑起来。对于入门级的飞控、平衡车、云台，完全够用。
4.6 避坑指南
我曾经在一个四轴项目里，发现飞机悬停时角度一直在缓慢漂移。查了半天，发现是 α 设成了 0.999。你想想看，陀螺仪权重这么大，加速度计几乎不起作用，不漂才怪。
后来我把 α 降到 0.98，问题立刻解决。
还有一次，我发现加速度计的角度跳得厉害。一查，原来是低通滤波没做。加速度计的数据直接拿来用，高频噪声全进来了。所以我的建议是：
- 加速度计数据先做低通滤波（比如滑动平均），再送进互补滤波
- 陀螺仪数据可以做高通滤波，去掉零偏
- α 的取值，建议在 0.95 ~ 0.99 之间调试
小技巧：如果你发现调参很麻烦，可以试试自适应互补滤波。让 α 随着运动剧烈程度自动变化——运动剧烈时增大 α（信任陀螺仪），静止时减小 α（信任加速度计）。
4.7 核心逻辑流程图
下面这张图，是我用 SVG 画的互补滤波核心流程。你看一眼就能明白整个数据流。
嗯，这张图其实已经说得很清楚了。陀螺仪走高通路径，加速度计走低通路径，最后在互补融合节点汇合，输出一个干净、稳定的角度。
4.8 什么时候用互补滤波？
我个人的建议是：
- 入门学习：一定要从互补滤波开始。它简单、直观，能帮你理解姿态解算的本质。
- 资源受限：如果你用的是 STM32F103 这种级别的 MCU，互补滤波是首选。
- 实时性要求高：互补滤波的计算延迟极低，适合 1kHz 以上的控制循环。
- 精度要求不高：如果你只需要 ±2° 的精度，互补滤波完全够用。
但如果你要做高精度导航、无人机编队、或者需要抗强振动环境，那还是得上卡尔曼滤波或者 Mahony 滤波。不过那是后面章节的内容了。
好了，互补滤波就讲到这里。代码我给你了，流程图也画了，剩下的就是你自己动手调一调。记住，调参的时候别急，一点一点来，手感会有的。
公众号:蓝海资料掘金营，微信deep3321