第3章姿态描述基础

3、姿态描述基础：欧拉角、方向余弦矩阵、四元数的定义与基本运算

各位同学，咱们今天聊点硬核的。搞惯导，说白了就是跟姿态干架。你得知道你的导弹、飞机或者机器人，脑袋朝哪、身子歪没歪。描述姿态的方法有好几种，但最常用的就是欧拉角、方向余弦矩阵和四元数。这三种东西，我这些年跟它们打交道最多，踩过的坑也不少，今天一次性给你们讲透。

3.1 欧拉角：最直观，但最危险

欧拉角是什么？说白了就是三个角度：航向角（ψ）、俯仰角（θ）、横滚角（γ）。你想象一下飞机：机头指向哪是航向，抬头低头是俯仰，左右倾斜是横滚。嗯，就这么简单。

我个人习惯用“3-2-1”转序，也就是先转航向，再转俯仰，最后转横滚。为什么？因为这样最符合人的直觉。你想想看，你站在地上，先转个身（航向），再抬头（俯仰），最后歪个脖子（横滚），是不是很自然？

定义公式：

从导航系（n系）到载体系（b系）的旋转矩阵，用欧拉角表示为：

C_n^b = R_x(γ) · R_y(θ) · R_z(ψ)

其中：

R_z(ψ) = [cosψ sinψ 0; -sinψ cosψ 0; 0 0 1]
R_y(θ) = [cosθ 0 -sinθ; 0 1 0; sinθ 0 cosθ]
R_x(γ) = [1 0 0; 0 cosγ sinγ; 0 -sinγ cosγ]

⚠️ 避坑指南：

我曾经在项目里吃过欧拉角的大亏。当时做无人机飞控，俯仰角到了90度，结果航向角和横滚角突然乱跳。这就是著名的“万向锁”问题。说白了，当俯仰角接近±90°时，航向和横滚的旋转轴重合了，你分不清哪个是哪个。所以，如果你做的是全姿态运动（比如战斗机、导弹），千万别只用欧拉角，否则会出大事。

3.2 方向余弦矩阵：稳定，但计算量大

方向余弦矩阵（DCM），说白了就是一个3×3的矩阵，里面每个元素都是两个坐标系轴之间的夹角余弦。你想想看，两个坐标系之间，一个轴对另一个轴，总共有9个夹角，所以矩阵有9个元素。

我记得刚入行时，老工程师跟我说：“小张，DCM是最稳的，但也是最笨的。”为什么？因为9个元素，每个都要算，而且还要保证正交性。你算着算着，矩阵可能就不正交了，还得重新归一化。麻烦是麻烦，但胜在不会出现万向锁。

定义：

方向余弦矩阵 C_n^b 是一个3×3的正交矩阵，满足：

C_n^b = [c11  c12  c13; c21  c22  c23; c31  c32  c33]

其中 c_ij = cos(θ_ij)，θ_ij 是导航系第i轴与载体系第j轴的夹角。

基本运算：

矩阵乘法：两个DCM相乘，相当于连续旋转。比如 C_b^n = C_n^b^T，因为它是正交矩阵，转置等于逆。
正交化：我建议每次更新后都做一次Gram-Schmidt正交化，否则误差会累积。公式如下：

// 伪代码：Gram-Schmidt正交化
c1 = C(:,1) / norm(C(:,1))
c2 = C(:,2) - (c1' * C(:,2)) * c1
c2 = c2 / norm(c2)
c3 = cross(c1, c2)  // 保证右手系

💡 我的经验：

在实际工程中，我很少直接用DCM做姿态更新，因为9个元素太费算力。但DCM有个好处：它可以直接用来做坐标变换。比如你要把加速度计测到的比力从载体系转到导航系，直接用DCM乘一下就行，非常直观。

3.3 四元数：轻巧，但抽象

四元数，说白了就是一个超复数。它由四个数组成：一个实部，三个虚部。写成 q = q0 + q1·i + q2·j + q3·k。你想想看，它其实是在描述一个旋转轴和旋转角度。轴是三维的，角度是一维的，加起来正好四个数。

我个人最喜欢用四元数，为什么？因为它没有万向锁，计算量小，而且插值平滑。我做过一个导弹的制导系统，姿态更新频率是1000Hz，如果用DCM，CPU直接冒烟。换成四元数，轻松搞定。

定义：

单位四元数 q = [q0, q1, q2, q3]^T，满足 q0^2 + q1^2 + q2^2 + q3^2 = 1。

它对应的旋转是：绕单位向量 u = [u1, u2, u3]^T 旋转角度 θ，则：

q0 = cos(θ/2)
q1 = u1·sin(θ/2)
q2 = u2·sin(θ/2)
q3 = u3·sin(θ/2)

基本运算：

乘法：两个四元数相乘，对应两次旋转。公式有点绕，但记住：

q = p ⊗ r = [p0*r0 - p1*r1 - p2*r2 - p3*r3;
              p0*r1 + p1*r0 + p2*r3 - p3*r2;
              p0*r2 - p1*r3 + p2*r0 + p3*r1;
              p0*r3 + p1*r2 - p2*r1 + p3*r0]

共轭： q* = [q0, -q1, -q2, -q3]，相当于反向旋转。
归一化：每次更新后，记得做 q = q / ||q||，否则误差会累积。

⚠️ 避坑指南：

我曾经在调试时发现，四元数更新后姿态突然跳变。查了半天，原来是忘了归一化。你想想看，四元数如果不归一化，它代表的就不是纯旋转，而是旋转加缩放。所以，每次更新后，一定要归一化，这是铁律。

3.4 三种方法的对比

好了，三种方法都讲完了。我给你们总结一下，方便你们选型时参考：

特性	欧拉角	方向余弦矩阵	四元数
参数数量	3个	9个	4个
直观性	★★★★★	★★☆☆☆	★★☆☆☆
万向锁	有	无	无
计算量	小	大	中
插值平滑性	差	中	好
工程常用场景	地面车辆、低动态	坐标变换、静态对准	飞行器、高动态

💡 我的建议：

如果你做的是初始对准，我建议这样搭配：

粗对准：用欧拉角，因为直观，方便调试。
精对准：用四元数，因为要频繁更新，计算量小。
坐标变换：用DCM，因为直接乘就行，不用来回转换。

嗯，说白了就是各取所长。我在实际项目中，经常是三种方法混着用。比如，先用欧拉角算个初值，转成四元数做滤波，最后用DCM输出结果。灵活一点，别死板。