3.1 多刚体动力学基础
3.1 多刚体动力学基础
本节摘要：多刚体动力学的全部内容可以压成一个方程：质量矩阵乘加速度加偏置力，等于执行器力矩加约束力。看似简单，五脏俱全——质量矩阵怎么从运动学树装配、偏置力里藏着什么、约束力从哪进出，决定了引擎的速度与精度，也决定了第五章控制器能借到哪些力。本节讲方程的每一项、装配它的两类递归算法、以及正逆动力学两个方向的工程用途。
上一章把样机造成了一棵运动学树；本节开始，引擎要让这棵树动起来。这是第三章的地基节：3.2 的接触、3.3 的积分全部发生在这个方程框架之内。
学习目标
阅读完本节，你应当能够：
- 写出多刚体系统的运动方程，逐项说明质量矩阵、偏置力、约束力的物理含义；
- 说明引擎如何从运动学树装配质量矩阵，为什么树状结构让这件事变得便宜；
- 用逆动力学接口回答「维持某姿态需要多大力矩」这类问题，并知道它在控制里的三个用途。
一、从单摆到机器：方程长什么样
单摆的动力学一句话就能讲清：重力沿切向的分量产生角加速度，质量与摆长决定惯性。多刚体系统不过是这个故事的「多维版」——每个自由度一个坐标，所有坐标摞成一个广义坐标向量 q，运动方程写成：
M 加速度加偏置力等于执行器力矩加约束力
用文字读一遍：质量矩阵 M 是「这台机器有多沉、沉在哪些方向」的完整描述——它随姿态变化，机械臂伸直与蜷缩时的 M 完全不同；加速度是未知数，是引擎每步真正要求的东西；偏置力把重力、离心力、科氏力、关节阻尼这些「不用你指挥就在的力」打包；执行器力矩是你的控制输入；约束力是接触、限位、等式约束给出的反作用力，3.2 的主角。
这个方程有两个对工程实践要紧的性质。其一，M 是对称正定的——不管机器多复杂，惯性在数学上永远表现良好，求解不会遇到除以零的噩梦（建模错误除外，第二章的惯量校验就是在守这道门）。其二，除约束力外方程是光滑的，这让「求导」成为合法操作：对输入力矩求导、对质量参数求导都有确定答案，第七章的可微分仿真吃的正是这口饭。
二、装配方程：树状结构便宜在哪里
方程里的每一项都要算出来，而算法的效率全看数据结构。世界上有大量刚体时，蛮力求 M 的代价是自由度数的三次方；沿树递归的两类算法把它降到线性——这是第二章说「树是刚需」的真正兑现。
正向类：从质量到方程。 复合刚体算法自叶向根走一遍：先把每个叶子自己的惯性算清，逐级向上「叠加」，每个节点把自己的惯量加上所有子树折算过来的惯量，最后按关节轴投影出 M 的每一列。它同时把科氏与重力项捎带算出，一遍递归全齐。引擎每一步都跑这套装配，快到什么程度？几百自由度的人形机器人在普通桌面 CPU 上，一步动力学只要几十微秒——这是第一章「单环境速度」优势的来源。
逆向类：从运动到力。 递归牛顿—欧拉算法走另一个方向：给定每个关节的位置、速度、加速度，自根向叶传递运动（每个刚体「被带着怎么动」就清楚了），再自叶向根汇聚所需的力。它直接回答逆动力学问题：要让机器走出这条轨迹，每个关节该出多大力。
两个方向在接口上各占一个函数：正向动力学算「已知力求运动」，mj_step 内部用的就是它；逆动力学是独立接口，工程价值常被低估。看一段可跑的代码：
import mujoco import numpy as np model = mujoco.MjModel.from_xml_path("arm6.xml") data = mujoco.MjData(model) # 摆出目标姿态并让所有关节静止 data.qpos[:] = [0.3, -0.5, 0.8, -1.2, 0.4, 0.0] data.qvel[:] = 0 mujoco.mj_forward(model, data) # 先算正运动学填充中间量 mujoco.mj_inverse(model, data) # 再问：保持这姿态要什么力矩 print("重力补偿力矩:", np.round(data.qfrc_inverse, 2))
这十几行就是一个可用的重力补偿器雏形。把 qfrc_inverse 直接喂给力控执行器，机械臂会「软」地停在任何姿态上，像被一只无形的手托着——协作臂的拖动示教就是这么实现的。
图 3-1 一个方程的解剖：五项从哪来、谁在每步变
三、正逆动力学的工程用法
用法一：重力补偿与拖动示教。 上面代码的扩展：实时算 qfrc_inverse 并前馈给力矩执行器，机器人对人的推拉不再抵抗，操作员直接把臂「摆」到目标位姿录轨迹。真实协作臂产品里这是出厂功能，仿真里一行接口就能原型验证。
用法二：负载辨识。 机械臂末端抓了未知重物，问：多重？把带载与空载两种状态的逆动力学结果相减，多出来的力矩就是负载贡献，质量与质心可以从关节空间的差量反解。某项目给抓取机械臂加了这个辨识环节，称重精度做到百分之五以内——没有加装任何称重传感器。
用法三：MPC 的内部模型。 第五章会展开，模型预测控制在每个决策步都要问「未来若干步我这样施力，机器会怎么动」，这个预演调用的正是正向动力学。MPC 对单次动力学调用延迟极其敏感，引擎的求解速度直接决定控制频率的上限——「单环境快」在这个场景里兑换成了「控制频率高」。
变式与边界：逆动力学假设你给的轨迹物理上可实现；给一条会穿墙的轨迹，它会「配合」地算出一组越墙所需的鬼力——约束力在逆接口里默认按等式处理，接触这类单边约束要靠正方向才有完整语义。记住分工：接触问题交给正方向，轨迹跟踪问题才找逆方向。
本节要点回顾
- 一个方程走天下：质量矩阵乘加速度加偏置力等于执行器力矩加约束力，机器再复杂也是它的展开；
- 树换来线性复杂度：复合刚体与递归牛顿—欧拉沿树递归装配方程，几百自由度也是微秒级；
- 逆动力学是接口不是理论：重力补偿、负载辨识、MPC 内部模型，三个高频工程用法；
- 正逆有分工：接触与仿真正方向包办，轨迹与补偿逆方向擅长，别用错场合。
方程里最硬的骨头还剩一根：约束力怎么算。下一节进入接触动力学——MuJoCo 用一套软约束凸优化把它变成可解、可微、可调的问题。