做平衡车调姿态我一开始压根没把“滤波”当回事心想无非就是把MPU6050的原始值读出来算个atan2角度丢给PID就完事。结果上车第一版车是起来了但站不住两秒就开始周期性抖动歪到一边去。后来我把角度波形导出来看了一眼才明白问题根源在于读到的东西根本不是真实倾角——加速度计在电机振动和车身加速时满是毛刺陀螺仪积分没几秒就开始漂移。从那以后我在平衡车项目里花在姿态估计算法上的时间比调PID还多。这篇文章想把这三种在平衡车圈子里最常见的姿态融合算法——互补滤波、卡尔曼滤波、清华滤波从头到尾拆开讲一遍。会先解释姿态角为什么这么难测再逐个讲清每个算法的原理、公式、代码和调参心得最后放一组我实车测试的对比数据。适合正在做两轮平衡车、正在纠结选哪个滤波算法、或者想搞懂卡尔曼滤波和互补滤波到底差在哪的读者。读完你至少能知道自己该选哪个以及为什么。1. 一个倾角为什么这么难测加速度计和陀螺仪都是“半残废”1.1 加速度计静止很准一动就废加速度计测倾角的原理谁都能听懂重力加速度在传感器三轴上的分量会随倾角变化用atan2(ax, az)就能算出当前倾斜角。这个方法没有任何积分累积只要传感器平放在桌面上读数可以长期保持稳定所以很多新手第一版代码都是用加速度计直接算角度。但加速度计有个致命弱点它分不清重力和外力。平衡车只要一被推动、一加速、一颠簸除了重力以外车身加减速产生的线性加速度同样会作用在传感器上。这时候加速度计实测到的加速度是“重力运动加速度”的混合体用这个值算出来的角度自然就飞了。电机振动下噪声更是尖锐静止时角度波动可能只有±0.5度电机一转起来能飘到±5度甚至更多。在典型的全速加速测试里加速度计角度最大会偏移真实倾角约25度根本没法用。1.2 陀螺仪短时准确长时漂移陀螺仪测的是角速度也就是单位时间转了多少度。理论上只要把角速度按时间积分就能得到角度变化量。而且陀螺仪测的是旋转本身不受直线加速的影响动态响应极其干脆没有相位滞后。问题出在“积分”这两个字上。陀螺仪输出有一条零偏也就是静止时读数并不严格为0可能是0.2度/秒也可能是-0.5度/秒。这条零偏会随时间、温度缓慢漂移。每次按周期dt累加哪怕只有0.05度/秒的零偏误差20秒也能累积出1度的偏差一分钟后就是约3度误差。真正的角度被淹没在这个不断长大的误差里车当然越来越歪。1.3 滤波本质做的是“融合”不是“降噪”所以姿态感知的核心不是“把噪声滤掉”二是在两个都是“半残废”的信号里各取所长。一句话总结就是短时间信陀螺仪长时间信加速度计。所有滤波算法包括后面要讲的三种本质上都是在为这个“什么频率信谁”的决策设计不同的实现方式。理解了这一点后面的公式就不会显得像天书。提示姿态融合算法解决的是“信号之间的互补”而不是像低通滤波那样单纯把波动压平。你用错了角度去看它永远调不明白。2. 互补滤波靠一个系数就稳住了车2.1 频率域里的“长处互补”互补滤波思路非常直观加速度计角度虽然噪声大但它是真实的绝对角度属于低频准、高频差陀螺仪积分角度短期很准但低频端会漂移属于高频好、低频差。两条曲线正好在频域上互补。那就在低频处多信加速度计高频处多信陀螺仪。实现方式就是把加速度计角度做低通滤波陀螺仪积分角度做高通滤波加起来就是融合后的角度。低通参数和高通参数设计成互补保证整个频段的信号增益都为1这就是“互补滤波”这个名字的来源。2.2 一阶互补滤波的公式与完整代码工程上最常用的一阶互补滤波递推公式长这样angle α * (angle gyro * dt) (1 - α) * acc_angle拆开看是两部分前半部分α * (angle gyro * dt)是上一时刻角度加上陀螺仪积分增量再乘权重α这相当于高通处理后半部分是加速度计计算角度乘权重(1-α)相当于低通处理。α越接近1陀螺仪权重越大对动态响应越敏感但漂移也越明显α越接近0加速度计权重越大波形越平滑但动态滞后越明显。实际参数整定一般不看α本身而看时间常数τ两者关系是α τ / (τ dt)τ可以理解为“相信陀螺仪多久之后开始拉向加速度计”典型值在0.5秒到1秒之间。比如dt5ms想取τ0.5sα0.5/0.505≈0.99。我车上实际用的就是α0.99dt5ms效果非常稳定。配合MPU6050的DMP思路写成的互补滤波代码很短整段直接用浮点运算就行// 互补滤波函数 // angle: 上一时刻融合角度单位度 // gyro: 陀螺仪Y轴角速度单位度/秒 // acc_angle: 加速度计计算角度单位度 // dt: 滤波周期单位秒 // alpha: 权重系数0.9~0.995之间 float complementary_filter(float angle, float gyro, float acc_angle, float dt, float alpha) { return alpha * (angle gyro * dt) (1.0f - alpha) * acc_angle; }就这一行车能站起来。很多人第一次看到这个代码都会愣一下就这么简单对就这么简单。我在STM32F103C8T6上跑这段代码加上外围的MPU6050读取和PID控制一个主循环总共也就几十微秒的额外开销几乎可以忽略。2.3 这个坑绕不开线性加速度就是来捣乱的互补滤波最大的软肋在动态场景下暴露无遗。平衡车起步、刹车、遇到障碍物被推了一下加速度计测到的不再是重力分解而是重力加上车身的线性加速度。这时的acc_angle本身就是错的你把加速度计的权重乘上任何一个系数结果依然是错的只是错多错少的问题。我在车上实测过小车被猛地往前推一下的瞬间互补滤波输出角度会瞬间跳偏10度以上然后靠后面的陀螺仪惯性慢慢拉回来。这个跳变对PID来说就是一次剧烈的冲击车反应不过来往往会往反方向猛打一下表现就是“一推就摔”。3. 卡尔曼滤波把“最优估计”搬进单片机3.1 状态模型与两个关键噪声卡尔曼滤波的思路和互补滤波完全不同。互补滤波是在频域上做加权卡尔曼滤波则是在时域里建立系统模型把“角度”和“陀螺仪零偏”当成两个需要估计的状态然后利用系统方程加观测数据每步都给出统计意义下的最优估计。平衡车场景下定义状态向量x [角度θ, 陀螺仪零偏b]^T系统方程写出来就是θ(k) θ(k-1) (ω(k) - b(k-1)) * dt w1 b(k) b(k-1) w2第一个方程的意思是新角度等于旧角度加上“实际角速度×时间”实际角速度等于陀螺仪读数减去零偏。第二个方程假设零偏在短时间内基本不变只受随机扰动w2影响。w1和w2就是过程噪声代表我们对模型的信任程度。观测方程就简单了加速度计算出来的角度就是测量值z(k) θ(k) v这里的v是测量噪声来自加速度计的振动、线性加速度干扰。卡尔曼滤波要做的事就是综合系统预测和测量值按噪声的统计特性算出一个最优权重再输出修正后的角度。3.2 五步滤波循环每一步都在干什么卡尔曼滤波的核心计算是五个公式我直接把这五步对应的语义讲清楚第一步状态预测x_pred A * x B * u把上一时刻的最优状态代入系统方程预测当前时刻状态。u是控制量在平衡车场景里一般没有明确控制输入直接为0。第二步协方差预测P_pred A * P * A^T QP代表我们对状态估计的置信程度Q是过程噪声协方差矩阵。这一步的实质是预测本身也有不确定度随着时间推移这个不确定度会变大。第三步计算卡尔曼增益K P_pred * H^T * (H * P_pred * H^T R)^(-1)卡尔曼增益K就是那个“最优权重”。如果测量噪声R小K会变大更相信加速度计如果过程噪声Q小说明系统模型可信K会变小更相信陀螺仪积分。第四步用测量值修正预测x x_pred K * (z - H * x_pred)用实际测到的加速度计角度和预测角度的差叫innovation或残差乘以K得到修正量。第五步更新协方差P (I - K * H) * P_pred修正完以后状态更可信了协方差变小进入下一轮迭代。在平衡车上应用时A、B、H都是常数矩阵代码可以化简成一个很小的定长计算块。以下是实际在STM32上验证过的完整卡尔曼滤波代码// 卡尔曼滤波参数 typedef struct { float Q_angle; // 角度过程噪声 float Q_gyro; // 零偏过程噪声 float R_angle; // 测量噪声 float angle; // 输出角度 float bias; // 陀螺仪零偏估计 float P[2][2]; // 误差协方差矩阵 } Kalman_t; void Kalman_Init(Kalman_t *k) { k-Q_angle 0.001f; k-Q_gyro 0.003f; k-R_angle 0.03f; k-angle 0.0f; k-bias 0.0f; k-P[0][0] 0.0f; k-P[0][1] 0.0f; k-P[1][0] 0.0f; k-P[1][1] 0.0f; } // 卡尔曼滤波主函数 // new_angle: 加速度计计算角度 // new_gyro: 陀螺仪角速度 // dt: 滤波周期 float Kalman_Filter(Kalman_t *k, float new_angle, float new_gyro, float dt) { float S, K0, K1; float P00, P01, P10, P11; float y, angle_err; // 1. 状态预测 k-angle dt * (new_gyro - k-bias); // 2. 协方差预测 P00 k-P[0][0] dt * (dt*k-P[1][1] - k-P[0][1] - k-P[1][0] k-Q_angle); P01 k-P[0][1] - dt * k-P[1][1]; P10 k-P[1][0] - dt * k-P[1][1]; P11 k-P[1][1] k-Q_gyro * dt; // 3. 计算卡尔曼增益 S P00 k-R_angle; K0 P00 / S; K1 P10 / S; // 4. 用加速度计角度修正 angle_err new_angle - k-angle; k-angle K0 * angle_err; k-bias K1 * angle_err; // 5. 更新协方差 k-P[0][0] P00 - K0 * P00; k-P[0][1] P01 - K0 * P01; k-P[1][0] P10 - K1 * P00; k-P[1][1] P11 - K1 * P01; return k-angle; }这段代码我在多个项目里用过速度上完全没问题一次完整迭代在STM32F103 72MHz下大约耗时30微秒左右比很多人想象中轻量得多。3.3 卡尔曼的调参心得和初始化技巧卡尔曼调参一直劝退新手的原因是R和Q没有明确的物理直觉。我的经验是把调参分成两步第一步定量级。R_angle代表你对加速度计的信任程度。传感器静止时如果角度噪声是±0.5度R可以先取0.01到0.1这个量级Q_angle和Q_gyro代表你对陀螺仪模型的信任取0.001到0.01之间比较常见。记住关键规律R相对越大越不信加速度计波形越平滑、动态响应越慢Q相对越大越不信陀螺仪积分波形越容易跟踪加速度计的毛刺。第二步看波形微调。实车调试时把融合角度和加速度计原始角度一起传到上位机显示从小R大Q开始逐步调整直到静止时波形平滑且被推一下时能快速真实跟手。初始化有个小技巧上电静止放置两秒把这段时间加速度计角度的平均值作为卡尔曼的初始角度陀螺仪零偏也可以用静止时的角速度均值预填充。这样做能大幅缩短起步阶段的收敛时间不然车刚上电会有一段时间角度是歪的直接导致起步猛冲。注意卡尔曼滤波不是“装上就一定比互补滤波好”的万能药。R和Q调得不对它的表现可能比一个随手设了α的互补滤波差得多。调参时间要预留到位。4. 清华滤波能自动修正陀螺仪零漂的折中算法4.1 “清华滤波”到底是个什么来头“清华滤波”这个说法在开源平衡车圈子里时常出现尤其在一些基于STM32的教程、比赛开源代码里经常能看到一个带零偏修正项的滤波函数被冠以这个名字。严格说它并不是某个官方机构发布的算法而是国内做平衡车的开发者们对某一类带陀螺仪零偏动态估计的姿态融合算法的统称因为早期流传的代码多源自这个圈子就叫开了。我接触到的所谓“清华滤波”本质上是一个互补滤波的改进版本。互补滤波把陀螺仪零偏当成一个未知常量靠加速度计的权重慢慢把角度拉回来而清华滤波的思路是在运动过程中实时估计那条零偏b然后从陀螺仪读数里减去b再积分。这样一来陀螺仪的权重可以相对提高动态响应更好同时又不会长期积累漂移。4.2 PI式偏差修正比互补滤波多出来的那一层清华滤波的核心结构可以看作一个比例积分PI调节器在驱动零偏估计。角度观测量和积分角度之间的误差被分成两路一路直接修正角度另一路累积起来修正陀螺仪零偏。用公式表达就是angle (gyro - bias) * dt; err acc_angle - angle; angle Kp * err * dt; // 比例项快速把角度拉向加速度计 bias Ki * err * dt; // 积分项把误差累积成零偏修正量和互补滤波对比就特别清晰。互补滤波相当于只有Kp这一路且Kp是隐含在α里的固定系数清华滤波额外加了一路积分器去“学习”零偏。零偏一旦被估计准陀螺仪积分的可信度大幅提升长期漂移问题就从根本上缓解了。Kp和Ki的整定思想是Kp越大角度跟踪加速度计越快但波形越毛糙Ki越大零偏收敛越快但过大会把瞬时误差当成长期零偏反而导致角度被误导。一般Ki比Kp小一个数量级比如Kp10Ki0.5。4.3 工程化代码和参数整定经验以下代码是我在平衡车上反复验证过的一个版本适合5ms到20ms的控制周期// 清华滤波参数 #define T_KP 10.0f // 比例系数角度修正强度 #define T_KI 0.5f // 积分系数零偏修正强度 typedef struct { float angle; float bias; float dt; } TsinghuaFilter_t; float tsinghua_filter(TsinghuaFilter_t *f, float gyro, float acc_angle) { float err; // 陀螺仪积分用实时零偏修正后的角速度 f-angle (gyro - f-bias) * f-dt; // 误差计算加速度计角度作为绝对参考 err acc_angle - f-angle; // 比例修正 f-angle T_KP * err * f-dt; // 积分修正零偏 f-bias T_KI * err * f-dt; return f-angle; }实际调参时我有一个比较省事的方法先把Ki设为0让整个滤波退化为一个纯比例修正的互补滤波这样先调Kp把角度跟踪调好然后缓慢加大Ki直到静止状态下车身能长时间保持不漂移为止。如果发现角度波形出现低频振荡或者推一下车之后回不到零位通常是Ki太大了往回退一半再微调。提示清华滤波的零偏修正机制对温度漂移尤其友好。MPU6050上电后随着发热陀螺仪零偏会明显变化普通互补滤波对此无能为力而清华滤波能通过积分器实时跟踪零偏变化长期稳定性好很多。5. 放在同一辆车上实测三种算法的真实差距5.1 测试条件与波形观察方法为了公平对比我把三种滤波器放进同一个工程里用同一个MPU6050数据源跑再分别输出三个角度通道到串口通过VOFA上位机软件在线画波形。硬件是一个STM32F103C8T6核心板加MPU6050电机驱动用DRV8833控制周期5ms滤波周期同样5ms。所有传感器原始数据不经过任何额外预处理直接喂给三种算法。每种算法都通过上位机波形观察三个关键指标静止时角度波动幅度、被推一下后的最大偏差响应冲击值、以及恢复到真实角度的时间恢复时间。5.2 静止、倾斜、被推着跑的对比结果三组典型场景的实测数据汇总如下测试场景互补滤波卡尔曼滤波清华滤波静止时角度波动±0.6度±0.3度±0.4度车辆突然前推时最大偏差约10度约4度约6度被推后恢复到真实角度的时间约0.8秒约0.3秒约0.5秒长跑1分钟后角度漂移约1.5度小于0.3度小于0.5度明显运动后是否出现相位滞后是较明显否轻微数据背后有几个值得玩味的细节。卡尔曼滤波的响应冲击值只有互补滤波的不到一半这是因为卡尔曼同时估计了零偏积分角度更接近真实值对误差的修正量不会那么大。清华滤波的恢复时间0.5秒介于两者之间但它的参数只用两行就能整定比卡尔曼省事太多。还有一个感受上的差异是波形平滑度。静止时卡尔曼的角度波形几乎是一条直线互补滤波还有可见的小锯齿清华滤波介于两者之间。这种平滑度差异在PID控制里影响很大——不平滑的角度微分会被D项放大导致电机出现高频抖动。5.3 资源开销和工程实现差异资源占用这块我把三种算法分别编进同一个工程实测结果如下算法Flash增量单次滤波耗时需要调的参数互补滤波约200字节约5微秒1个α清华滤波约400字节约8微秒2个系数(Kp,Ki)卡尔曼滤波约1KB约30微秒3个噪声参数(Q_angle,Q_gyro,R_angle)以STM32F103的性能来说30微秒在5ms控制周期里只占0.6%哪怕是老旧的F103C8T6也完全带得动。所以性能本身不是选型的决定性因素真正影响选择的是开发周期和稳定性。6. 最后还是得说句大实话你根本不需要“最优”滤波6.1 我的选型逻辑和实际项目建议如果现在有人问我做平衡车用哪个滤波我的答案很直接大部分人应该从互补滤波起步先让车站起来然后换成清华滤波把零漂问题解决了这时候你的平衡车已经能稳定工作卡尔曼滤波不是你做普通两轮平衡车的必需品除非你追求极致性能或者车要长时间自主运行。做竞赛或者做产品原型我推荐直接上清华滤波或卡尔曼。清华滤波胜在参数少、调试快鲁棒性好卡尔曼胜在精度上限高、波形干净但代价是调参要耐心而且一旦硬件换了传感器安装方向、换了车身重量分布就要重新验证一遍噪声参数是否还合适。6.2 卡尔曼滤波“降维”使用的一个思路卡尔曼调参难主要是因为Q和R缺乏直觉这个门槛劝退了不少人。我分享一个自己常用的取巧做法先用清华滤波把车调稳跑起来然后从清华滤波的输出里反推卡尔曼的初始参数。具体做法是把清华滤波的融合角度和加速度计角度差值统计一下方差这个方差直接作为R的初值再把静止时角速度的偏差方差作为Q_gyro的初值。这样得到的卡尔曼参数通常第一版就能基本稳定剩下的微调就轻松多了。6.3 姿态滤波这条路后面还能怎么走平衡车只是姿态感知的最小载体同一套算法搬到自平衡机器人、双轮载人车、云台稳定器上面对的也是同一类问题。如果后续想深入下去可以研究显式互补滤波基于四元数配合BFL或者直接上带IMU的EKF框架原理和卡尔曼在平衡车上做的事一脉相承只是状态量从二维扩到更高维度。最后分享一个我自己踩过的坑无论用哪种滤波传感器安装位置和防震处理永远比算法本身影响更大。我之前把MPU6050直接焊在电机驱动板上滤波怎么调都抖后来用软排线加减震泡棉把传感器独立出来后同样一套代码波形立刻干净了。姿态估计算法是锦上添花扎实的硬件基础才是那匹锦。