学飞控、做平衡车第一个绕不开的算法就是互补滤波。别被名字吓到它其实就是“哥俩好互相拉一把”的加权平均。1. 痛点为什么一个传感器搞不定假设你要做一个自平衡小车或者无人机飞控首先得知道当前倾斜了多少度角度。通常我们会用两种传感器来测角度陀螺仪测角速度反应极快你一动它就知道。但它有个致命缺点——零漂。静止时它也可能输出微小的值积分久了角度会不受控制地飘走1 分钟飘出几十度。加速度计测重力分量绝对稳定静止时能靠重力算出精准角度。但它极度怕震动电机一启动噪声全来了角度跳得像心电图。结论陀螺仪是“短期靠谱长期跑偏”加速度计是“长期准确短期跳脚”。2. 互补滤波的核心思想高频与低频的分工互补滤波的思路极其简单、优雅既然陀螺仪动态好高频信号准加速度计静态好低频信号准那我们就把它们通过滤波器组合起来陀螺仪的数据通过高通滤波器相信突变不信缓慢漂移加速度计的数据通过低通滤波器相信缓慢变化不信突跳噪声两者相加正好互补成全频带都准确的角度。用人话讲角度绝大部分时间跟着陀螺仪快速跑但每隔一刹那偷偷用加速度计的角度往回拽一点点把漂移拉回来。3. 最经典的数学公式一看就懂离散化的一阶互补滤波公式只有一行a n g l e α × ( a n g l e − 1 g y r o × d t ) ( 1 − α ) × a c c A n g l e angle \alpha \times (angle_{-1} gyro \times dt) (1 - \alpha) \times accAngleangleα×(angle−1​gyro×dt)(1−α)×accAngle参数拆解每个都很重要符号含义单位a n g l e \mathrm{angle}angle当前融合后的角度你要的结果deg / rada n g l e − 1 \mathrm{angle}_{-1}angle−1​上一时刻融合的角度deg / radg y r o \mathrm{gyro}gyro陀螺仪测得的角速度∘ / s ^\circ/\mathrm{s}∘/sd t dtdt程序循环时间采样周期s \mathrm{s}sa c c A n g l e \mathrm{accAngle}accAngle加速度计计算出的瞬时倾角degα \alphaα滤波系数核心0 ∼ 1 0 \sim 10∼14. 核心系数α \alphaα到底怎么调附时间常数α \alphaα决定了你“更信谁”当α 0.98 \alpha 0.98α0.98意味着 98% 信任陀螺仪积分2% 信任加速度计。漂移修正很慢但抗震动极好。当α 0.60 \alpha 0.60α0.60两者权重接近角度响应变慢且容易引入噪声。实际工程中α \alphaα通常取0.90 ∼ 0.999 0.90 \sim 0.9990.90∼0.999。更专业的调参方式是看时间常数τ \tauττ α × d t 1 − α \tau \frac{\alpha \times \mathrm{d}t}{1 - \alpha}τ1−αα×dt​如果想在 0.1秒内修正漂移算出的α \alphaα就小一点如果电机震动大想让修正慢一点α \alphaα就无限趋近于 1。5. 一个超形象的“开车类比”把互补滤波想象成开车回家陀螺仪 车速表。告诉你当前瞬时速度反应灵敏但车轮打滑或磨损时里程表积分会慢慢不准。加速度计 路边偶尔出现的里程牌。绝对准确但不是时时刻刻都有。互补滤波就是平时按车速表跑每当路过一个里程牌就悄悄地微调一下里程表读数不让误差累积太大。6. 保姆级 C 语言代码复制即用对于 STM32、Arduino 或任何单片机你只需这么几行// 定义结构体方便管理typedefstruct{floatangle;// 融合后的角度floatdt;// 采样时间 (s)floatalpha;// 滤波系数 (0.9~0.999)}ComplementaryFilter_t;// 初始化voidComplementary_Init(ComplementaryFilter_t*f,floatdt,floatalpha){f-angle0.0;f-dtdt;f-alphaalpha;}// 更新函数传入陀螺仪角速度和加速度计计算的角度floatComplementary_Update(ComplementaryFilter_t*f,floatgyro,floataccAngle){// 陀螺仪积分预测floatpredictf-anglegyro*f-dt;// 互补融合f-anglef-alpha*predict(1.0f-f-alpha)*accAngle;returnf-angle;}// ---------- 主程序调用示例 ----------// 假设加速度计算出俯仰角为 5.2°陀螺仪 Z 轴角速度为 0.5°/sdt0.01sfloatpitchComplementary_Update(filter,0.5,5.2);// pitch 结果 ≈ 0.98*(前一角度0.005) 0.02*5.27. 进阶避坑指南写给新手加速度计计算角度时记得加低通滤波如果电机震动大建议先对a c c A n g l e \mathrm{accAngle}accAngle滑动平均或二阶低通再喂给互补滤波。陀螺仪要去零偏上电静止时取前100次陀螺仪平均值作为零偏之后每次读值都减去它否则漂移修不回来。d t dtdt要实时计算不要用固定死值最好用定时器时间戳计算d t dtdt否则α \alphaα的有效时间常数会变导致动作忽软忽硬。8. 互补滤波 vs 卡尔曼滤波横向对比经常有人问为啥不用高大上的卡尔曼对比项互补滤波卡尔曼滤波计算量极小加减乘除较大矩阵求逆调参难度调一个 α 就行调 Q 和 R 矩阵很难适用场景资源紧张的 MCU、入门项目高精度、高动态、有 GPS 等复杂场景效果够用有轻微滞后理论最优但调不好不如互补对于平衡车、飞控入门、舵机稳定互补滤波绝对 YYDS永远的神。结语互补滤波就像一位聪明的和事佬让暴躁的陀螺仪和安静的加速度计握手言和。它不需要复杂的线性代数短短几行代码就能让你的设备获得稳定、低延迟的姿态信息。你的第一个平衡车、自稳云台、四轴飞行器都可以从这一行公式开始。赶紧去试试吧如果你觉得有帮助欢迎点赞、收藏、评论让更多人看到