刚体动力学入门：牛顿-欧拉方程与三维姿态

质点动力学只需要一个 F = ma，但真实物体有形状、有转动。刚体动力学的任务就是回答两个问题：质心怎么平动、刚体怎么绕质心转动。前者和质点几乎一样，后者需要引入一个新的物理量——惯量张量。

刚体 = 质量 + 惯量张量 + 位姿。平动看牛顿第二定律，转动看欧拉方程，二者通过受力与力矩耦合。

1. 从质点走向刚体

刚体是”任意两点间距离恒定”的质点系。这个约束带来两个直接好处：内部所有点的运动可以由质心平动加绕质心转动完全描述，因此自由度从无限多降到 6 个——三个平移、三个旋转（外加一个长度为一的姿态约束）。

1.1 位姿：位置与姿态

位置用一个三维向量 r 表示，姿态则更讲究。工程上常见三种表示：

表示方式	参数量	优点	缺点
欧拉角	3	直观易调参	存在万向锁，插值不平滑
旋转矩阵	9	可直接作用于向量	冗余，长期积分会失去正交性
四元数	4	无万向锁，插值平滑，数值稳定	不直观，需要归一化

仿真与姿态控制几乎一律选四元数：它计算量小、没有奇点，而且满足单位长度约束后能保证长时间积分不发散。

2. 惯量张量：转动版的”质量”

质量描述”改变直线运动有多难”，惯量则描述”改变转动有多难”。对一般形状的刚体，它是一个 3×3 对称矩阵：

I = \int ρ(r) ((r\cdotr)E - r\otimesr) dV (1)

对规则几何体，可以直接写出解析表达式。比如边长为 a、b、c、质量为 m 的长方体，在自身主轴坐标系下惯量是对角的：

I xx = m(b²+c²)/12，I yy = m(a²+c²)/12，I zz = m(a²+b²)/12 (2)

关键在于：惯量张量必须定义在刚体自身坐标系下。只要刚体不变形，它在体坐标系里就是常量，这能省掉每次积分都重新积分的巨大开销；需要世界系下的惯量时，再做一次旋转即可。

3. 牛顿-欧拉方程

平动部分与质点完全一致，转动部分则要小心：角动量 L = Iω，但它是在惯性系下守恒的。把对时间求导放到体坐标系中，就会出现一个额外项，这正是陀螺效应的来源。

m\cdota = F | I\cdotω̇ + ω \times (I\cdotω) = τ (3)

其中 ω 是角速度，τ 是合外力矩。ω × (Iω) 这一项常被新手忽略，结果就是仿真里陀螺永远转不对方向。求解 ω̇ 时需要对 I 求逆：

ω̇ = I -1 (τ - ω \times (I\cdotω)) (4)

4. 最小可运行实现

把上面的公式翻译成 NumPy，二十来行就能跑通一个自由旋转的刚体。姿态用四元数表示，每个时间步先更新角速度，再用四元数微分方程更新姿态，最后归一化。

import numpy as np

def box_inertia(m, a, b, c):
    # 长方体在主轴系下的惯量张量（对角阵）
    return np.diag([m*(b*b+c*c)/12,
                    m*(a*a+c*c)/12,
                    m*(a*a+b*b)/12])

def step(state, dt, torque):
    omega, q = state
    I = box_inertia(1.0, 2.0, 1.0, 0.5)
    I_inv = np.linalg.inv(I)

    # 欧拉方程：含 ω 与角动量的叉乘项（陀螺力矩）
    omega_dot = I_inv @ (torque - np.cross(omega, I @ omega))
    omega = omega + omega_dot * dt

    # 四元数微分：q̇ = 0.5 * ω ⊗ q
    wx, wy, wz = omega
    Omega = np.array([[0, -wx, -wy, -wz],
                      [wx,  0,  wz, -wy],
                      [wy, -wz,  0,  wx],
                      [wz,  wy, -wx,  0]])
    q = q + 0.5 * (Omega @ q) * dt
    q = q / np.linalg.norm(q)      # 归一化，防止数值漂移
    return omega, q

这段代码用的是显式欧拉积分。对快速旋转的刚体，它容易积累误差甚至发散，工程里通常升级为 RK4，或者在角速度更新后做能量校正。不过作为理解方程的第一版实现，已经足够看清每个物理量在做什么。

omega = np.array([0.1, 3.0, 0.1])      # 绕 y 轴快速自旋
q = np.array([1.0, 0.0, 0.0, 0.0])      # 单位四元数
tau = np.zeros(3)                   # 无力矩，自由旋转

for _ in range(1000):
    omega, q = step((omega, q), 0.001, tau)
print(q)   # 姿态持续变化，说明陀螺项在起作用