第一次把 MPU6050 焊上自己的小四轴时我踩过一个大坑直接用加速度计反三角算角度结果电机一转上位机里的角度曲线就像得了帕金森一样疯狂抖动。改成陀螺仪积分静止时确实丝滑可过了半分多钟就开始慢慢往一个方向漂越漂越远。后来真正把飞控算法的姿态解算这层窗户纸捅破——用互补滤波把两者按频率特性揉在一起世界才清净了。这篇就聊聊我理解的互补滤波加速度计和陀螺仪为什么单独都不好用一阶互补滤波公式到底在干什么以及在 STM32 MPU6050 上怎么把它写成能飞的代码。想认真搞飞控算法的朋友不管是自己打板做小四轴还是纯粹想弄懂姿态解算的原理这篇都适合你。1. 传感器原罪加速度计和陀螺仪为什么单独都不可用1.1 加速度计测姿态静止很准一动就骗人先看加速度计。它的物理模型是“比力传感器”测量的是机体受到的加速度矢量。飞行器静止悬停时加速度计唯一感受到的加速度就是重力加速度的反方向所以从三轴分量就能反推出当前姿态角。以常规机体坐标系为例x 轴朝前、y 轴朝右、z 轴朝下静止时加速度计读数约为 (0, 0, 1g)。此时roll 滚转角atan2(ay, az)pitch 俯仰角atan2(-ax, sqrt(ay*ay az*az))这里面没有任何积分过程测量是绝对式的所以长期来看它不会漂移。这正是加速度计最宝贵的特性低频稳定。但问题也极其明显。飞行器一旦开始加减速加速度计上叠加的就不再只有重力了发动机拉力、空气阻力、旋翼气流都会变成额外的“运动加速度”。比如飞机以 0.5g 的水平加速度往前冲加速度计合成矢量的方向会明显偏离真正的重力方向解算出来的俯仰角可能凭空偏出好几度。你看着姿态角在跳但机体其实根本没转这就是运动加速度污染。更要命的是电机高速旋转带来的振动。无刷电机的换向振动、螺旋桨高频切割空气的气流扰动全都会被加速度计接收。直接拿原始数据算角度出来的曲线全是毛刺。就算在软件里加低通滤波器滤波也会引入相位延迟姿态响应变迟钝飞控的角速度环和姿态环都会受影响。1.2 陀螺仪积分瞬间贼准时间一长全是债陀螺仪测量的是角速度单位是 deg/s 或 rad/s。要得到角度必须对时间积分θ(t) θ(0) ∫ ω(τ) dτ积分的好处是它完全不受线加速度干扰你飞机怎么乱飞只要转速真是那么多角度增量就是准的。而且陀螺仪高频响应极好动态性能碾压加速度计。坏处呢它有一个隐藏的“债主”叫零偏。陀螺仪静止时输出不应该是 0但实际芯片总存在一个微小的偏置电压或者说 bias比如 0.1°/s。你即使做了上电校准把静止时的平均值减掉了可温度一变化、供电电压一波动bias 还会偷偷跑回来。0.1°/s 的零偏听起来微不足道积上 300 秒就是 30° 的漂移。四轴飞个五分钟姿态从“水平”漂成“倒扣”都不是玩笑。用一个生活类比陀螺仪像记忆力特别好但从来不检查作业的学生短时间能背得滚瓜烂熟时间一长错题就累积到没法收拾。加速度计像每道题都要翻书对答案的同学答案正确但速度太慢还经常被窗外的动静带偏。1.3 频域里它俩是互补的把两个传感器的误差拿到频域里看就特别有意思陀螺仪高频段准确低频段有漂移误差集中在低频加速度计低频段准确高频段噪声大误差集中在高频。一个是高通友好型一个是低通友好型。那自然的处理方式就是让陀螺仪信号通过高通滤波器让加速度计信号通过低通滤波器再把两者加起来不就得到一个全频段都靠谱的姿态估计了吗这就是“互补滤波”这个名字的真正来源——不是简单的加权平均而是在频率域上做互补拼接。这也是为什么互补滤波特别适合飞控算法简单、计算量小、不需要复杂的噪声协方差模型在 MCU 资源紧张的场景下非常有价值。2. 一阶互补滤波公式拆解一个 α 值背后的权衡2.1 离散递推式怎么理解最经典的一阶互补滤波离散形式长这样θ[k] (1 - α) · (θ[k-1] ω[k] · Δt) α · θ_acc[k]拆开看每一项都很直白θ[k-1] ω[k]·Δt陀螺仪从上一时刻积分得到的当前角度预测值θ_acc[k]加速度计当前直接解算出的角度观测值α加速度计的信任权重取值范围 0~1。如果α 0滤波输出就是纯陀螺仪积分如果α 1输出就是纯加速度计角度。实际飞控里 α 一般取 0.02~0.2 之间也就是大多数时候信任陀螺仪加速度计只负责慢慢地把积分漂移“拽回来”。这个公式的工程含义很舒服短期剧烈运动时陀螺仪积分主导姿态跟手长期静止或匀速时加速度计观测持续修正消除漂移。2.2 α 与时间常数为什么不能随手填一个数很多新手看到 α 就直接填 0.5觉得“一半一半”最平衡这是典型误区。滤波器的响应速度可以用等效时间常数 τ 来描述。对一阶互补滤波当 α 远小于 1 时近似有τ ≈ Δt / α对应的截止频率fc ≈ α / (2π · Δt)举个例子。假设姿态解算跑在 500Hz也就是 Δt 2ms如果 α 0.1τ ≈ 20ms修正收敛得很快但加速度计噪声也会泄进来如果 α 0.02τ ≈ 100ms滤波输出更丝滑但姿态估计对真实角度的跟随会明显变慢如果 α 0.5τ ≈ 4ms那基本上就是加速度计在主导陀螺仪积分反而成了配角动态性能直接崩掉。调参的时候不要凭空猜 α先想明白你希望姿态估计在多长时间内收敛到加速度计的参考方向。想要更跟手的动态响应α 可以稍微大一点想要更平滑的静态输出α 就小一点。这个时间尺度的概念比死记“α 取 0.1”靠谱得多。2.3 欧拉角版本的互补滤波roll/pitch 能补yaw 没救网上很多入门例程会先给一个最朴素的欧拉角互补滤波把 roll 和 pitch 先用加速度计算出来再套递推式float roll_est 0.0f, pitch_est 0.0f; float alpha 0.1f; float gx_rad, gy_rad; // 陀螺仪角速度单位 rad/s float ax, ay, az; // 加速度计单位 g float dt; // 采样周期单位 s float roll_acc atan2f(ay, az) * 57.29578f; float pitch_acc atan2f(-ax, sqrtf(ay*ay az*az)) * 57.29578f; roll_est (1.0f - alpha) * (roll_est gx_rad * dt * 57.29578f) alpha * roll_acc; pitch_est (1.0f - alpha) * (pitch_est gy_rad * dt * 57.29578f) alpha * pitch_acc;这个版本入门看没有问题但它有个天生缺陷yaw 偏航角补不了。因为重力方向永远垂直于水平面加速度计根本无法分辨飞机绕重力轴转了多少度也就是重力矢量在 yaw 轴上没有任何投影信息所以 yaw 只能靠陀螺仪积分漂移是必然的。要解决 yaw 漂移要么加磁力计做绝对航向参考要么升级到四元数版本把加速度计和磁力计的参考矢量统一放进一个框架里修正。这也是为什么后面我们最终会走到 Mahony 风格的四元数互补滤波。3. STM32MPU6050 的工程实现从寄存器配置到 Mahony 风格互补滤波3.1 传感器初始化和单位换算先说硬件底子。MPU6050 通过 I2C 接口通信AD0 接地时从机地址是 0x68。飞控应用里我建议加速度计量程选 ±8g 或 ±16g因为真实飞行中机体运动加速度很容易超过 2g选 ±2g 虽然精度高但在大机动时会饱和。陀螺仪量程建议选 ±500dps 或 ±1000dps±250dps 在快速翻滚时也可能爆量程。初始化代码大致如下#define MPU6050_ADDR 0x68 void MPU6050_Init(void) { // 复位传感器 MPU_Reg_Write(MPU6050_ADDR, 0x6B, 0x80); delay_ms(100); // 唤醒并选择 Z 轴陀螺作为内部时钟源稳定性更好 MPU_Reg_Write(MPU6050_ADDR, 0x6B, 0x03); // 陀螺仪配置±500dps灵敏度 65.5 LSB/(°/s) MPU_Reg_Write(MPU6050_ADDR, 0x1B, 0x08); // 加速度计配置±8g灵敏度 4096 LSB/g MPU_Reg_Write(MPU6050_ADDR, 0x1C, 0x10); // 数字低通滤波器 DLPF 配置这里选了一个折中频段 // 实际值需要结合你的采样率和控制频率调整 MPU_Reg_Write(MPU6050_ADDR, 0x1A, 0x05); }关键在于单位换算这一步是新手翻车重灾区。加速度计原始值是 int16要除以灵敏度系数得到 g陀螺仪原始值要除以灵敏度得到 deg/s再乘上 0.0174533 转成 rad/s。因为后面的四元数积分公式明确要求角速度单位是 rad/s如果你拿 deg/s 直接算姿态会疯狂乱转。int16_t ax_raw, ay_raw, az_raw; int16_t gx_raw, gy_raw, gz_raw; float ax ax_raw / 4096.0f; // 单位 g float ay ay_raw / 4096.0f; float az az_raw / 4096.0f; float gx (gx_raw - gyro_bias[0]) / 65.5f * 0.0174533f; // 单位 rad/s float gy (gy_raw - gyro_bias[1]) / 65.5f * 0.0174533f; float gz (gz_raw - gyro_bias[2]) / 65.5f * 0.0174533f;3.2 零偏校准上电第一件事陀螺仪零偏不校掉后面所有功夫都白费。最可靠的校准方式就是上电后静置采样若干次求平均把平均原始值作为 bias 存起来。注意校准时机尽量避开刚上电那两三秒因为芯片内部时钟和电源还没稳定此时 bias 会突变。#define GYRO_SAMPLE_NUM 500 float gyro_bias[3] {0.0f, 0.0f, 0.0f}; void Gyro_Calib(void) { float sum[3] {0.0f, 0.0f, 0.0f}; for (int i 0; i GYRO_SAMPLE_NUM; i) { read_gyro_raw(gx_raw, gy_raw, gz_raw); sum[0] gx_raw; sum[1] gy_raw; sum[2] gz_raw; delay_ms(1); } gyro_bias[0] sum[0] / GYRO_SAMPLE_NUM; gyro_bias[1] sum[1] / GYRO_SAMPLE_NUM; gyro_bias[2] sum[2] / GYRO_SAMPLE_NUM; }更讲究的场景还会做温度补偿把零偏随温度变化的曲线拟合出来在飞行中实时查表校正。但对大多数自研小四轴来说上电校准加“静止时定期重新校准”已经够用了。3.3 四元数更新为什么内部状态量不用欧拉角初次接触姿态解算的人都会问既然最后输出还是要转成欧拉角我直接用欧拉角当内部状态不行吗答案是不行核心原因是万向节锁。当俯仰角达到 ±90° 时滚转和偏航的旋转轴会重合欧拉角自由度退化姿态解算会出现奇异点。四轴飞行器理论上确实可以飞出不大于 90° 的俯仰但你想做翻滚动作、倒飞或者面对复杂机动时用欧拉角当内部状态就是埋雷。四元数用四个分量描述三维旋转没有奇异点代价只是多了一维约束——模长必须恒等于 1。更新公式长这样q_dot 0.5 · q ⊗ (0, ωx, ωy, ωz)离散化后就是常见的一阶龙格库塔q0 0.5f * dt * (-q1 * gx - q2 * gy - q3 * gz); q1 0.5f * dt * ( q0 * gx q2 * gz - q3 * gy); q2 0.5f * dt * ( q0 * gy - q1 * gz q3 * gx); q3 0.5f * dt * ( q0 * gz q1 * gy - q2 * gx);每步更新后必须归一化否则积分的模长误差会不断累积姿态就会逐渐失真。3.4 Mahony 互补滤波的落地代码Mahony 算法本质上就是一种互补滤波用加速度计的重力参考矢量和四元数推算出的重力投影矢量做叉积叉积结果近似等于姿态误差再把这个误差用 PI 补偿器反馈到陀螺仪角速度上。最关键的一点是用修正后的角速度去做四元数积分。完整实现如下这个函数建议放在固定采样周期的定时器中断里调用typedef struct { float q0, q1, q2, q3; // 四元数 float ex_int, ey_int, ez_int; // 积分误差项 float dt; // 姿态解算采样周期单位 s } ahrs_t; void MahonyAHRSupdate(ahrs_t *ahrs, float gx, float gy, float gz, // 陀螺仪rad/s已减零偏 float ax, float ay, float az) // 加速度计单位 g { float norm; float vx, vy, vz; float ex, ey, ez; float halfT 0.5f * ahrs-dt; float Kp 2.0f; float Ki 0.005f; // 加速度计归一化 norm sqrtf(ax * ax ay * ay az * az); if (norm 1e-6f) return; ax / norm; ay / norm; az / norm; // 根据当前四元数计算重力在机体坐标系下的投影 vx 2.0f * (ahrs-q1 * ahrs-q3 - ahrs-q0 * ahrs-q2); vy 2.0f * (ahrs-q0 * ahrs-q1 ahrs-q2 * ahrs-q3); vz ahrs-q0 * ahrs-q0 - ahrs-q1 * ahrs-q1 - ahrs-q2 * ahrs-q2 ahrs-q3 * ahrs-q3; // 叉积误差加速度计测量方向与推算方向之间的偏差 ex ay * vz - az * vy; ey az * vx - ax * vz; ez ax * vy - ay * vx; // 误差积分项用来消除稳态漂移 ahrs-ex_int Ki * ex * ahrs-dt; ahrs-ey_int Ki * ey * ahrs-dt; ahrs-ez_int Ki * ez * ahrs-dt; // 补偿角速度 gx Kp * ex ahrs-ex_int; gy Kp * ey ahrs-ey_int; gz Kp * ez ahrs-ez_int; // 四元数一阶积分更新 ahrs-q0 halfT * (-ahrs-q1 * gx - ahrs-q2 * gy - ahrs-q3 * gz); ahrs-q1 halfT * ( ahrs-q0 * gx ahrs-q2 * gz - ahrs-q3 * gy); ahrs-q2 halfT * ( ahrs-q0 * gy - ahrs-q1 * gz ahrs-q3 * gx); ahrs-q3 halfT * ( ahrs-q0 * gz ahrs-q1 * gy - ahrs-q2 * gx); // 归一化 norm sqrtf(ahrs-q0 * ahrs-q0 ahrs-q1 * ahrs-q1 ahrs-q2 * ahrs-q2 ahrs-q3 * ahrs-q3); if (norm 1e-6f) return; ahrs-q0 / norm; ahrs-q1 / norm; ahrs-q2 / norm; ahrs-q3 / norm; }最后需要把四元数转成欧拉角给上位机显示或者给控制环用float roll atan2f(2.0f * (q0*q1 q2*q3), q0*q0 - q1*q1 - q2*q2 q3*q3) * 57.29578f; float pitch asinf(2.0f * (q0*q2 - q1*q3)) * 57.29578f; float yaw atan2f(2.0f * (q0*q3 q1*q2), q0*q0 q1*q1 - q2*q2 - q3*q3) * 57.29578f;如果你发现 roll 或者 pitch 的符号是反的优先检查你的机体坐标轴定义和 MPU6050 安装方向不要急着乱改公式。4. 调参与飞控应用现场我在实际调试中踩过的具体坑4.1 Kp 和 Ki 的调参手感Mahony 互补滤波的调参其实比很多人想得简单。Kp 是比例修正增益决定“加速度计把陀螺仪漂移拉回来”的力度Ki 是积分增益负责消除长时间静置时残留的稳态误差。我的经验是先设 Ki 0只调 KpKp 太小姿态估计对加速度计的修正不敏感长时间悬停还是会有缓慢漂移Kp 太大加速度计的高频噪声会被放大姿态曲线重新变成“心电图”而且大机动时运动加速度干扰也会被放大。一个可行的调参起点200~500Hz 采样率下Kp 取 1.0~3.0Ki 取 0.001~0.01。调 Ki 时一点一点加加到静止几分钟不再漂为止。加多了你会发现猛烈转动后姿态会有明显的“过冲回弹”。现象可能原因调参/排查方向姿态曲线高频抖动Kp 过大 / 加速度计振动噪声降低 Kp检查减震安装长时间缓慢漂移零偏没校干净 / Ki 太小重新校准缓慢增大 Ki快速转动后角度跟手度差Kp 太小 / 采样周期不稳增大 Kp固定采样周期推油门瞬间角度跳变运动加速度干扰用加速度模长检测并降权静止时角度慢慢朝一个方向翻符号反了 / 零偏没减干净检查坐标轴方向和叉积符号4.2 保持采样周期稳定是第一优先级姿态解算对 dt 极其敏感。如果把 Mahony 更新放在主循环里而主循环同时又处理遥控器接收、电机输出、LED 闪烁那么每次循环的时间都不一样。dt 一会儿 2ms 一会儿 8ms积分的随机误差会直接转化为姿态的随机漂移而且这种漂移极难排查。我踩过一次特别深的坑芯片在冷启动时定时器初始化慢了半拍前几百个姿态解算周期 dt 全是乱的导致解锁后飞控差点直接翻机。后来把所有跟“时间”相关的逻辑都用硬件定时器统一管理姿态解算固定放在 1ms 定时器中断里问题立刻消失。如果实在没办法把解算放进中断至少要在主循环里用时间戳测量真实 dt喂给滤波函数而不是用固定常量。4.3 电机振动加速度计噪声的工程来源电机一转加速度计输出就会叠加大量高频振动。这种噪声在飞行中很难完全靠软件消除。你可以给加速度计信号加数字低通但滤波本身会带来相位延迟姿态环性能反而可能下降。工程上更推荐先做硬件减震传感器和飞控板之间加一层硅胶减震垫或海绵把高频振动从物理上隔掉。很多商品飞控的传感器都封装在减震结构里不是因为传感器怕摔而是因为高频振动会直接毁掉姿态解算的精度。软件层面如果发现静止时姿态曲线已经足够平但解锁后噪声骤增优先检查减震不要一上来就把 Kp 往下猛降。4.4 大机动下加速度计会“说谎”加速度计的参考矢量在正常情况下就是重力方向但大机动时运动加速度会混进来导致加速度计给出的方向偏离真实重力。典型现象是快速推油门时姿态角瞬间跳一下然后慢慢回正。一个实用的保护策略实时计算加速度计模长norm sqrt(ax²ay²az²)。如果 norm 明显偏离 1g比如大于 1.3 或小于 0.7就认为此刻运动加速度干扰很大应该降低加速度计的修正权重甚至直接切换到纯陀螺仪积分模式等 norm 恢复到 1g 附近再恢复互补修正。这也符合互补滤波的思想故障信号不可信时就少信它一些。4.5 一个判断算法是否正常的小技巧把四元数转出来的 roll/pitch/yaw 和角速度曲线同时打印到上位机。静止时roll 和 pitch 应当保持在 ±1° 以内yaw 允许有小幅漂移但速度很慢快速来回转动机体角度曲线应当跟手但不过冲猛地停下姿态应当迅速收敛到当前真实姿态没有长时间的回摆。如果静止时 roll/pitch 在缓慢爬升多半是陀螺仪零偏没校干净如果一转动就疯狂跳变多半是 Kp 太大或者坐标系符号反了。用这条流程排查比瞎调参数高效得多。5. 在飞控体系里看互补滤波它处在什么位置还有哪些路可走5.1 开源飞控里的互补滤波影子很多初级开发者以为开源飞控现在都用卡尔曼滤波互补滤波已经过时了。实际上不是这样。ArduPilot 早期版本大量使用的 DCM 方向余弦矩阵算法本质上就是方向余弦矩阵框架下的互补滤波用加速度计和磁力计修正陀螺仪积分的方向误差。PX4 也保留了基于四元数互补滤波的姿态估计器因为它在计算量、鲁棒性、调试复杂度之间取得了很好的平衡。只有在需要更高精度、更多传感器融合的场景才会升级到 EKF 这类基于状态协方差估计的算法。所以互补滤波不是玩具而是一套有深厚工程基础的姿态融合方案。你在学习阶段把它彻底搞懂后面看 EKF 也不会那么劝退。5.2 为什么 MPU6050 有 DMP还要自己写互补滤波MPU6050 内部集成了一个数字运动处理器 DMP可以直接输出四元数省掉你写滤波算法的工作。听起来很美好但实际工程里 DMP 有几个不太舒服的点DMP 固件是 InvenSense 提供的库闭源、有授权限制在产品化时存在法律和可控性风险DMP 输出频率和融合逻辑是封死的你想临时改一下融合权重或者接入自定义传感器数据根本插不进手姿态解算的学习价值几乎为零因为内部怎么运作你完全看不见。相比之下自己写 Mahony 互补滤波也就几十行代码算力开销极小代码完全可控还能随意扩展磁力计。如果你想做的是自研飞控或者深度定制我强烈建议至少把互补滤波版本调通再考虑是否上 DMP。5.3 什么情况下该继续用互补滤波什么情况下该上卡尔曼我的判断标准很简单学习阶段 / 低成本小四轴 / 固定翼姿态参考互补滤波足够别自我感动式上卡尔曼多旋翼长时间高精度悬停、混入GPS/气压计/磁力计/视觉等多种传感器上扩展卡尔曼或误差状态卡尔曼更合适因为卡尔曼框架能显式建模传感器噪声和观测模型传感器只有一个 IMU且算力极其紧张互补滤波依然是性价比极高的选择。从互补滤波过渡到卡尔曼时你会发现卡尔曼里的“预测”对应陀螺仪积分“更新”对应加速度计观测修正思路和互补滤波一脉相承只是把固定增益换成了随时间变化的卡尔曼增益。所以先把互补滤波的血肉吃透真的不是弯路。我自己调完这套算法后最大的体会是姿态解算这件事传感器原始数据的质量远比算法本身重要。零偏没校干净、采样周期不稳定、坐标系符号反了这些问题靠再高级的滤波算法也救不回来。先把原始数据、时间基准、坐标定义这三样东西弄得干干净净哪怕只用一个简单的一阶互补滤波也能让飞机稳稳地飞起来。如果你正被姿态角乱跳折磨先别急着折腾复杂算法回头看看这三样八成问题就出在那里。