姿态解算算法（一）：欧拉角法与方向余弦矩阵

各位同学，咱们今天聊聊姿态解算里最基础、也最绕不开的两个方法——欧拉角法和方向余弦矩阵。说实话，我刚入行那会儿，觉得这东西不就是几个角度来回转嘛，有啥难的？直到第一次在飞控上遇到万向锁，飞机直接失控翻滚，我才老老实实回去啃数学。嗯，咱们今天就把这块硬骨头啃透。

一、欧拉角法：最直观的姿态表示

欧拉角法，说白了就是用三个角度来描述一个刚体在三维空间里的朝向。你想想看，飞机在空中，无非就是俯仰、偏航、滚转这三个动作。我习惯用 Z-Y-X 的顺序来旋转，也就是先绕Z轴转偏航角 ψ，再绕Y轴转俯仰角 θ，最后绕X轴转滚转角 φ。

为什么用这个顺序？因为我在实际项目中试过其他顺序，比如Z-X-Y，结果发现跟惯导传感器的输出对不上，调试起来特别别扭。后来我查了资料才知道，航空领域约定俗成就是Z-Y-X，咱们做工程，最好别跟行业标准对着干。

每个旋转矩阵长这样：

R(ψ) = [cosψ  -sinψ  0;  sinψ  cosψ  0;  0  0  1]
R(θ) = [cosθ   0     sinθ;  0    1    0;  -sinθ  0  cosθ]
R(φ) = [1      0     0;  0  cosφ  -sinφ;  0  sinφ  cosφ]

把它们乘起来，就得到完整的姿态矩阵。嗯，这里要注意，矩阵乘法不满足交换律，顺序错了，结果就全错了。我曾经有个同事，把旋转顺序搞反了，调了三天没找到bug，最后发现是这里的问题。

二、方向余弦矩阵：另一种视角

方向余弦矩阵（DCM），其实跟欧拉角法描述的是一回事，只是角度不同。DCM里的每个元素，是两个坐标系之间坐标轴夹角的余弦值。说白了，就是告诉你「机体系的X轴在导航系里指向哪里」。

我个人觉得，DCM比欧拉角更「硬核」一些。为什么？因为DCM没有万向锁问题，而且更新起来更平滑。但代价是——它有9个元素，计算量比欧拉角的3个参数大得多。

对比项	欧拉角法	方向余弦矩阵
参数数量	3个	9个
直观性	高（俯仰/偏航/滚转）	低（全是余弦值）
万向锁	有	无
计算量	小	大
适用场景	俯仰角不大时	全姿态运动

三、欧拉角微分方程：从角度到角速度

咱们做导航，光知道姿态还不够，还得知道姿态怎么变。这就引出了欧拉角微分方程。它把欧拉角的变化率跟机体角速度（陀螺仪测的那个）联系起来了。

公式长这样：

[φ̇; θ̇; ψ̇] = [1  sinφ·tanθ  cosφ·tanθ;  
                0  cosφ       -sinφ;  
                0  sinφ/cosθ  cosφ/cosθ] · [p; q; r]

其中 p、q、r 是陀螺仪测得的机体角速度。

我在做无人机飞控时，遇到过俯仰角拉到85度以上，姿态解算直接崩了。后来我加了个判断：如果 |θ| > 80°，就切换到四元数法。嗯，这是工程上的折中方案。

四、万向锁问题：绕不开的坑

万向锁，说白了就是当俯仰角达到 ±90° 时，偏航和滚转变得无法区分。你想想看，飞机机头朝上垂直爬升，这时候你让它偏航和让它滚转，效果是一样的——都是绕垂直轴转。

为什么会这样？因为三个旋转轴不再互相独立了。两个轴重合了，自由度从3降到了2。我见过有人用欧拉角做全姿态模拟，结果在万向锁附近，姿态估计直接发散。

五、方向余弦矩阵更新：如何让DCM动起来

DCM的更新，用的是下面这个微分方程：

Ṙ = R · [ω]×

其中 [ω]× 是角速度的反对称矩阵：

[ω]× = [0   -r    q;  
         r    0   -p;  
        -q    p    0]

实际工程中，我们不会直接解这个微分方程，而是用数值积分。我常用的是一阶龙格-库塔法，简单够用：

R(t+dt) = R(t) · (I + [ω]× · dt)

但这里有个坑——这样更新出来的R，可能不再是严格的正交矩阵了。因为数值误差会累积。所以每隔几步，就得做一次正交化修正。我一般每10步做一次，用牛顿法迭代修正。

这个公式看着简单，但效果很好。我实测过，1000步迭代后，误差能控制在10⁻⁶以内。

六、优缺点对比：什么时候用哪个？

好了，咱们来总结一下。我个人在实际项目中的选择原则是这样的：

• 欧拉角法： 适合俯仰角变化范围小（比如地面车辆、固定翼平飞），需要直观显示姿态的场景。优点是计算快、好理解。缺点是万向锁、不能全姿态。
• 方向余弦矩阵： 适合全姿态运动（比如特技飞行、机器人手臂）。优点是没奇异性、更新平滑。缺点是计算量大、需要正交化维护。
• 四元数法： 这个咱们下节课讲。提前剧透一下——它结合了两者的优点，是现代导航系统的主流选择。

我记得有一次做水下机器人的姿态估计，俯仰角经常到 ±90°，我果断用了DCM。虽然计算量大了点，但胜在稳定。后来换到四元数，效果更好。

嗯，今天就先讲到这里。下节课咱们聊四元数法，那才是真正的大杀器。回去之后，建议你把欧拉角微分方程手推一遍，别光看。我当年就是推了三遍，才真正理解为什么会有万向锁。

有什么问题，咱们课后交流。