Chapter5: 逆动力学
Inverse Dynamics

求解逆动力学问题是在计算产生指定刚体系统加速度所需要的力。逆动力学的计算在运动控制系统、轨迹规划和优化(比如，机器人和动画)、机械设计等方面都有应用。有时也是正动力学算法的一部分。我们将在第 6 章中介绍正动力学。逆动力学的计算可以归结为如下的公式: $$ \boldsymbol{\tau} = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}) $$ 其中，$\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}$分别是广义的位置、速度、加速度向量，$\boldsymbol{\tau}$ 是广义的力向量。 model 是刚体系统的系统模型。

本章介绍一种计算运动树逆动力学算法。闭关系统的算法将在第 8 章中介绍。运动树的逆动力学特别容易计算，这是我们先介绍它的原因。该算法被称为递归牛顿欧拉算法(recursive Newton-Euler algorithm)，它是运动树逆动力学的最简单有效的算法。其计算复杂度是 $O(n)$，这意味着计算复杂度随着树中刚体数量或者关节变量的数量线性增长。

我们首先介绍该算法的基本形式，然后添加一些一些细节。对比空间矢量版本与原始 3D 矢量版本的算法。本章还涉及了算法复杂度的内容，解释了为什么递归算法如此高效。

5.1 算法复杂度 Algorithm Complexity

我们把算法的计算成本定义为其运算次数operations count —— 即执行算法过程中需要运行的算术运算次数。如果算法涉及到实数运算，比如说动力学算法，那么通常只统计浮点运算的次数。因此，整形运算在统计过程中都被忽略了，比如计算数组索引、循环变量的递增等。

算法的计算复杂度 computational Complexity 是用来度量计算量是如何随着问题规模增加的。我们用大写字母 $O$ 表示它的量级。如果我们说一个算法的复杂度是 $O(n^p)$，其中 $n$ 用来描述问题规模，那么当 $n \to \infty$ 时计算量以 $n^p$ 的量级增长。更准确的说，如果一个算法的复杂度是 $O(f(n))$，那么存在正数 $C_{min}, C_{max}$ 和 $n_{min}$，对于所有的 $n > n_{min}$，使得计算量最少是 $C_{min}\cdot f(n)$，但不会超过 $C_{max} \cdot 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))$。提及复杂度的时候，我们通常选择函数集合中形式最简单的那个。比如说，如果一个算法的时间成本可以写成多项式 $a_0 + a_1 n + \cdots + a_p n^p$，那么与该多项式对应的最简单的形式是 $n^p$，所以我们说其计算复杂度是 $O(n^p)$。一般情况下，计算复杂度只与$f(n)$ 中增长最快的那项有关。如果函数 $f(n)$ 收敛到一个常数，我们说 $f(n)$ 是 $O(1)$ 的。

算法的复杂度取决于我们如何统计问题的规模。比如，我们说矩阵求逆的算法复杂度是 $O(n^3)$的，因为对一个 $n \times n$ 的矩阵求逆，当$n \to \infty$是计算量将与 $n^3$ 成比例。这里描述算法规模的因子是矩阵的维度$n$。如果 $n$ 表示矩阵中元素的数量，那么其计算复杂度就是$O(n^{1.5})$。更一般的，如果 $m$ 和 $n$ 是描述问题规模的两个不同因子，并且 $m = g(n)$，那么$O(f(m)) = O(f(g(n))$。

对于一个动力学算法，问题的规模是刚体系统的规模。如果系统是一个运动树，那么有两个指标来衡量描述系统规模: 刚体的数量$N_B$，或者自由度的数量$n$。如果树中每个关节至少有一个自由度，那么这两个指标满足关系 $N_B ≤ n ≤ 6N_B$，这意味着: $$ \begin{equation}\label{equa_5_1} O(N_B^p) = O(n^p) \end{equation} $$ 因此，在描述计算复杂度的时候，没有不必要区分$N_B$ 和 $n$。

5.2 递归关系 Recurrence Relations

现代动力学算法大多都是递归的，它比以前非递归的方法快好几个数量级。它们被称为递归算法，是因为它们利用了递归关系计算中间结果。正是递归关系决定了算法效率，所以我们有必要花点时间研究一下递归关系式如何工作的。

递归关系recurrence relation是根据之前的元素定义序列中下一个元素的表达式。这样的表达式都有一个初始值，定义者递归关系的起点。比如，斐波那契数列，$0, 1, 1, 2, 3, 5, 8, 13, \cdots$，其递归关系式为 $F_i = F_{i-1} + F_{i-2}$，初值为 $F_0 = 0, F_1 = 1$。在刚体系统中，如果刚体都是按照4.1.2节中的规则编码的，那么刚体的速度，$\boldsymbol{v}_0, \boldsymbol{v}_1, \boldsymbol{v}_2, \cdots$ 可以由递归关系 $\boldsymbol{v}_i = \boldsymbol{v}_{\lambda(i)} + \boldsymbol{S}_i \dot{\boldsymbol{q}}_i$，以及给定初值$\boldsymbol{v}_0$来唯一确定。

我们先分别看一下有无递归关系时如何计算刚体速度的。为了简化问题，我们假设系统是一个无分支的运动树，各关节自由度都是 1。这意味着$\lambda(i) = i - 1$，因此关节 $i$ 的传导的速度是关节轴向量 $\boldsymbol{s}_i$ 和关节速度变量$\dot{q}_i$的乘积，刚体速度的递归关系可以写成: $$ \begin{equation}\label{equa_5_2} \boldsymbol{v}_i = \boldsymbol{v}_{i-1} + \boldsymbol{s}_i \dot{q}_i \end{equation} $$ 第0个刚体是个基座，有系统的初始条件 $\boldsymbol{v}_0 = \boldsymbol{0}$，因此刚体速度的非递归公式就是: $$ \begin{equation}\label{equa_5_3} \boldsymbol{v}_i = \sum_{j=1}^i \boldsymbol{s}_i \dot{q}_i \end{equation} $$ 令 $\mathrm{m}$ 和 $\mathrm{a}$ 分别表示向量的数乘运算和向量加法的计算成本。式($\ref{equa_5_2}$)的计算成本就是$\mathrm{m} + \mathrm{a}$。而式($\ref{equa_5_3}$)的计算成本则是$i \mathrm{m} + (i - 1) \mathrm{a}$。利用式($\ref{equa_5_2}$)计算$n$个刚体的速度，其代价为$n(\mathrm{m} + \mathrm{a})$，而式($\ref{equa_5_3}$)的代价为$\frac{1}{2}\left(n(n+1)\mathrm{m} + n(n-1)\mathrm{a}\right)$。因此式($\ref{equa_5_2}$)的计算复杂度就是$O(n)$，而式($\ref{equa_5_3}$)的计算复杂度则是$O(n^2)$。低效的根源不难看出，如果我们用式($\ref{equa_5_3}$)完成计算任务，那么需要计算: $$ \begin{array}{rcl} \boldsymbol{v}_1 & = & \boldsymbol{s}_1 \dot{q}_1 \\ \boldsymbol{v}_2 & = & \boldsymbol{s}_1 \dot{q}_1 + \boldsymbol{s}_2 \dot{q}_2 \\ & \vdots & \\ \boldsymbol{v}_n & = & \boldsymbol{s}_1 \dot{q}_1 + \boldsymbol{s}_2 \dot{q}_2 + \cdots + \boldsymbol{s}_n \dot{q}_n \end{array} $$ 乘积 $\boldsymbol{s}_1 \dot{q}_1$被计算了$n$遍，但实际只需要一次。类似的$\boldsymbol{s}_2 \dot{q}_2$计算了$n-1$次， $\boldsymbol{s}_1 \dot{q}_1 + \boldsymbol{s}_2 \dot{q}_2$ 也被计算了$n-1$次，等等。递归关系提供了一种避免这些重复计算的机制，这就是它们高效的原因。

当计算更加复杂时，递推关系的效率优势会更大。例如，无分支链中物体加速度的递推关系为: $$ \begin{equation}\label{equa_5_4} \boldsymbol{a}_i = \boldsymbol{a}_{i-1} + \boldsymbol{s}_i \ddot{q}_i + \boldsymbol{v}_i \times \boldsymbol{s}_i \dot{q}_i \end{equation} $$ 它仅仅是式($\ref{equa_5_2}$)关于时间的导数，其非递归的表达式为: $$ \begin{equation}\label{equa_5_5} \boldsymbol{a}_i = \sum_{j=1}^i \boldsymbol{s}_j \ddot{q}_j + \sum_{j=1}^i \sum_{k=1}^{j-1} \left(\boldsymbol{s}_k \dot{q}_k\right) \times \left(\boldsymbol{s}_j \dot{q}_j\right) \end{equation} $$ 这两个式子中，我们都认为$\dot{\boldsymbol{s}}_i = \boldsymbol{v}_i \times \boldsymbol{s}_i$。式($\ref{equa_5_4}$)的计算复杂度是$O(n)$的，而式($\ref{equa_5_5}$)的计算复杂度则是$O(n^3)$。方程 ($\ref{equa_5_5}$) 和 ($\ref{equa_5_5}$) 直接用关节轴矢量以及关节速度和加速度变量来表达刚体的速度和加速度。这种形式的方程称为封闭式的(closed form)。刚体系统的运动方程写成封闭的形式，有: $$ \begin{equation}\label{equa_5_6} \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 \end{equation} $$ 其中$H_{ij}$是广义的惯性矩阵，$C_{ijk}$ 是离心力和科氏力的系数，$g_i$ 是重力项。早些年，采用类似这样的方程求解动力学的非递归算法的计算复杂度通常都是 $O(n^4)$ 的。递归牛顿欧拉算法第一次出现，人们就发现，对于一个6自由度的机械臂，该算法比早先的非递归方法 Uicker/Kahn 算法快了 100 倍(Hollerbach, 1980)。在高效递归算法发明之前，闭式方程是描述运动方程的标准方法。

5.3 递归牛顿欧拉算法 The Recursive Newton-Euler Algorithm

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

计算树中每个刚体的速度和加速度
计算产生加速度所需的力
计算作用在刚体上的力通过关节传导的力。

这就是递归牛顿欧拉算法的步骤。假设运动树是按照第 4 章的设定，由 $\lambda(i), \mu(i), \boldsymbol{S}_i, \boldsymbol{I}_i$ 建模的。接下来我们来看每一步所需的计算公式。

第一步: 令 $\boldsymbol{v}_i$ 和 $\boldsymbol{a}_i$ 为刚体 $i$ 的速度和加速度。运动树中任何刚体的速度都是由器父节点的速度和关节的速度之和定义的，即: $$ \begin{equation}\label{equa_5_7} \boldsymbol{v}_i = \boldsymbol{v}_{\lambda(i)} + \boldsymbol{S}_i \dot{\boldsymbol{q}} \qquad (\boldsymbol{v}_0 = \boldsymbol{0}) \end{equation} $$ 加速度的递推关系可以通过对该方程微分获得，即 $$ \begin{equation}\label{equa_5_8} \boldsymbol{a}_i = \boldsymbol{a}_{\lambda(i)} + \boldsymbol{S}_i \ddot{\boldsymbol{q}}_i + \dot{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i \qquad (\boldsymbol{a}_0 = \boldsymbol{0}) \end{equation} $$ 从 $1$ 到 $N_B$ 遍历 $i$ 连续迭代这两个公式，就得到了运动树中每个刚体的速度和加速度。

第二步: 令 $\boldsymbol{f}_i^{B}$ 表示作用在刚体 $i$ 上的力。根据运动方程，可以写出该力与加速度之间的关系: $$ \begin{equation}\label{equa_5_9} \boldsymbol{f}_i^B = \boldsymbol{I}_i \boldsymbol{a}_i + \boldsymbol{v}_i \times^* \boldsymbol{I}_i \boldsymbol{v}_i \end{equation} $$

图 5.1. 作用在刚体 $i$ 上的力

第三步: 根据图 5.1，令 $\boldsymbol{f}_i$ 为从刚体$\lambda(i)$ 通过关节 $i$ 作用到刚体 $i$ 的力， $\boldsymbol{f}_i^x$为作用在刚体$i$上的外力。外力可以用来模拟包括力场、物理接触等在内的各种环境的影响。它们是算法的输入，即，它们的值应当是已知的。那么作用到刚体$i$上的力则是: $$ \boldsymbol{f}_i^B = \boldsymbol{f}_i + \boldsymbol{f}_i^x - \sum_{j \in \mu(i)} \boldsymbol{f}_j $$ 整理之后可以写出关节力: $$ \begin{equation}\label{equa_5_10} \boldsymbol{f}_i = \boldsymbol{f}_i^B - \boldsymbol{f}_i^x + \sum_{j \in \mu(i)} \boldsymbol{f}_j \end{equation} $$

从 $N_B$ 到 $1$ 遍历 $i$ 连续迭代，就可以计算出运动树中每个关节的空间力。这个方程不需要初值，因为任何没有子节点的刚体其 $\mu(i)$ 都是空集。

计算了每个关节的空间力后，还需要计算关节处的广义力，可由下式给出(参考式(3.37)): $$ \begin{equation}\label{equa_5_11} \boldsymbol{\tau}_i = \boldsymbol{S}_i^T \boldsymbol{f}_i \end{equation} $$ 式(($\ref{equa_5_7}$)到式($\ref{equa_5_11}$) 以最简单的形式描述了迭代牛顿欧拉算法。外力可用于模拟各种环境影响，包括重力。然而，把均匀的重力场建模成基座的虚拟加速度(fictitious acceleration)更合适。为此，我们只需要将方程($\ref{equa_5_8}$)的初值替换为$\boldsymbol{a}_0 = -\boldsymbol{a}_g$即可，其中$\boldsymbol{a}_g$是重力加速度矢量。如果使用这个技巧，那么$\boldsymbol{a}_i$和$\boldsymbol{f}_i^B$的计算值将不再是刚体$i$的真实加速度和力，而是分别带有重力加速度和力矢量的偏移量。刚体$i$的重力为$\boldsymbol{I}_i\boldsymbol{a}_g$。

算法特征(Algorithm Features)

方程(($\ref{equa_5_7}$)和($\ref{equa_5_8}$) 涉及到数据从根节点到叶子节点传播的过程。方程($\ref{equa_5_9}$)和($\ref{equa_5_11}$) 描述了每个刚体的局部计算describe calculations that are local to each body。因此，递归牛顿欧拉算法是一种两遍算法，它先向外(从根到叶)遍历树，计算刚体的速度和加速度。然后向内(从叶到根)遍历，计算关节力。刚体力的计算可以放到第一次遍历过程中也可以放到第二次，通常第一次是更好的选择。

该算法不仅仅计算了$\boldsymbol{\tau}_i$。其他中间量，例如$\boldsymbol{v}_i$ 和 $\boldsymbol{f}_i$，有时也很有用。比如，一个机械工程师要选择一个具有足够强度的关节轴承，以承受运行期间的动态反作用力dynamic reaction forces。这些力都可以根据 $\boldsymbol{f}_i$ 给出。

通过将一些输入设置为零，我们可以计算动力学的一些特定组成部分。比如，把外力、关节速度和加速度项设置为零，但保留虚拟基座加速度，那么算法仅计算重力补偿项，即静态情况下能够平衡重力的项。这些术语常用于机器人控制系统。或者，我们将重力、加速度和外力项全部设置为零，但保持关节速度，则算法仅计算科氏力和离心力。如果十分关心计算效率，那么可以使用该算法的精简版本来执行这些专门的计算。比如，只需要计算重力补偿项时，只需要: $$ \begin{equation}\label{equa_5_12} \boldsymbol{f}_i = -\boldsymbol{I}_i \boldsymbol{a}_g + \sum_{j\in \mu(i)} \boldsymbol{f}_j \end{equation} $$ $$ \begin{equation}\label{equa_5_13} \boldsymbol{\tau}_i = \boldsymbol{S}_i^T \boldsymbol{f}_i \end{equation} $$ 要计算实际的重力，就用$\boldsymbol{I}_i \boldsymbol{a}_g$ 替代 $-\boldsymbol{I}_i \boldsymbol{a}_g$。

刚体坐标系下的方程(Equations in Body Coordinates)

方程($\ref{equa_5_7}$) 至($\ref{equa_5_11}$) 的算法不涉及任何特定的坐标系。实际上，如果与刚体 $i$ 相关的计算在刚体 $i$ 坐标系中进行，则该算法效果最佳。这是该算法的刚体坐标版本。现在我们把递归牛顿欧拉算法的方程转换到刚体坐标系下。变量 $\boldsymbol{v}_i, \boldsymbol{a}_i, \boldsymbol{S}_i, \boldsymbol{I}_i, \boldsymbol{f}_i, \boldsymbol{f}_i^B$ 现在都是在刚体$i$坐标系下的变量。注意系统模型已经提供了刚体坐标系下的$\boldsymbol{S}_i, \boldsymbol{I}_i$。方程($\ref{equa_5_7}$) 和 ($\ref{equa_5_8}$)写成刚体坐标系下为: $$ \begin{equation}\label{equa_5_14} \boldsymbol{v}_i = {}^i\boldsymbol{X}_{\lambda(i)} \boldsymbol{v}_{\lambda(i)} + \boldsymbol{S}_i \dot{\boldsymbol{q}}_i \qquad (\boldsymbol{v}_0 = \boldsymbol{0}) \end{equation} $$ $$ \begin{equation}\label{equa_5_15} \boldsymbol{a}_i = {}^i\boldsymbol{X}_{\lambda(i)} \boldsymbol{a}_{\lambda(i)} + \boldsymbol{S}_i \ddot{\boldsymbol{q}}_i + \mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i + \boldsymbol{v}_i \times \boldsymbol{S}_i \dot{\boldsymbol{q}}_i \qquad (\boldsymbol{a}_0 = -\boldsymbol{a}_g) \end{equation} $$ 与原始的方程相比，做了以下修改：在$\boldsymbol{v}_{\lambda(i)}$ 和 $\boldsymbol{a}_{\lambda(i)}$ 上应用了坐标变换${}^i\boldsymbol{X}_{\lambda(i)}$，将他们从刚体$\lambda(i)$的坐标系转换到刚体$i$的坐标系下。用矩阵$\mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i + \boldsymbol{v}_i \times \boldsymbol{S}_i \dot{\boldsymbol{q}}_i$替代矩阵$\dot{\boldsymbol{S}}_i$。其中$\mathring{\boldsymbol{S}}_i$是$\boldsymbol{S}_i$在刚体坐标系下的表观导数(apparent derivative)。我们还借此机会用 $\boldsymbol{a}_0 = −\boldsymbol{a}_g$ 代替了原来的初始加速度值，来模拟重力的影响。

关节的计算函数jcalc也可以支持 $\boldsymbol{S}_i, \boldsymbol{v}_{Ji} = \boldsymbol{S}_i \dot{\boldsymbol{q}}_i, \boldsymbol{c}_{Ji} = \mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i$ 在刚体$i$坐标系下的计算。因此，我们将式($\ref{equa_5_14}$,$\ref{equa_5_15}$)改写如下，以便利用这些数据: $$ \begin{align}\label{equa_5_16} \boldsymbol{v}_{Ji} & = \boldsymbol{S}_i \dot{\boldsymbol{q}}_i \\ \boldsymbol{c}_{Ji} & = \mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i \\ \boldsymbol{v}_{i} & = {}^i\boldsymbol{X}_{\lambda(i)} \boldsymbol{v}_{\lambda(i)} + \boldsymbol{v}_{Ji} \qquad (\boldsymbol{v}_0 = \boldsymbol{0}) \\ \boldsymbol{a}_{i} & = {}^i\boldsymbol{X}_{\lambda(i)} \boldsymbol{a}_{\lambda(i)} + \boldsymbol{S}_i \ddot{\boldsymbol{q}}_i + \boldsymbol{c}_{Ji} + \boldsymbol{v}_i \times \boldsymbol{v}_{Ji} \qquad (\boldsymbol{a}_0 = -\boldsymbol{a}_g) \end{align} $$ 在刚体坐标系版本下式($\ref{equa_5_9}$)和($\ref{equa_5_11}$)仍保持不变，但是式($\ref{equa_5_10}$)将变成: $$ \begin{equation}\label{equa_5_20} \boldsymbol{f}_i = \boldsymbol{f}_i^B - {}^i\boldsymbol{X}_0^* \boldsymbol{f}_i^x + \sum_{j \in \mu(i)} {}^i\boldsymbol{X}_j^* \boldsymbol{f}_j \end{equation} $$ 在这个方程中，我们假设外力全部来自系统外部，因此将他们都以绝对坐标系表示基座坐标系, 刚体0的坐标系。方程($\ref{equa_5_16}$)至(20)以及($\ref{equa_5_9}$)和($\ref{equa_5_11}$)就是递归牛顿欧拉算法的刚体坐标版本的方程。

算法细节(Algorithm Details)

表 5.1中给出了该算法的刚体坐标版本的伪代码，以及基础版本和刚体坐标版本的方程。请注意，刚体坐标版本中的符号 $\boldsymbol{v}_i, \boldsymbol{a}_i$ 等都是以刚体坐标表示。而基础版本中的相同符号表示抽象向量和坐标向量，他们都是在一个相同的坐标系下的。

表 5.1 递归牛顿欧拉方程和算法.
The recursive Newton-Euler equations and algorithm

从伪代码中的两个 for 循环可以清晰看出该算法的两遍循环。符号$\boldsymbol{X}_J, \boldsymbol{v}_J, \boldsymbol{c}_J$ 都是一些局部变量，它们在第一个循环的每次迭代时取得新值。如果树中包含一些关节，其速度变量不是位置变量的导数，就用符号$\boldsymbol{\alpha}_i,\dot{\boldsymbol{\alpha}}_i$替代其中的变量$\dot{\boldsymbol{q}}_i, \ddot{\boldsymbol{q}}_i$。如果树包含的环节明确与时间相关，那么 jcalc 将自动使用式(3.32)和 (3.44) 替代式($\ref{equa_5_16}$,17)，计算 $\boldsymbol{v}_J$ 和 $\boldsymbol{c}_J$。伪代码中的唯一变化是将当前时间作为第四个参数传递给 jcalc。唯一使用 ${}^i\boldsymbol{X}_0$ 的地方是将外力从基础坐标系转换到刚体坐标系下。如果没有外力，那么计算这些变换的 if 语句可以省略。

实现方程($\ref{equa_5_20}$)时，我们需要将$\mu(i)$下的计算结果转换到$\lambda(i)$下。将方程($\ref{equa_5_20}$) 写成伪代码的形式如下面左图所示。

然而，也可以以不同的顺序完成相同的计算，得到的结果是一样的，如上面右侧的伪代码所示。将此代码片段与算法的其余部分合并时，第一个循环的主体将成为算法中第一个循环主体的最后一行。

表 5.2 原始递归牛顿欧拉方程.
The original recursive Newton-Euler equations

5.4 原始版本 The Original Version

最早递归牛顿欧拉算法是 Luh et al. (1980a) 在一篇论文中提出的，后来由 Brady et al. (1982) 再次发表。最初算法的公式里面使用的是 3D 向量，与5.3节中描述的算法有着很大的差别。但是，这两个算法实际上基本相同，惟一的差别就是一个用空间加速度，另一个用的是传统的加速度(参见2.11)。

表5.2中给出了一个应用原始算法的特殊案例，其中所有的关节都是旋转关节。使用 3D 向量表示刚体动力学的一个缺点就是，公式对于关节类型敏感。在 Luh et al. (1980a) 中描述的算法分别对旋转和平动关节给出了公式，其完整形式类似下面的分类形式: $$ variable = \begin{cases} one expression & \text{if joint is revolute} \\ another expression & \text{if joint is prismatic} \end{cases} $$ 此表中 3D 向量符号的选择是本书与其他书所使用的符号之间的折衷。该表中的方程是针对无分支的运动树编写的，与其他地方描述该算法的方式一致。而把它们修改的在分支树上也可以工作，是一件简单的事情。

表 5.2中还给出了原算法中的 3D 向量，与表 5.1中算法所用的空间向量之间的对应关系。只有两个地方 3D 向量与空间向量中相应 3D 分量不匹配：线性加速度和刚体力矩(net body moment)。后者的差异个微不足道，如果使用 $\boldsymbol{N}_i$ 计算 $\boldsymbol{n}_i$ 时，该差异会立即得到纠正。这是因为 $N_i$ 参考的是质心而不是刚体坐标系原点计算的。前者的差异影响还是挺大的，因为加速度会从一个刚体传播到下一个刚体。脚注 1: 根据第 2.11 节，经典加速度是坐标系中空间速度的表观导数，该坐标系的纯线速度等于原点处的刚体固定点的速度。鉴于此，可以将两种算法之间的差异解释如下：表5.1中的算法是在与当前时刻的刚体坐标系重合的静止坐标系中表达方程；而表 5.2 中的算法在一个非旋转坐标系中表达方程，该坐标系恰好与当前时刻的主体坐标系重合，并且具有与刚体坐标系原点相同的线速度。

原始算法的一个有趣的特点是，线速度从未在任何后续表达式中出现过，因此不需要计算。这是经典加速度的一个特殊性质。如果省略速度计算，那么该算法会比空间版本稍微高效一点。但是，差异只有几个百分点，并且很容易被其他与效率相关的问题掩盖掉，例如编程语言的选择、计算机硬件等。

5.5 背景材料 Additional Notes

出现在 Luh et al. (1980a) 中的算法就是我们通常认为的递归牛顿欧拉算法。然而，将牛顿欧拉运动方程与递归计算策略相结合的想法早于该算法几年。特别是，人们可以在 Stepanenko and Vukobratovic (1976) 以及 Orin et al.(1979) 的文章中看到递归牛顿欧拉算法的早期形式。此后该算法得到了完善和优化，其中一些优化将在第 10 章中介绍。 He and Goldenberg (1989) 以及 Balafoutis et al. (1988) 是我们找到的运行最快的两个版本，后者也出现在 Balafoutis and Patel (1991) 中。在这些算法被发明的时候，拉格朗日动力学比牛顿欧拉动力学更受欢迎。这促使 Hollerbach 设计了一种递归拉格朗日算法，并比较了各种递归和非递归算法的计算复杂度 (Hollerbach，1980)。这项工作将 $O(n^4)$ 复杂度的以 Uicker/Kahn 算法(Kahn 和 Roth，1971)为代表的非递归方法，与$O(n)$ 复杂度的递归算法进行了对比。

机器人界当时研究此类算法的目标是，开发一种足够快的逆动力学算法，可以用于机器人运动的实时控制。从 Luh et al. (1980b) 中可以找到一个早期的例子。后来操作空间的表述方法最终变得更加流行(Khatib，1987)。当时计算机的速度相对较慢，因此有一些研究人员探索了并行计算 (Lathrop，1985)，而其他研究人员则研究符号优化技术(Murray and Neuman，1984; Neuman and Murray，1987)。一个有趣的研究方向是开发有效的算法来计算逆动力学相对于设计参数的偏导数。 Fang and Pollard (2003) 就是一个例子。

Chapter5: 逆动力学Inverse Dynamics