刚体运动学方程：位置与姿态的数学描述
各位同学，今天我们来啃一块硬骨头——刚体运动学方程。说实话，这部分内容在飞控仿真里属于「地基」级别的存在。你想想看，飞机在天上飞，我们总得知道它往哪飞、头朝哪吧？这就是位置运动学和姿态运动学要干的事。
我个人习惯把运动学分成两半来看：位置运动学管「飞机在空间里怎么移动」，姿态运动学管「飞机怎么转」。两者合起来，就是六自由度仿真里那六个自由度——三个平动、三个转动。
核心要点：运动学只描述运动，不关心力。它回答「是什么」，不回答「为什么」。动力学才管「为什么」。这个界限要划清楚。
位置运动学：地面坐标系下的位移
位置运动学其实很简单。我们通常把飞机的位置用地面坐标系（也叫惯性坐标系）下的三个坐标表示：x、y、z。速度呢？就是位置对时间的导数。
公式长这样：
dx/dt = u * cos(θ) * cos(ψ) + v * (sin(φ)*sin(θ)*cos(ψ) - cos(φ)*sin(ψ)) + w * (cos(φ)*sin(θ)*cos(ψ) + sin(φ)*sin(ψ))
dy/dt = u * cos(θ) * sin(ψ) + v * (sin(φ)*sin(θ)*sin(ψ) + cos(φ)*cos(ψ)) + w * (cos(φ)*sin(θ)*sin(ψ) - sin(φ)*cos(ψ))
dz/dt = -u * sin(θ) + v * sin(φ) * cos(θ) + w * cos(φ) * cos(θ)看着是不是有点晕？别急，我来拆解一下。这里的 u、v、w 是飞机机体坐标系下的三个速度分量，φ、θ、ψ 是三个欧拉角。说白了，就是把机体坐标系下的速度，通过旋转矩阵变换到地面坐标系。
我的经验：我在做第一个六自由度仿真时，死活调不对位置输出。后来发现是坐标变换顺序搞反了。记住：先转航向ψ，再转俯仰θ，最后转滚转φ。顺序错了，飞机就往反方向飞了。
姿态运动学：欧拉角微分方程
姿态运动学描述的是飞机姿态角的变化率。我们常用的欧拉角——滚转角φ、俯仰角θ、航向角ψ——它们的变化率跟机体的角速度 p、q、r 有关系。
公式如下：
dφ/dt = p + q * sin(φ) * tan(θ) + r * cos(φ) * tan(θ)
dθ/dt = q * cos(φ) - r * sin(φ)
dψ/dt = (q * sin(φ) + r * cos(φ)) / cos(θ)这里有个坑，我当年踩过。你看第三个公式，分母是 cos(θ)。当俯仰角θ接近 ±90° 时，cos(θ) 趋近于0，dψ/dt 就爆炸了。这就是著名的万向锁问题。
警告：欧拉角微分方程在俯仰角 ±90° 时存在奇异性。如果你的仿真场景涉及垂直爬升或俯冲，千万别用欧拉角，否则仿真会直接崩掉。我曾经在做一个战斗机机动仿真时，就因为没注意这个，结果飞机翻了个跟头后姿态就乱套了。
四元数微分方程：避开奇异性
那怎么解决万向锁呢？答案是用四元数。四元数用四个参数表示姿态，没有奇异性问题。它的微分方程形式也简洁很多。
四元数 q = [q0, q1, q2, q3]^T，微分方程：
dq0/dt = 0.5 * (-q1*p - q2*q - q3*r)
dq1/dt = 0.5 * ( q0*p + q2*r - q3*q)
dq2/dt = 0.5 * ( q0*q - q1*r + q3*p)
dq3/dt = 0.5 * ( q0*r + q1*q - q2*p)写成矩阵形式更漂亮：
dq/dt = 0.5 * Ω * q
其中 Ω = [ 0 -p -q -r
p 0 r -q
q -r 0 p
r q -p 0 ]我个人特别喜欢四元数，原因有三：
- 没有奇异性，随便你怎么转
- 计算量小，只有四个参数
- 插值平滑，适合做姿态控制
避坑指南：四元数需要归一化。每次更新完四元数后，记得做 q = q / ||q||。我刚开始做仿真时忘了这步，结果四元数越积越大，姿态完全失真。后来养成了习惯，每次积分完都归一化一下。
方向余弦矩阵微分方程
方向余弦矩阵（DCM）是另一种姿态表示方法。它是一个 3×3 的旋转矩阵，把机体坐标系转换到地面坐标系。它的微分方程形式也很直观：
dC/dt = C * S(ω)
其中 S(ω) = [ 0 -r q
r 0 -p
-q p 0 ]C 是方向余弦矩阵，S(ω) 是角速度的反对称矩阵。这个方程的本质是：旋转矩阵的变化率，等于当前旋转矩阵乘以角速度的叉乘矩阵。
嗯，这里要注意一点。DCM 有 9 个参数，但只有 3 个自由度，所以它必须满足正交性约束。每次更新后，需要做正交化处理，否则矩阵会慢慢「漂移」。
| 表示方法 | 参数数量 | 奇异性 | 计算量 | 适用场景 |
|---|---|---|---|---|
| 欧拉角 | 3 | 有（±90°俯仰） | 小 | 常规飞行，小角度机动 |
| 四元数 | 4 | 无 | 中 | 全姿态，大机动 |
| 方向余弦矩阵 | 9 | 无 | 大 | 需要直接使用旋转矩阵的场景 |
知识体系总览
下面这张图是我自己画的，把刚体运动学的核心逻辑串起来了。你一看就明白：
从这张图可以看得很清楚：刚体运动学方程分两大块——位置和姿态。姿态部分又有三种表示方法，各有各的适用场景。我个人建议，做常规仿真用欧拉角，做全姿态仿真用四元数，需要频繁做坐标变换的用DCM。
总结一下：位置运动学把机体速度转到地面坐标系，姿态运动学描述飞机怎么转。三种姿态表示方法中，四元数是最实用的——没有奇异性，计算量适中，插值平滑。我现在的项目里，90%的情况都用四元数。
好了，刚体运动学方程就讲到这里。记住：运动学是仿真的骨架，动力学是血肉。骨架搭好了，后面加力、加力矩就顺了。
公众号:蓝海资料掘金营，微信deep3321