031、DCM姿态更新算法

飞控算法从入门到精通 · 031 | DCM姿态更新算法

从一次炸机说起

去年夏天调试一架四轴，飞控板是自研的STM32F4，IMU用的MPU9250。起飞后悬停大约十秒，突然机身开始缓慢自旋，然后不可控地翻倒。黑匣子日志里，欧拉角在最后三秒出现了明显的“跳变”——俯仰角从15度瞬间跳到-170度，然后又跳回来。当时用的是经典的Mahony互补滤波，采样率400Hz，陀螺零偏已经校准过。

排查到最后，问题出在姿态更新环节：我用了欧拉角微分方程直接积分，当俯仰角接近90度时，万向锁导致计算发散。这不是算法本身的问题，而是姿态表示方法的选择问题。从那以后，我彻底转向了DCM（方向余弦矩阵）姿态更新算法，再没遇到过类似的奇异性崩溃。

DCM是什么，为什么用它

方向余弦矩阵，本质上是一个3×3的旋转矩阵，描述从机体坐标系到导航坐标系的旋转关系。每个元素是两个坐标系对应轴之间的夹角余弦值。比如矩阵第一行第一列，就是机体x轴与导航x轴夹角的余弦。

用DCM的好处很直接：

• 没有万向锁，任何姿态下都能稳定计算
• 不需要三角函数，全是乘加运算，对嵌入式MCU友好
• 便于融合加速度计和磁力计，因为矩阵的每一列都有明确的物理意义

代价是9个元素需要维护，比四元数的4个参数多了一倍多。但在Cortex-M4上，这点计算量根本不是问题。