📚 课程目录第3章姿态运动学

3. 姿态运动学

姿态运动学解决的核心问题是：当航天器以角速度 ω 相对惯性空间转动时，描述姿态的参数如何随时间变化。它不涉及力矩与惯量（那属于姿态动力学），只关心几何关系。本节围绕四条主线展开：欧拉角运动学方程、方向余弦矩阵微分方程、四元数运动学方程，以及工程实现中的离散化更新与归一化处理。

一、参考约定与角速度定义

先明确符号约定，全文保持一致：设 ω_bi^b 表示本体系 b 相对惯性系 i 的角速度，在本体系下表达。欧拉角采用 3-2-1（偏航 ψ—俯仰 θ—滚转 φ）旋转顺序，即从惯性系到本体系依次绕 Z、Y、X 轴旋转。姿态参数统一表示“本体系相对惯性系”的取向。

二、欧拉角运动学方程与奇异点

对 3-2-1 顺序，欧拉角速率与本体角速度的关系为：

φ̇ = p + q·sinφ·tanθ + r·cosφ·tanθ
θ̇ = q·cosφ − r·sinφ
ψ̇ = (q·sinφ + r·cosφ) / cosθ

其中 p、q、r 为 ω_bi^b 在本体系三轴的分量。观察方程可见，φ̇ 与 ψ̇ 都含有 tanθ 或 1/cosθ 因子。当 θ = ±90° 时，cosθ = 0，方程发散，这就是欧拉角奇异点（万向节死锁）。物理含义是：俯仰角到达 90° 时，偏航轴与滚转轴在空间中重合，两个自由度退化为一个，无法唯一确定 φ 与 ψ 的分配。

工程上应对奇异点有三种思路：一是限制工作范围，使 θ 远离 ±90°（例如对近地指向卫星，俯仰通常不超过 ±60°）；二是改用无奇异的姿态参数（四元数或方向余弦矩阵）；三是切换欧拉角旋转顺序，把奇异点移到不常用的姿态区域。需要强调的是，奇异点是参数化本身的缺陷，不是航天器物理上不能转动。

三、方向余弦矩阵微分方程

设 C_bi 为从惯性系到本体系的坐标变换矩阵，其运动学方程为：

Ċ_bi = −[ω_bi^b ×] · C_bi

其中 [ω×] 是由 p、q、r 构成的反对称矩阵。该方程是线性的、无奇异的，但矩阵有 9 个元素且需满足正交性约束 C·C^T = I，数值积分中会逐渐失去正交性，需要定期正交化（如 Gram-Schmidt 或极分解）。因此它多用于理论推导和误差分析，实时姿态解算更常用四元数。

四、四元数运动学方程

单位四元数 q = [q₀, q₁, q₂, q₃]^T 描述本体系相对惯性系的姿态，其运动学方程为：

q̇ = 0.5 · q ⊗ [0, p, q, r]^T

展开为矩阵形式（便于编程）：

q̇0 = 0.5·(−q1·p − q2·q − q3·r)
q̇1 = 0.5·( q0·p + q2·r − q3·q)
q̇2 = 0.5·( q0·q − q1·r + q3·p)
q̇3 = 0.5·( q0·r + q1·q − q2·p)

四元数只有 4 个分量、无三角函数奇异，且约束仅为模长为 1，是星载姿态确定与控制中最常用的姿态参数。其代价是物理直观性差，需通过转换公式还原为欧拉角供显示或判读。

五、离散化更新与归一化处理

星载计算机以固定周期 T 运行，需要把连续微分方程转为离散递推。设第 k 步姿态四元数为 q(k)，本周期内角速度近似常值，则增量转角 Δθ = |ω|·T，转轴单位向量 n̂ = ω/|ω|。增量四元数为：

Δq = [cos(Δθ/2), sin(Δθ/2)·n̂x, sin(Δθ/2)·n̂y, sin(Δθ/2)·n̂z]^T
q(k+1) = q(k) ⊗ Δq

当 Δθ 很小时（典型控制周期 0.1 s、角速度 1°/s，Δθ ≈ 0.1°），可用一阶近似 Δq ≈ [1, 0.5·p·T, 0.5·q·T, 0.5·r·T]^T，再归一化。这种“一阶更新 + 归一化”在星上实现简单、计算量小，是工程上最常见的做法。

归一化不可省略。由于浮点舍入和近似，q(k+1) 的模长会偏离 1，若不修正，误差会随积分步数累积，表现为姿态缓慢漂移。归一化公式为：

q_norm = q / sqrt(q0² + q1² + q2² + q3²)

下面给出一段可直接参考的伪代码，展示单步更新流程：

输入: q = [q0,q1,q2,q3], ω = [p,q,r], T
1. Δθ = sqrt(p²+q²+r²) · T
2. 若 Δθ < 1e-8: 返回 q（无转动）
3. n̂ = ω / |ω|
4. Δq = [cos(Δθ/2), sin(Δθ/2)·n̂]
5. q_new = q ⊗ Δq          // 四元数乘法
6. norm = sqrt(Σ q_new_i²)
7. q_new = q_new / norm     // 归一化
8. 返回 q_new

教学示例：设 T = 0.1 s，ω = [0.01, 0.02, −0.005] rad/s，则 |ω| ≈ 0.0229 rad/s，Δθ ≈ 0.00229 rad。此时一阶近似与精确增量四元数的差异在 1e-6 量级，对姿态精度影响可忽略。若角速度升至 0.5 rad/s，Δθ ≈ 0.05 rad，建议改用精确增量四元数或二阶积分，以抑制圆锥误差。

常见错误：只做四元数乘法而忘记归一化，或归一化时误用未更新的旧模长。另一个易错点是把 ω 的坐标系搞混——四元数运动学中的 ω 必须是本体系分量，若误用惯性系分量，姿态更新方向会完全错误。

六、参数选择与工程判断

综合来看，欧拉角适合人机交互与任务规划，但必须避开奇异点；方向余弦矩阵适合理论推导与坐标变换，实时积分需正交化；四元数适合实时姿态传播与控制，配合归一化即可长期稳定运行。实际星上软件通常以四元数为主状态量，仅在遥测下传和地面显示时转换为欧拉角。判断更新是否正常，可观察四元数模长是否稳定在 1±1e-6 以内，以及由四元数反算的欧拉角是否连续无跳变。