2. 多体动力学建模基础
多体系统动力学的建模过程，本质上是在“用尽量少的坐标描述尽量复杂的机械系统”这一目标下，完成从物理对象到可求解数学模型的转换。本节围绕四个相互衔接的环节展开：刚体运动学描述、动力学方程建立、约束建模、广义坐标选择与系统组装。理解这条链路，才能明白为什么同一个机构可以用不同方式建模，以及不同选择会带来怎样的求解代价。
一、刚体运动学：位姿、速度与加速度的递推
刚体在空间中的状态由位置和姿态共同决定。位置用质心或参考点的三维坐标表示，姿态用方向余弦矩阵、欧拉角或四元数表示。以方向余弦矩阵 A 为例，刚体上任意一点 p 在全局坐标系中的位置可写为：
r_p = r_i + A_i * s_p'
其中 r_i 是刚体参考点位置，s_p' 是该点在局部坐标系中的常矢量。对时间求导得到速度关系：
v_p = v_i + ω_i × (A_i * s_p')
这里 ω_i 是刚体角速度矢量。再求导得到加速度，会额外出现向心加速度和科氏加速度项。实际建模中，通常不直接对方向余弦矩阵求导，而是引入角速度的反对称矩阵 ω̃，满足 Ȧ = ω̃ A，这样姿态运动学方程更简洁，也便于与动力学方程联立。
对于由多个刚体组成的系统，运动学量需要沿拓扑关系递推。以开链系统为例，从基座向末端依次计算每个刚体的位姿、速度和加速度，父体的运动作为子体的输入。这种递推方式计算量小、结构清晰，是许多多体仿真软件内部采用的基本策略。
二、动力学方程：从牛顿-欧拉到拉格朗日
单个刚体的动力学可用牛顿-欧拉方程描述：
m * a = F
I * α + ω × (I * ω) = M
其中 m 为质量，I 为惯性张量，F 和 M 分别为合外力和合力矩。牛顿-欧拉方法物理意义直观，适合递推计算，但约束力需要显式求解。
对于含约束的多体系统，更常用的是拉格朗日方程。设广义坐标为 q，动能 T，势能 V，则：
d/dt(∂T/∂q̇) - ∂T/∂q + ∂V/∂q = Q
其中 Q 为非保守广义力。拉格朗日方法自动消去理想约束力，方程数量等于广义坐标数，适合系统级建模。实际仿真中，常将拉格朗日方程整理为如下形式：
M(q) * q̈ + C(q, q̇) * q̇ + G(q) = Q
其中 M 为质量矩阵，C 为科氏与离心项，G 为重力项。这一形式是后续数值积分和控制器设计的基础。
三、约束建模：完整约束与非完整约束
约束是连接刚体、限制相对运动的数学关系。常见约束包括旋转铰、平移铰、球铰、万向节、齿轮副和接触约束等。按数学形式可分为完整约束和非完整约束。
完整约束可写成位置层面的代数方程：
Φ(q, t) = 0
例如旋转铰约束两个刚体上某点重合，并限制三个相对转动自由度。对时间求导得到速度约束：
Φ_q * q̇ = -Φ_t
再求导得到加速度约束：
Φ_q * q̈ = γ
其中 Φ_q 为约束雅可比矩阵，γ 为加速度约束右端项。非完整约束则只能写成速度层面的不可积关系，例如车轮纯滚动约束。建模时需要根据实际运动特点判断约束类型，否则会导致模型自由度错误或求解失败。
在数值求解中，完整约束容易产生违约问题，即位置和速度约束随时间漂移。常用处理方式包括Baumgarte稳定化、坐标投影和增广拉格朗日法。Baumgarte方法在加速度约束中引入位置和速度反馈项：
Φ_q * q̈ = γ - 2α * Φ̇ - β² * Φ
其中 α、β 为稳定化参数，需根据系统刚度和积分步长选取。参数过小约束漂移明显，过大则可能引入高频振荡。
四、广义坐标选择与系统组装
广义坐标的选择直接决定模型规模和求解效率。常见方案有三类：
- 笛卡尔坐标：每个刚体用6个坐标描述，约束方程数量多，但质量矩阵稀疏、组装规则统一，适合复杂拓扑和接触问题。
- 相对坐标：用铰链相对运动变量描述，坐标数等于自由度，方程维数低，但质量矩阵稠密，适合开链和树状系统。
- 混合坐标：对部分刚体用笛卡尔坐标，对部分铰链用相对坐标，兼顾稀疏性和低维数，常见于车辆和机器人模型。
系统组装是把各刚体的动力学方程和约束方程按拓扑关系拼装成整体方程。以笛卡尔坐标为例，组装后的指标-3微分代数方程形式为：
[ M Φ_qᵀ ] [ q̈ ] [ Q ]
[ Φ_q 0 ] [ λ ] = [ γ ]
其中 λ 为拉格朗日乘子，对应约束力。组装过程包括：确定广义坐标向量、计算各刚体质量矩阵和广义力、计算约束雅可比矩阵、按节点和铰链关系填充整体矩阵。对于闭环系统，还需处理冗余约束和奇异构型。
五、一个可操作的建模检查流程
实际建模时，可按以下顺序检查，避免返工：
- 明确惯性参考系和重力方向，统一单位制。
- 列出每个刚体的质量、质心位置和惯性张量，确认惯性张量的参考点和坐标系。
- 定义铰链类型和连接关系，写出每个约束的
Φ(q,t)=0表达式。 - 选择广义坐标，统计自由度数，验证
自由度 = 坐标数 - 独立约束数。 - 组装质量矩阵、广义力向量和约束雅可比矩阵，检查矩阵对称性和稀疏性。
- 设置积分步长和约束稳定化参数，先做静平衡或小位移测试，再逐步增加驱动和载荷。
- 观察约束违约量、能量变化和求解器迭代次数，判断模型是否稳定。
下面用一张图概括从物理系统到可求解方程组的建模链路，重点体现运动学、动力学、约束和组装之间的输入输出关系。
这张图强调了一个容易被忽略的事实：运动学、动力学、约束和组装并不是四个独立步骤，而是相互反馈的整体。数值求解得到的状态会反过来影响运动学量的计算，约束违约又需要通过稳定化或投影修正。建模时如果只关注方程形式而忽略这条反馈链，往往会在仿真中出现能量漂移、约束失效或求解器不收敛。
最后需要强调，广义坐标和约束形式的选择没有绝对优劣，只有是否匹配当前系统拓扑、载荷特征和求解精度要求。对于同一台机械臂，用相对坐标建模可能只有6个自由度，用笛卡尔坐标则可能有42个坐标加36个约束。前者方程少但矩阵稠密，后者方程多但矩阵稀疏。实际工程中，应结合仿真软件的能力、实时性要求和模型复用需求综合决定，而不是机械套用某一种方案。