3. 多体动力学仿真原理
机器人虚拟样机的可信度，最终取决于动力学求解器能否在每一帧正确回答一个问题：在当前关节位置、速度和外力作用下，各关节需要多大的驱动力矩，才能产生符合物理规律的运动。多体动力学仿真正是围绕这一问题展开的。它把机器人抽象为由刚体、关节和约束组成的多体系统，通过建立并求解运动方程，得到关节加速度、约束反力和接触力，再交给积分器推进状态。理解拉格朗日方程、牛顿-欧拉递推、约束求解以及接触摩擦模型这四条主线，是判断仿真结果是否可信的基础。
一、从能量到方程：拉格朗日方程的作用
拉格朗日方法从系统能量出发建立动力学方程。对由 n 个关节变量 q 描述的机器人，定义拉格朗日函数 L = T − V，其中 T 为系统动能，V 为势能，则运动方程为：
d/dt(∂L/∂q̇) − ∂L/∂q = Q
展开后得到标准形式：
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ + JᵀF_ext
M(q) 惯性矩阵，随位形变化
C(q,q̇) 科氏力与离心力项
G(q) 重力项
τ 关节驱动力矩
JᵀF_ext 末端或接触点外力映射到关节空间拉格朗日方程的优势在于无需显式分析关节处的约束反力，适合串联、树形结构的机器人建模。以一台 6 自由度机械臂为例，若在 q = [0, −π/2, π/2, 0, π/2, 0] 位形下计算，惯性矩阵 M 为 6×6 对称正定矩阵，其对角元反映各关节等效惯量，非对角元反映关节间耦合。当机械臂伸展时，靠近基座的关节等效惯量显著增大，这正是仿真中“伸得越远越难控”的数学来源。实际实现时，M、C、G 通常由递归算法或符号推导生成，而非手工展开。
二、牛顿-欧拉递推：高效计算关节力矩
牛顿-欧拉方法采用另一种思路：对每个连杆分别列写牛顿方程（平动）和欧拉方程（转动），再通过递推消去内部约束力。它分为两个阶段：
- 前向递推：从基座到末端，依次计算各连杆的角速度、角加速度和质心线加速度，得到惯性力和惯性力矩。
- 后向递推：从末端到基座，利用力与力矩平衡，逐关节求出所需驱动力矩和关节约束反力。
牛顿-欧拉递推的计算复杂度为 O(n)，与拉格朗日显式展开的 O(n³) 或 O(n²) 相比，在自由度较多时优势明显。因此，主流仿真引擎（如 MuJoCo、Bullet、ODE 以及 MATLAB 的 Simscape Multibody）在底层多采用递推形式计算 M(q)q̈ + C + G。对使用者而言，这意味着：当模型自由度增加时，仿真步长内的计算量近似线性增长，而不是爆炸式增长。若发现仿真在 20 自由度以上明显变慢，通常不是递推算法本身的问题，而是接触检测或约束求解成为瓶颈。
三、约束求解：把关节和接触写成方程
机器人中的关节、闭链、接触都可以统一表示为约束。设约束函数为 φ(q, t) = 0，对其求导得到速度约束 J q̇ = 0，再求导得到加速度约束 J q̈ + J̇ q̇ = 0。将加速度约束与运动方程联立，得到带约束的动力学方程：
[ M Jᵀ ] [ q̈ ] [ τ − Cq̇ − G ]
[ J 0 ] [ λ ] = [ −J̇q̇ ]
λ 为约束反力（拉格朗日乘子）实际仿真中很少直接求解这个鞍点系统，而是采用更稳定的方法。常见做法包括：用 Baumgarte 稳定化把位置和速度约束误差反馈进加速度约束，抑制数值漂移；或用投影法在每一步积分后把 q 和 q̇ 投影回约束流形。对于闭链机构，约束反力 λ 直接对应连杆间的内力，若 λ 出现异常尖峰，往往意味着模型存在过约束或初始装配误差。
下面这张图概括了从模型输入到状态推进的完整数据流，以及四条主线的位置关系。
四、接触与摩擦：仿真中最容易失真的环节
接触模型决定机器人末端、足端或夹爪与环境的交互力。工程上常用两类方法。第一类是罚函数法：允许微小穿透 δ，法向力 F_n = k·δ + c·δ̇，k 为接触刚度，c 为阻尼。它实现简单、计算快，但 k 过大会导致数值刚性、步长被迫减小，k 过小则穿透明显、接触“发软”。第二类是互补条件法（LCP）：要求 δ ≥ 0、F_n ≥ 0、δ·F_n = 0，即不穿透、不拉拽、接触或分离二选一。它物理上更严格，但求解更复杂，MuJoCo 等引擎采用软约束近似来兼顾稳定性。
切向摩擦通常采用库仑模型：|F_t| ≤ μF_n。当切向力未达到 μF_n 时，接触点保持粘滞；超过后转为滑动，摩擦力方向与相对滑动速度相反。仿真中常见的“机器人抓取物体后缓慢滑落”或“足端打滑”，往往不是控制问题，而是 μ 取值、法向力估计或静动摩擦切换处理不当。例如，抓取一个质量 0.5 kg 的圆柱体，若设定 μ = 0.4，则所需最小夹紧力约为 F_n ≥ mg/(2μ) ≈ 6.1 N（两侧夹持），若仿真中夹紧力仅给 4 N，物体必然滑落——这类量级估算应在建模阶段完成，而不是等仿真失败后再猜。
五、实现中的参数与验证要点
把上述理论落到仿真工程，需要关注几个具体参数。积分步长 h 通常取 0.5～2 ms；对于含高刚度接触的场景，h 需进一步减小，或改用隐式积分。接触刚度 k 的选取应与步长匹配：k 过大时系统固有频率升高，显式积分容易发散。约束求解的 Baumgarte 系数一般取 0.1～0.2，过大将引入人工阻尼，过小则约束漂移明显。
验证动力学实现是否正确，可采用两条低成本判据。其一，能量守恒检查：在无驱动、无摩擦的自由摆动中，系统总机械能应近似守恒，若持续单调增大，说明积分器或约束稳定化引入了能量。其二，重力静平衡检查：令 q̇ = q̈ = 0，此时 τ = G(q)，将仿真输出的关节力矩与解析重力项对比，误差应在数值精度范围内。这两项检查不需要额外硬件，却能在早期发现大部分建模与求解错误。