第3章刚体运动学基础

3. 刚体运动学基础：姿态、角速度与四元数

各位同学，今天我们聊聊刚体运动学。说实话，这部分内容是整个惯导系统的“地基”。你想想看，飞行器在天上怎么转、怎么翻，我们怎么知道它当前的脸朝哪边？靠的就是姿态描述。我个人习惯把姿态、角速度和四元数这三样东西，看作是“描述飞行器怎么动”的三把钥匙。

3.1 姿态描述：从欧拉角到旋转矩阵

最早接触姿态，大家肯定都是从欧拉角开始的。俯仰、滚转、偏航，听起来很直观。我在项目中遇到过一位同事，他特别喜欢用欧拉角做控制，因为“一看就懂”。确实，欧拉角的好处是物理意义明确，三个角度就能描述一个刚体在空间中的朝向。

但这里有个坑——万向锁。说白了，就是当俯仰角接近±90°时，滚转和偏航会变得无法区分。我曾经在调试一个无人机仿真时，发现飞机翻了个跟头后姿态解算直接炸了，查了半天，就是万向锁在作怪。所以，如果你做的是全姿态飞行（比如特技无人机），欧拉角就不太够用了。

那怎么办？用旋转矩阵。旋转矩阵是一个3×3的正交矩阵，它没有奇点问题。但代价是——9个参数，冗余度太高。你想想看，每次更新都要保证它是正交的，否则数值误差会越积越多。嗯，这里要注意，实际工程中很少直接用旋转矩阵做递推，更多是用它做坐标变换。

核心结论：

欧拉角：直观，但有万向锁，适合小角度运动
旋转矩阵：无奇点，但冗余，适合坐标变换
四元数：无奇点，无冗余，适合递推计算

3.2 角速度：刚体转动的“速度计”

姿态描述的是“位置”，那角速度描述的就是“变化率”。在惯导系统中，我们通常用陀螺仪来测量角速度。陀螺仪输出的就是机体坐标系下的三轴角速度，单位是rad/s。

这里有个关键点：角速度本身不是姿态的导数。为什么？因为姿态参数（比如欧拉角）的导数与角速度之间有一个非线性变换关系。我建议你记住下面这个公式，它经常出现在代码里：

// 欧拉角速率与机体角速度的关系（ZYX顺序）
// 假设 phi = 滚转, theta = 俯仰, psi = 偏航
// p, q, r 为机体角速度

phi_dot   = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta);
theta_dot = q*cos(phi) - r*sin(phi);
psi_dot   = q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta);

你看，当theta接近90°时，tan(theta)会趋于无穷大，这就是万向锁的数学本质。所以，如果你用欧拉角做姿态更新，一定要避开大俯仰角。

3.3 四元数：优雅的数学工具

四元数是我个人最喜欢的姿态描述方式。它只有4个参数，没有奇点，而且更新公式非常简洁。说白了，四元数就是一个超复数，由一个实部和三个虚部组成：

q = [q0, q1, q2, q3]^T
其中 q0 = cos(θ/2)
     [q1, q2, q3]^T = sin(θ/2) * [ex, ey, ez]^T
     θ 为旋转角度，[ex, ey, ez] 为旋转轴单位向量

四元数的好处在于，它用一次旋转（绕一个轴转θ角）代替了三次旋转。你想想看，这多符合刚体运动的物理本质——刚体从一个姿态到另一个姿态，其实只需要绕一个轴转一次就够了。

工程小技巧：

我在项目中习惯用四元数做姿态递推，但最终输出给控制律时，会转成欧拉角。因为飞控手还是习惯看俯仰、滚转、偏航。转换公式如下：

// 四元数转欧拉角（ZYX顺序）
phi   = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1^2 + q2^2));
theta = asin(2*(q0*q2 - q3*q1));
psi   = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2));

3.4 四元数运动学方程

有了四元数，我们怎么更新它？答案是四元数运动学方程。这个方程把四元数的导数与角速度联系了起来：

dq/dt = 0.5 * Ω(ω) * q

其中 Ω(ω) 是角速度的反对称矩阵形式：
Ω(ω) = [ 0,  -p,  -q,  -r;
         p,   0,   r,  -q;
         q,  -r,   0,   p;
         r,   q,  -p,   0 ]

这个方程在离散时间下怎么用？我建议用一阶龙格-库塔法（RK1）或者更精确的二阶方法。下面是一个简单的离散更新代码：

// 四元数离散更新（RK1）
// 输入：当前四元数 q, 角速度 [p, q, r], 采样时间 dt
// 输出：更新后的四元数 q_new

float norm = sqrt(p*p + q*q + r*r);
if (norm > 1e-10) {
    float theta = norm * dt;
    float sin_half = sin(theta / 2.0);
    float cos_half = cos(theta / 2.0);
    
    // 旋转轴单位向量
    float ex = p / norm;
    float ey = q / norm;
    float ez = r / norm;
    
    // 增量四元数
    float dq0 = cos_half;
    float dq1 = ex * sin_half;
    float dq2 = ey * sin_half;
    float dq3 = ez * sin_half;
    
    // 四元数乘法
    q_new[0] = q[0]*dq0 - q[1]*dq1 - q[2]*dq2 - q[3]*dq3;
    q_new[1] = q[0]*dq1 + q[1]*dq0 + q[2]*dq3 - q[3]*dq2;
    q_new[2] = q[0]*dq2 - q[1]*dq3 + q[2]*dq0 + q[3]*dq1;
    q_new[3] = q[0]*dq3 + q[1]*dq2 - q[2]*dq1 + q[3]*dq0;
    
    // 归一化
    norm = sqrt(q_new[0]*q_new[0] + q_new[1]*q_new[1] + 
                q_new[2]*q_new[2] + q_new[3]*q_new[3]);
    q_new[0] /= norm;
    q_new[1] /= norm;
    q_new[2] /= norm;
    q_new[3] /= norm;
}

注意：

四元数必须保持单位模长！每次更新后一定要做归一化。我曾经因为忘记归一化，导致姿态解算在长时间飞行后漂移了十几度，查了两天才找到原因。这个坑，大家千万别踩。

3.5 知识体系总览

下面这张图是我自己整理的，把本章的核心逻辑串了起来。你可以看到，从角速度出发，通过四元数运动学方程得到四元数，再转换成欧拉角或旋转矩阵，最终服务于导航与控制。

3.6 本章小结

好了，总结一下今天的内容。刚体运动学的基础就是三件事：

姿态描述：欧拉角直观但有奇点，旋转矩阵无奇点但冗余，四元数是最佳折中
角速度：陀螺仪直接测量，但注意它和姿态导数不是简单的积分关系
四元数更新：用运动学方程离散化，别忘了归一化

我个人建议，如果你刚开始做惯导，先从四元数入手。虽然数学上比欧拉角多绕一步，但后面你会发现，它带来的便利远大于初期的学习成本。嗯，今天就到这里，大家回去可以试着写一个四元数更新的代码，跑一跑看看效果。

公众号:蓝海资料掘金营，微信deep3321