4. 刚体运动学与关节约束方程

多体动力学仿真的第一步，是把机械系统抽象为“刚体 + 关节”的拓扑结构。刚体提供惯量与几何，关节则限制刚体之间的相对运动。要让仿真器知道机器人如何运动、能运动到哪些位形，就必须建立两套方程：一套描述刚体自身的速度与加速度如何递推，另一套描述关节对相邻刚体施加的约束。本节围绕这两套方程展开，并给出转动、移动、球铰、虎克铰四类常见关节的约束写法与自由度计算方法。

一、坐标系与位姿描述约定

为避免符号混乱，本节统一约定：每个刚体 i 上固连一个连体坐标系 {i}，原点取在质心或关节中心，姿态用旋转矩阵 R_i 表示，位置用向量 r_i 表示。全局惯性系记为 {O}。刚体 i 的广义坐标取为 q_i = [r_i^T, p_i^T]^T，其中 p_i 是姿态参数（欧拉角或四元数）。角速度 ω_i 定义在连体系下，与姿态参数导数的关系为 ω_i = G_i(p_i)·ṗ_i，G_i 是姿态参数到角速度的映射矩阵。全文与图示均采用这一约定。

二、速度与加速度递推

对于开链机器人，从基座向末端逐体递推是最自然的做法。设父刚体为 i-1，子刚体为 i，关节 i 连接二者。子刚体质心速度由父刚体速度、关节相对运动共同决定：

v_i = v_{i-1} + ω_{i-1} × r_{i-1,i} + R_i · (v_rel)
ω_i = ω_{i-1} + R_i · (ω_rel)

其中 r_i-1,i 是父刚体质心指向关节点的向量，v_rel、ω_rel 是关节在连体系下的相对速度。对加速度求导，会额外出现科氏项与向心项：

a_i = a_{i-1} + α_{i-1} × r_{i-1,i} + ω_{i-1} × (ω_{i-1} × r_{i-1,i})
      + R_i · (a_rel + 2·ω_i × v_rel + ω_i × (ω_i × r_rel))

式中 α 为角加速度，a_rel 为关节相对加速度。递推的关键在于：每经过一个关节，都要把父体的运动“传递”到子体，再叠加关节自身的相对运动。递推顺序必须与拓扑顺序一致，闭环机构需要先断开为生成树，再用约束方程补回断开的关节。

三、关节约束方程与自由度计算

关节约束的本质，是限制两个刚体之间某些相对运动分量。设关节在父体上的连接点为 P_i-1，在子体上的连接点为 P_i，则约束方程可统一写成 Φ(q_i-1, q_i, t) = 0。对时间求导得到速度约束 Φ_q·q̇ = ν，再求导得到加速度约束 Φ_q·q̈ = γ，其中 Φ_q 是约束雅可比矩阵，γ 汇集了速度二次项。仿真器正是通过求解带约束的动力学方程，得到满足约束的加速度与约束力。

自由度计算遵循“总坐标数减去独立约束数”的原则。若系统有 n 个刚体、每个刚体 6 个位姿坐标，则总坐标数为 6n；每个关节贡献 c 个独立约束，则系统自由度 DOF = 6n − Σc。例如一个由 3 个转动关节串联的平面机械臂，n=3，每个转动关节约束 5 个相对自由度，DOF = 18 − 15 = 3，与直观一致。

四、四类关节的约束描述方法

转动关节（Revolute）：允许两刚体绕某一固定轴相对转动，其余 5 个相对自由度被约束。约束写法为：连接点位置重合（3 个标量方程），且两刚体上各取一个与转轴垂直的向量保持正交（2 个标量方程）。在仿真中通常用“点重合 + 两轴平行”来表达，转轴方向由关节坐标系指定。

移动关节（Prismatic）：允许沿某一固定轴相对平移，约束 5 个相对自由度。约束写法为：两刚体上对应点沿垂直于移动轴的两个方向的分量相等（2 个方程），且两刚体相对姿态保持不变（3 个方程）。移动关节不产生角速度，递推时 ω_rel = 0，只有线速度沿轴方向。

球铰（Spherical）：允许三个方向的相对转动，约束 3 个平移自由度，保留 3 个转动自由度。约束写法最简单：两刚体连接点位置重合，即 r_P,i-1 − r_P,i = 0。球铰没有姿态约束，因此递推时相对角速度是三维的，需要三个转动参数描述。

虎克铰（Universal）：又称十字万向节，允许绕两个正交轴转动，约束 4 个相对自由度。约束写法为：连接点位置重合（3 个方程），加上一个“两刚体上某向量保持共面”的约束（1 个方程），从而只保留两个转动自由度。虎克铰常用于需要传递扭矩又允许一定角度偏转的场合，如差速器、传动轴。

五、约束雅可比与仿真实现要点

无论哪类关节，最终都要组装成全局约束雅可比矩阵 Φ_q。它的每一行对应一个约束方程，每一列对应一个广义坐标。对于转动关节，位置约束行只涉及两个刚体的平移坐标，姿态约束行则涉及姿态参数。组装完成后，动力学方程可写成：

[ M   Φ_q^T ] [ q̈ ]   [ Q_e ]
[ Φ_q   0   ] [ λ  ] = [ γ   ]

其中 M 是质量矩阵，Q_e 是广义力，λ 是拉格朗日乘子，物理意义是关节约束力。求解该线性方程组即可得到满足约束的加速度与约束力。实际编程时，建议先按拓扑顺序完成速度、加速度递推，再统一组装约束雅可比，最后求解。若出现约束漂移，可引入 Baumgarte 稳定项，将位置和速度约束的偏差按比例反馈到加速度约束中。

掌握速度、加速度递推与关节约束方程后，就能把任意机器人拓扑转化为可求解的动力学方程组。下一阶段将在此基础上引入质量矩阵与广义力，完成完整的动力学建模。