2. 刚体动力学基础回顾

柔性体与刚体联合仿真的第一步，是把刚体部分“说清楚”。刚体动力学提供了描述物体位姿、速度、加速度以及力与运动因果关系的统一语言。若刚体模型本身存在坐标系混乱、约束冗余或方程形式选择不当，后续引入柔性体后误差会被进一步放大。因此本章围绕刚体运动描述、牛顿-欧拉方程、拉格朗日方程、广义坐标与约束、以及刚体系统建模流程五个要点展开，目标是让读者能够独立写出一个多刚体系统的运动方程，并理解不同建模路线的适用边界。

一、刚体运动的描述：位姿、速度与加速度

刚体在三维空间中的运动由位置和姿态共同决定，合称位姿。位置用质心或参考点的位置矢量 r ∈ R³ 描述，姿态用旋转矩阵 R ∈ SO(3) 或单位四元数 q = [q₀, q₁, q₂, q₃]ᵀ 描述。旋转矩阵满足正交性与行列式为 1 的约束，即 RᵀR = I、det(R) = 1；四元数则需满足 ‖q‖ = 1。工程实现中，四元数因无奇异、插值平滑而被广泛采用，但旋转矩阵在推导中更直观。

速度分为线速度与角速度。线速度通常取质心速度 v = ṙ；角速度 ω 是姿态变化率的载体，需明确其表达坐标系：ω 在惯性系下记为 ω^I，在体坐标系下记为 ω^b，二者通过 ω^I = R ω^b 联系。角速度与旋转矩阵的关系为 Ṙ = [ω^I]× R = R [ω^b]×，其中 [·]× 表示反对称矩阵。加速度则通过对速度求导得到，注意角加速度 α = ω̇ 同样需要标明参考系。方向与正负号约定必须在建模开始前固定，例如统一采用右手系、Z 轴向上、角速度按右手螺旋方向为正。

二、牛顿-欧拉方程：力与运动的直接因果

牛顿-欧拉方程把刚体运动拆成平动与转动两部分。平动部分为 m a = F，其中 m 为质量，a 为质心加速度，F 为合外力。转动部分在体坐标系下写为 I^b ω̇^b + ω^b × (I^b ω^b) = τ^b，其中 I^b 为体坐标系下的惯性张量，τ^b 为合外力矩。若在惯性系下表达，则形式为 I^I ω̇^I + ω^I × (I^I ω^I) = τ^I，但 I^I = R I^b Rᵀ 随时间变化，计算量更大，因此工程中多采用体坐标系形式。

牛顿-欧拉方法的优势在于物理意义清晰、易于逐体施加约束力与外力，适合链式或树状多体系统。其代价是需要显式处理约束力，方程数量随刚体数增加而增长。对于含闭链或大量接触的模型，约束力求解会成为主要计算负担。

三、拉格朗日方程：从能量出发的广义坐标描述

拉格朗日方程从能量角度建立运动方程，形式为 d/dt(∂L/∂q̇) − ∂L/∂q = Q，其中 L = T − V 为拉格朗日函数，T 为动能，V 为势能，q 为广义坐标，Q 为广义力。对于刚体系统，动能包含平动与转动两部分：T = ½ m vᵀv + ½ ωᵀ I ω。拉格朗日方法自动消去理想约束力，方程数量等于系统自由度数，适合自由度较少、约束关系明确的系统。

其代价是推导过程涉及大量偏导数，且对非保守力、接触力需要额外引入广义力项。实际建模中，常先用拉格朗日方法获得符号方程，再通过代码生成或数值积分求解。对于含柔性体的联合仿真，拉格朗日框架更易与有限元模态坐标耦合，因为两者都以广义坐标为基本变量。

四、广义坐标与约束：自由度的真实来源

广义坐标是描述系统位形的最小独立变量集合。一个自由刚体在三维空间有 6 个自由度，N 个自由刚体组成的无约束系统有 6N 个自由度。当系统存在约束时，实际自由度减少。约束分为完整约束与非完整约束：完整约束可写成 Φ(q, t) = 0，例如铰链、滑轨、球铰；非完整约束不可积分为位置形式，例如纯滚动条件。

约束的数学处理有两种主流方式。第一种是拉格朗日乘子法，将约束力作为乘子引入方程，得到指标-3 的微分代数方程；第二种是缩并法，通过约束雅可比矩阵 Φ_q 的零空间构造独立广义坐标，直接消去约束。前者实现简单但需处理漂移，后者方程规模小但零空间构造复杂。工程中常配合 Baumgarte 稳定化或投影法抑制约束漂移。

五、刚体系统建模流程与方程选择

一个可复现的刚体建模流程通常包含以下环节。第一步，定义对象：列出每个刚体的质量 m、质心位置、惯性张量 I^b，并确认单位制（建议 SI）。第二步，建立坐标系：指定惯性系原点与轴向，为每个刚体建立体坐标系，明确旋转顺序与正方向。第三步，选择广义坐标：根据自由度确定 q 的维数，例如平面双摆取两个关节角，空间机械臂取六个关节角加基座位姿。第四步，施加约束：写出约束方程 Φ(q,t) = 0，并计算雅可比矩阵 Φ_q。第五步，选择方程形式：自由度少、约束简单时用拉格朗日方程；需要显式约束力或系统规模大时用牛顿-欧拉方程。第六步，数值积分并检查约束漂移、能量守恒与物理合理性。

以一个平面双摆为例，两杆长度均为 l = 0.5 m，质量均为 m = 1 kg，质心位于杆中点，重力加速度 g = 9.81 m/s²。取两杆与竖直方向夹角 θ₁、θ₂ 为广义坐标，系统自由度为 2。动能与势能分别为：

T = ½ m l² [ (5/3) θ̇₁² + (2/3) θ̇₁ θ̇₂ cos(θ₁−θ₂) + (1/3) θ̇₂² ]
V = −m g l [ (3/2) cos θ₁ + (1/2) cos θ₂ ]

代入拉格朗日方程可得两个耦合的二阶微分方程。若初始条件取 θ₁ = 60°、θ₂ = 30°、角速度为零，用四阶龙格-库塔法以 dt = 1e-3 s 积分 10 s，可观察到两杆角度呈准周期摆动，总能量在 ±0.5% 内波动。若能量持续单调漂移，通常说明步长过大或方程符号有误；若角度出现非物理跳变，应检查约束雅可比与坐标系定义。

对于后续要与柔性体耦合的模型，建议优先采用拉格朗日框架并以广义坐标为接口。柔性体的模态坐标可以自然并入 q，约束方程也可统一处理。若必须使用牛顿-欧拉方程，则需在刚柔连接处显式传递力与力矩，并保证界面点位置、速度连续。无论选择哪条路线，建模流程中的对象定义、坐标系约定与约束检查都不可省略，它们是联合仿真稳定收敛的基础。