飞控算法里，姿态解算一直是绕不开的坎。我最早接触飞控时，第一件事就是被“互补滤波”这四个字反复折磨，网上资料一抓一大把，但真正能把它讲清楚、能直接拿去跑飞的却不多。当时我手里就一块MPU6050加一块STM32，用的正是互补滤波算法，从四元数姿态解算到最终在屏幕上看到稳定的姿态角，中间踩了好些坑。这期内容我就把姿态解算里最经典的互补滤波从头到尾拆一遍，把这个算法的原理、推导、代码实现和调参经验一次性讲透，给还在入门路上折腾的人一条相对顺的路。
很多人把姿态解算想得很玄乎，其实它就是解决一个问题：怎么知道飞行器现在“朝哪、歪多少”。在飞控领域，姿态通常是拿三轴角度来描述的，也就是横滚（Roll）、俯仰（Pitch）和偏航（Yaw）。MPU6050这颗六轴传感器能够提供三轴加速度计和三轴陀螺仪的数据，加速度计能感受重力，陀螺仪能感受角速度，但单独用任何一个都不可靠。互补滤波做的事情，就是让两者“搭伙过日子”，把各自的优点拿出来用，缺点互相补掉。这个算法原理不复杂、计算量又小，放在STM32这类单片机上非常合适，也是小四轴、自平衡车、云台这类项目里最常见的姿态解算方案之一。这篇文章适合正在用STM32和MPU6050做飞控或机器人项目的朋友，尤其适合那种“原理看了不少，但一写代码就卡壳”的选手。
1. 姿态解算要解决什么问题，为什么绕不开互补滤波
1.1 飞控里的姿态到底是怎么定义的
飞行器在空中飞行，机身本身就是一个刚体，我们需要用一个相对直观的方式描述它的朝向。工程上最常见的就是欧拉角：绕自身坐标系Z轴旋转得到偏航角（Yaw），绕Y轴旋转得到俯仰角（Pitch），绕X轴旋转得到横滚角（Roll）。这三组角度合在一起，就可以确定飞行器的姿态。
但欧拉角有个很尴尬的毛病，就是“万向锁”问题。当俯仰角接近正负90度时，偏航和横滚的旋转轴会重合，系统失去一个自由度。这个问题在飞行器做大机动时是会真实出现的，比如你做翻滚动作时，某些瞬间俯仰角会到90度附近，这时候用欧拉角做解算就会出奇异，数学上直接崩掉。所以实际飞控代码里，姿态几乎都是拿四元数来存的，四元数没有奇异问题，而且更新时只需要算几个乘法加法，非常轻量。四元数到欧拉角的转换代码最后也只是几个三角函数的调用而已。
这也是为什么飞控的姿态解算模块，本质上就是一个“输入陀螺仪和加速度计的原始数据，输出能代表当前姿态的四元数”的过程。你只要看到那些姿态解算的例程里在疯狂更新四元数，就知道这个模块做的是什么事了。理解了这一步，后面再去看互补滤波的代码就不会觉得天书一样了。
1.2 只用加速度计会怎样，只用陀螺仪又会怎样
先说加速度计。它测的是什么？是比力，通俗点说，传感器感受到的加速度。静止不动的时候，加速度计的输出就是1g的重力加速度，方向竖直向下。所以我们可以利用重力在机体坐标系三个轴上的投影，反推出机体的倾角。这听起来很完美，而且它的优势是“绝对测量”，不管时间过了多久，加速度计只要感受到重力，就能告诉你哪个方向是“下”，不会漂移。
但加速度计的致命伤在于它特别怕运动。一旦机体有平移加速度，比如飞行器侧飞、往前冲刺或者机架在剧烈振动，加速度计输出的就不只是重力一个分量了，而是重力加运动加速度的混合值。此时你拿它算出来的角度就全是噪声和毛刺。我做测速小车的时候观察过，电机一拉满油门，加速度计数据狂跳，直接用加速度计算角度的后果就是姿态角抖得跟筛子一样。
再说陀螺仪。陀螺仪测的是角速度，也就是机体绕三轴转动的快慢。想要得到角度，只需要把角速度对时间做积分就行。它的好处是特别“稳”，短时间内积分出来的角度很干净，不受振动的干扰。但坏处也致命：陀螺仪有零偏，哪怕静止时，输出也不是精确的0，而是一个很小的偏移量。这个偏移量积分一分钟可能看不出问题，积分十分钟，角度就会一路漂到几十度去，根本没法用。
所以结论很直接：加速度计长于长期，陀螺仪长于短期，互补滤波就是让两者在频域上做一次“取长补短”。低频段相信加速度计，高频段相信陀螺仪，拼出一个既没有毛刺也不会漂移的姿态角。这个思路简单到让人觉得不可能有用，但它确实在无数飞控上稳定运行了十几年。
2. 互补滤波的核心思想，以及它背后的数学表达
2.1 从频域理解“互补”两个字
互补滤波的“互补”，指的是加速度计和陀螺仪的信号特征在频率域上是互补的。把姿态看作一个随时间变化的信号，陀螺仪积分出来的姿态在低频部分存在严重的漂移，而在高频部分噪声很小；加速度计解算出来的姿态在低频部分非常准确，也就是长期稳定不漂移，但高频部分充满了运动加速度带来的噪声。
所以我们可以设计一个一阶高通滤波器来处理陀螺仪积分结果，滤掉它的低频漂移；再设计一个一阶低通滤波器来处理加速度计的姿态，滤掉它的高频噪声。然后把两者相加，得到最终的姿态估计。因为高通和低通滤波器的截止频率是同一个值，两者频率特性反向互补，相加之后正好等于全通，所以称为“互补滤波”。这是最原始的一阶互补滤波，在单片机上的代码只要三五行就能写完，很多简单平衡小车的姿态解算就是用的这种形式。
但一阶互补滤波有一个明显不足：它相当于用固定系数去配比加速度计和陀螺仪的权重，无法根据飞行器的运动状态动态调整。悬停时加速度计很干净，可以多信一些；猛加速时加速度计全是噪声，应该少信甚至完全不信。一阶互补做不到这种自适应。这也是很多项目后期会转向卡尔曼滤波或更高阶姿态解算算法的原因。然而互补滤波计算量小、响应快、实现简单，对于大多数飞控场景已经足够好，而且它其实可以再做升级。
2.2 从一阶互补到PI互补滤波的演进逻辑
标准的一阶互补滤波公式可以写成这样：angle = (0.98) * (angle + gyro * dt) + (0.02) * acc_angle。这个0.98和0.02加起来等于1，系数越大越信任陀螺仪，越小越信任加速度计。它做起来很简单，但它有一个隐含问题：这个固定权重意味着它既不能根据动态调整，也无法抑制陀螺仪的常值漂移。陀螺仪的零偏是慢变的，如果积分一直在累积一个固定误差，简单权重再配也有积累。
所以工程上更常用的是Mahony提出的PI互补滤波方案。它的核心思想是：先用陀螺仪积分更新四元数，得到当前姿态；然后把加速度计的测量值与当前姿态推算出来的“重力方向参考值”做叉积，得到姿态误差；这个误差经过PI控制器，反馈到陀螺仪的角速度值上，对陀螺仪的零偏进行在线修正。如果陀螺仪有漂移，反馈回路会自动产生一个修正角速度把它拉回来。相当于把原来固定系数的那点“信任度”，换成了一个自适应的闭环修正机制。
这个思路在理解上可以这样类比：陀螺仪是那个“凭感觉闷头走”的人，走着走着会偏向，加速度计则是站在高处发号施令的“向导”，时不时喊一嗓子纠正方向。PI控制器就是那个协调员，向导喊的偏差小就少纠一点，偏差大就多纠一点，纠多了就让记忆里记住，以后主动修正。这个机制比固定权重聪明得多，也稳定得多。
2.3 四元数运算和互补滤波的结合点
既然实际代码里姿态用四元数存储，那互补滤波就得在四元数的框架下工作。整个过程可以分为几步：读取陀螺仪角速度，加上PI修正量，带入四元数微分方程进行积分更新；读取加速度计，归一化后得到测量方向；用当前四元数计算出理论重力方向；两者叉积得到误差，送进PI控制器；最终输出更新后的四元数。
这里先简单提一下四元数微分方程的形态：设四元数为q = [qw, qx, qy, qz]，角速度为ω = [ωx, ωy, ωz]，则四元数的导数满足一个矩阵乘法，展开之后是四个简单的线性方程。实际工程中我们一般不直接解微分方程，而是用一阶龙格-库塔近似：q += 0.5 * q ⊗ ω * dt。这个公式写出来的代码就是四个乘加运算，性能开销很小。互补滤波和四元数结合，整个姿态解算一帧算下来，在STM32F103这类芯片上也就几十微秒的事情，哪怕跑1000Hz都毫无压力。
3. 基于STM32和MPU6050的互补滤波完整实现
3.1 硬件和工程准备
我这里用的是最常见的STM32F103C8T6最小系统板加MPU6050模块，MPU6050通过I2C接口连接。STM32的I2C硬件外设速度慢且容易出错，我习惯用GPIO模拟I2C，稳定性和时序都好掌控。MPU6050的供电接3.3V，I2C上拉电阻一般模块上已经有了，连接就四根线：VCC、GND、SCL、SDA。
初始化配置注意三件事：一是量程设置，陀螺仪我习惯设成±500dps，加速度计设成±4g，这样既有足够量程又能保留一定精度，适合大多数飞行场景；二是低通滤波器配置，MPU6050内部自带数字低通，我一般把DLPF带宽设成20Hz左右，能有效滤掉部分振动噪声；三是采样率，这里我设置MPU6050以100Hz输出数据，但姿态解算代码控制在1kHz的循环里跑，两者不冲突。
代码里定义的关键变量包括一个姿态四元数q0、q1、q2、q3，以及后面PI控制器要用的积分误差。资源占用上，这个解算代码除了几个浮点运算外不占额外内存，对STM32来说是绰绰有余。工程框架按照“初始化MPU6050—读取原始数据—互补滤波解算—输出四元数和欧拉角”的流程组织，模块化清晰，方便后续替换成卡尔曼滤波或其他算法。
3.2 原始数据读取和预处理
MPU6050的原始输出是16位有符号整数，需要先除以对应量程的灵敏度系数才能转成有量纲的物理值。±500dps对应的灵敏度是65.5 LSB/(°/s)，±4g对应的是8192 LSB/g。所以在读取之后，第一件事就是把原始值除以这两个系数。
这一步是很多人容易忽视的坑。如果你灵敏度系数选错了，后续滤波的收敛速度会明显异常，姿态也会歪。我自己踩过，一度调了半天发现P参数对不上，最后查出来是加速度计量程设的±2g但代码里按±4g的系数在换算。这种低级的单位错误，往往比算法本身更折磨人。
另外还有一个细节：陀螺仪的原始数据最好去做一次零点校正。MPU6050上电静止时，陀螺仪三轴输出并不一定正好是0，可能有个几十LSB的偏移。这个偏移不消除，积分起来就会让姿态缓慢漂移。虽然互补滤波最后能通过加速度计把姿态拉回来，但校正后整体收敛更快更稳。校正方法很简单，上电后静止采样几百次陀螺仪数据求平均，把平均值当作零偏减掉就行。多花几十行代码，效果立竿见影。
3.3 互补滤波代码实现和逐步讲解
下面是完整的互补滤波姿态解算核心代码，基于Mahony算法实现，我加了一些注释方便阅读：
// 姿态四元数全局变量 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // PI控制器参数，需要根据实际情况调整 float Kp = 2.0f; float Ki = 0.05f; float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // 互补滤波主函数 // gx, gy, gz: 陀螺仪角速度，单位rad/s // ax, ay, az: 加速度计原始值，单位g // dt: 解算周期，单位秒 void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; gx = gx * 0.01745329f; // 把deg/s转成rad/s gy = gy * 0.01745329f; gz = gz * 0.01745329f; // 加速度计测量值归一化，得到单位向量 recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // 用当前四元数推算重力向量在机体坐标系下的分量 halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // 计算重力向量与加速度计测量值的叉积，得到姿态误差 halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // 如果叉积不为0，说明姿态有误差，用PI控制器修正陀螺仪 if (Ki > 0.0f) { integralFBx += Ki * halfex * dt; integralFBy += Ki * halfey * dt; integralFBz += Ki * halfez * dt; gx += integralFBx; gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; integralFBy = 0.0f; integralFBz = 0.0f; } gx += Kp * halfex; gy += Kp * halfey; gz += Kp * halfez; // 四元数微分方程积分，一阶龙格-库塔近似 qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * dt; q1 += (qa * gx + qc * gz - q3 * gy) * 0.5f * dt; q2 += (qa * gy - qb * gz + q3 * gx) * 0.5f * dt; q3 += (qa * gz + qb * gy - qc * gx) * 0.5f * dt; // 重新归一化四元数，防止累积误差导致四元数模长偏离1 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }代码里的invSqrt是快速平方根倒数算法，就是那个经典的数字魔法，在STM32这种没有硬件浮点单元的单片机上能显著提速。如果你用的是带FPU的M4内核，可以直接用数学库的sqrt和除法，差别不大。
关键点在于“叉积误差”的计算。你看代码里算出halfvx、halfvy、halfvz，是用当前四元数换算出的重力方向，如果姿态完全准确，这个向量应该和加速度计测量向量重合。一旦两者有夹角，叉积就会产生一个垂直于两向量的误差向量，其大小正比于角度误差。拿这个误差去修正陀螺仪值，就实现了闭环反馈。
需要注意这里计算的都是“半”向量，所以后面叉积结果也叫halfex，实际使用时PI系数要相应放大两倍。很多人在移植别处代码时没注意这个细节，发现Kp要设得特别大才能跟上姿态变化，实际上就是半向量和全向量的尺度差异引起的。
3.4 欧拉角输出和上位机显示验证
解算完的四元数最终要变成可读的欧拉角输出。转换公式如下：先用四元数算出旋转矩阵的元素，再通过反三角函数提取三个角度。横滚角和俯仰角可以用atan2算出，偏航角同理。公式在代码里是几行三角函数，不复杂。
// 将四元数转换成欧拉角 float roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f; float pitch = asinf(2.0f * (q0 * q2 - q3 * q1)) * 57.29578f; float yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f;注意，上面代码里的pitch计算公式当俯仰角达到正负90度时会出现定义域问题，这正是欧拉角本身的奇异点导致。因为飞控内部姿态是用四元数存的，就算转换出来的欧拉角在临界处跳变，只要你不拿它直接做控制，系统的姿态还不会乱。在实际飞控代码里，控制环使用的往往是旋转矩阵或四元数，欧拉角更多只用于显示和调试。
调试时我习惯把四元数或者欧拉角通过串口打印出来，在PC端用VOFA+或者匿名上位机看波形。把MPU6050平放不动，理论上横滚和俯仰应该保持在0度附近小幅波动，波动范围在正负0.5度以内算是健康。然后手动翻转板子，观察角度能不能快速跟随实际物理姿态。如果每次翻过去之后有小幅回弹或持续偏移，就说明PI参数还没调好，需要微调。
4. 调参方法和工程中的常见问题排查
4.1 Kp和Ki的调节顺序与手感
互补滤波就两个参数，Kp和Ki，但别小看这两个数，调起来是有讲究的。我建议先令Ki=0，只调Kp。把板子拿在手里快速晃动，观察屏幕或上位机上的姿态角响应。如果角度一会儿超前一会儿滞后，跟不上手的动作，说明Kp太小，增大它；如果角度出现高频抖动、来回震荡，说明Kp太大，减小它。反复试，找到一个“既不迟钝也不振铃”的临界值，这就是基准Kp。
Kp定好后再逐步加Ki。Ki的作用是消除稳态误差，也就是长时间保持同一个姿态时，陀螺仪零偏导致角度缓慢偏移的那个量。调Ki时把板子静止放在桌上，观察输出的角度能否长时间稳定不动。如果发现一分钟内角度缓慢漂移，就增大Ki。但Ki也不能太大，太大了会让角度出现缓慢的等幅振荡，频率很低但很恶心。
一个经验数据供参考，在100Hz解算频率下，Kp大约在1.0到4.0之间，Ki在0.01到0.1之间。这只是起步值，务必根据自己硬件现场调。不同MPU6050个体、不同安装方式，最优参数都会略微不同，但不会差到量级的程度。
4.2 MPU6050振动干扰的过滤技巧
飞控装在四轴或者小车上以后，机架振动会通过机械结构传导到MPU6050上，此时单独靠互补滤波算法去压制振动不太现实，毕竟互补滤波的高频抑制能力有限。常见的做法是三管齐下：第一，用减振棉或减震球把MPU6050和机架隔开，这是最物理也最有效的一招；第二，在MPU6050内部配置数字低通滤波器，把带宽降到20Hz甚至更低；第三，在读取原始数据之后再做一次滑动平均或软件低通滤波。
我自己的经验是，先上减震棉，再调MPU6050的DLPF，一般就够用了，软件端不需要额外做滤波。软件滤波虽然能压噪声，但会引入额外的相位延迟，导致姿态角响应变慢，这在飞控里是个很要命的隐患。如果非要软件滤波，也尽量选择零相移的方案，比如在记录一段时间后再做离线处理，但这无法用于实时控制。实时控制里，我更倾向于硬件减震加传感器内部滤波器，效果最好。
4.3 常见故障速查表
下面这些是我做飞控过程中实际遇到过的问题，整理成表格方便快速定位。
| 现象 | 可能原因 | 处理方式 |
|---|---|---|
| 静止时角度缓慢漂移 | 陀螺仪零偏未校准；Ki过小 | 上电静止采样求零偏并扣除；增大Ki |
| 快速运动时角度跟不上 | Kp太小；解算频率太高导致响应滞后 | 增大Kp；降低解算频率到与数据更新率匹配 |
| 角度高频抖动 | Kp太大；加速度计噪声过大；机架振动传导 | 减小Kp；增加DLPF滤波；加装减震棉 |
| 角度长时间等幅振荡 | Ki偏大 | 减小Ki |
| 翻转板子后角度弹回 | P增益不够或加速度计权重过高 | 增大Kp，查看量程设置是否正确 |
| 偏航角受加速度计影响 | 纯互补滤波缺少磁力计，偏航可观测性差 | 接受少量漂移；或另加磁力计做融合 |
其中最后一条值得多说一句：纯六轴姿态解算，也就是只有加速度计和陀螺仪，对偏航角来说并没有绝对参考。加速度计只能确定“下”的方向，也就是横滚和俯仰，但无法感知机头指向“北”还是“东”。所以偏航角长时间运行一定会缓慢漂移，这是物理原理决定的，不是算法缺陷。想要稳定偏航，必须加磁力计，用地球磁场做绝对参考。如果你的项目对偏航精度有要求，建议提前预留磁力计接口。
4.4 从互补滤波到更高级算法的升级路径
当你的项目跑通互补滤波之后，你会遇到它的性能天花板：它只有一个固定的截止频率，无法根据运动剧烈程度动态调整对加速度计和陀螺仪的信任度。这时候可以往两个方向走：一个是改用自适应增益的互补滤波，根据加速度计模长偏离1g的程度动态调节权重，运动小的时候多信加速度计，运动大的时候几乎只信陀螺仪，这个方法实现简单但效果提升明显；另一个就是上卡尔曼滤波或者更现代的动捕算法，本质都是在概率框架下给加速度计和陀螺仪建模，以最优方式估计姿态。
从我个人的经验来看，如果只是为了做小四轴或者平衡车，互补滤波已经完全够用，完全没有必要为了“高级”而强行上卡尔曼滤波。卡尔曼滤波的计算量大、调试门槛高，很多情况下调出来的效果还没有一个参数得当的互补滤波稳定。先把互补滤波吃透，理解其中的物理含义和参数敏感性，再根据需求决定是不是要升级，这条路比一开始就啃卡尔曼滤波要靠谱得多。
5. 写在最后
互补滤波这个算法，是我觉得姿态解算里性价比最高的入门选择。它不需要高深的数学，计算量很小，在STM32上跑得飞快，而且只要参数调得好，效果不输很多复杂算法。我在实际项目中反复体会到，真正让姿态解算稳定的，往往不是算法的选择，而是对传感器特性的理解和对细节的把控。比如量程配置对不对、陀螺仪零偏有没有校准、采样时间准不准、机架振动有没有隔离，这些点任何一个出差错，再高级的算法也救不回来。
如果看完这篇文章，你想自己动手试一遍，我建议从一块MPU6050加一块STM32最小板开始，把上面的代码敲进去，先不急着调参，把上位机波形调出来，让姿态角实时显示在屏幕上。亲眼看到板子倾斜时角度跟着变，那种感觉和你只看公式是完全不一样的。等这一套流程跑通，恭喜你，飞控的地基算是打牢了。后面无论是继续优化姿态解算，还是上PID控制、写飞行控制环，都会顺手很多。
