4、姿态解算算法（一）：互补滤波原理、Mahony算法详解、代码实现。
各位同学，欢迎来到姿态解算的第一讲。
飞控里什么最核心？我个人觉得，姿态解算排第一。你想想看，如果连飞机当前是正着还是歪着都搞不清楚，那后面的PID控制、位置估算全白搭。今天咱们就啃这块硬骨头——互补滤波和Mahony算法。
4.1 为什么需要姿态解算？
说白了，飞控就是个“盲人”。它没有眼睛，只能靠传感器去“摸”世界。陀螺仪告诉它转得有多快，加速度计告诉它重力朝哪边，磁力计告诉它北在哪。
但问题是，每个传感器都有毛病：
- 陀螺仪：短时间准，但时间一长就飘。我做过一个测试，让IMU静止不动，纯积分陀螺仪数据，10分钟后姿态偏了快30度。
- 加速度计：长时间稳，但一有震动就炸。飞机电机一转，加速度计读数全是噪声。
- 磁力计：能定航向，但容易被电机磁场干扰。
所以，我们需要一个“聪明”的算法，把这三个传感器的优点结合起来，缺点互相抵消。这就是姿态解算的核心思想——数据融合。
核心观点：姿态解算不是选一个最好的传感器，而是让所有传感器“投票”，取一个最靠谱的结果。
4.2 互补滤波原理——直觉上的理解
互补滤波这个名字听起来挺唬人，其实道理很简单。
陀螺仪的高频数据（快速变化）很准，但低频（缓慢漂移）不准。加速度计和磁力计正好反过来：低频稳，高频噪。互补滤波就是做一个“频率分工”：
- 陀螺仪负责快速响应——飞机突然转一下，全靠它。
- 加速度计/磁力计负责长期修正——把陀螺仪慢慢拉回正确位置。
数学上就是一个加权平均：
最终姿态 = α × (陀螺仪积分) + (1 - α) × (加速度计/磁力计估算)这里的α通常取0.98或0.99。为什么？因为陀螺仪更可信，权重就大一点。
我的经验：α值不是死的。如果你用的陀螺仪噪声大，α可以降到0.95；如果加速度计特别烂（比如廉价的MEMS），α可以升到0.995。我一般在调试时先设0.98，然后看悬停时的姿态波动，再微调。
4.3 Mahony算法详解——从理论到实现
Mahony算法是互补滤波的一种经典实现。它不直接融合角度，而是融合四元数。为什么用四元数？因为避免了欧拉角的万向锁问题，计算量也小。
4.3.1 算法流程
Mahony的核心就三步：
- 预测：用陀螺仪数据更新四元数（积分）。
- 观测：用加速度计和磁力计数据，算出当前姿态的“误差”。
- 修正：用这个误差去补偿陀螺仪的漂移，然后重复。
下面这张图把整个流程串起来了：
4.3.2 误差到底怎么算？
这里有个关键点——叉积。我刚开始学的时候也懵，为什么两个向量叉一下就是误差？
你想想看，加速度计测出来的重力方向（在机体坐标系下）是 [ax, ay, az]。同时，我们根据当前四元数，也能算出一个“预测的重力方向”。如果两个方向完全一致，叉积为0，说明没误差。如果它们有夹角，叉积的大小正比于夹角的正弦值，方向就是旋转轴。
这个叉积，就是陀螺仪的“漂移指示器”。
注意：叉积只在角度偏差较小时近似线性。如果飞机倒置（偏差接近180度），叉积会变小甚至为0，导致修正失效。所以Mahony算法在初始化时，一定要保证飞机大致水平。
4.4 代码实现——手写一个Mahony
光说不练假把式。下面是我在实际项目中用过的Mahony核心代码，去掉了工程冗余，保留了最精华的部分。
// Mahony 互补滤波核心函数
// gx, gy, gz: 陀螺仪数据 (rad/s)
// ax, ay, az: 加速度计数据 (归一化后)
// mx, my, mz: 磁力计数据 (归一化后)
// dt: 采样时间 (s)
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz, float dt)
{
float recipNorm;
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3;
float q3q3;
// 1. 加速度计归一化
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 2. 磁力计归一化
recipNorm = invSqrt(mx*mx + my*my + mz*mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// 3. 用当前四元数计算预测的重力方向
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// 预测重力向量 (在机体坐标系下)
float halfvx = q1q3 - q0q2;
float halfvy = q0q1 + q2q3;
float halfvz = q0q0 - 0.5f + q3q3;
// 4. 计算误差 (叉积)
float ex = (ay * halfvz - az * halfvy);
float ey = (az * halfvx - ax * halfvz);
float ez = (ax * halfvy - ay * halfvx);
// 5. PI补偿器
integralFBx += Ki * ex * dt;
integralFBy += Ki * ey * dt;
integralFBz += Ki * ez * dt;
gx += Kp * ex + integralFBx;
gy += Kp * ey + integralFBy;
gz += Kp * ez + integralFBz;
// 6. 四元数积分 (一阶龙格-库塔)
q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * dt;
q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * dt;
q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * dt;
// 7. 归一化四元数
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}4.4.1 关键参数说明
| 参数 | 作用 | 典型值 | 我的建议 |
|---|---|---|---|
| Kp | 比例增益，决定误差修正的强度 | 0.5 ~ 2.0 | 先设1.0，如果姿态震荡就减小 |
| Ki | 积分增益，消除稳态误差 | 0.001 ~ 0.05 | 我一般设0.005，太大容易低频振荡 |
| dt | 采样周期，必须准确 | 0.001 ~ 0.005 | 用定时器测量，不要用延时估算 |
避坑指南：我曾经在Ki上吃过亏。有一次为了快速收敛，把Ki设到了0.1，结果飞机悬停时像“点头”一样周期性晃动。后来才明白，Ki太大相当于给陀螺仪偏置一个错误的修正，反而引入了低频振荡。记住：Ki宁小勿大。
4.5 实际调试中的几个坑
代码写完了，不代表就能跑。我总结几个实际项目中容易踩的坑：
- 传感器坐标系不一致：陀螺仪和加速度计的坐标系可能反了。我见过有人把X轴和Y轴搞反，结果飞机一推油门就翻。上电后先手动旋转，看数据变化方向对不对。
- 归一化不能省：加速度计和磁力计数据必须归一化。如果不归一化，叉积的大小会受加速度幅值影响，导致修正强度不稳定。
- 四元数归一化：积分步长累积误差会让四元数模长偏离1，必须每步都归一化。否则姿态会慢慢“缩”成一个点。
- 磁力计干扰：电机大电流时磁场畸变严重。我一般只在起飞前用磁力计做一次航向初始化，起飞后就只用陀螺仪积分航向，靠加速度计修正俯仰和横滚。
警告：不要在电机高速旋转时依赖磁力计做航向修正。我曾经在测试时没注意这个，飞机在空中突然“甩头”90度，差点炸机。后来加了磁力计置信度判断，当电机油门超过30%时，降低磁力计权重。
4.6 小结
好了，这一讲的内容就到这。互补滤波和Mahony算法，说白了就是“陀螺仪做主角，加速度计和磁力计做配角”。主角负责动作戏，配角负责把跑偏的主角拉回来。