Part II · 第 8 章动力学 · 8.2

牛顿-欧拉递推

机器人仿真器和 1 kHz 的力矩控制器每个周期都要回答同一个问题：关节要出多大力矩，才能让每根连杆按指定的加速度运动？拉格朗日方程给出了答案，但符号展开会随自由度爆炸。递推牛顿-欧拉算法 (RNEA) 换了一种想法：把牛顿第二定律和欧拉方程逐根连杆写下来，先从基座向末端把速度和加速度一级一级传出去，再从末端向基座把力和力矩一级一级收回来，计算量只与关节数 $n$ 成正比。

约 35 分钟前置：1.8 叉积 · 5.8 伴随 · 5.10 PoE · 7.4 速度运动学 · 7.6 静力学与 wrench · 8.1 拉格朗日动力学 6 个交互

1为什么机器人需要它

8.1 节用拉格朗日方程得到了动力学方程 $M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)=\boldsymbol\tau$（第 8 章始终用 $\mathbf q=(\theta_1,\dots,\theta_n)$ 表示关节坐标，偏置项记作 $\mathbf b=C\dot{\mathbf q}+\mathbf g$）。真实的机器人软件需要的是它的两种"算法形式"：

逆动力学 $\boldsymbol\tau=\mathrm{ID}(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})$：给定想要的运动，算出需要的力矩。力矩前馈、计算力矩控制（Part IV）、重力补偿、从关节电流估计接触力，用的都是它。
正动力学 $\ddot{\mathbf q}=\mathrm{FD}(\mathbf q,\dot{\mathbf q},\boldsymbol\tau)$：给定力矩，算出机器人会怎么动。物理仿真器每个时间步都在积分它。

7 自由度的机械臂要在 1 kHz 下算，30 多个自由度的人形机器人也一样。对一个 6 轴机械臂手推拉格朗日方程，$M$、$C$ 的符号展开已经要写满好几页；自由度再多，项数增长得更快，没有人会把它们手抄进控制器。递推牛顿-欧拉算法（Luh–Walker–Paul，1980）的特点是：没有任何符号展开，每根连杆只做固定次数的向量运算，所以总计算量是 $O(n)$。

它的思想朴素到几乎是常识：每根连杆都是一个刚体，刚体满足 $\mathbf F=m\mathbf a$ 和欧拉方程；连杆之间的相互作用力只通过关节传递。于是先向外传运动，再向内传力，一趟就算完。

2直觉：向外传运动，向内传力

先想一条鞭子。你抖动手腕（关节 1），整根鞭子的运动是一级一级传下去的：第 $i$ 段的运动，等于第 $i-1$ 段已经有的运动，再加上关节 $i$ 自己多转的那一部分。这就是向前递推 (forward pass)，只涉及运动学：角速度 $\omega_i$、角加速度 $\alpha_i$、质心加速度 $\mathbf a_{c_i}$。

质心加速度怎么来的？7.4 节和 1.8 节的刚体速度公式再求一次导：刚体上离参考点 $\mathbf r$ 处的一点，加速度是

$$ \mathbf a_{p} \;=\; \hl{inh}{\mathbf a_{o}} \;+\; \hl{euler}{\alpha\,\hat{\mathbf z}\times\mathbf r} \;-\; \hl{centri}{\omega^{2}\,\mathbf r}. $$

三项各有各的含义：继承项（参考点自己的加速度）、欧拉项（角加速度造成的切向加速度，垂直于 $\mathbf r$）、向心项（匀速转动也有，指向转轴）。下面的 FIG 1 用三根箭头把这三项接在一起，合成的琥珀色箭头就是质心加速度。点"下一步"，让递推从基座走到末端。

FIG 1向前递推：速度与加速度逐级外传

平面三连杆臂，均匀细杆：长度 1.0 / 0.8 / 0.5 m，质量 2.0 / 1.6 / 1.0 kg（密度 2 kg/m），质心在杆的中点。灰色箭头是继承的加速度，紫色是欧拉项 $\alpha\hat{\mathbf z}\times\mathbf r_c$，蓝色是向心项 $-\omega^2\mathbf r_c$，琥珀色虚线箭头是三者之和 $\mathbf a_c$，青色细箭头是质心速度。加速度箭头的比例尺显示在读数里。圆弧是 $\omega_i$。拖动关节上的圆圈改变姿态。

试一试

点"下一步"，一直走到第 3 根连杆。外面的连杆为什么一直"带着"里面所有关节的角速度？
把所有 $\ddot\theta$ 设为 0（按钮"θ̈ = 0"），而让第 3 根连杆的质心仍有大于 0.5 m/s² 的加速度。没有任何东西在加速，加速度从哪来？

核心洞见

向前递推只需要"上一根连杆的结果"和"本关节的 $\theta,\dot\theta,\ddot\theta$"，每根连杆做固定多的运算。不管链有多长，每一级都只看它的邻居，所以总代价与 $n$ 成正比。

3动手玩：向后递推，一根连杆一张受力图

运动都知道了，质心加速度 $\mathbf a_{c_i}$ 和角加速度 $\alpha_i$ 已经在手里。现在反过来，从末端往基座走。取出第 $i$ 根连杆，只看它受到的力：

关节 $i$ 处，前一根连杆通过关节推它：力 $\mathbf f_i$，力矩 $\boldsymbol\mu_i$；
关节 $i+1$ 处，后一根连杆反推它：由牛顿第三定律，是 $-\mathbf f_{i+1}$ 和 $-\boldsymbol\mu_{i+1}$；
重力 $m_i\mathbf g$ 作用在质心。

这条连杆做 $\mathbf a_{c_i}$ 的运动，所以三个力之和必须等于 $m_i\mathbf a_{c_i}$：只有 $\mathbf f_i$ 是未知的，立刻解出。力矩平衡同理解出 $\boldsymbol\mu_i$。而关节 $i$ 的电机在转轴方向上出的力矩，正好就是 $\tau_i=\mu_i$。算完 $\mathbf f_i,\boldsymbol\mu_i$，它们就是下一根（更靠近基座的）连杆要用的 $\mathbf f_{i+1},\boldsymbol\mu_{i+1}$。末端是递推的起点：手爪对环境施加的力 $\mathbf F_{\text{tip}}$ 就是 $\mathbf f_{n+1}$（空载时为 0）。

FIG 2向后递推：受力图与关节力矩

τ₁

τ₂

τ₃

同一条三连杆臂。"下一步"从末端开始，逐根连杆画出受力图：关节 $i$ 处的 $\mathbf f_i$（用关节的颜色），关节 $i+1$ 处的 $-\mathbf f_{i+1}$，质心处的重力（紫色）和合力 $m\mathbf a$（青色虚线）。圆弧是力矩 $\mu_i$ 与 $-\mu_{i+1}$ 的方向。拖动琥珀色手柄设定手爪施加给环境的力 $\mathbf F_{\text{tip}}$（1 m ↔ 10 N），拖动关节圆圈改变姿态，滑块设定 $\dot\theta,\ddot\theta$。"全部显示"画出每个关节处的 $\mathbf f_i$。手爪力为 0 时琥珀色手柄与第 3 个关节圆圈重合（优先抓到力的手柄），先把它拖开。

点"下一步"，从末端走到关节 1，让三个力矩都出现。哪个关节的力矩最大？为什么？
按"静止"（$\dot\theta=\ddot\theta=0$），手爪力为 0，打开重力，把手臂摆成竖直向上。三个力矩都应当接近 0（$\max|\tau|<0.3$）。这是稳定平衡吗？
用手爪力把 $\tau_1$ 调到几乎为 0（$|\tau_1|<0.4$），同时 $\|\mathbf F_{\text{tip}}\|\ge 1$ N。$\mathbf F_{\text{tip}}$ 对基座的力矩在抵消什么？（提示：先按“静止”，姿态接近竖直时重力力矩小，手爪力够用。）

静力学是它的特例

令 $\dot\theta=\ddot\theta=0$，向后递推就只剩重力和手爪力。读数里的"$\mathrm{RNEA}(\mathbf F)-\mathrm{RNEA}(0)-J^\top\mathbf F$"检验了这一点：手爪力引起的力矩恰好是 3.4 节的 $J^\top\mathbf F$（7.6 节把它推广到 wrench）。RNEA 对 $\mathbf F_{\text{tip}}$ 是线性的，所以这个差在任何运动状态下都成立。

4第二、第三视角：它就是拉格朗日方程

递推 = 拉格朗日？

牛顿-欧拉从"力平衡"出发，拉格朗日从"能量"出发，两条路没有共同的中间步骤，却应当给出同一个 $\boldsymbol\tau$。FIG 3 把它们放在一起比：一边是 RNEA 的递推结果，另一边是完全独立写成的 $M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)$，其中 $M=\sum_i\big(m_iJ_{c_i}^\top J_{c_i}+I_iJ_{\omega_i}^\top J_{\omega_i}\big)$（质心雅可比，8.3 节详细讲）、$C$ 由 $M$ 的偏导数（Christoffel 符号）得到、$\mathbf g=\sum_i m_iJ_{c_i}^\top(0,g)^\top$。右图把力矩拆成三块：惯性 $M\ddot{\mathbf q}$、速度项 $C\dot{\mathbf q}$、重力 $\mathbf g$，三块首尾相接，应当恰好落在 RNEA 的琥珀色菱形上。两连杆时还会和 8.1 节的闭式解比较。

FIG 3RNEA = 拉格朗日

姿态（拖动圆圈）

右图每行一个关节：红色是 $M\ddot{\mathbf q}$，紫色是 $C\dot{\mathbf q}$，蓝色是 $\mathbf g$，沿水平方向（N·m）首尾相接；琥珀色菱形是 RNEA 给出的 $\tau_i$。"最大差"是 RNEA 与独立拉格朗日计算之间的最大绝对误差，只来自浮点舍入。"随机状态"每次随机抽取 $\mathbf q,\dot{\mathbf q},\ddot{\mathbf q}$。

连按 5 次"随机状态"。每次两种算法都吻合到 $10^{-10}$ 以内吗？
关掉重力，设 $\ddot\theta=0$，让各关节以非零速度转动，使 $\|\boldsymbol\tau\|>0.3$。没有加速度、没有重力，为什么还需要力矩？

用 RNEA 一列一列地造出质量矩阵

RNEA 是 $\boldsymbol\tau$ 的黑盒，而 $\boldsymbol\tau=M\ddot{\mathbf q}+\mathbf b$，其中 $\mathbf b=C\dot{\mathbf q}+\mathbf g$ 与 $\ddot{\mathbf q}$ 无关。令 $\dot{\mathbf q}=0$，再令 $\ddot{\mathbf q}=\mathbf e_j$（只有关节 $j$ 单位加速），有 $\mathrm{RNEA}(\mathbf q,0,\mathbf e_j)=M\mathbf e_j+\mathbf g$，也就是 $M$ 的第 $j$ 列加上重力项；再算一次 $\mathrm{RNEA}(\mathbf q,0,0)=\mathbf g$，相减：

$$ M(\mathbf q)\,\mathbf e_j \;=\; \mathrm{RNEA}(\mathbf q,\,0,\,\mathbf e_j)\;-\;\mathrm{RNEA}(\mathbf q,\,0,\,0),\qquad j=1,\dots,n. $$

这就是 Walker–Orin（1982）的想法：用 $n$ 次（关掉重力就只要 $n$ 次，开着重力是 $n+1$ 次）$O(n)$ 的 RNEA 调用拼出 $M$，再解线性方程 $M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b$，就得到正动力学，其中 $\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)$。FIG 4 一列一列地做这件事。

FIG 4逐列拼出 $M(\mathbf q)$

M₁ⱼ

M₂ⱼ

M₃ⱼ

左图：让关节 $j$ 以 $1\ \mathrm{rad/s^2}$ 加速、其余关节不加速（$\dot\theta=0$），琥珀色箭头是各连杆质心此时的加速度（0.5 m ↔ 1 m/s²），紫色圆弧是关节 $j$ 的加速度。右侧的三个条是这次实验所需的关节力矩减去重力项，即 $M$ 的第 $j$ 列。切换"重力"可以看到：开着重力时，两次调用的重力项相互抵消，$M$ 不变。

依次点出三列，让矩阵完整。核对：它是对称的吗？
拖动关节，把手臂折叠起来，使 $M$ 出现一个小于 $-0.1$ 的非对角元，同时最小特征值仍是正的。惯性耦合的符号为什么会变？

一个思想，几种看法

力的视角：每根连杆受力平衡，力沿着链向内累加（RNEA）。能量的视角：写出总动能 $T=\tfrac12\dot{\mathbf q}^\top M\dot{\mathbf q}$ 和势能，求导（拉格朗日，8.1 节）。矩阵的视角：同一个映射 $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\mapsto\boldsymbol\tau$，对 $\ddot{\mathbf q}$ 是仿射的，斜率就是 $M$。计算的视角：RNEA 是一个"黑盒函数"，有了函数就能用有限次调用恢复出 $M$，这是现代库（Pinocchio、RBDL、Drake）里"一个核心，多个接口"的设计原因。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 与 FIG 2 里对应的箭头会被加粗。

平面 RNEA 全貌

平面 $n$ 连杆臂，所有转轴垂直于纸面，$\theta_i$ 是关节角。第 $i$ 根连杆：质量 $m_i$，质心处的转动惯量 $I_i$（均匀细杆是 $m_il_i^2/12$），$\mathbf r_i$ 为关节 $i$ 指向关节 $i+1$ 的向量，$\mathbf r_{c_i}$ 为关节 $i$ 指向质心的向量。所有向量都写在世界系里。角量是标量，叉积 $\mathbf a\times\mathbf b=a_xb_y-a_yb_x$ 也是标量。记 $\omega_0=\alpha_0=0$，$\mathbf a_1$ 是关节 1（基座）的加速度，先取 $\mathbf a_1=(0,\,g)$（原因见下面的"重力技巧"）。

向前（$i=1\to n$）：

$$ \begin{aligned} \hl{omega}{\omega_i} &= \omega_{i-1}+\dot\theta_i, \qquad \alpha_i = \alpha_{i-1}+\ddot\theta_i,\\ \mathbf a_{c_i} &= \hl{inh}{\mathbf a_i} + \hl{euler}{\alpha_i\hat{\mathbf z}\times\mathbf r_{c_i}} - \hl{centri}{\omega_i^2\,\mathbf r_{c_i}},\\ \mathbf a_{i+1} &= \mathbf a_i + \alpha_i\hat{\mathbf z}\times\mathbf r_i - \omega_i^2\,\mathbf r_i . \end{aligned} $$

向后（$i=n\to1$）：起点 $\mathbf f_{n+1}=\mathbf F_{\text{tip}}$（手爪施加给环境的力），$\mu_{n+1}=0$。

$$ \begin{aligned} \hl{force}{\mathbf f_i} &= \mathbf f_{i+1} + m_i\,\mathbf a_{c_i},\\ \hl{moment}{\mu_i} &= \mu_{i+1} + I_i\alpha_i + \mathbf r_{c_i}\times\mathbf f_i + (\mathbf r_i-\mathbf r_{c_i})\times\mathbf f_{i+1},\\ \tau_i &= \mu_i . \end{aligned} $$

来源。第 $i$ 根连杆受到 $\mathbf f_i$（作用在关节 $i$）、$-\mathbf f_{i+1}$（作用在关节 $i+1$）和重力 $m_i\mathbf g$。牛顿第二定律：$\mathbf f_i-\mathbf f_{i+1}+m_i\mathbf g=m_i\mathbf a^{\text{真实}}_{c_i}$。把重力移到右边，$\mathbf f_i-\mathbf f_{i+1}=m_i(\mathbf a^{\text{真实}}_{c_i}-\mathbf g)$，括号里就是上面递推里的 $\mathbf a_{c_i}$（见下）。对质心取矩（欧拉方程）：$I_i\alpha_i=\mu_i-\mu_{i+1}+(-\mathbf r_{c_i})\times\mathbf f_i+(\mathbf r_i-\mathbf r_{c_i})\times(-\mathbf f_{i+1})$，移项就是上面的 $\mu_i$；重力过质心，不产生力矩。

重力技巧。令 $\mathbf g=(0,-g)$，把基座当作以 $\mathbf a_1=-\mathbf g=(0,+g)$ 向上加速。这样每根连杆的质心加速度都多出一个 $-\mathbf g$，而 $m_i(\mathbf a^{\text{真实}}_{c_i}-\mathbf g)$ 正好就是 $m_i\mathbf a_{c_i}$：重力不再需要单独处理。好像你在一部以 $g$ 向上加速的电梯里，重力和"电梯加速"无法区分。

平行轴定理。递推用的是质心惯量 $I_i$。一根均匀杆绕自己端点的惯量是 $ml^2/3=ml^2/12+m(l/2)^2$，多出来的 $m(l/2)^2$ 在递推里已经由叉积项 $\mathbf r_{c_i}\times m_i\mathbf a_{c_i}$ 提供，不能再算一次。

检验。静止时（$\omega=\alpha=0$，$\mathbf F_{\text{tip}}=0$）$\mathbf f_i=\sum_{k\ge i}m_k(0,g)$，关节 $i$ 要托住它外面所有连杆的重量；一根水平的均匀杆 $\tau=\mathbf r_c\times\mathbf f=(l/2)\,mg$。

空间版本（Modern Robotics 的写法）

三维里角速度、速度、力、力矩都是三维向量，惯量是 $3\times3$ 矩阵。把它们成对地装进 6 维向量，递推就和平面版长得一样了。约定（与全课一致）：twist $\mathcal V=(\boldsymbol\omega,\mathbf v)$ 角部分在前，wrench $\mathcal F=(\boldsymbol\mu,\mathbf f)$ 力矩在前。坐标系 $\{0\}$ 固定在基座；$\{i\}$ 固定在连杆 $i$ 上，原点在它的质心；$M_{i-1,i}$ 是零位 ($\mathbf q=0$) 时 $\{i\}$ 相对 $\{i-1\}$ 的位姿；$\mathcal A_i$ 是关节 $i$ 的螺旋轴在 $\{i\}$ 系中的表示（5.10 节的 $S_i$ 经 $\mathrm{Ad}_{M_{0,i}^{-1}}$ 变到 $\{i\}$ 中）。则

$$ T_{i,i-1}=e^{-[\mathcal A_i]\theta_i}\,M_{i,i-1}, \qquad \mathcal V_0=0,\quad \dot{\mathcal V}_0=(\mathbf 0,\,-\mathbf g). $$ $$ \begin{aligned} \text{forward:}\quad \mathcal V_i &= \mathrm{Ad}_{T_{i,i-1}}\mathcal V_{i-1} + \mathcal A_i\dot\theta_i,\\ \dot{\mathcal V}_i &= \mathrm{Ad}_{T_{i,i-1}}\dot{\mathcal V}_{i-1} + \mathrm{ad}_{\mathcal V_i}\mathcal A_i\,\dot\theta_i + \mathcal A_i\ddot\theta_i,\\[4pt] \text{backward:}\quad \mathcal F_i &= \mathrm{Ad}_{T_{i+1,i}}^{\top}\mathcal F_{i+1} + \mathcal G_i\dot{\mathcal V}_i - \mathrm{ad}_{\mathcal V_i}^{\top}\mathcal G_i\mathcal V_i,\\ \tau_i &= \mathcal F_i^{\top}\mathcal A_i,\qquad \mathcal G_i=\begin{bmatrix}\mathcal I_i&0\\0&m_i\mathbf 1_3\end{bmatrix}. \end{aligned} $$

其中 $\mathcal F_{n+1}$ 是末端坐标系 $\{n+1\}$ 中手爪对环境的 wrench，$T_{n+1,n}$ 是常量。各项来源：

$\mathrm{Ad}_{T_{i,i-1}}\mathcal V_{i-1}$：把前一根连杆的 twist 从 $\{i-1\}$ 搬到 $\{i\}$（5.8 节的伴随）；$\mathcal A_i\dot\theta_i$：本关节自己多转出的 twist。这是平面里 $\omega_i=\omega_{i-1}+\dot\theta_i$ 的 6 维版本。
$\mathrm{ad}_{\mathcal V}$是 twist 的李括号矩阵：对 $\mathcal V=(\boldsymbol\omega,\mathbf v)$ 和任意 twist $\mathcal W$，$[\mathcal V,\mathcal W]=\mathrm{ad}_{\mathcal V}\mathcal W$， $\mathrm{ad}_{\mathcal V}=\begin{bmatrix}[\boldsymbol\omega]_\times&0\\ [\mathbf v]_\times&[\boldsymbol\omega]_\times\end{bmatrix}$。它也是伴随的导数：若 $T(t)=e^{-[\mathcal A]\theta(t)}M$，则 $\tfrac{d}{dt}\mathrm{Ad}_T=-\dot\theta\,\mathrm{ad}_{\mathcal A}\mathrm{Ad}_T$。
$\dot{\mathcal V}_i$ 的来源：把 $\mathcal V_i$ 的式子对时间求导，$\dot{\mathcal V}_i=\mathrm{Ad}_T\dot{\mathcal V}_{i-1}-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathrm{Ad}_T\mathcal V_{i-1}+\mathcal A_i\ddot\theta_i$。用 $\mathrm{Ad}_T\mathcal V_{i-1}=\mathcal V_i-\mathcal A_i\dot\theta_i$、$\mathrm{ad}_{\mathcal A}\mathcal A=0$ 和 $\mathrm{ad}_{\mathcal X}\mathcal Y=-\mathrm{ad}_{\mathcal Y}\mathcal X$，得到 $-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathcal V_i=\mathrm{ad}_{\mathcal V_i}\mathcal A_i\dot\theta_i$，这一项在平面里就是向心项（$\hat z\times$ 的二次叠加）。
$\mathcal G_i\dot{\mathcal V}_i-\mathrm{ad}^\top_{\mathcal V_i}\mathcal G_i\mathcal V_i$：单个刚体在自身质心系里的牛顿-欧拉方程。把 $\mathcal V=(\boldsymbol\omega,\mathbf v)$ 代入，它等于 $(\mathcal I\dot{\boldsymbol\omega}+\boldsymbol\omega\times\mathcal I\boldsymbol\omega,\; m(\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v))$：第一项是欧拉方程（含陀螺项 $\boldsymbol\omega\times\mathcal I\boldsymbol\omega$），第二项是 $m\mathbf a$（体坐标里的真实加速度是 $\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v$）。平面里陀螺项消失，只剩 $I\alpha$ 与 $m\mathbf a$。
$\mathrm{Ad}^\top_{T_{i+1,i}}\mathcal F_{i+1}$：把后一根连杆通过关节 $i+1$ 传来的 wrench 换算到 $\{i\}$ 中。为什么是伴随的转置？功率不变：$\mathcal F_{i+1}^\top\mathcal V_{i+1}=\mathcal F_{i+1}^\top\mathrm{Ad}_{T_{i+1,i}}\mathcal V_i=(\mathrm{Ad}^\top\mathcal F_{i+1})^\top\mathcal V_i$（7.6 节）。
$\tau_i=\mathcal F_i^\top\mathcal A_i$：电机沿螺旋轴做功，$\tau_i\dot\theta_i=\mathcal F_i^\top(\mathcal A_i\dot\theta_i)$。

数值核对。我们把平面三连杆臂嵌入三维（转轴沿 $z$，连杆沿 $x$ 放置，$\mathcal A_i=(0,0,1,\,0,\,l_i/2,\,0)$，$\mathcal I_i=\mathrm{diag}(0,0,m_il_i^2/12)$，平面运动只用到 $zz$ 分量），随机抽取 300 个状态和手爪力，空间递推与平面递推的 $\boldsymbol\tau$ 在 $3\times10^{-14}$ 内一致；平面递推又与独立的拉格朗日公式（FIG 3）在同样的量级内一致。

树与浮动基座。树结构只要把"上一根"换成"父连杆"，向后一步对所有子连杆的 $\mathrm{Ad}^\top\mathcal F$ 求和。8.3 节会看到 $M(\mathbf q)=\sum_iJ_i^\top\mathcal G_iJ_i$（$J_i$ 是连杆 $i$ 质心系的体雅可比）。

平面 ↔ 空间

$\omega,\alpha,\mathbf a_c\;\leftrightarrow\;\mathcal V,\dot{\mathcal V}$；$\mathbf f,\mu\;\leftrightarrow\;\mathcal F$；$(m,I)\;\leftrightarrow\;\mathcal G$；"$\alpha\hat z\times\mathbf r-\omega^2\mathbf r$" $\leftrightarrow$ "$\mathrm{Ad}\,\dot{\mathcal V}+\mathrm{ad}_{\mathcal V}\mathcal A\dot\theta$"。两套公式是同一个算法在两种记号下的写法：平面版适合看清物理，空间版适合写代码，因为没有分情况，也不需要明确地写出每个叉积。

6机器人中的它：仿真与计算量

用 RNEA 做正动力学：让三连杆臂掉下来

仿真需要的是正动力学。把 FIG 4 的办法接上 2.5 节的 ODE 积分：每个时间步里，先用 RNEA 算出偏置项 $\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)=C\dot{\mathbf q}+\mathbf g$，再逐列拼出 $M$，解 $M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b$，然后用四阶 Runge–Kutta 积分。电机不出力（$\boldsymbol\tau=0$，可加粘性摩擦 $-b\dot{\mathbf q}$），手臂在重力下自由摆动。无摩擦的保守系统总能量 $E=T+V$ 应当不变，漂移的大小反映积分器的误差，是对整个动力学模型和积分器的一次体检。

FIG 5由 RNEA 驱动的正动力学仿真

能量 (J) 随时间 (s)

拖动关节圆圈摆出初始姿态（只能在时间为 0 时拖动），点"放手"开始仿真。右图：动能 $T$（红）、势能 $V$（蓝，以基座高度为 0）、总能量 $E$（琥珀色）。读数里的"漂移"是 $\max|E-E_0|/|E_0|$（仅在 $b=0$ 时有意义）。步长 $\Delta t$ 是积分步长；第 3 根杆很轻，绕质心的转动惯量只有 0.021 kg·m²，对步长最敏感。

放手，让仿真至少运行 3 s（$b=0$）。能量漂移是多少？
把步长加大，直到能量漂移超过 1%（$b=0$）。这是物理失真还是数值失真？

为什么不直接用拉格朗日公式？计算量

FIG 6 数出了这一页上几种实现的乘加次数（把每个 rnea、lagr 内部的标量运算累加起来，这是一个计数模型，不是计时 benchmark；真实库的常数项完全不同，但增长的阶不变）。四条线：① RNEA 求逆动力学；② 调用 $n$ 次 RNEA 拼出 $M$；③ 在 ② 基础上再解线性方程，得到正动力学；④ 先显式算 $M$、$\partial M/\partial\mathbf q$、$C$、$\mathbf g$，再相乘求逆动力学。④ 的阶是 $n^4$（单独算 $M$ 是 $n^3$），①是 $n$。链越长，差距越大。

FIG 6操作数 vs 关节数

每个点是该关节数下在这一页的代码里实际数出的乘加次数，链沿用三种连杆循环。横纵轴都取对数时，$n^k$ 是斜率为 $k$ 的直线。切换成线性坐标，红线几乎立刻冲出图外。竖线是当前的 $n$；读数里的斜率是 $n=20\to40$ 之间测得的局部斜率。

把 $n$ 拉到 30（人形机器人的量级）。显式拉格朗日比 RNEA 贵多少倍？

真实系统里

软件库：Pinocchio（rnea、crba、aba）、RBDL、KDL（ChainIdSolver_RNE）、Drake（MultibodyPlant::CalcInverseDynamics）、MuJoCo（mj_inverse 内部用 RNE）。它们的核心就是本节的算法，只是用更精细的空间代数和树结构实现。正动力学：Walker–Orin 的做法是 $O(n^3)$（要解线性方程）；Featherstone 的关节体算法 (ABA) 用"关节体惯量"把链从末端向基座逐级吸收，正动力学也是 $O(n)$，这是仿真大型机器人的标准算法；组合刚体算法 (CRBA) 则是求 $M$ 的常用方法。控制：计算力矩控制和逆动力学前馈每个周期调用一次 RNEA（Franka 等机器人的库提供质量矩阵、科氏力、重力项）；参数辨识：RNEA 对惯性参数是线性的，$\boldsymbol\tau=Y(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\boldsymbol\pi$，用激励轨迹的数据做最小二乘（Atkeson–An–Hollerbach，1986）；真实机器人的质量、质心位置、惯量从来不会准确知道，还有转子惯量、齿轮摩擦和线缆的影响（9.4 节），所以模型算出的 $\boldsymbol\tau$ 总有偏差，这也是反馈和自适应存在的理由。

7常见误区

误区 1：忘了每个量写在哪个坐标系里

平面版里所有向量都在世界系，所以可以直接相加。三维 RNEA 的每个量（$\mathcal V_i$、$\dot{\mathcal V}_i$、$\mathcal F_i$）都在各自连杆的坐标系里，相加之前必须用 $\mathrm{Ad}$ 换到同一个系。手爪力 $\mathcal F_{n+1}$ 在手爪系里，而传感器读数常常在基座系或腕部传感器系，直接塞进去会得到错的力矩。症状：静止时 $\tau$ 对，运动起来就错；或者只在某些姿态下错。

误区 2：重力符号错了，或者算了两遍

重力技巧是令基座加速度 $\dot{\mathcal V}_0=(\mathbf 0,-\mathbf g)$，$\mathbf g$ 是重力加速度向量（指向下），所以 $\dot{\mathcal V}_0$ 指向上。符号取反，机器人就像在天花板上受力。另一个常见错误是：已经用了基座加速度技巧，又在每根连杆上加了一次 $m_i\mathbf g$，重力项变成两倍。把 $\mathbf q$ 全部置零、$\dot{\mathbf q}=\ddot{\mathbf q}=0$，水平杆的 $\tau$ 应当是 $+mgl/2$（FIG 2），用它做自检。

误区 3：质心惯量和关节惯量混用

CAD 软件给出的惯量常常是绕零件原点或绕关节轴的，而 RNEA 的 $\mathcal G_i$ 要的是绕质心、写在质心坐标系里的惯量。平行轴定理：$I_{\text{关节}}=I_c+md^2$。均匀杆绕端点是 $ml^2/3$，绕质心是 $ml^2/12$，相差 4 倍。用错的后果是偏大的惯量：参数看上去合理，但模型预测的力矩系统性偏高，机器人的响应也与仿真对不上。

误区 4：把固定基座的 RNEA 用在浮动基座上

本节假设基座固定：$\dot{\mathcal V}_0$ 已知（0 或重力技巧），基座的反力是算出来的"副产品"。四足或人形机器人的躯干是浮动的，有 6 个额外的自由度（9.2 节），躯干的加速度本身是未知量：要么把 6 个自由度加入广义坐标，用 $M\ddot{\mathbf q}$ 的 $n+6$ 维形式，要么给定躯干的加速度，从躯干的 6 个方程里解出接触力。直接对 $n$ 个关节算 RNEA，只会得到"如果躯干被钉住"时的关节力矩。

误区 5：模型的 $\tau$ 不是电机该出的力矩

RNEA 给的是关节轴上需要的力矩，在刚体、理想关节、参数准确的前提下。真实的电机还要克服转子惯量（乘以减速比的平方）、齿轮和轴承的摩擦、线缆的拉力，传动还有弹性。把 RNEA 直接作为电流指令的前馈，通常会留下百分之几到几十的力矩误差，所以必须有反馈（Part IV），并常常辨识摩擦和转子惯量。

8小练习

1. 一根均匀水平杆，质量 $m$，长 $l$，一端由电机关节托住并保持静止。重力加速度 $g$。电机需要的力矩是？

静止时 $\omega=\alpha=0$，用重力技巧 $\mathbf a_c=(0,g)$，$\mathbf f=m(0,g)$，$\mu=\mathbf r_c\times\mathbf f=(l/2)\cdot mg$。直观上是重心（在杆中点）的重量乘以杠杆臂 $l/2$。$mgl/3$ 来自把端点惯量 $ml^2/3$ 错用到静力学上。

2. 太空中（无重力），同样的均匀杆绕一端的关节以恒定角速度 $\omega$ 旋转。关节电机需要的力矩是？

$\alpha=0$，质心加速度只有向心项 $-\omega^2\mathbf r_c$，所以关节要提供沿杆方向的力 $\mathbf f=-m\omega^2\mathbf r_c$，大小 $m\omega^2l/2$，这是力，不是力矩。力矩 $\mu=I\alpha+\mathbf r_c\times\mathbf f=0+0$，因为 $\mathbf f$ 与 $\mathbf r_c$ 平行。单根杆绕固定端恒速旋转不需要力矩；多关节链里速度项才会产生力矩（在 FIG 3 里关掉重力、令 $\ddot\theta=0$ 就能看到）。

3. 一个 6 自由度机械臂，在关掉重力的情况下，要用 RNEA 拼出完整的 $6\times6$ 质量矩阵 $M(\mathbf q)$，最少要调用几次 RNEA？

每次调用令 $\dot{\mathbf q}=0$、$\ddot{\mathbf q}=\mathbf e_j$，得到 $M$ 的一整列。需要 6 列，所以 6 次。开着重力时，每次要减去 $\mathrm{RNEA}(\mathbf q,0,0)$，共 7 次。每次是 $O(n)$，总共 $O(n^2)$。利用 $M$ 的对称性可以少算，CRBA 就是这样把 $O(n^2)$ 做得更紧。

4. 世界系 $z$ 向上，重力加速度向量是 $\mathbf g=(0,0,-9.81)$。用"基座加速度"技巧时，三维 RNEA 的初始值 $\dot{\mathcal V}_0$ 是？（twist 角部分在前）

$\dot{\mathcal V}_0=(\mathbf 0,-\mathbf g)=(0,0,0,\,0,0,+9.81)$：线加速度部分（后三个分量）向上。选项 B 把重力放进了角加速度部分，选项 A 的符号反了。

5. 做正动力学时，先调用 $\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)$，它得到的是什么？

$\boldsymbol\tau=M\ddot{\mathbf q}+\mathbf b$，令 $\ddot{\mathbf q}=0$ 就只剩 $\mathbf b=C\dot{\mathbf q}+\mathbf g$。若再令 $\dot{\mathbf q}=0$，才只剩下 $\mathbf g$（选项 C）。然后解 $M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b$ 得到加速度。

9延伸阅读

J. Y. S. Luh, M. W. Walker & R. P. C. Paul, On-Line Computational Scheme for Mechanical Manipulators, J. Dynamic Systems, Measurement, and Control 102(2), 1980 — 递推牛顿-欧拉算法的原始论文
M. W. Walker & D. E. Orin, Efficient Dynamic Computer Simulation of Robotic Mechanisms, J. Dynamic Systems, Measurement, and Control 104(3), 1982 — 用 RNEA 逐列拼出质量矩阵并求正动力学
R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008 — 空间向量代数，RNEA、CRBA、ABA 的权威讲解
K. Lynch & F. Park, Modern Robotics, Ch. 8, Sec. 8.3 — 本节空间版本的出处：Newton–Euler 逆动力学
B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Ch. 7 — 动力学：拉格朗日与牛顿-欧拉两种形式及动力学参数辨识
C. G. Atkeson, C. H. An & J. M. Hollerbach, Estimation of Inertial Parameters of Manipulator Loads and Links, Int. J. Robotics Research 5(3), 1986 — 惯性参数辨识：动力学对参数线性