本系列是我学习机器人动力学的讲义，我将它搬运到博客中。这系列文章默认读者已经掌握机器人运动学相关的知识。
主要内容计划安排如下，专栏地址：微鉴道长机器人动力学专栏
后文内容不定期更新，欢迎关注：

【机器人动力学】（一）：刚体惯性与动能
【机器人动力学】（二）：机械臂动力学方程与计算
【机器人动力学】（三）：第三章：力矩控制与接触交互
【机器人动力学】（四）：动力学回归与最小参数集
【机器人动力学】（五）：最优激励与测量数据处理
【机器人动力学】（六）：本体、摩擦与工具参数辨识

第二章：机械臂动力学方程与计算

上一章把连杆的质量分布、Jacobian 和整机质量矩阵联系起来。加上重力、速度耦合及外部作用，就能建立完整的关节动力学方程。能量法便于看清这些项的来源，随后介绍的递归算法则沿连杆逐步完成计算。

2.1 动力学方程中的惯性、速度与重力项

由能量得到力矩

设动能为 $T$，势能为 $U$，二者之差 $L=T-U$ 称为拉格朗日函数，单位仍为 J。经典力学里有一个结论，叫作欧拉–拉格朗日方程。本文直接拿来用，就不展开它的推导或证明了。这个方程通过下面的关系，把能量和关节力矩联系起来：

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}}-\frac{\partial L}{\partial q}=\tau .$$

这里 $q$ 是关节位置，$\dot{q}$ 是关节速度，$t$ 是时间；$\partial /\partial q$ 和 $\partial /\partial \dot{q}$ 分别表示对位置和速度求偏导。对于 $n$ 个转动关节，对每个关节坐标使用同一关系，得到 $n$ 维力矩向量 $\tau$，各分量单位为 N·m。右侧汇总驱动、接触和摩擦等作用，重力通过势能 $U$ 计入左侧。六轴、七轴机械臂同样适用这个方程。

仍用单连杆作一个简短的例子。连杆绕固定的水平关节轴在竖直平面摆动，质量为 $m$，质心到关节轴的距离为 $\ell$，绕该轴的标量惯量为第 1.2 节得到的 $I_{O,u}$。它已包含质心偏移的贡献，本例的质量矩阵为 $M=[I_{O,u}]$。$m,\ell ,I_{O,u}$ 的单位分别为 kg、m、kg·m²。

以竖直向下为转角 $q$ 的零位，角度采用 rad；以质心最低位置为势能零点，重力加速度大小记为 $g_0$，单位为 m/s²。动能和势能分别为：

$$T=\frac{1}{2}{I}_{O,u}{\dot{q}}^2,U=m{g}_0\ell (1-\cos q).$$

忽略摩擦和外部接触，令电机力矩 ${\tau }_a$ 的正方向与 $q$ 一致。将这两个能量表达式代入欧拉–拉格朗日方程并化简，得到：

$${\tau }_a=I_{O,u}\ddot{q}+m{g}_0\ell \sin q.$$

第一项是产生角加速度所需的惯性力矩，第二项是抵消重力所需的力矩。连杆静止在水平位置时，电机需要提供大小为 $m{g}_0\ell$ 的保持力矩；连杆开始加速时，还要计入 $I_{O,u}\ddot{q}$。前面建立的轴向惯量，在这里直接成为角加速度的系数。

通用动力学方程与速度项

对于整台机械臂，把第 1.2 节得到的 $T=\frac{1}{2}{\dot{q}}^{T}M(q)\dot{q}$ 和重力势能 $U(q)$ 代入同一个方程并整理，便得到：

$$M(q)\ddot{q}+c(q,\dot{q})+g(q)=\tau .$$

$M\ddot{q}$ 是与关节加速度有关的惯性力矩，$g(q)=\partial U/\partial q$ 是抵消重力所需的力矩，$c(q,\dot{q})$ 汇总运动速度引起的力矩。三者都是 $n$ 维向量，分量单位为 N·m。速度项来自动能随构型的变化：一个关节运动时，上下游连杆的运动方向和惯性耦合也会随之改变。

在关节方程中，通常把与速度平方 ${\dot{q}}_j^2$ 成正比的部分称为离心项，把含有不同关节速度乘积 ${\dot{q}}_j{\dot{q}}_k$ 的部分称为科里奥利项；$j,k$ 为关节编号。这些项共同组成 $c$。实现中也常写成 $c=C(q,\dot{q})\dot{q}$，其中 $C$ 是将这些速度系数组织起来的 $n\times n$ 矩阵。

单连杆绕固定轴转动时，$I_{O,u}$ 为常数，动能与转角无关，因而该例的 $c=0$。匀速转动时，质心仍具有大小为 $\ell {\dot{q}}^2$ 的向心加速度。产生这一加速度的合力径向分量称为向心力，其作用线经过关节轴，对该轴的力矩为零；关节支承力与重力等共同提供这部分径向合力。向心加速度体现在连杆受力中，关节电机的力矩则由绕轴取矩后的关系决定。

在转动坐标系中，径向惯性效应通常用向外的离心惯性力表示。科里奥利效应与相对运动及参考系的转动共同有关，例如肘部运动时，它所依附的上臂也随肩关节转动。多关节系统即使满足 $\ddot{q}=0$，也可能需要传递速度相关力矩。

进一步区分作用来源，令 ${\tau }_a$ 为关节侧实际驱动力矩，${\tau }_{\mathrm{ext}}$ 为环境作用于机械臂的广义力矩，${\tau }_f$ 为驱动器需要克服的摩擦项。三者均为 $n$ 维向量，分量单位为 N·m。按 $\tau ={\tau }_a+{\tau }_{\mathrm{ext}}-{\tau }_f$ 的符号约定，得到：

$$M(q)\ddot{q}+c(q,\dot{q})+g(q)+{\tau }_f={\tau }_a+{\tau }_{\mathrm{ext}}.$$

在模型和外部作用确定后，关节所需驱动力矩一般由位置 $q$、速度 $\dot{q}$ 和加速度 $\ddot{q}$ 共同决定。静态保持时，$\dot{q}=\ddot{q}=0$，惯性加速度项和速度项消失，驱动器需要平衡重力、外部接触与静态摩擦等作用。

例如，黏性摩擦可写为 ${\tau }_f=B_v\dot{q}$，其中 $B_v$ 是由各关节黏性系数组成的非负对角矩阵，系数单位为 N·m·s/rad。后文沿用上述符号约定，并用 $h=c+g$ 简记刚体偏置项。

能量表达与递归计算

拉格朗日方法从动能和势能建立关节方程，牛顿–欧拉方法从每根连杆的受力和运动建立方程。在相同的刚体模型、约束和外部作用下，两者满足同一套动力学关系。能量表达便于理解 $M,c,g$ 的结构，递归算法便于利用连杆连接关系组织计算。

工程中常用递归牛顿–欧拉算法求逆动力学：沿基座到末端计算各连杆的运动，再沿末端到基座汇总受力，得到各关节所需力矩。这里的前向、后向指沿连杆树的遍历方向，整个算法求解的是逆动力学。正动力学则可通过质量矩阵方程求解加速度，也可采用铰接体算法进行递归计算。后两节将分别展开这些计算过程。

动力学方程也能用来描述机械能的收支。先看一个关节：力矩 ${\tau }_j$ 推动它转过一小段角度 $d{q}_j$，所做的功为 $d{W}_j={\tau }_{j}d{q}_j$；除以经过的时间，就得到瞬时功率 $P_j={\tau }_j{\dot{q}}_j$。这里 $W_j$ 表示做功，单位为 J，$P_j$ 的单位为 W。将各关节的功率加起来，一组关节力矩 $\tau$ 对应的总功率为：

$$P=\sum _{j=1}^n{\tau }_j{\dot{q}}_j={\dot{q}}^T\tau .$$

从完整动力学方程出发，经过推导可以得到下面的关系。这里省略推导，直接看它的物理含义：

$$\frac{d}{dt}(T+U)={\dot{q}}^T({\tau }_a+{\tau }_{\mathrm{ext}}-{\tau }_f).$$

左侧是动能与势能之和的变化率，也就是机械能增加或减少的速度。右侧的 ${\dot{q}}^T{\tau }_a$ 是驱动器功率，${\dot{q}}^T{\tau }_{\mathrm{ext}}$ 是环境作用的功率，${\dot{q}}^T{\tau }_f$ 是摩擦消耗的功率，单位均为 W。前两项为正时向机械臂输入能量，为负时从机械臂取出能量；摩擦消耗的部分从中扣除。因此，这个式子表示：驱动器和环境输入的功率，扣掉摩擦消耗的功率，就是机械能增加的速度。

重力的作用已经计入势能 $U$。例如，抬高物体时，克服重力所做的功会增加物体的重力势能。完整方程中的惯性项与速度项共同对应总动能 $T$ 的变化，重力项对应势能 $U$ 的变化，整理后就得到左侧的 $d(T+U)/dt$。这个能量收支关系也将用于后续的阻抗控制与被动性分析。

2.2 递归逆动力学的运动与受力传递

逆动力学（Inverse Dynamics，ID）给定关节位置、速度和加速度，求产生这些运动所需的驱动力矩：

$${\tau }_a=M\ddot{q}+h+{\tau }_f-{\tau }_{\mathrm{ext}}.$$

沿期望轨迹计算力矩前馈、评估轨迹的力矩需求，以及由测量运动建立参数回归关系，都属于逆动力学的应用。

前面用空间速度 $V=[\omega ;v]$ 同时描述旋转和平移。与之配对的空间力为：

$$F=\left[\begin{array}{c}n\\ f\end{array}\right],P=F^{T}V=n^T\omega +f^{T}v.$$

$n$ 是关于所选参考点的力矩，$f$ 是合力，$v$ 是同一参考点的线速度。标量 $P$ 是这一组力和力矩对刚体所做的瞬时功率，单位为 W；因此力矩与角速度、力与线速度必须采用相同的坐标轴和参考点。本文的递归公式采用角量在前的排列；线量在前的表示也很常见，两者通过一致的行列置换相互转换。

本节按串联机械臂的连接顺序编号。对于 $n$ 轴机械臂，固定基座记为 $0$，从基座到末端的移动连杆依次记为 $1,\dots ,n$；第 $i$ 个关节连接连杆 $i-1$ 和连杆 $i$。因此，当前连杆 $i$ 的父连杆就是 $i-1$。在坐标变换公式中，用 $p=i-1$ 简写父连杆编号。六阶矩阵 $X_i$ 把父连杆的空间运动转换到子连杆的坐标系与原点，同时处理旋转与原点平移。在旋量理论中，这个矩阵对应刚体变换的伴随矩阵 $\mathrm{Ad}$。按本文角量在前的排列，定义为：

$${}^i{T}_p=\left[\begin{array}{cc}{R}_{ip}& r_{ip}\\ 0& 1\end{array}\right],X_i={\mathrm{Ad}}_{{}^i{T}_p}=\left[\begin{array}{cc}{R}_{ip}& 0\\ [r_{ip}{]}_{\times }{R}_{ip}& R_{ip}\end{array}\right].$$

${}^i{T}_p$ 是把父坐标系中的点坐标转换到子坐标系的 $4\times 4$ 齐次变换；$R_{ip}$ 是对应的 $3\times 3$ 旋转矩阵，$r_{ip}$ 是从子原点指向父原点、用子坐标轴表达的位移，单位为 m。$[r_{ip}{]}_{\times }$ 是前面定义的叉乘矩阵。伴随矩阵的左下块计入参考点平移带来的线速度变化。如果已有的是正向运动学中的 ${}^p{T}_i$，则先取逆，得到这里所用的 ${}^i{T}_p$。

本节的空间向量写法与旋量理论采用相同的刚体运动关系。实现中常见的区别包括角量与线量的排列顺序，以及速度所用的参考点和表达坐标轴。例如，设连杆原点在世界坐标系中的位置为 $r_O$，其实际线速度为 ${\dot{r}}_O$；以世界原点为参考点的空间旋量，其线性部分为 $v_{\mathrm{s}}={\dot{r}}_O-\omega \times r_O$，式中各向量均用世界坐标轴表达。将参考点移到连杆原点，再按连杆坐标轴表达，就能与本文的 $v$ 对应。这些约定统一后，各种写法的计算结果一致。

对同一个空间运动作坐标变换，若 $V_i=X_i{V}_p$，则六维力沿同一方向变换时满足 $F_i=X_i^{-T}{F}_p$。正文从子连杆向父连杆传递力，使用反向关系 $F_p=X_i^T{F}_i$，这可以由功率不变得到：

$$F_i^T{V}_i=F_i^T{X}_i{V}_p=(X_i^T{F}_i{)}^T{V}_p.$$

这一对变换保持功率不变，并将参考点平移引起的力臂效应包含在矩阵中。

空间递归还使用两个叉乘算子：

$$\begin{array}{rl}V\times ={\mathrm{ad}}_V& =\left[\begin{array}{cc}[\omega {]}_{\times }& 0\\ [v{]}_{\times }& [\omega {]}_{\times }\end{array}\right],\\ V{\times }^{\ast }& =-(V\times {)}^T=-{\mathrm{ad}}_V^T.\end{array}$$

两个算子都是 $6\times 6$ 矩阵，式中的 $0$ 是三阶零矩阵。小写 ${\mathrm{ad}}_V$ 对应正文的运动叉乘算子 $V\times$，作用于另一个六维运动向量；它的负转置对应力叉乘算子 $V{\times }^{\ast }$，作用于动量和力。上标 $\ast$ 用来区分这两个算子。大写 $\mathrm{Ad}$ 由位姿变换构造，用于坐标变换；小写 $\mathrm{ad}$ 由运动向量构造，用于计算运动引起的叉乘和耦合项。后文的 $V{\times }^{\ast }\mathcal{I}V$ 就是对空间动量 $\mathcal{I}V$ 作这种运算。

考虑七轴机械臂同时抬起前臂并旋转手腕的过程。首先根据各关节运动计算每根连杆的速度和加速度，再计算产生这些运动所需的空间力。腕部的作用传给前臂，前臂的作用继续传给肘部和肩部。

递归 Newton–Euler 算法（RNEA）按这两个方向完成逆动力学计算：前向遍历求运动，后向遍历汇总受力。每根连杆只需处理本体与相邻连接。下面按串联机械臂逐根计算；对于有分支的机器人，也可以沿各分支的连接关系使用这一算法。

下面用连杆 $i-1$ 和连杆 $i$ 描述一步递推。以肘关节和前臂为例，前臂是当前连杆，上臂是父连杆；肩部运动已经包含在上臂的运动中。$V_i,A_i$ 分别记录当前连杆的六维空间速度和空间加速度，统一用当前连杆的坐标轴表达。下面先按纯运动讨论，最后再加入重力。

$S_i$ 描述当前关节允许的相对运动方向，称为运动子空间。单自由度转动关节的参考点位于转轴上时，$S_i=[u_i;0]$，其中 $u_i$ 是在当前连杆坐标系中表达的单位轴向量，$0$ 是三维零向量。关节角速度为 ${\dot{q}}_i$ 时，它产生的相对空间速度为 $V_{J,i}=S_i{\dot{q}}_i$：相对角速度沿 $u_i$，轴上参考点的相对平移速度为零。这里采用在连杆坐标系中保持不变的 $S_i$。

速度的两项

当前连杆的速度由两部分叠加得到：

$$V_i=X_i{V}_{i-1}+V_{J,i}.$$

第一项 $X_i{V}_{i-1}$ 是父连杆带来的运动。肘关节保持角度不变、肩关节抬起手臂时，前臂会跟着上臂一起运动。这部分速度先在父连杆坐标系中给出，再由 $X_i$ 转到前臂的原点和坐标轴下。$X_i$ 同时处理坐标轴转向和参考点之间的距离，因此也能计入肘部随上臂转动产生的线速度。

第二项 $V_{J,i}=S_i{\dot{q}}_i$ 是当前关节额外带来的相对运动。上臂保持静止、肘关节弯曲时，前臂的运动就来自这一项。肩、肘同时运动时，两部分相加。${\dot{q}}_i$ 的单位为 rad/s；六维速度中，角速度分量的单位为 rad/s，线速度分量的单位为 m/s。

加速度的三项

速度随时间变化，就得到加速度。按照上面的速度关系求导并整理，可以得到下面的递推式，这里直接解释各项的含义：

$$A_i=X_i{A}_{i-1}+S_i{\ddot{q}}_i+V_i\times V_{J,i}.$$

第一项 $X_i{A}_{i-1}$ 传递父连杆已有的空间加速度。例如，肩关节开始加速抬臂，上臂的加速度会继续传到前臂；肘关节保持角度不变时，前臂也会随之加速。父连杆的 $A_{i-1}$ 采用父坐标轴，乘以 $X_i$ 后便采用当前连杆的原点和坐标轴。

第二项 $S_i{\ddot{q}}_i$ 来自当前关节速度的变化。肘关节弯曲得越来越快时，${\ddot{q}}_i$ 就会通过这一项给前臂增加相对角加速度。${\ddot{q}}_i$ 的单位为 rad/s²。当前关节匀速转动时，这一项为零。

第三项 $V_i\times V_{J,i}$ 计入两种运动同时发生时的速度耦合。当前关节运动会使父、子坐标系之间的变换 $X_i$ 随时间变化，把这个变化对速度的影响整理出来，就得到这一项。

这里的六维运动叉乘就是前面的小 $\mathrm{ad}$ 运算，也满足 $V\times V=0$。从它的矩阵定义可以直接看到，角向部分为 $\omega \times \omega =0$，线性部分为 $v\times \omega +\omega \times v=0$。把 $V_i=X_i{V}_{i-1}+V_{J,i}$ 代入第三项：

$$\begin{array}{rl}{V}_i\times V_{J,i}& =(X_i{V}_{i-1}+V_{J,i})\times V_{J,i}\\ & =(X_i{V}_{i-1})\times V_{J,i}+\underset{0}{\underbrace{V_{J,i}\times V_{J,i}}}\\ & =(X_i{V}_{i-1})\times V_{J,i}.\end{array}$$

展开后，留下的正是父连杆传来的运动与当前关节相对运动之间的耦合。实现中已经算出了 $V_i$，因此常直接使用 $V_i\times V_{J,i}$ 这一简洁写法，两种形式完全等价。

例如，肩部绕竖直轴匀速转动，腕部同时绕随手臂转向的水平轴匀速转动。两处转速都保持不变，腕部转轴的空间方向仍在变化，因此腕部连杆的角速度向量也在变化，需要计入相应的角加速度。

父连杆静止时，$X_i{V}_{i-1}=0$，第三项为零。当前关节保持角度不变时，$V_{J,i}=0$，第三项也为零，此时连杆的运动由父连杆继续传递。两部分运动同时存在时，第三项的大小还取决于运动方向。递推中的三项都在当前连杆坐标系中表达后，才能相加。

随体坐标中的加速度

为了把上面的 $A_i$ 与实际运动对应起来，可以把六维向量看成两组三维向量。$V_i$ 的六个数字依次是角速度的三个分量和参考点线速度的三个分量。坐标轴画在连杆上、随连杆一起转动；这些分量的变化同时受到运动本身和坐标轴转向的影响。

在尚未加入重力修正时，本文的 $A_i$ 按这些随体速度分量对时间求导来组织。记参考点 $O_i$ 的实际线速度为 $v_{O_i}$、实际线加速度为 $a_{O_i}$，连杆的角速度与角加速度分别为 ${\omega }_i,{\alpha }_i$，则：

$$V_i=\left[\begin{array}{c}{\omega }_i\\ v_{O_i}\end{array}\right],A_i=\left[\begin{array}{c}{\alpha }_i\\ a_{O_i}-{\omega }_i\times v_{O_i}\end{array}\right].$$

式中各向量都用连杆坐标轴表达。$A_i$ 的第一组三个分量是角加速度，单位为 rad/s²；第二组三个分量是线速度在随体坐标中的变化率，单位为 m/s²。坐标轴自身的转动带来 ${\omega }_i\times v_{O_i}$ 这一项，所以随体分量的变化率要从实际线加速度中减去它。要恢复实际线加速度，就把 $A_i$ 的第二组三个分量与 ${\omega }_i\times v_{O_i}$ 相加。

例如，连杆绕固定轴匀速转动时，连杆上某个点的线速度在随体坐标中的分量可以保持不变，此时对应的线性变化率为零。这个点在空间中仍沿圆周运动，其向心加速度由 ${\omega }_i\times v_{O_i}$ 给出。这样，匀速转动、随体分量保持不变和空间中的向心加速度就能对应起来。

重力在递推中的处理

固定基座在整个运动过程中保持静止，空间速度 $V_0$ 为六维零向量，下标 $0$ 表示基座。单独计算运动加速度时，基座空间加速度也取 $A_0=0$。计算包含重力的逆动力学时，可以利用同一套递推把重力一起计入。

设物理重力加速度向量为 $g_{\mathrm{phys}}$，指向下方，单位为 m/s²。对于质量为 $m$、实际线加速度为 $a$ 的物体，用 $f$ 表示支撑、驱动等作用的合力，牛顿方程可写为 $f=m(a-g_{\mathrm{phys}})$。物体静止时，$a=0$，所需支撑力就是向上的 $-m{g}_{\mathrm{phys}}$。这给出了在加速度中放入 $-g_{\mathrm{phys}}$ 的原因。

在六维递推中，把基座的角加速度分量设为零、线性分量设为 $-g_{\mathrm{phys}}$：

$$V_0=0,A_0=\left[\begin{array}{c}0\\ -g_{\mathrm{phys}}\end{array}\right].$$

这里的 $g_{\mathrm{phys}}$ 用基坐标轴表达，矩阵中的 $0$ 是三维零向量。这个初值用来计算支撑重力所需的作用，基座在物理上仍保持静止。它沿串联机械臂逐根传递后，各连杆后续的惯性力计算就包含了重力贡献。机械臂完全静止时，各连杆速度都为零，受力递推仍能据此算出关节需要提供的重力保持力矩。采用这个初值后，$A_i$ 中也包含了相应的重力修正。

连杆自身的需求与下游传来的作用

已知各连杆的运动后，先计算每根连杆自身需要的空间力，用 $F_i^{\mathrm{self}}$ 表示：

$$F_i^{\mathrm{self}}={\mathcal{I}}_i{A}_i+V_i{\times }^{\ast }{\mathcal{I}}_i{V}_i-F_{i,\mathrm{ext}}.$$

上标 $\mathrm{self}$ 表示当前连杆本体，${\mathcal{I}}_i$ 是它关于本体原点的空间惯量，$F_{i,\mathrm{ext}}$ 是环境施加在该连杆上的已知六维力，均采用当前连杆的原点和坐标轴。第一项对应空间加速度引起的惯性作用，第二项对应运动速度引起的惯性作用，第三项扣除环境已经提供的作用。这个公式对每一根连杆都适用。

一根连杆还需要带动它后面的部分。例如前臂运动时，既要满足前臂自身的惯性需求，还要通过腕部带动手和工具。用 $F_i$ 表示汇总了当前连杆及全部下游连杆需求的完整空间力，后向递推就可以写成：

$$\begin{array}{rl}{F}_i& =F_i^{\mathrm{self}}+X_{i+1}^T{F}_{i+1}\\ & ={\mathcal{I}}_i{A}_i+V_i{\times }^{\ast }{\mathcal{I}}_i{V}_i-F_{i,\mathrm{ext}}+X_{i+1}^T{F}_{i+1}.\end{array}$$

第一部分是当前连杆自身的需求；第二部分是下一根连杆传来的作用。$F_{i+1}$ 已经包含连杆 $i+1$ 及其全部下游连杆的需求，用连杆 $i+1$ 的原点和坐标轴表达。$X_{i+1}$ 对应从连杆 $i$ 到连杆 $i+1$ 的运动变换，所以用它的转置 $X_{i+1}^T$，就能把这份空间力传回连杆 $i$，并计入参考点改变带来的力矩。

因此，计算 $F_i$ 时需要先知道 $F_{i+1}$，顺序自然是从末端往基座走。上式用于 $i=n-1,\dots ,1$，其中 $n$ 是末端连杆编号。末端作为递推起点，直接取 $F_n=F_n^{\mathrm{self}}$。这里将固定工具的惯性并入末端连杆，工具与环境的接触作用计入 $F_{n,\mathrm{ext}}$。

从末端逐根算到基座

对七轴机械臂，后向计算的前三步为：

$$\begin{array}{rl}{F}_7& =F_7^{\mathrm{self}},\\ F_6& =F_6^{\mathrm{self}}+X_7^T{F}_7,\\ F_5& =F_5^{\mathrm{self}}+X_6^T{F}_6.\end{array}$$

第七连杆先算出自己的需求。第六连杆将这份需求转换到自身坐标系，再加上第六连杆自己的需求，得到 $F_6$。第五连杆接到的 $F_6$ 已包含第六、七连杆的作用，再加上第五连杆自身的需求，得到 $F_5$。如此继续到第一连杆，肩部就接收了整条手臂的需求。

如果把当前已算完的连杆记为 $i$，同一步计算也可以写成 $F_{i-1}=F_{i-1}^{\mathrm{self}}+X_i^T{F}_i$。这与前向递推的方向相对应：前向由 $i-1$ 算出 $i$ 的运动，后向由 $i$ 算出 $i-1$ 需要承受的作用。

每算出一根连杆的完整空间力，就从中提取沿当前关节自由度的驱动力矩，作为该关节电机的力矩需求；完整空间力继续向前一根连杆传递。

$${\tau }_i=S_i^T{F}_i.$$

这里 ${\tau }_i$ 是 RNEA 求出的第 $i$ 个关节刚体驱动力矩，尚未加入关节摩擦。投影使用的 $F_i$ 已包含下游需求，因此 ${\tau }_6$ 会包含带动第七连杆和工具所需的贡献。这个投影关系也可以由功率理解：关节相对运动对应的功率为 $F_i^T{S}_i{\dot{q}}_i={\tau }_i{\dot{q}}_i$。

肩部运动时，腕关节可以保持相对角度不变，腕部连杆仍会随手臂一起运动，具有相应的空间速度和加速度。前向递推中的 $X_i{V}_{i-1}$ 记录了这部分继承运动；后向递推再将产生这些运动所需的作用传回肩部。

关节电机调节沿关节允许运动方向的力矩，转动关节的轴承还可能承受径向力、轴向力和其他方向的弯矩。这些作用都保留在完整的六维空间力 $F_i$ 中，并通过 $X_i^T{F}_i$ 继续传给前一根连杆。递推既能得到电机所需的标量关节力矩，也保留了连杆之间传递的完整受力信息。

对具有 $n$ 个关节的串联机械臂，RNEA 前后各遍历一次，计算复杂度为 $O(n)$，直接利用各连杆的空间惯量求出关节力矩。

2.3 质量矩阵与正动力学

正动力学（Forward Dynamics，FD）给定当前状态和作用力矩，求当前加速度：

$$M\ddot{q}={\tau }_a+{\tau }_{\mathrm{ext}}-h-{\tau }_f.$$

它用于仿真和运动预测。加速度经过时间积分后，才能得到下一时刻的速度与位置。因此，一次正动力学计算与一个完整仿真步骤分别对应力学求解和状态演化两个环节。

以下 ID 仅包含刚体惯性与重力，摩擦和外力单独处理。令加速度为零，逆动力学返回速度与重力偏置；再令速度为零，则只剩重力项：

$$h(q,\dot{q})=\mathrm{ID}(q,\dot{q},0),g(q)=\mathrm{ID}(q,0,0).$$

质量矩阵的第 $j$ 列记为 $M_{:j}$，其中冒号表示取该列全部行。它可以通过单位加速度求得；$e_j$ 沿用前述单位向量，在这里作为第 $j$ 轴为单位角加速度、其余轴为零的数值输入：

$$M_{:j}(q)=\mathrm{ID}(q,0,e_j)-g(q).$$

这相当于逐个计算各关节加速度对全部关节惯性力矩的影响。重复调用 ID 能够构造质量矩阵，也便于建立直观的计算关系。上式默认 ID 保留重力；若将重力加速度输入设为零，返回结果就可以直接填入第 $j$ 列。

将重力和末端外力也作为可设置的输入，就能用同一套 ID 分别提取下面各项。各行均使用当前关节位置 $q$，并关闭摩擦等附加项；按表中输入调用 ID，即得到最后一列的结果。$g_{\mathrm{phys}}$ 是在基坐标系中表达的物理重力加速度，单位为 m/s²；$F_{\mathrm{tip}}$ 表示环境施加给机器人的末端空间力。

计算内容	关节速度	关节加速度	末端外力	重力加速度	ID 结果
质量矩阵第 $j$ 列	$0$	$e_j$	$0$	$0$	$M_{:j}(q)$
速度相关力矩	$\dot{q}$	$0$	$0$	$0$	$c(q,\dot{q})$
重力补偿力矩	$0$	$0$	$0$	$g_{\mathrm{phys}}$	$g(q)$
平衡末端外力的力矩	$0$	$0$	$F_{\mathrm{tip}}$	$0$	$-J^T{F}_{\mathrm{tip}}$

第一行依次取 $j=1,\dots ,n$，每次把返回的完整关节力矩向量填入对应列。第二行得到的是 $c=C\dot{q}$ 这个力矩向量。最后一行的 $J$ 是末端雅可比，与 $F_{\mathrm{tip}}$ 使用相同的参考点、坐标系和角／线分量顺序。若给定的是机器人希望施加给环境的空间力 $F_d$，环境反作用为 $F_{\mathrm{tip}}=-F_d$，对应的关节驱动力矩便是 $J^T{F}_d$。

这些量可以用于 FD，也可以用于力矩前馈、重力补偿等计算。采用质量矩阵形式计算 FD 时，需要先取得 $M$、偏置 $h=c+g$，以及已知外力对应的广义力矩。ID 在这里充当提取动力学系数的计算工具：表中的零速度和单位加速度都是在模型中人为设置的输入，用来单独算出某一项；当前状态下的实际关节加速度留到最后求解。这个过程全部在模型内完成。

采用这种方法计算 FD 时，先求出当前状态下的 $M$ 和偏置 $h$，再把已知驱动力矩、外力和摩擦合成为净加速作用，最后求解 $M\ddot{q}={\tau }_{\mathrm{net}}$，其中净加速力矩 ${\tau }_{\mathrm{net}}={\tau }_a+{\tau }_{\mathrm{ext}}-h-{\tau }_f$。表中最后一行给出的是平衡外力所需的驱动力矩，其符号与这里的 ${\tau }_{\mathrm{ext}}=J^T{F}_{\mathrm{tip}}$ 相反。质量矩阵是对称正定矩阵时，可以采用 Cholesky 或适当的对称分解，直接求得当前关节加速度。

ID 与 FD 在无未知接触、输入量充分给定时相互对应。若末端已经抵住刚性表面，环境反力又没有给定，只有驱动力矩还不足以确定加速度；环境将同时限制运动并产生反力。接触控制中的动力学分析需要把这种关系一并纳入，而自由空间 FD 对应的是外部作用已知的情况。

仿真还需要将加速度积分。模型与力矩相同、积分步长不同，数值轨迹也可能不同；摩擦的零速切换和接触刚度会进一步影响积分的稳定性。因此，ID/FD 的瞬时一致性与长时间仿真的轨迹质量属于相互联系但不同的计算性质。

复合刚体算法（CRBA）采用另一种组织方式。它先从末端向基座汇总子树的空间惯量，再把复合惯量与各关节运动方向投影，形成质量矩阵元素。复合惯量描述子树被视为一个整体时的惯性。对串联臂生成完整稠密质量矩阵，一般需要 $O(n^2)$ 计算。

铰接体算法（ABA）利用刚体树逐级消去和恢复关节加速度，可以在无额外约束的树形系统中以线性复杂度直接求正动力学，省去逐列调用 ID 构造质量矩阵的步骤。因此，调用 ID 是实现 FD 的一种途径，工程中也可以用 CRBA 构造质量矩阵后求解方程，或直接用 ABA 求加速度。RNEA、CRBA 和 ABA 分别组织力矩、质量矩阵与加速度的计算。