032、四元数姿态更新算法

飞控算法从入门到精通 · 032 · 四元数姿态更新算法

从一次炸机说起

去年夏天调试一架四轴，飞控板是自研的STM32F4方案，IMU用的BMI088。地面站里姿态显示一切正常，解锁、起飞、悬停，前两分钟稳得像块石头。第三分钟开始，飞机突然朝一个方向缓慢倾斜，我以为是风，打了点反向杆，没反应。接着倾斜加速，遥控器拉满也救不回来，眼睁睁看着它侧翻炸地。

事后分析日志，发现一个诡异现象：陀螺仪积分出来的姿态角，在炸机前30秒开始与加速度计/磁力计融合后的姿态出现不可逆的偏差。问题出在姿态更新算法——我用的还是最原始的欧拉角微分方程，在俯仰角接近90度时，方程出现了奇异点，积分直接发散。

那次之后，我把整个姿态解算模块重写，核心换成了四元数更新算法。今天这篇笔记，就把四元数姿态更新的工程实现掰开揉碎讲清楚。

为什么非要用四元数

很多刚入门的朋友喜欢用欧拉角，因为直观。但欧拉角有三个致命问题：

1. 万向锁：当俯仰角接近±90°时，滚转和偏航的旋转轴重合，丢失一个自由度。这在固定翼或特技飞行中几乎是必然遇到的。
2. 三角函数计算量大：每次更新都要算sin/cos，在Cortex-M4上跑1000Hz更新率，CPU占用率直接飙到30%以上。
3. 插值困难：两个姿态之间做平滑过渡，欧拉角插值出来的路径是扭曲的。

四元数没有这些问题。它用四个参数表示三维旋转，没有奇异点，计算只需要乘法