2. 姿态运动学与动力学入门

姿态估计的核心任务，是回答“航天器当前朝向哪里、正在怎样转动”。要回答这个问题，必须先把两件事分开：一是姿态本身如何随角速度变化，这属于姿态运动学；二是角速度如何被力矩改变，这属于刚体姿态动力学。前者描述几何关系，后者描述物理因果。二者通过角速度耦合在一起，构成姿态估计与控制的完整模型基础。

一、姿态运动学：角速度如何驱动姿态变化

姿态运动学只关心几何，不涉及质量、力矩。设本体系相对参考系的角速度在本体系下表示为 ω = [ωx, ωy, ωz]ᵀ，单位 rad/s。若用方向余弦矩阵 C_bn（本体系到参考系）描述姿态，则运动学方程为：

Ċ_bn = C_bn · [ω×]
[ω×] = [ 0   -ωz   ωy
         ωz   0   -ωx
        -ωy  ωx    0 ]

这个式子说明：姿态变化率不是角速度本身，而是角速度构成的反对称矩阵左乘当前姿态。也就是说，同样的角速度，在不同姿态下引起的姿态变化方向不同，这正是“耦合”的第一层含义。若航天器绕本体系 z 轴以 0.1 rad/s 旋转，而当前本体系 z 轴恰好指向参考系 x 轴，那么姿态变化会体现在参考系的 x 轴方向，而不是 z 轴方向。

方向余弦矩阵有 9 个元素但只有 3 个自由度，直接积分会产生正交性漂移，因此工程上更常用四元数。设四元数 q = [q0, q1, q2, q3]ᵀ，其中 q0 为标量部分，矢量部分对应旋转轴与半角正弦。四元数微分方程为：

q̇ = 0.5 · q ⊗ [0, ωx, ωy, ωz]ᵀ

展开为矩阵形式更便于实现：

q̇0 = -0.5(ωx·q1 + ωy·q2 + ωz·q3)
q̇1 =  0.5(ωx·q0 + ωz·q2 - ωy·q3)
q̇2 =  0.5(ωy·q0 - ωz·q1 + ωx·q3)
q̇3 =  0.5(ωz·q0 + ωy·q1 - ωx·q2)

注意这里的乘法顺序：q ⊗ ω 与 ω ⊗ q 结果不同，取决于角速度定义在本体系还是参考系。本课程统一约定：角速度 ω 定义在本体系，四元数 q 表示本体系相对参考系的旋转。若切换参照系，必须显式说明，否则符号会整体反号。

四元数积分常用一阶欧拉法或二阶龙格库塔。以采样周期 Δt = 0.01 s、角速度 ω = [0.02, -0.01, 0.05] rad/s 为例，一阶更新为 q(k+1) = q(k) + q̇·Δt，随后必须归一化 q ← q/||q||。若不归一化，几十秒后模长会偏离 1，姿态解算出现明显偏差。这是实际实现中最容易被忽略的一步。

二、刚体姿态动力学：力矩如何改变角速度

运动学告诉我们角速度如何改变姿态，但没有说明角速度从何而来。刚体转动动力学给出答案。在本体系下，欧拉方程为：

J·ω̇ + ω × (J·ω) = T

其中 J 为惯量矩阵，T 为外力矩（含控制力矩、干扰力矩）。展开成分量形式：

Jx·ω̇x = (Jy - Jz)·ωy·ωz + Tx
Jy·ω̇y = (Jz - Jx)·ωz·ωx + Ty
Jz·ω̇z = (Jx - Jy)·ωx·ωy + Tz

式中 (Jy - Jz)·ωy·ωz 等项称为陀螺耦合项。它表明：即使没有外力矩，只要三轴角速度同时非零且惯量不相等，角速度也会互相激励。例如一颗 Jx=100, Jy=120, Jz=80 kg·m² 的航天器，初始 ω=[0.05, 0.02, 0.01] rad/s，在零力矩下 ωx 会因 ωy·ωz 项缓慢变化，ωy、ωz 也随之耦合。这就是“角速度与姿态变化耦合”的第二层含义：动力学让角速度自身演化，运动学再把演化后的角速度映射为姿态变化。

三、耦合关系的整体图景

把两部分放在一起看：控制力矩 T 通过动力学改变角速度 ω，角速度通过运动学改变姿态 q，而姿态又决定了本体系下重力梯度力矩、气动力矩等干扰力矩的方向，从而反过来影响 T 和 ω。这是一个闭环耦合链。姿态估计的任务，正是利用陀螺测得的 ω 和星敏感器、太阳敏感器测得的姿态量测，在这条链上做最优融合。

四、一个可复现的数值示例

下面用一段伪代码展示从力矩到姿态的完整一步更新，便于理解耦合顺序。设 J = diag(100, 120, 80)，Δt = 0.01 s，初始 ω = [0.05, 0.02, 0.01]，q = [1,0,0,0]，控制力矩 T = [0.01, 0, 0] N·m。

# 1. 动力学：先更新角速度
wx_dot = ((Jy-Jz)*wy*wz + Tx) / Jx
wy_dot = ((Jz-Jx)*wz*wx + Ty) / Jy
wz_dot = ((Jx-Jy)*wx*wy + Tz) / Jz
wx += wx_dot * dt;  wy += wy_dot * dt;  wz += wz_dot * dt

# 2. 运动学：再用新角速度更新四元数
q0 += 0.5*(-wx*q1 - wy*q2 - wz*q3) * dt
q1 += 0.5*( wx*q0 + wz*q2 - wy*q3) * dt
q2 += 0.5*( wy*q0 - wz*q1 + wx*q3) * dt
q3 += 0.5*( wz*q0 + wy*q1 - wx*q2) * dt

# 3. 归一化，防止模长漂移
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
q0/=norm; q1/=norm; q2/=norm; q3/=norm

按上述参数运行一步后，ωx 会因 Tx 增大，ωy、ωz 因陀螺耦合项出现微小变化；四元数 q1 开始偏离 0，表示本体系绕参考系 x 轴产生小角度旋转。若把 Tx 置零，ω 仍会因 ω×(J·ω) 项缓慢演化，姿态随之漂移——这正是无控航天器需要持续估计姿态的原因。

五、从模型到估计的衔接

本节建立的两组方程，正是后续卡尔曼滤波等估计算法的过程模型。状态量通常取 x = [q, ω]ᵀ，其中 q 由运动学方程预测，ω 由动力学方程预测；量测方程则由星敏感器等给出 q 的观测。理解角速度与姿态变化之间的耦合，才能正确写出状态转移矩阵，也才能在陀螺漂移、力矩扰动存在时判断估计结果是否合理。下一阶段的学习，将围绕如何在这条耦合链上做最优融合展开。