3. 姿态运动学方程数值仿真

姿态运动学描述的是“姿态参数如何随角速度变化”，它本身不涉及力矩和惯量，因此是姿态动力学仿真之前必须单独验证的一环。工程上常见的做法是：给定一条已知的角速度时间历程，分别用欧拉角运动学方程和四元数运动学方程积分，再把结果与解析解或高精度参考解对比，从而判断模型实现是否正确、积分器是否够用、奇异点是否被妥善处理。

一、两套运动学微分方程

设本体系相对参考系的角速度在本体系下表示为 ω = [ωx, ωy, ωz]^T，单位为 rad/s。若采用 3-2-1 转序（先绕 Z 偏航 ψ，再绕 Y 俯仰 θ，最后绕 X 滚转 φ），欧拉角运动学方程为：

φ̇ = ωx + tanθ·(ωy·sinφ + ωz·cosφ)
θ̇ = ωy·cosφ − ωz·sinφ
ψ̇ = (ωy·sinφ + ωz·cosφ) / cosθ

可以看到，θ̇ 与 φ、ψ 无关，是三者中唯一“干净”的通道；而 φ̇ 和 ψ̇ 都含有 tanθ 或 1/cosθ 因子。当 θ → ±90° 时，这两项发散，方程出现奇异，这就是欧拉角表示法的固有缺陷。

四元数运动学方程则没有这个问题。设姿态四元数 q = [q0, q1, q2, q3]^T（q0 为标量部），则：

q̇ = 0.5 · Ω(ω) · q
Ω(ω) = [ 0   −ωx  −ωy  −ωz
        ωx    0   ωz  −ωy
        ωy  −ωz    0   ωx
        ωz   ωy  −ωx    0 ]

该方程对任意姿态都成立，无奇异点，代价是四元数有冗余（4 个分量表示 3 个自由度），积分后需要归一化以抑制数值漂移。

二、积分器选择

运动学方程是常微分方程初值问题，常用积分器有三类。显式欧拉法最简单，但一阶精度、误差累积快，只适合快速验证方程形式，不适合长时仿真。四阶龙格-库塔（RK4）是工程默认选择：固定步长、四阶精度，在 h=0.01 s 时对典型卫星姿态运动已能给出 1e-6 量级误差。当角速度变化剧烈或需要长时间高精度时，改用自适应步长的 RK45（Dormand-Prince），通过局部误差估计自动调整步长，代价是每步计算量更大、步长不固定。

选择积分器时还要注意：四元数方程虽然线性，但 Ω(ω) 随 ω 变化，若 ω 由动力学耦合给出，方程整体仍是非线性的，不能因为“四元数方程看起来线性”就降低积分器阶数。工程上建议先用 RK4 固定步长跑通，再用 RK45 复核，两者结果一致才认为实现正确。

三、奇异点处理

欧拉角奇异点出现在 θ = ±90°。实际处理有三种思路。第一种是限幅：在 θ 接近 ±90° 时对 tanθ 和 1/cosθ 做限幅，例如把 |θ| 限制在 89.5° 以内，避免数值溢出，但会引入姿态误差，只适合 θ 不会真正穿越 90° 的场景。第二种是切换转序：当 θ 接近 90° 时改用 3-1-3 或 3-2-1 的替代转序，使新的“中间角”远离奇异，代价是转序切换逻辑复杂、易出错。第三种是根本方案：内部用四元数积分，只在需要输出或显示时把四元数转成欧拉角，此时即使 θ 穿越 90°，四元数本身也不会发散，转换后的欧拉角在奇异附近会出现跳变，但这是表示问题而非积分问题。

四、仿真精度对比方法

精度对比不能只看“曲线像不像”，要构造可量化的判据。常用做法是：给定解析可积的角速度（例如绕单轴常值旋转），解析解为 θ(t) = ωt，把数值解与解析解逐点作差，统计最大绝对误差和末值误差。对于无解析解的一般 ω(t)，用 RK45 小容差解作为参考解，比较不同积分器和步长下的误差收敛阶。

下面给出一个可直接使用的对比记录表，字段按运动学仿真实际需要设置：

表中数值为示例性结果，用于说明量级关系，实际数值随 ω(t)、步长和实现细节变化。判读时关注三点：一是误差是否随步长减小按预期阶数下降（RK4 应约 16 倍/步长减半）；二是四元数范数是否始终保持在 1±1e-12 内，若持续漂移说明归一化没做或做错；三是把四元数结果转成欧拉角后与欧拉角直接积分结果对比，在远离奇异处应一致，在奇异附近欧拉角路径应出现明显偏差或跳变。

五、实现要点小结

• 角速度 ω 必须明确参考系：本体系下 ω 与参考系下 ω 的 Ω 矩阵形式不同，混用会导致符号错误。
• 欧拉角方程中 φ、θ、ψ 的转序必须与 Ω 矩阵、四元数转欧拉角的公式一致，建议全文统一 3-2-1。
• RK4 实现时，k1~k4 的 ω 取值时刻要与状态更新时刻对应，常值 ω 下不敏感，时变 ω 下必须对齐。
• 精度对比至少包含“远离奇异”和“穿越奇异”两组工况，否则无法暴露欧拉角路径的缺陷。
• 输出四元数时同时输出 ‖q‖ 曲线，作为数值健康度的直接指标。

完成本节后，应能独立写出欧拉角与四元数两套运动学右端函数，选择 RK4 或 RK45 完成积分，并用误差统计表判断实现是否正确、奇异点是否被妥善处理。这是后续姿态动力学仿真和控制器闭环验证的基础。