刚体动力学算法 Chapter 5 逆动力学

Chapter 5 Inverse Dynamics

逆动力学是指在刚体系统中寻找产生给定加速度所需的力。逆动力学计算在运动控制系统、轨迹设计和优化（例如用于机器人和动画角色）、机械设计以及某些正向动力学算法中都有应用。最后一个应用在第6章中介绍。逆动力学计算可以用以下方程表示：
$$\mathbf{\tau }=\mathrm{ID}(\mathrm{model},\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}),$$

其中 $\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}$ 和 $\mathbf{\tau }$ 分别是广义位置、速度、加速度和力的向量，而 model 是特定刚体系统的系统模型。

本章介绍了一种计算运动树逆动力学的算法。闭环系统的算法将在第8章中出现。运动树的逆动力学计算特别容易，这就是我们为什么从这个问题开始的原因。该算法称为递归牛顿-欧拉算法，它是最简单、最高效的算法。它的计算复杂度为 $O(n)$，这意味着计算量与树中的刚体或关节变量数量呈线性增长。首先介绍基本形式的算法，然后添加实现细节，最后将空间向量版本与原始的3D向量版本进行比较。本章还涵盖算法复杂度的主题，并解释了为什么递归算法如此高效。

5.1 算法复杂度

我们将算法的计算成本定义为其操作数量，即在执行算法过程中执行的算术操作次数。如果算法涉及实数运算（如动力学算法），则习惯上仅计算浮点操作。因此，忽略了整数操作，例如计算数组索引或递增循环变量所需的操作。

算法的计算复杂度是衡量成本如何随问题规模增长的一种指标。我们使用“大O”符号来指定这个数量。如果一个算法的复杂度被描述为 $O\left(n^p\right)$，其中 $n$ 是问题的规模，那么当 $n\to \infty$ 时，成本将与 $n^p$ 成比例增长。更准确地说，如果存在正常数$C_{\min }，C_{\max }$ 和 $n_{\min }$，使得计算成本至少为 $C_{\min }f(n)$，但对于所有 $n>n_{\min }$，不超过 $C_{\max }f(n)$ ，则称算法具有 $O(f(n))$ 复杂度。

表达式 $O(f(n))$ 可以解释为所有在 $n\to \infty$ 时与 $f(n)$ 成比例增长的函数集合。因此，像“算法$X$是 $O(f(n))$ ”这样的说法意味着 $X$ 的成本，表示为 $n$ 的函数，是集合 $O(f(n))$ 的元素。类似地，表达式 $O(f(n))=O(g(n))$ 是一个集合相等，意味着 $g(n)\in O(f(n))$ 且反之亦然。在引用复杂度时，我们选择集合中最简单的函数作为 $O$ 的参数。例如，如果算法的精确成本由多项式 $a_0+a_{1}n+\cdots +a_p{n}^p$ 给出，则与该多项式成比例增长的最简单函数是 $n^p$，因此我们引用复杂度为 $O\left(n^p\right)$ 。通常，只有 $f$ 中增长最快的组成部分才重要。如果 $f$ 收敛到常数，则称 $f$ 为 $O(1)$。

算法的复杂度取决于问题规模的衡量方式。例如，矩阵求逆的复杂度被认为是 $O\left(n^3\right)$ ，因为求逆一个 $n\times n$ 矩阵的成本随着 $n\to \infty$ 而以 $n^3$ 成比例增长。这个数字依赖于 $n$ 是矩阵的维度。如果 $n$ 是矩阵中元素的数量，那么复杂度数字将是 $O\left(n^{1.5}\right)$。更一般地，如果 $m$ 和 $n$ 是问题规模的两种不同衡量方式，并且 $m=g(n)$，那么 $O(f(m))=O(f(g(n)))$。

对于动力学算法，问题的规模是刚体系统的规模。如果系统是一个运动链，那么有两种合理的系统规模衡量方式：刚体数量 $N_B$ 和自由度数量 $n$。如果链中的每个关节至少有一个自由度，则这两个数字满足 $N_B\le n\le 6N_B$，这意味着

$$O\left(N_B^p\right)=O\left(n^p\right).$$

因此，在引用复杂度时，无需区分 $N_B$ 和 $n$。

5.2 递归关系

现代动力学算法是递归的，它们比非递归的前身快几个数量级。它们之所以被称为递归，是因为它们利用递归关系来计算中间结果的序列。正是递归关系的使用导致了它们的高效性；因此，让我们花一点时间来看看这些递归关系是如何工作的。递归关系是一个公式，它根据前面的元素定义了序列中的下一个元素。这个公式连同一组初始值提供了序列的递归定义。例如，斐波那契数列$0,1,1,2,3,5,8,13,\dots$可以通过递归关系 $F_i=F_{i-1}+F_{i-2}$ 和初始值 $F_0=0$ 和 $F_1=1$ 来定义。对于刚体系统，在根据 第4.1.2节 的规则编号的情况下，刚体速度序列 ${\mathbf{v}}_0,{\mathbf{v}}_1,{\mathbf{v}}_2,\dots$可以通过递归关系 ${\mathbf{v}}_i={\mathbf{v}}_{\lambda (i)}+{\mathbf{S}}_i{\dot{\mathbf{q}}}_i$ 和给定的 ${\mathbf{v}}_0$ 的初始值来定义。

让我们来看看使用递归关系和不使用递归关系计算刚体速度的任务。为了简化问题，假设系统是一个无分支的运动链，具有1自由度关节。这意味着 $\lambda (i)=i-1$ ，并且关节 $i$ 上的速度是关节轴向量 $s_i$ 与标量关节速度变量 ${\dot{q}}_i$ 的乘积。刚体速度的递归关系如下：

$${\mathbf{v}}_i={\mathbf{v}}_{i-1}+{\mathbf{s}}_i{\dot{q}}_i\text{\hspace{1em}(5.2)}$$

在初始条件 ${\mathbf{v}}_0=0$ 下，刚体速度的非递归公式为：

$${\mathbf{v}}_i=\sum _{j=1}^i{\mathbf{s}}_j{\dot{q}}_j\text{\hspace{1em}(5.3)}$$

设 $\mathrm{m}$ 和 $\mathrm{a}$ 分别表示标量-向量乘法和向量加法的成本。方程5.2 的一次应用成本为 $\mathrm{m}+\mathrm{a}$，方程5.3 的成本为 $i\mathrm{m}+(i-1)$ a。通过方程5.2 计算前 $n$ 个刚体速度的成本为 $n(\mathrm{m}+\mathrm{a})$，通过方程5.3 的成本为 $\frac{1}{2}(n(n+1)\mathrm{m}+n(n-1)\mathrm{a})$。因此，使用方程5.2 可以得到高效的 $O(n)$ 计算，而方程5.3 导致低效的 $O\left(n^2\right)$ 计算。

这种低效的原因并不难理解。如果我们使用方程5.3 进行计算，那么我们最终需要计算的是
$$\begin{array}{rl}{\mathbf{v}}_1& ={\mathbf{s}}_1{\dot{q}}_1\\ {\mathbf{v}}_2& ={\mathbf{s}}_1{\dot{q}}_1+{\mathbf{s}}_2{\dot{q}}_2\\ & ⋮\\ & ⋮\\ {\mathbf{v}}_n& ={\mathbf{s}}_1{\dot{q}}_1+{\mathbf{s}}_2{\dot{q}}_2+\cdots +{\mathbf{s}}_n{\dot{q}}_n.\end{array}$$

在这种情况下，${\mathbf{s}}_1{\dot{q}}_1$ 的乘积被计算了 $n$ 次，而实际上只需要计算一次。同样地，$s_2{\dot{q}}_2$ 的乘积被计算了 $n-1$ 次，和 ${\mathbf{s}}_1{\dot{q}}_1+{\mathbf{s}}_2{\dot{q}}_2$ 的和被计算了 $n-1$ 次，依此类推。因此，递归关系提供了一种系统的方法来避免在计算相关量的序列时不必要的重复，这是其高效性的秘诀。

当计算更加复杂时，递归关系的计算优势甚至更大。例如，在无分支链中刚体加速度的递归关系是
$${\mathbf{a}}_i={\mathbf{a}}_{i-1}+{\mathbf{s}}_i{\ddot{q}}_i+{\mathbf{v}}_i\times {\mathbf{s}}_i{\dot{q}}_i\text{\hspace{1em}(5.4)}$$

这正是 公式5.2 的时间导数，而非递归公式则为：

$${\mathbf{a}}_i=\sum _{j=1}^i{\mathbf{s}}_j{\ddot{q}}_j+\sum _{j=1}^i\sum _{k=1}^{j-1}\left({\mathbf{s}}_k{\dot{q}}_k\right)\times \left({\mathbf{s}}_j{\dot{q}}_j\right)\text{\hspace{1em}(5.5)}$$

（在这两种情况下，我们假设${\dot{\mathbf{s}}}_i={\mathbf{v}}_i\times {\mathbf{s}}_i$ 。）通过公式5.4计算加速度是一个$O(n)$的计算；但是通过公式5.5计算是一个$O\left(n^3\right)$的计算。

公式5.3和5.5直接用其基本要素——关节轴向量、关节速度和加速度变量来表示身体速度和加速度。这样的方程被称为闭式形式方程。刚体系统的运动方程在闭式形式下可以表示为：

$${\tau }_i=\sum _{j=1}^n{H}_{ij}{\ddot{q}}_i+\sum _{j=1}^n\sum _{k=1}^n{C}_{ijk}{\dot{q}}_j{\dot{q}}_k+g_i\text{\hspace{1em}(5.6)}$$

其中$H_{ij}$是广义惯量矩阵的元素，$C_{ijk}$是离心力和科氏力的系数，$g_i$是重力项。早期的非递归算法通过类似这样的方程计算动力学，并且它们的计算复杂性通常为$O\left(n^4\right)$。递归算法效率更高，其计算复杂性可以降低至$O(n)$。当递归的牛顿-欧拉算法首次出现时，相对于非递归的前身——Uicker/Kahn算法，对于一个6自由度的机械臂，其速度约快100倍（Hollerbach, 1980）。在高效的递归算法发明之前，闭式形式方程是表示和评估运动方程的标准方式。

5.3 递归牛顿-欧拉算法

一般运动树的逆动力学可以按照以下三个步骤计算：

1. 计算运动树中每个刚体的速度和加速度。
2. 计算产生这些加速度所需的力。
3. 计算从作用于刚体上的力传递到关节的力。

这些是递归牛顿-欧拉算法的步骤。现在让我们推导出每个步骤的方程，假设树由量$\lambda (i),\mu (i),{\mathbf{S}}_i$和 ${\mathbf{I}}_i$来建模，如第4章所解释的那样。

图5.1：作用在刚体 i 上的力

步骤1：设 ${\mathbf{v}}_i$ 和 ${\mathbf{a}}_i$ 分别为刚体 $i$ 的速度和加速度。在一个运动树中，任何刚体的速度可以递归地定义为其父刚体的速度和连接它与父刚体的关节处的速度之和。因此，
$${\mathbf{v}}_i={\mathbf{v}}_{\lambda (i)}+{\mathbf{S}}_i{\dot{\mathbf{q}}}_i\cdot \left({\mathbf{v}}_0=0\right)\text{\hspace{1em}(5.7)}$$

（初始值以括号形式给出）。通过对该方程进行求导得到加速度的递推关系：

$${\mathbf{a}}_i={\mathbf{a}}_{\lambda (i)}+{\mathbf{S}}_i{\ddot{\mathbf{q}}}_i+{\dot{\mathbf{S}}}_i{\dot{\mathbf{q}}}_i.\left({\mathbf{a}}_0=0\right)\text{\hspace{1em}(5.8)}$$

使用这两个公式进行连续迭代，其中 $i$ 的取值范围从 1 到 $N_B$，可以计算出运动树中每个刚体的速度和加速度。

步骤2：设 ${\mathbf{f}}_i^B$ 为作用在刚体 $i$ 上的净力。这个力与刚体 $i$ 的加速度之间存在如下运动方程的关系：
$${\mathbf{f}}_i^B={\mathbf{I}}_i{\mathbf{a}}_i+{\mathbf{v}}_i{\times }^{\ast }{\mathbf{I}}_i{\mathbf{v}}_i\text{\hspace{1em}(5.9)}$$

步骤3：参考图5.1，设${\mathbf{f}}_i$为从刚体$\lambda (i)$传递到刚体$i$的关节$i$处的力，${\mathbf{f}}_i^x$为作用在刚体$i$上的外力（如果有的话）。外力可以模拟各种环境影响，如力场、物理接触等。它们被视为算法的输入；也就是说，它们的值被假定为已知。刚体$i$上的净力则为：
$${\mathbf{f}}_i^B={\mathbf{f}}_i+{\mathbf{f}}_i^x-\sum _{j\in \mu (i)}{\mathbf{f}}_j,$$

这个方程可以重新排列为关节力的递推关系：

$${\mathbf{f}}_i={\mathbf{f}}_i^B-{\mathbf{f}}_i^x+\sum _{j\in \mu (i)}{\mathbf{f}}_j\text{\hspace{1em}(5.10)}$$

使用这个公式进行连续迭代，其中 $i$ 的取值范围从$N_B$递减至1，可以计算出运动树中每个关节处的空间力。对于这个公式，不需要初始值，因为任何没有子刚体的刚体都会使$\mu (i)$为空集。

在计算了每个关节处的空间力之后，只需计算关节处的广义力，即：

$${\mathbf{\tau }}_i={\mathbf{S}}_i^{\mathrm{T}}{\mathbf{f}}_i\text{\hspace{1em}(5.11)}$$

（参考方程3.37）。方程5.7到5.11描述了递归牛顿-欧拉算法的最简形式。

外力可以用来模拟各种环境影响，包括重力。然而，可以更高效地将均匀重力场建模为基座的虚拟加速度。为此，我们只需将方程5.8的初始值替换为${\mathbf{a}}_0=-{\mathbf{a}}_g$，其中${\mathbf{a}}_g$为重力加速度矢量。如果使用这个技巧，那么计算得到的${\mathbf{a}}_i$和${\mathbf{f}}_i^B$不再是刚体$i$的真实加速度和净力，而是分别被重力加速度和重力力矢量所偏移。（刚体$i$上的重力力为${\mathbf{I}}_i{\mathbf{a}}_g$）。

算法特点

方程5.7和5.8涉及从根部到叶子的数据传播；方程5.10涉及从叶子到根部的数据传播；而方程5.9和5.11描述了每个刚体局部的计算。因此，递归牛顿-欧拉算法是一个两遍过程：首先进行外部传递，计算速度和加速度；然后进行内部传递，计算关节力。计算刚体力可以分配给第一次或第二次传递中的任意一次。通常选择第一次传递。

这个算法不仅计算${\mathbf{\tau }}_i$，还可以计算其他量，比如${\mathbf{v}}_i$和${\mathbf{f}}_i$，有时候也是有用的。例如，机械设计师的任务之一是选择足够强度的关节轴承以承受运行期间的动态反作用力。这些力可以从${\mathbf{f}}_i$中计算得到。

通过将某些输入设为零，可以计算出动力学的特定分量。例如，如果将外部力、关节速度和加速度项设为零，但保留虚拟基底加速度，则算法只计算重力补偿项——静态平衡重力的项。这样的项在机器人控制系统中被使用。另外，如果将重力、加速度和外部力项都设为零，但保留关节速度，那么算法只计算科氏力和离心力的项。如果对效率有要求，那么可以使用简化版本的算法来执行这些专门的计算。例如，如果目标是仅计算重力补偿项，那么算法简化为：

$$\begin{array}{rl}{\mathbf{f}}_i& =-{\mathbf{I}}_i{\mathbf{a}}_g+\sum _{j\in \mu (i)}{\mathbf{f}}_j\\ {\mathbf{\tau }}_i& ={\mathbf{S}}_i^{\mathrm{T}}{\mathbf{f}}_i.\end{array}$$

要计算实际的重力项，将$-{\mathbf{I}}_i{\mathbf{a}}_g$替换为${\mathbf{I}}_i{\mathbf{a}}_g$。

刚体坐标下的方程

方程5.7到5.11表达了算法而不涉及任何特定的坐标系。在实际应用中，如果将与刚体$i$相关的计算以刚体$i$的坐标系（如第4.2节中定义的）进行，则算法效果最佳。这是算法的刚体坐标（或链路坐标）版本。

让我们将递归牛顿-欧拉算法的方程转换为刚体坐标。量${\mathbf{v}}_i,{\mathbf{a}}_i,{\mathbf{S}}_i,{\mathbf{I}}_i,{\mathbf{f}}_i$和${\mathbf{f}}_i^B$现在都被认为是以刚体$i$的坐标系表示的。注意，系统模型已经以这些坐标提供了${\mathbf{S}}_i$和${\mathbf{I}}_i$。用刚体坐标表示，方程5.7和5.8变为：

$${\mathbf{v}}_i={}^i{\mathbf{X}}_{\lambda (i)}{\mathbf{v}}_{\lambda (i)}+{\mathbf{S}}_i{\dot{\mathbf{q}}}_i\left({\mathbf{v}}_0=0\right)$$

和

$${\mathbf{a}}_i={}^i{\mathbf{X}}_{\lambda (i)}{\mathbf{a}}_{\lambda (i)}+{\mathbf{S}}_i{\ddot{\mathbf{q}}}_i+{\stackrel{\circ }{S}}_i{\dot{\mathbf{q}}}_i+{\mathbf{v}}_i\times {\mathbf{S}}_i{\dot{\mathbf{q}}}_i.\left({\mathbf{a}}_0=-{\mathbf{a}}_g\right)$$

与原方程相比，进行了以下更改：插入了坐标转换，将${\mathbf{v}}_{\lambda (i)}$和${\mathbf{a}}_{\lambda (i)}$从刚体$\lambda (i)$的坐标系转换为刚体$i$的坐标系；并且将矩阵${\dot{\mathbf{S}}}_i$替换为${\stackrel{\circ }{S}}_i+{\mathbf{v}}_i\times {\mathbf{S}}_i$，其中${\stackrel{\circ }{S}}_i$是在刚体坐标中的${\mathbf{S}}_i$的显式导数。我们还利用这个机会，将原始的初始加速度值替换为${\mathbf{a}}_0=-{\mathbf{a}}_g$以模拟重力的影响。

关节计算例程jcalc能够以刚体$i$的坐标系提供量${\mathbf{S}}_i$，${\mathbf{v}}_{\mathrm{J}i}={\mathbf{S}}_i{\dot{\mathbf{q}}}_i$和${\mathbf{c}}_{\mathrm{J}i}={\dot{\mathbf{S}}}_i{\dot{\mathbf{q}}}_i$。因此，为了利用可用的数据，我们修改方程5.14和5.15如下：

$$\begin{array}{rl}{\mathbf{v}}_{\mathrm{J}i}& ={\mathbf{S}}_i{\dot{\mathbf{q}}}_i\\ {\mathbf{c}}_{\mathrm{J}i}& ={\stackrel{\circ }{S}}_i{\dot{\mathbf{q}}}_i\\ {\mathbf{v}}_i& ={}^i{\mathbf{X}}_{\lambda (i)}{\mathbf{v}}_{\lambda (i)}+{\mathbf{v}}_{\mathrm{J}i}\left({\mathbf{v}}_0=0\right)\\ {\mathbf{a}}_i& ={}^i{\mathbf{X}}_{\lambda (i)}{\mathbf{a}}_{\lambda (i)}+{\mathbf{S}}_i{\ddot{\mathbf{q}}}_i+{\mathbf{c}}_{\mathrm{J}i}+{\mathbf{v}}_i\times {\mathbf{v}}_{\mathrm{J}i}.\left({\mathbf{a}}_0=-{\mathbf{a}}_g\right)\end{array}\text{\hspace{1em}(5.16-19)}$$

在刚体坐标系版本中，方程5.9和5.11保持不变，但是方程5.10变为：

$${\mathbf{f}}_i={\mathbf{f}}_i^B-{}^i{\mathbf{X}}_0^{\ast }{\mathbf{f}}_i^x+\sum _{j\in \mu (i)}{}^i{\mathbf{X}}_j^{\ast }{\mathbf{f}}_j\text{\hspace{1em}(5.20)}$$

在这个方程中，我们假设外部力来自系统外部，并且假设它们以绝对坐标（即刚体0的坐标）表示。方程5.16到5.20与方程5.9和5.11一起，是递归牛顿-欧拉算法的刚体坐标版本的方程。

算法细节

表格5.1展示了算法的刚体坐标版本的伪代码，同时给出了基本版本和刚体坐标版本的方程。请注意，刚体坐标版本中的符号 ${\mathbf{v}}_i,{\mathbf{a}}_i$ 等是以刚体坐标表示的，而基本版本中相同的符号表示抽象向量或未指定公共坐标系中的坐标向量。

从伪代码中的两个for循环可以看出算法的两次通过结构。符号 ${\mathbf{X}}_{\mathrm{J}},{\mathbf{v}}_{\mathrm{J}}$ 和 ${\mathbf{c}}_{\mathrm{J}}$ 是局部变量，每次第一个循环迭代时都会取新值。如果树中包含速度变量不是位置变量的导数的关节，则只需将符号 ${\dot{\mathbf{q}}}_i$ 和 ${\ddot{\mathbf{q}}}_i$ 分别替换为 ${\mathbf{\alpha }}_i$ 和 ${\dot{\mathbf{\alpha }}}_i$。同样地，如果树中包含任何显式依赖于时间的关节，则 jcalc 将使用方程3.32和3.41而不是5.16和5.17自动计算 ${\mathbf{v}}_{\mathrm{J}}$ 和 ${\mathbf{c}}_{\mathrm{J}}$；伪代码中唯一的变化是将当前时间作为jcalc的第四个参数传递。${}^i{\mathbf{X}}_0$ 仅用于将外部力从基座标转换为刚体坐标。如果没有外部力，则可以省略计算这些变换的if语句。

在实现方程5.20时，我们将其从使用 $\mu (i)$ 的计算转换为使用 $\lambda (i)$ 的计算。方程5.20的直译伪代码如下：

然而，通过以下代码片段，可以按不同的顺序进行相同的计算，但结果是一样的：

合并此代码片段与算法的其余部分后，第一个循环体变成了算法中第一个循环体的最后一行。

5.4 原始版本

递归牛顿-欧拉算法最初是由Luh等人（1980a）在一篇论文中描述的，该论文在Brady等人（1982）中再次发表。这个最初的算法是使用3D向量来构成的，表面上看起来与5.3节中描述的算法有很大不同。然而，这两种算法实际上几乎是相同的，唯一的显著区别是其中一个使用空间加速度，而另一个使用经典加速度（见§2.11）。

表5.2显示了原始算法的一个特殊情况，即所有关节都是旋转关节。使用3D向量来表达刚体动力学的一个缺点是方程对关节类型敏感。该算法原始方程式：
$$\begin{array}{rl}{\mathbf{\omega }}_i& ={}^i{\mathbf{E}}_{i-1}{\mathbf{\omega }}_{i-1}+\mathbf{z}{\dot{q}}_i\\ {\mathbf{v}}_i& ={}^i{\mathbf{E}}_{i-1}\left({\mathbf{v}}_{i-1}+{\mathbf{\omega }}_{i-1}\times {}^{i-1}{\mathbf{r}}_i\right)\\ {\dot{\mathbf{\omega }}}_i& ={}^i{\mathbf{E}}_{i-1}{\dot{\mathbf{\omega }}}_{i-1}+\mathbf{z}{\ddot{q}}_i+\left({}^i{\mathbf{E}}_{i-1}{\mathbf{\omega }}_{i-1}\right)\times \mathbf{z}{\dot{q}}_i\\ {\dot{\mathbf{v}}}_i& ={}^i{\mathbf{E}}_{i-1}\left({\dot{\mathbf{v}}}_{i-1}+{\dot{\mathbf{\omega }}}_{i-1}\times {}^{i-1}{\mathbf{r}}_i+{\mathbf{\omega }}_{i-1}\times {\mathbf{\omega }}_{i-1}\times {}^{i-1}{\mathbf{r}}_i\right)\\ {\mathbf{F}}_i& =m_i\left({\dot{\mathbf{v}}}_i+{\dot{\mathbf{\omega }}}_i\times {\mathbf{c}}_i+{\mathbf{\omega }}_i\times {\mathbf{\omega }}_i\times {\mathbf{c}}_i\right)\\ {\mathbf{N}}_i& ={\mathbf{I}}_i^{cm}{\dot{\mathbf{\omega }}}_i+{\mathbf{\omega }}_i\times {\mathbf{I}}_i^{cm}{\mathbf{\omega }}_i\\ {\mathbf{f}}_i& ={\mathbf{F}}_i+{}^i{\mathbf{E}}_{i+1}{\mathbf{f}}_{i+1}\\ {\mathbf{n}}_i& ={\mathbf{N}}_i+{\mathbf{c}}_i\times {\mathbf{F}}_i+{}^i{\mathbf{E}}_{i+1}{\mathbf{n}}_{i+1}+{}^i{\mathbf{r}}_{i+1}\times {}^i{\mathbf{E}}_{i+1}{\mathbf{f}}_{i+1}\\ {\tau }_i& ={\mathbf{z}}^{\mathrm{T}}{\mathbf{n}}_i\end{array}$$

符号：

$$\begin{array}{rl}& {\mathbf{\omega }}_i,{\dot{\mathbf{\omega }}}_i\text{ angular velocity and acceleration of body }i\\ & {\mathbf{v}}_i,{\dot{\mathbf{v}}}_i\text{ linear velocity and acceleration of the origin of }{F}_i\\ & \text{ (body }i\text{ coordinate frame) }\\ & {\mathbf{F}}_i,{\mathbf{N}}_i\text{ net force and moment on body }i\text{, expressed at its }\\ & \text{ centre of mass }\\ & {\mathbf{f}}_i,{\mathbf{n}}_i\text{ force and moment exerted on body }i\text{ through joint }i\\ & m_i,{\mathbf{c}}_i,{\mathbf{I}}_i^{cm}\text{ mass, centre of mass, and rotational inertia about }\\ & \text{ centre of mass for body }i\text{ in }{F}_i\text{ coordinates }\\ & {}^i{\mathbf{E}}_{i-1}3\times 3\text{ orthogonal rotation matrix transforming from }\\ & F_{i-1}\text{ to }{F}_i\text{ coordinates }\\ & {}^{i-1}{\mathbf{r}}_i\text{ position of origin of }{F}_i\text{ relative to }{F}_{i-1}\text{, expressed in }\\ & F_{i-1}\text{ coordinates }\\ & {\dot{q}}_i,{\ddot{q}}_i,{\tau }_i\text{ joint }i\text{ velocity, acceleration and force variables }\\ & \mathbf{z}\text{ direction of joint }i\text{ axis in }{F}_i\text{ coordinates }\end{array}$$

与表5.1中的版本对应关系：

v ^ i = [ ω i v i ] a ^ i = [ ω ˙ i v ˙ i − ω i × v i ] f ^ i B = [ N i + c i × F i F i ] f ^ i = [ n i f i ] i X i − 1 = [ i E i − 1 0 − i E i − 1 i − 1 r i × i E i − 1 ] I ^ i = [ I i c m − m i c i × c i × m i c i × − m i c i × m i 1 ] S i = [ z 0 ] z = [ 0 0 1 ] λ ( i ) = i − 1 μ ( i ) = { i + 1 } \begin{aligned} & \hat{\boldsymbol{v}}_{i}=\left[\begin{array}{c} \boldsymbol{\omega}_{i} \\ \boldsymbol{v}_{i} \end{array}\right] \quad \hat{\boldsymbol{a}}_{i}=\left[\begin{array}{c} \dot{\boldsymbol{\omega}}_{i} \\ \dot{\boldsymbol{v}}_{i}-\boldsymbol{\omega}_{i} \times \boldsymbol{v}_{i} \end{array}\right] \quad \hat{\boldsymbol{f}}_{i}^{B}=\left[\begin{array}{c} \boldsymbol{N}_{i}+\boldsymbol{c}_{i} \times \boldsymbol{F}_{i} \\ \boldsymbol{F}_{i} \end{array}\right] \quad \hat{\boldsymbol{f}}_{i}=\left[\begin{array}{c} \boldsymbol{n}_{i} \\ \boldsymbol{f}_{i} \end{array}\right] \\ & { }^{i} \boldsymbol{X}_{i-1}=\left[\begin{array}{cc} { }^{i} \boldsymbol{E}_{i-1} & \mathbf{0} \\ -{ }^{i} \boldsymbol{E}_{i-1}{ }^{i-1} \boldsymbol{r}_{i} \times & { }^{i} \boldsymbol{E}_{i-1} \end{array}\right] \quad \hat{\boldsymbol{I}}_{i}=\left[\begin{array}{cc} \boldsymbol{I}_{i}^{c m}-m_{i} \boldsymbol{c}_{i} \times \boldsymbol{c}_{i} \times & m_{i} \boldsymbol{c}_{i} \times \\ -m_{i} \boldsymbol{c}_{i} \times & m_{i} \mathbf{1} \end{array}\right] \\ & \boldsymbol{S}_{i}=\left[\begin{array}{l} \boldsymbol{z} \\ \mathbf{0} \end{array}\right] \quad \boldsymbol{z}=\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right] \quad \lambda(i)=i-1 \quad \mu(i)=\{i+1\} \end{aligned} ​v^i​=[ωi​vi​​]a^i​=[ω˙i​v˙i​−ωi​×vi​​]f^​iB​=[Ni​+ci​×Fi​Fi​​]f^​i​=[ni​fi​​]iXi−1​=[iEi−1​−iEi−1​i−1ri​×​0iEi−1​​]I^i​=[Iicm​−mi​ci​×ci​×−mi​ci​×​mi​ci​×mi​1​]Si​=[z0​]z= ​001​ ​λ(i)=i−1μ(i)={i+1}​

表5.2：根据Luh等人（1980a）描述的原始递归牛顿-欧拉方程适用于旋转和平移关节，其中包含以下形式的方程式…

（注：接下来应该是具体的方程式形式，但由于你的输入不完整，无法提供详细内容。）
$$\text{ variable }=\{\begin{array}{ll}\text{ one expression }& \text{ if joint is revolute }\\ \text{ another expression }& \text{ if joint is prismatic. }\end{array}$$

本表中选择使用$3D$向量符号是为了在本书和其他列出该算法的书籍之间达成折衷。该表中的方程是针对一个无分支的运动树编写的，与其他地方描述该算法的方式一致，但很容易将其修改为适用于分支树。

表5.2还显示了原始算法中的$3D$向量与表5.1中显示的空间向量之间的对应关系。只有两个地方$3D$向量与空间向量的对应的$3D$分量不匹配：线性加速度和净物体力矩。后者是一个微不足道的差异，当使用${\mathbf{N}}_i$来计算${\mathbf{n}}_i$时，它会立即得到纠正。这是由于${\mathbf{N}}_i$是在质心处计算而不是在物体坐标系的原点处计算所导致的。前者是一个非微不足道的差异，因为它会影响从一个物体传播到下一个物体的量。

（根据2.11节，经典加速度是在一个坐标系中，该坐标系具有纯线性速度，等于固定于物体原点的点的速度，空间速度的明显导数。根据这一点，可以将两个算法之间的差异解释如下：表5.1中的算法在与当前时刻的物体坐标系重合的静止坐标系中表示方程；而表5.2中的算法在与当前时刻的物体坐标系重合、且与物体坐标系原点具有相同线性速度的非旋转坐标系中表示方程。）

原始算法的一个有趣特点是线性速度在任何后续表达式中都没有被使用，因此不需要进行计算。这是使用经典加速度产生的特殊属性。如果省略速度计算，那么该算法比空间版本稍微更高效。然而，这种差异仅仅是几个百分点，并且在其他与效率相关的问题上很容易被忽略，比如编程语言的选择、计算机硬件等等。

5.5 附加注释

Luh等人（1980a）提出的算法通常被称为递归牛顿-欧拉算法。然而，将牛顿-欧拉运动方程与递归评估策略相结合的想法在几年前就有了。特别是，在Stepanenko和Vukobratovic（1976）以及Orin等人（1979）的早期工作中可以看到递归牛顿-欧拉算法的初步形式。该算法随后经过改进和优化，其中一些优化方法在第10章中进行了介绍。最快的两个已发表版本可以在He和Goldenberg（1989）以及Balafoutis等人（1988）中找到，后者还在Balafoutis和Patel（1991）中提到。当时机器人技术领域进行这类研究的动力是为了开发一种逆动力学算法，以便在实时控制机器人运动中能够快速使用。Luh等人（1980b）中可以找到一个早期的例子，尽管最终操作空间的形式化变得更受欢迎（Khatib，1987）。当时计算机的相对较慢速度使得一些研究人员开始研究并行计算机实现的可能性（Lathrop，1985），而其他人则研究符号优化技术（Murray和Neuman，1984；Neuman和Murray，1987）。有一个有趣的新方向是开发高效的算法来计算相对于设计参数的逆动力学的偏导数。Fang和Pollard（2003）提供了一个例子。

在该算法被发明的时候，拉格朗日动力学比牛顿-欧拉动力学更受欢迎，这促使Hollerbach设计了递归拉格朗日算法，并比较了各种递归和非递归算法的计算复杂性（Hollerbach，1980）。这项工作对比了非递归方法（如Uicker/Kahn算法（Kahn和Roth，1971））的$O\left(n^4\right)$复杂性与递归算法的$O(n)$复杂性。