033、姿态解算中的数值积分方法
飞控算法从入门到精通 | 033、姿态解算中的数值积分方法
一、一次真实炸机带来的思考
去年调试一款自研四旋翼，飞控板用的STM32F405，IMU是BMI088。室内悬停一切正常，拉到室外有风环境，飞机突然开始高频抖动，三秒后直接翻倒炸机。回传日志一看，姿态角在0.5秒内从5度跳变到45度，欧拉角曲线像被狗啃过一样。当时第一反应是陀螺仪坏了，但重新上电自检又正常。
后来用示波器抓SPI总线，发现陀螺仪数据本身没问题，问题出在姿态解算的积分环节——我用的是一阶欧拉积分，步长4ms，在角速度变化剧烈时，数值误差被快速累积，最终导致姿态发散。那次之后我彻底明白了：姿态解算的精度，一半靠传感器，一半靠积分方法。
二、为什么数值积分在飞控里这么要命
飞控的姿态解算本质是在解微分方程：
[
\dot{q} = \frac{1}{2} q \otimes \omega
]
或者用旋转矩阵形式：
[
\dot{R} = R [\omega]_\times
]
这些方程在连续时间域是精确的，但我们的MCU只能离散采样。陀螺仪输出的是角速度采样值，我们需要用这些离散点去近似连续积分。误差来源有两个：一是采样间隔内的角速度变化被忽略（截断误差），二是浮点运算的舍入误差。在飞控这种实时性要求高的场景，步长通常只有2-10ms，截断误差是主要矛盾。