3、姿态解算入门：加速度计与陀螺仪原理、互补滤波原理、Mahony姿态解算算法
好，咱们进入正题。姿态解算，说白了就是回答一个问题：“飞机现在朝哪边？”
你想想看，飞控要控制姿态，首先得知道姿态是什么。这就像你闭着眼睛走路，得先知道自己是面朝北还是面朝南。姿态解算就是飞控的“眼睛”和“内耳”。
我个人习惯把姿态解算分成两步：传感器读数据，然后算法算姿态。今天咱们就把这两步拆开揉碎了讲。
3.1 加速度计：它能告诉你什么？
加速度计，顾名思义，测的是加速度。但这里有个坑——它测的是比力，不是单纯的运动加速度。
什么意思呢？
静止放在桌面上的加速度计，它读出来的是 (0, 0, 9.8) m/s²。明明没动，为什么有加速度？因为重力。重力加速度方向向下，而加速度计的敏感轴感受到的是“支撑力”带来的反作用。
嗯，这里要注意：加速度计无法区分重力加速度和运动加速度。这是它的天生缺陷。
核心结论：
- 加速度计可以测出重力方向 → 从而算出俯仰角和横滚角
- 但它对偏航角（Yaw）无能为力——因为重力方向不随偏航变化
- 有运动加速度时，测量值会“污染”，导致姿态估计不准
我在项目中遇到过一个问题：四旋翼快速前飞时，加速度计读到的“重力方向”明显偏了，导致姿态估计出现几度的误差。后来怎么解决的？用陀螺仪来补偿。
3.2 陀螺仪：角速度的忠实记录者
陀螺仪测的是角速度，单位是 °/s 或 rad/s。它没有加速度计那种“被运动加速度污染”的问题。
但陀螺仪有另一个毛病——漂移。
你想想看，陀螺仪输出的是角速度，要得到角度就得积分。积分嘛，误差会累积。哪怕零点几度的偏差，积分几秒钟就变成几度了。
避坑指南：
我曾经调试一架固定翼，陀螺仪零偏没校准好，起飞后偏航角以每秒0.5°的速度漂移。30秒后，飞机以为自己在直飞，实际已经转了15°。结果嘛……嗯，那次炸机让我记住了：陀螺仪零偏校准是必修课。
陀螺仪的优点和缺点都很明显：
| 传感器 | 优点 | 缺点 |
|---|---|---|
| 加速度计 | 长期稳定（不漂移） | 受运动加速度干扰，高频噪声大 |
| 陀螺仪 | 动态响应快，不受运动加速度影响 | 有零偏漂移，低频误差累积 |
看到没？这两个传感器正好互补。一个低频好、高频差；另一个高频好、低频差。那把它们融合起来，不就是完美的姿态估计吗？
3.3 互补滤波：最简单的融合思路
互补滤波的思路特别直观：
- 陀螺仪算出的角度，用高通滤波器滤掉低频漂移
- 加速度计算出的角度，用低通滤波器滤掉高频噪声
- 两者相加，得到融合后的姿态
公式长这样：
角度 = α × (角度_陀螺仪) + (1 - α) × (角度_加速度计)其中 α 是融合系数，通常取 0.98 左右。α 越大，越信任陀螺仪；α 越小，越信任加速度计。
我建议新手先从互补滤波入手。它简单、直观、计算量小。在STM32F103这种老芯片上都能跑得飞起。
调参小技巧：
α 的取值跟你的传感器更新频率有关。一般经验是：
- 陀螺仪更新率 1kHz，加速度计 100Hz → α ≈ 0.98
- 如果加速度计更新率更低，α 要适当调大
我习惯先设 α=0.98，然后看悬停时的角度波动。如果波动太大，说明加速度计权重不够，调小 α；如果响应太慢，调大 α。
3.4 Mahony姿态解算算法：工程界的经典
互补滤波虽然简单，但有个问题——它处理的是标量角度，不是完整的姿态表示。实际飞控里，我们更常用四元数来表示姿态。
Mahony算法就是基于四元数的互补滤波改进版。它由Robert Mahony提出，在开源飞控里用得特别多。
核心思想就三步：
- 用陀螺仪积分更新四元数（短期预测）
- 用加速度计和磁力计计算误差（长期修正）
- 用PI控制器修正陀螺仪偏差（消除漂移）
你看，它把姿态解算问题，转化成了一个控制问题。用PI控制器来修正陀螺仪的零偏，这个思路非常巧妙。
代码实现大概长这样（简化版）：
// Mahony算法核心步骤（伪代码）
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az) {
// 1. 归一化加速度计数据
norm = sqrt(ax*ax + ay*ay + az*az);
ax /= norm; ay /= norm; az /= norm;
// 2. 用当前四元数估计重力方向
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 3. 计算误差（叉积）
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;
// 4. PI控制器修正陀螺仪
exInt += ex * Ki;
eyInt += ey * Ki;
ezInt += ez * Ki;
gx += Kp*ex + exInt;
gy += Kp*ey + eyInt;
gz += Kp*ez + ezInt;
// 5. 用修正后的角速度更新四元数
// ... 四元数微分方程
}关键参数说明：
- Kp：比例增益，决定对加速度计的信任程度。Kp越大，修正越快，但噪声也越大。
- Ki：积分增益，用于消除陀螺仪零偏。Ki太小，漂移消不掉；Ki太大，会引入低频振荡。
我调Mahony参数的经验是：先设Ki=0，只调Kp。等姿态能跟住了，再慢慢加Ki。Ki从0.001开始试，每次翻倍，直到出现振荡再退回上一档。
3.5 实际工程中的注意事项
讲完了原理，说几个实际干活时容易踩的坑：
- 加速度计数据一定要先低通滤波。机臂振动会带来大量高频噪声，不滤波的话，姿态会跟着抖。
- 陀螺仪数据要减去零偏。每次上电后，静止采集100个点取平均，作为零偏值。
- 磁力计慎用。室内有钢筋、电机有大电流，都会干扰磁力计。我一般只用加速度计+陀螺仪做俯仰和横滚，偏航用磁力计辅助或者干脆不用。
一个血的教训：
我曾经在电机全速运转时测试姿态，发现Mahony算法输出的角度剧烈跳动。查了半天，发现是电机电流产生的磁场干扰了加速度计的测量轴。后来在加速度计和飞控板之间加了减震海绵，问题解决。
所以，传感器安装的机械隔离，有时候比算法本身更重要。
3.6 小结
这一章咱们聊了：
- 加速度计测重力方向，但怕运动加速度
- 陀螺仪测角速度，但会漂移
- 互补滤波把两者简单融合
- Mahony算法用四元数+PI控制器，更鲁棒
下一章，咱们会深入讲四元数的数学原理，以及如何用四元数做姿态控制。到时候你会发现，理解了四元数，整个飞控的姿态部分就通了一半。
嗯，今天就到这儿。回去可以试试在STM32上跑一下Mahony算法，看看悬停时的角度波动。有问题随时交流。