4. 多体系统运动学与动力学方程

在联合仿真中，多体系统动力学方程是“被求解的核心对象”。无论接口如何设计、求解器如何协同，最终都要落到一组关于广义坐标 q、广义速度 q̇ 与拉格朗日乘子 λ 的微分代数方程上。本节围绕这组方程从原理到组装，梳理一条可操作的实现路径。

一、从虚功原理到拉格朗日方程

虚功原理是动力学建模的出发点：系统在任意虚位移 δq 下，惯性力、外力与约束力所做虚功之和为零。对由 n 个刚体组成的系统，设第 i 个体质心位置为 ri、质量为 mi、惯量张量为 Ji，则虚功方程可写为：

Σᵢ [ mᵢ δrᵢᵀ ( r̈ᵢ − g ) + δθᵢᵀ ( Jᵢ ω̇ᵢ + ω̃ᵢ Jᵢ ωᵢ − τᵢ ) ] = 0

将 ri、θi 用广义坐标 q 表示后，虚位移可写成 δri = (∂ri/∂q) δq。代入并整理，即得到第二类拉格朗日方程：

d/dt ( ∂T/∂q̇ )ᵀ − ( ∂T/∂q )ᵀ = Q

其中 T 为系统动能，Q 为广义力（含外力、弹簧阻尼力、驱动等）。若系统存在约束，则需引入约束力项，方程扩展为：

M(q) q̈ + C(q, q̇) + Φ<sub>q</sub>ᵀ λ = Q<sub>ext</sub>

这里 M 为质量矩阵，C 为科氏力与离心力项，Φq 为约束雅可比矩阵，λ 为拉格朗日乘子。这一形式是绝大多数多体仿真软件（如 ADAMS、RecurDyn、Simpack）内部实际组装的方程。

二、约束建模：完整约束与非完整约束

约束决定了系统的自由度与方程结构。常见约束包括：

• 完整约束（几何约束）：如旋转副、平移副、球铰，形式为 Φ(q, t) = 0，其雅可比 Φq 直接进入方程。
• 非完整约束：如轮式车辆的纯滚动条件，形式为 Φ(q, q̇, t) = 0，需在速度层处理。
• 驱动约束：将某广义坐标或相对运动指定为时间函数，如 q1 − A sin(ωt) = 0，本质是时变完整约束。

约束建模的关键是写出每个约束的雅可比行。以平面旋转副为例，连接点 Pi 与 Pj 重合，约束为两个标量方程：

Φ = [ xᵢ + lᵢ cosθᵢ − xⱼ − lⱼ cosθⱼ ;
      yᵢ + lᵢ sinθᵢ − yⱼ − lⱼ sinθⱼ ] = 0

对 q = [xi, yi, θi, xj, yj, θj] 求偏导，即得 2×6 的雅可比块。所有约束的雅可比按行堆叠，形成全局 Φq。

三、刚柔耦合项：柔性体如何进入方程

当系统中存在柔性体时，其变形需用模态坐标 ξ 描述。柔性体上任意点的位置可写为：

r = R + A ( r₀ + Φ<sub>f</sub> ξ )

其中 R 为浮动坐标系原点位置，A 为方向余弦矩阵，r₀ 为未变形位置，Φf 为模态振型矩阵。对时间求导后，动能中会出现刚柔耦合项，典型形式为：

T = ½ [ q̇ᵀ M<sub>rr</sub> q̇ + 2 q̇ᵀ M<sub>rf</sub> ξ̇ + ξ̇ᵀ M<sub>ff</sub> ξ̇ ]

其中 Mrf 即刚柔耦合质量矩阵，它随刚体姿态变化，是柔性多体系统区别于纯刚体系统的核心。代入拉格朗日方程后，Mrf 会同时出现在刚体方程与柔性方程中，形成双向耦合。若忽略 Mrf，则退化为“单向耦合”，在大变形或高速旋转场景下会带来显著误差。

四、方程组装流程

实际仿真中，方程组装通常按以下顺序进行：

1. 定义广义坐标 q，确定每个刚体/柔性体的位姿与模态坐标。
2. 计算各体质量矩阵 Mi 与广义力 Qi，柔性体还需计算 Mrf、Mff 与刚度矩阵 Kf。
3. 按约束类型写出 Φ(q, t) 与雅可比 Φq，并组装全局约束矩阵。
4. 将各体矩阵按自由度映射到全局位置，形成全局 M、C、Qext。
5. 与约束方程联立，得到微分代数方程组，交给求解器（如 BDF、Radau、Newmark）积分。

下面这张图展示了从虚功原理到最终微分代数方程的组装链路，以及刚柔耦合项在其中的位置。

五、一个可操作的组装示例

考虑一个平面双连杆+末端柔性梁的简化模型。设连杆 1 长 L1=0.4 m、质量 m1=1.2 kg，连杆 2 长 L2=0.3 m、质量 m2=0.8 kg，末端柔性梁取前两阶模态，模态质量 mf1=0.05 kg、mf2=0.03 kg，模态刚度 kf1=120 N/m、kf2=480 N/m。广义坐标取 q = [θ1, θ2, ξ1, ξ2]。

组装时，先写出两连杆的 2×2 质量矩阵块，再叠加柔性模态的 2×2 对角块，最后将刚柔耦合项 Mrf 填入对应位置。若在 t=0 时给 θ1 一个 0.5 rad 的初始角，并施加 τ1=2 N·m 的驱动力矩，仿真 2 s 后可观察：

• 刚体转角 θ1 随时间增大，θ2 因耦合产生振荡；
• 柔性模态 ξ1 在启动瞬间被激发，幅值约 0.8 mm，随后按 kf1/mf1 对应的频率衰减；
• 若将 Mrf 置零，ξ1 的峰值会下降约 30%，说明耦合项对瞬态响应影响显著。

六、联合仿真中的方程交接

在联合仿真架构下，多体求解器通常负责组装并积分上述方程，而控制器、液压、电机等子系统通过接口交换力、位移或速度。此时需注意：接口变量必须与广义坐标或广义力对应，避免出现“力加到错误自由度”或“位移单位不一致”的问题。例如，若多体侧输出的是关节角 θ，控制器侧输入的是弧度，则无需转换；若控制器输出的是力矩 τ，则直接作为广义力 Q 的一部分加入对应行。

方程组装不是一次性动作，而是每个时间步都可能重新计算 M(q)、C(q, q̇) 与 Φq。对于刚柔耦合系统，Mrf 随姿态变化，若采用模态坐标且模态数较少，可在线更新；若模态数较多，则需权衡精度与效率，必要时采用模态截断或子结构缩减。