4. 姿态运动学方程：欧拉角、四元数与姿态解算
各位同学，今天我们来聊聊姿态运动学。说白了，就是研究卫星怎么“转”的数学描述。我刚开始接触这个领域时，觉得不就是个旋转嘛，有什么难的？后来在项目中吃过亏，才发现这里面的门道真不少。
姿态运动学不涉及力和力矩，只关心角度、角速度这些几何量之间的关系。嗯，咱们先从一个最直观的表示方法说起——欧拉角。
4.1 欧拉角运动学方程
欧拉角大家应该不陌生，就是俯仰、偏航、滚转这三个角度。我习惯用ZYX顺序，也就是先绕Z轴转偏航角ψ，再绕Y轴转俯仰角θ，最后绕X轴转滚转角φ。
那么问题来了：已知三个欧拉角的角速度，怎么得到卫星本体的角速度？反过来，已知本体角速度，怎么解算欧拉角的变化率？
这里直接给结论。从本体角速度到欧拉角速度的转换关系是：
ωx = φ̇ - ψ̇·sinθ
ωy = θ̇·cosφ + ψ̇·sinφ·cosθ
ωz = -θ̇·sinφ + ψ̇·cosφ·cosθ写成矩阵形式更清晰：
[ωx] [1 0 -sinθ ] [φ̇]
[ωy] = [0 cosφ sinφ·cosθ] [θ̇]
[ωz] [0 -sinφ cosφ·cosθ] [ψ̇]反过来，我们更常用的是从本体角速度求欧拉角速度：
[φ̇] [1 sinφ·tanθ cosφ·tanθ ] [ωx]
[θ̇] = [0 cosφ -sinφ ] [ωy]
[ψ̇] [0 sinφ/cosθ cosφ/cosθ ] [ωz]⚠️ 奇异性问题
看到分母上的cosθ了吗？当θ = ±90°时，cosθ = 0，方程就奇异了。我在做某型对地观测卫星时，就遇到过俯仰角接近90°的情况，解算直接发散。这就是所谓的“万向锁”问题。
所以，欧拉角虽然直观，但只适合小角度机动或者有特殊约束的场景。如果你要搞全姿态机动，我建议你直接上四元数。
4.2 四元数运动学方程
四元数是什么？你可以把它理解成一个“超复数”，有四个分量：一个实部加三个虚部。写成q = [q0, q1, q2, q3]ᵀ，其中q0是标量部分，q1,q2,q3是矢量部分。
四元数最大的好处就是——没有奇异性。你随便转，它都能优雅地描述。我个人特别喜欢这一点，写代码时不用考虑边界情况，省心不少。
四元数的运动学方程长这样：
q̇ = 0.5 · q ⊗ ω其中⊗表示四元数乘法，ω是本体角速度的四元数形式[0, ωx, ωy, ωz]ᵀ。
展开写成矩阵形式：
[q̇0] [ 0 -ωx -ωy -ωz] [q0]
[q̇1] = [ ωx 0 ωz -ωy] [q1] · 0.5
[q̇2] [ ωy -ωz 0 ωx] [q2]
[q̇3] [ ωz ωy -ωx 0 ] [q3]💡 核心要点
四元数必须保持单位化，即q0² + q1² + q2² + q3² = 1。每次更新后最好做一次归一化，否则误差会累积。我曾经因为忘了归一化，仿真跑了半小时姿态就飘到天上去了……
4.3 姿态解算方法
有了运动学方程，接下来就是怎么解算的问题了。说白了，就是给定角速度测量值，怎么实时更新姿态。
我常用的方法有三种，咱们一个一个说。
4.3.1 欧拉法（一阶龙格-库塔）
最简单粗暴的方法：
q(t+Δt) = q(t) + q̇(t) · Δt优点是简单，缺点是精度低。如果你仿真步长很小（比如1ms以下），凑合能用。但步长大了就会发散，我一般不推荐。
4.3.2 四阶龙格-库塔法（RK4）
这是工程中最常用的方法。精度高，稳定性好。代码实现也不复杂：
def rk4_step(q, omega, dt):
k1 = 0.5 * quat_multiply(q, omega)
k2 = 0.5 * quat_multiply(q + 0.5*dt*k1, omega)
k3 = 0.5 * quat_multiply(q + 0.5*dt*k2, omega)
k4 = 0.5 * quat_multiply(q + dt*k3, omega)
q_new = q + (dt/6.0)*(k1 + 2*k2 + 2*k3 + k4)
return q_new / norm(q_new) # 归一化🔧 实用技巧
我在项目中一般用RK4，步长取0.01秒，精度完全够用。如果你做实时仿真，可以考虑用二阶龙格-库塔，计算量小一些，精度也还行。
4.3.3 解析法（精确解）
当角速度矢量方向不变时，存在解析解：
q(t+Δt) = [cos(θ/2), (ω̂)·sin(θ/2)] ⊗ q(t)其中θ = |ω|·Δt，ω̂ = ω/|ω|。
这个方法精度最高，但只适用于角速度方向恒定的情况。实际中很少直接用，不过可以用来做理论验证。
4.4 知识体系总览
说了这么多，咱们用一张图来总结一下本章的核心逻辑：
从这张图可以看得很清楚：输入是角速度，经过不同的运动学方程和解算方法，最终得到更新后的姿态。我个人强烈推荐四元数+RK4的组合，通用性强，精度高，代码也好写。
📌 本章小结
- 欧拉角直观但有奇异性，适合小角度场景
- 四元数无奇异性，适合全姿态机动
- 姿态解算推荐RK4，步长取0.01秒
- 四元数每次更新后必须归一化
好了，姿态运动学就讲到这里。下一章咱们聊聊姿态动力学，也就是卫星为什么会转、怎么让它转的问题。到时候我会分享一个我踩过的坑——关于飞轮饱和的，挺有意思。