Chapter6:正动力学:惯性矩阵法
Forward Dynamics: Inertia Matrix Methods

正动力学是给定作用力计算刚体系统的加速度。它主要用于仿真，有时被称为'直接动力学(direct dynamics)'或者简称'动力学(dynamics)'。本章和下一章中，我们介绍运动树的正动力学。闭环系统的动力学将在第 8 章中介绍。求解运动树的正动力学问题有两种主要的方案:① 对整个系统构建一个运动方程，并求解它得到加速度。② 将加速度沿着关节，从一个刚体传播到下一个刚体。本章中，我们讨论第一种方法，第 7 章讨论第二种方法。

第一种方法需要计算一个 $n \times n$ 的关节空间惯性矩阵，然后分解该矩阵，求解 $n$ 个关于加速度变量的线性方程组。此类算法在最坏情况下具有 $O(n^3)$ 的复杂度，因此有时被称为 $O(n^3)$ 算法。实际上，将它们称为 $O(nd^2)$ 算法会更准确一些，其中 $d$ 是树的深度。第二种方法需要多次遍历运动树，每次遍历都对每个刚体执行固定数量的计算。这种方法的复杂度是 $O(n)$，因此更适合刚体数量较多的系统。如果刚体的数量较少，或者运动树的深度较小，那么 $O(nd^2)$ 算法计算速度可以媲美甚至稍微超过 $O(n)$ 算法。此外，$O(nd^2)$ 算法也是闭环动力学算法的重要组成部分。我们将在第 10 章中讨论这两种方法之间的效率权衡。

本章从如何使用关节空间惯性矩阵求解正向动力学问题开始。然后第 6.2 节介绍了复合刚体(composite-rigid-body)算法，这是这类惯性矩阵算法中最快的；第 6.3 节介绍算法最初是如何被发现和描述的。接下来的两节介绍分支引起的稀疏现象，并提出了两种利用这一现象的分解算法。这种稀疏性使得计算关节空间惯性矩阵的复杂度仅为 $O(nd)$ 且分解它的成本仅为 $O(nd^2)$。最后，6.6 节介绍正动力学问题的一些背景材料。

6.1 关节空间惯性矩阵 The Joint-Space Inertia Matrix

运动树的运动方程可以写成矩阵的形式: $$ \begin{equation}\label{equa_6_1} \boldsymbol{\tau} = \boldsymbol{H}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{f}^x) \end{equation} $$ 其中，$\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}, \boldsymbol{\tau}$ 分别是关节位置、速度、加速度和力，$\boldsymbol{f}^x$ 是外力， $\boldsymbol{H}$ 是关节空间惯性矩阵，$\boldsymbol{C}$是关节空间的偏置力。$\boldsymbol{H}$ 和 $\boldsymbol{C}$ 是运动方程的系数矩阵， $\boldsymbol{\tau}$ 和 $\ddot{\boldsymbol{q}}$ 是变量。这个方程中 $\boldsymbol{H}$ 是关于 $\boldsymbol{q}$ 的函数， $\boldsymbol{C}$ 则是关于 $\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{f}^x$ 的函数。这个方程中最有用的信息是，$\boldsymbol{H}$ 只与位置变量有关。如果系统处于静止的状态，并且除了 $\boldsymbol{\tau}$ 没有其它作用力，那么 $\boldsymbol{C} = \boldsymbol{0}$，上式可以简化为 $\boldsymbol{\tau} = \boldsymbol{H}\ddot{\boldsymbol{q}}$。

在正动力学问题中，$\boldsymbol{\tau}$ 是给定的，需要计算的是 $\ddot{\boldsymbol{q}}$。本章中描述的方法通过如下的过程求解问题:

计算 $\boldsymbol{C}$
计算 $\boldsymbol{H}$
求解方程 $\boldsymbol{H}\ddot{\boldsymbol{q}} = \boldsymbol{\tau} - \boldsymbol{C}$ 得到 $\ddot{\boldsymbol{q}}$。

这三步的计算复杂度分别是 $O(n), O(nd), O(nd^2)$，其中 $d$ 是树的深度，有 $d ≤ n$，因此最坏的情况下，步骤2,3 的复杂度是 $O(n^2), O(n^3)$。有一种极端的情况，$d$有一个定的上限，那么这三步的计算复杂度都是 $O(n)$。下面我们会看到，步骤1可以简单地通过一个逆动力学算法得到，所以本章主要考虑步骤2,3。

$\boldsymbol{H}$ 是一个对称的正定的 $n\times n$ 的矩阵。通常将其划分成 $N_B \times N_B$ 的矩阵块，对于一个运动树有 $N_B = N_J$。如果 $n_i$ 是关节 $i$ 的自由度，那么矩阵块 $\boldsymbol{H}_{ij}$ 的尺寸是 $n_i \times n_j$。如果 $p_i = \sum_{k=1}^i n_k$，那么第 $i$ 行的矩阵块block-row $i$包含行$p_{i-1} + 1$ 到行 $p_i$，并且第 $i$ 列的矩阵块block-column $j$包含列$p_{j-1} + 1$ 到列 $p_j$。物理意义上，$\boldsymbol{H}_{ij}$描述了关节 $j$ 的加速度和关节 $i$ 的力之间的关系。如果树中的每个关节都只有一个自由度，那么 $n = N_B$，$\boldsymbol{H}_{ij}$就是一个$1 \times 1$ 的矩阵。

假设我们有一个类似第 5 章中描述的计算逆动力学的函数，$\text{ID}$，完成如下计算: $$ \boldsymbol{\tau} = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}, \boldsymbol{f}^x) $$ 与式($\ref{equa_6_1}$)对比，我们可以得出: $$ \begin{equation}\label{equa_6_2} \boldsymbol{C} = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{0}, \boldsymbol{f}^x) \end{equation} $$ $$ \begin{equation}\label{equa_6_3} \boldsymbol{H}\ddot{\boldsymbol{q}} = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}, \boldsymbol{f}^x) - \boldsymbol{C} \end{equation} $$ 因此，调用一次函数 $\text{ID}$ 就可以求得 $\boldsymbol{C}$，矩阵$\boldsymbol{H}$ 与任意加速度向量的乘积，可以通过两次调用 $\text{ID}$ 的差得到。现在我们定义一个求差逆动力学differential inverse dynamics函数，记为$\text{ID}_\delta$，计算给定加速度时关节力的变化，其形式为: $$ \begin{equation}\label{equa_6_4} \text{ID}_\delta(model, \boldsymbol{q}, \ddot{\boldsymbol{q}}) = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}, \boldsymbol{f}^x) - \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{0}, \boldsymbol{f}^x) \end{equation} $$

表 6.1 计算 $\boldsymbol{\tau} = \boldsymbol{H}\ddot{\boldsymbol{q}}$ 的方程和算法.
Equations and algorithm to calculate $\boldsymbol{\tau} = \boldsymbol{H}\ddot{\boldsymbol{q}}$

根据这个方程，我们可以计算 $\boldsymbol{H}$ 中的第 $\alpha$ 列: $$ \boldsymbol{H}\boldsymbol{\delta}_\alpha = \text{ID}_\delta (model, \dot{\boldsymbol{q}}, \boldsymbol{\delta}_\alpha) $$ 其中，$\boldsymbol{\delta}_{\alpha}$是一个 n 维的坐标向量，只有其中第 $\alpha$ 个元素为 1，其它元素全为 0。将 $\alpha$ 从 1 遍历到 n 重复调用该式，我们就可以计算出 $\boldsymbol{H}$的一列。这一思想是 Walker and Orin (1982) 中描述的方法 1 和 2 的基础。

式($\ref{equa_6_4}$)右侧中有很多项都可以被约去。特别的，$\dot{\boldsymbol{q}}$ 和 $\boldsymbol{f}^x$ 就被约去了，所以左侧 $\text{ID}_\delta$ 的参数列表中没有列出这两项。因此在计算 $\text{ID}_\delta$ 时，我们有可能得到一个极简化的递归牛顿欧拉算法。相关的方程和代码已经在表6.1 中给出了。在这个伪代码中，省去了 $\boldsymbol{S}_i$ 和 ${}^i\boldsymbol{X}_{\lambda(i)}$ 的计算，因为它们可以通过计算 $\boldsymbol{C}$ 顺便得到。

虽然，表6.1中的算法简单、直接。但这并不是计算 $\boldsymbol{H}$ 效率最高的方法。该问题的最快算法是复合刚体算法，这是下一节的主题。

6.2 复合刚体算法 The Composite-Rigid-Body Algorithm

一棵运动树的动能是他的各个刚体的动能之和，即: $$ \begin{equation}\label{equa_6_5} T = \frac{1}{2} \sum_{k=1}^{N_B} \boldsymbol{v}_k^T \boldsymbol{I}_k \boldsymbol{v}_k \end{equation} $$ 其中刚体 $k$ 的速度为: $$ \begin{equation}\label{equa_6_6} \boldsymbol{v}_k = \sum_{i \in \kappa(k)} \boldsymbol{S}_i \dot{\boldsymbol{q}}_i \end{equation} $$ 其中，$\kappa(k)$ 是支撑刚体$k$的关节集合，即，从基座到刚体$k$的路径上的所有关节(参见§4.1.4 中，关于 $\kappa(i), v(i), \mu(i)$ 的描述)。将式($\ref{equa_6_6}$)代入式($\ref{equa_6_5}$)，有: $$ \begin{equation}\label{equa_6_7} T = \frac{1}{2} \sum_{k = 1}^{N_B} \sum_{i \in \kappa(k)} \sum_{j \in \kappa(k)} \dot{\boldsymbol{q}}_i^T \boldsymbol{S}_i^T \boldsymbol{I}_k \boldsymbol{S}_j \dot{\boldsymbol{q}}_j \end{equation} $$ 现在，上式是关于三元组 $i, j, k$ 的综合，其中 $i$ 和 $j$ 是支撑刚体 $k$ 的关节。因此，上式可以整理为: $$ \begin{equation}\label{equa_6_8} T = \frac{1}{2} \sum_{i = 1}^{N_B} \sum_{j = 1}^{N_B} \sum_{k \in v(i) \cap v(j)} \dot{\boldsymbol{q}}_i^T \boldsymbol{S}_i^T \boldsymbol{I}_k \boldsymbol{S}_j \dot{\boldsymbol{q}}_j \end{equation} $$ 其中，$v(i) \cap v(j)$ 关节 $i$ 和关节 $j$ 共同支撑的刚体集合。式($\ref{equa_6_8}$) 仍是遍历三元组 $i,j,k$ 的和，其中 $k$ 是关节 $i$ 和关节 $j$ 共同支撑的刚体，它与式($\ref{equa_6_7}$)等价。$v(i) \cap v(j)$ 可以表示为: $$ \begin{equation}\label{equa_6_9} v(i) \cap v(j) = \begin{cases} v(i) & \text{if } i \in v(j) \\ v(j) & \text{if } j \in v(i) \\ \emptyset & \text{otherwise} \end{cases} \end{equation} $$ 其中，$\emptyset$ 表示空集。关节空间惯性矩阵可以用来写出运动树的动能公式: $$ \begin{equation}\label{equa_6_10} T = \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{H} \dot{\boldsymbol{q}} = \frac{1}{2} \sum_{i=1}^{N_B} \sum_{j=1}^{N_B} \dot{\boldsymbol{q}}_i^T \boldsymbol{H}_{ij} \dot{\boldsymbol{q}}_j \end{equation} $$ 联合式($\ref{equa_6_8}$)可以得出: $$ \begin{equation}\label{equa_6_11} \boldsymbol{H}_{ij} = \sum_{k \in v(i) \cap v(j)} \boldsymbol{S}_i^T \boldsymbol{I}_k \boldsymbol{S}_j \end{equation} $$ 现在我们引入一个新的量 $\boldsymbol{I}_i^c$ 用来表示以刚体 $i$ 为根节点的子树的惯性。我们把这棵子树整体看做是一个复合的刚体，这也是为啥该算法被称为复合刚体算法。 $\boldsymbol{I}_i^c$ 可以写成如下的形式: $$ \begin{equation}\label{equa_6_12} \boldsymbol{I}_i^c = \sum_{j \in v(i)} \boldsymbol{I}_j \end{equation} $$ 也可以写成递归的形式: $$ \begin{equation}\label{equa_6_13} \boldsymbol{I}_i^c = \boldsymbol{I}_i + \sum_{j \in \mu(i)} \boldsymbol{I}_j^c \end{equation} $$ 联合式($\ref{equa_6_11}$, $\ref{equa_6_12}$, $\ref{equa_6_9}$)，我们可以得到关节惯性矩阵的方程: $$ \begin{equation}\label{equa_6_14} \boldsymbol{H}_{ij} = \begin{cases} \boldsymbol{S}_i^T \boldsymbol{I}_i^c \boldsymbol{S}_j & \text{if } i \in v(j) \\ \boldsymbol{S}_i^T \boldsymbol{I}_j^c \boldsymbol{S}_j & \text{if } j \in v(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 公式($\ref{equa_6_13}$, $\ref{equa_6_14}$)是组合刚体算法的基础方程。对于无分支树的特殊情况，可以简化为: $$ \begin{equation}\label{equa_6_15} \boldsymbol{I}_i^c = \boldsymbol{I}_i + \boldsymbol{I}_{i+1}^c \end{equation} $$ $$ \begin{equation}\label{equa_6_16} \boldsymbol{H}_{ij} = \boldsymbol{S}_i^T \boldsymbol{I}_{max(i,j)}^c \boldsymbol{S}_j \end{equation} $$

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

在刚体坐标系下，式($\ref{equa_6_13}$)可以写作: $$ \begin{equation}\label{equa_6_17} \boldsymbol{I}_i^c = \boldsymbol{I}_i + \sum_{j \in \mu(i)} {}^i\boldsymbol{X}_j^* \boldsymbol{I}_j^c {}^j\boldsymbol{X}_i \end{equation} $$ 原理上，式($\ref{equa_6_14}$)也可以写成刚体坐标系下: $$ \begin{equation}\label{equa_6_18} \boldsymbol{H}_{ij} = \begin{cases} \boldsymbol{S}_i^T \cdot \boldsymbol{I}_i^c \cdot {}^i\boldsymbol{X}_j \cdot \boldsymbol{S}_j & \text{if } i \in v(j) \\ \boldsymbol{S}_i^T \cdot {}^i\boldsymbol{X}_j^* \cdot \boldsymbol{I}_j^c \cdot \boldsymbol{S}_j & \text{if } j \in v(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 给这个公式设计一个高效的计算策略，我们还需要定义一个新的量，${}^{j}\boldsymbol{F}_i$。它是刚体$j$坐标系下 $\boldsymbol{I}_i^c \boldsymbol{S}_i$ 的值，即，${}^{j}\boldsymbol{F}_i = {}^j\boldsymbol{X}_i^* \boldsymbol{I}_i^c \boldsymbol{S}_i$。计算 $\boldsymbol{H}$ 时，我们需要为每个满足 $j \in k(i)$ 的 $i,j$ 对计算 ${}^{j}\boldsymbol{F}_i$。它们可以通过如下公式递归地计算得到: $$ \begin{equation}\label{equa_6_19} {}^{\lambda(j)}\boldsymbol{F}_i = {}^{\lambda(j)} \boldsymbol{X}_j^* \cdot {}^j\boldsymbol{F}_i, \qquad \left({}^i\boldsymbol{F}_i = \boldsymbol{I}_i^c \boldsymbol{S}_i\right) \end{equation} $$ $i$ 需要遍历从 1 到 $N_B$ 的所有值，针对每个 $i$，$j$ 则需要通过 $i, \lambda(i), \lambda(\lambda(i)), \cdots$ 一直回溯到根节点。关节空间惯性矩阵可以表示成: $$ \begin{equation}\label{equa_6_20} \boldsymbol{H}_{ij} = \begin{cases} {}^j\boldsymbol{F}_i^T \cdot \boldsymbol{S}_j & \text{if } i \in v(j) \\ \boldsymbol{H}_{ji}^T & \text{if } j \in v(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 式($\ref{equa_6_17}$, $\ref{equa_6_19}$, $\ref{equa_6_20}$) 是在刚体坐标系下的复合刚体算法。

表 6.2 聚合刚体算法.
The composite rigid body equations and algorithm

算法细节 Algorithm Details

表6.2 中显示了复合刚体算法的刚体坐标版本的伪代码，及其基本方程和刚体坐标算法的方程。基础版本中的符号$\boldsymbol{I}_i, \boldsymbol{I}_i^c, \boldsymbol{S}_i$ 抽象张量或在未指定的公共坐标系中表达的张量，而刚体坐标方程中用相同的符号表示刚体$i$坐标系下的量。伪代码中没有给出计算 $\boldsymbol{S}_i, {}^i\boldsymbol{X}_{\lambda(i)}$ 的代码，因为这些量可以通过递归牛顿欧拉算法，在计算公式($\ref{equa_6_1}$) 中的 $\boldsymbol{C}$ 时同时得到。

在实现方程($\ref{equa_6_17}$)时，我们将其从计算 $\mu(i)$ 转换为计算 $\lambda(i)$。可以将方程($\ref{equa_6_17}$)直译为下面左侧的伪代码：

在不改变计算结果的情况下，我们可以调整一些计算的顺序，如上面右侧的代码片段所示。该算法有两个准备的环节，将 $\boldsymbol{H}$ 设置为 0，并将每个 $\boldsymbol{I}_i^c$ 设置为 $\boldsymbol{I}_i$。置 $\boldsymbol{H}$ 为 0，是为了提醒读者，代码的其余部分将仅处理 $\boldsymbol{H}$ 中的非零子矩阵。这个环节并不是必须的，取决于如何使用矩阵 $\boldsymbol{H}$。我们在一个循环中实现方程($\ref{equa_6_17}$, $\ref{equa_6_19}$, $\ref{equa_6_20}$)。局部变量 $\boldsymbol{F}$ 被初始化为 $\boldsymbol{I}_i^c \boldsymbol{S}_i$，就可以得到方程中的 ${}^i\boldsymbol{F}_i$。在 while 循环中从刚体 $i$ 到基座，遍历每个$j \in \kappa(i)$，将这个变量 $\boldsymbol{F}$ 逐个进行坐标变换，计算子矩阵 $\boldsymbol{H}_{i,j}$ 和 $\boldsymbol{H}_{ji}$。注意，只有访问到上三角对应的矩阵块，才需要设置 $\boldsymbol{H}_{ji}$。

图 6.1. 加速度 $\dot{\boldsymbol{q}} = \boldsymbol{\delta}_\alpha$的运动树。

如果 $d$) 是运动树的深度，则对于任何 $i$ 值，while 循环的执行次数一定不超过 $d − 1$ 次。因此，复合刚体算法的计算复杂度不能超过 $O(nd)$，并且 $\boldsymbol{H}$ 中非零元素的数量最多为 $O(nd)$。详细的计算复杂度分析见§10.3.2。

6.3 物理解释 A Physical Interpretation

本节我们介绍复合刚体算法的另一种推导方式，它更接近原始版本，目的是展示促使在该算法出现的一些物理内涵。

正如我们之前提到的，如果运动树是静止的，并且除了 $\boldsymbol{\tau}$ 之外没有其它力作用，那么运动方程可以简化为 $\boldsymbol{\tau} = \boldsymbol{H}\ddot{\boldsymbol{q}}$。在这些条件下，如果我们置 $\ddot{\boldsymbol{q}} = \boldsymbol{\delta}_\alpha$，那么 $\boldsymbol{\tau}$将是 $\boldsymbol{H}$ 中的第 $\alpha$ 列。图 6.1中描述了当 $\dot{\boldsymbol{q}} = \boldsymbol{\delta}_\alpha$ 时的运动树。$\ddot{\boldsymbol{q}}$ 中第 $\alpha$ 个元素对应着一个关节。我们称它拥有这个变量，称之为关节$i$。类似的，我们将 $\boldsymbol{S}_i$ 中对应第 $\alpha$ 个变量的那一列记为 $\boldsymbol{s}_\alpha$，这是一个空间运动向量。这棵树的关节 $i$ 上的加速度为 $\boldsymbol{s}_\alpha$，其他所有关节上的加速度为零。因此，由关节 $i$ 支撑的子树中每个物体的加速度为 $\boldsymbol{s}_\alpha$，并且所有其他物体的加速度为零。即: $$ \begin{equation}\label{equa_6_21} \alpha_j = \begin{cases} \boldsymbol{s}_\alpha & \text{if } j \in v(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 记 $\boldsymbol{f}_j^B$ 和 $\boldsymbol{f}_j$ 分别是作用在刚体 $j$ 上的力和通过关节$j$传导的到刚体上的力。因为没有速度项，所以 $\boldsymbol{f}_j^B$ 可以按照下式计算: $$ \begin{equation}\label{equa_6_22} \boldsymbol{f}_j^B = \boldsymbol{I}_j \boldsymbol{a}_j = \begin{cases} \boldsymbol{I}_j \boldsymbol{s}_\alpha & \text{if } j \in v(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 现在，关节 $j$ 是子树 $ν(j)$ 与系统其余部分之间的唯一联系。由于除了通过关节传递的力之外，没有任何力作用在物体上，因此 $\boldsymbol{f}_j$ 一定是作用在子树 $ν(j)$ 上的力。此外，子树上的力也是子树中刚体上的力的总和，所以我们有 $$ \begin{equation}\label{equa_6_23} \boldsymbol{f}_j = \sum_{k \in v(j)} \boldsymbol{f}_k^B \end{equation} $$ 联立这两个式子，有: $$ \boldsymbol{f}_j = \begin{cases} \sum_{k \in v(j)} \boldsymbol{I}_k \boldsymbol{s}_\alpha & \text{if } j \in v(i) \\ \sum_{k \in v(i)} \boldsymbol{I}_k \boldsymbol{s}_\alpha & \text{if } j \in k(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} $$ 简化为: $$ \begin{equation}\label{equa_6_24} \boldsymbol{f}_j = \begin{cases} \boldsymbol{I}_j^c \boldsymbol{s}_\alpha & \text{if } j \in v(i) \\ \boldsymbol{I}_i^c \boldsymbol{s}_\alpha & \text{if } j \in k(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 参考式($\ref{equa_6_12}$)，最终，我们可以通过下式计算关节力: $$ \begin{equation}\label{equa_6_25} \boldsymbol{\tau}_j = \boldsymbol{S}_j^T \boldsymbol{f}_j = \begin{cases} \boldsymbol{S}_j^T \boldsymbol{I}_j^c \boldsymbol{s}_\alpha & \text{if } j \in v(i) \\ \boldsymbol{S}_j^T \boldsymbol{I}_i^c \boldsymbol{s}_\alpha & \text{if } j \in k(i) \\ \boldsymbol{0} & \text{otherwise} \end{cases} \end{equation} $$ 这正是矩阵$\boldsymbol{H}$ 中 $\alpha$ 列的子向量。式($\ref{equa_6_24}$)中的第2中情况，揭示了发现复合刚体算法的两个物理内涵: ① $\boldsymbol{f}_i$ 是关节运动向量与复合刚体惯量的乘积。② 如图 6.1中深灰色的关节所示，对于所有关节 $j \in \kappa(i)$ 都有 $\boldsymbol{f}_j = \boldsymbol{f}_i$。因此该算法由如下四个部分构成:

通过式($\ref{equa_6_17}$) 计算复合刚体的惯量。
在刚体 $i$ 的坐标系下计算 $\boldsymbol{f}_i = \boldsymbol{I}_i^c \boldsymbol{s}_\alpha$。
将 $\boldsymbol{f}_i$ 转换到 $\kappa(i)$ 中各个刚体的坐标系下。
计算 $\boldsymbol{H}$ 中主对角线上方 $\alpha$ 列部分的元素，并将其复制到主对角线下方 $\alpha$ 行对应部分，由于 $\boldsymbol{H}$ 矩阵对称，两者相同。

该算法与6.2 节中的算法之间的唯一区别是，后者以块的形式处理列，而不是单独处理列，一个块是属于单个关节的所有列。如果关节 $i$ 有 $n_i$ 个自由度，那么方程($\ref{equa_6_19}$)中 ${}^i\boldsymbol{F}_i$ 只是一个 $6 \times n_i$ 的矩阵，它的列都是力 $\boldsymbol{f}_i = \boldsymbol{I}_i^c \boldsymbol{s}_\alpha$，其中 $\alpha$ 对应着关节 $i$。

图 6.2. 分支引起的稀疏性示例图。

6.4 分支引起的稀疏性 Branch-Induced Sparsity

式($\ref{equa_6_14}$)意味着，因为连通性的影响，$\boldsymbol{H}$ 中的一些元素都是 0。只要 $i$ 不是 $j$ 的祖先或者后代，也不等于 $j$，那么 $\boldsymbol{H}_{ij}$ 就等于0。只要 $i$ 和 $j$ 分别位于树的两个不同的分支上时，就会出现这种情况，我们称这种现象为分支引起的稀疏性 branch-induced sparsity。

图 6.2 中显示了一些简单的连接图及其产生的稀疏模式。示例 (a) 描述了一种极端情况，其中每个主体都直接连接到基座上，产生了最大可能数量的稀疏性。在这种情况下，每个非对角子矩阵都为零。更一般地，如果基座具有多个子节点，那么 $\boldsymbol{H}$ 将总是块对角阵(或其对称排列)。示例 (b) 描述了一种与(a)相反的极端情况，树没有分支，因此不存在分支引起的稀疏性。示例（c）描述了一种典型的稀疏模式；示例（d）和（e）描述了编号方案的影响，同一个图的两种不同编号产生不同的对称排列的稀疏性。本质上，(d)和(e)的两个矩阵具有相同的底层稀疏模式，编号的选择不会影响我们如何利用稀疏性。

通常情况下，分支引起的稀疏性会影响 $\boldsymbol{H}$ 的大部分元素。我们可以使用以下经验法则粗略估计其效益：如果 $\boldsymbol{H}$ 的密度为 $\rho$，那么计算它的成本大约是相同大小的稠密 $\boldsymbol{H}$ 的 $\rho$ 倍，而分解它的成本大约是相同大小的稠密矩阵的 $\rho^2$ 倍。 $\rho$ 被定义为 $\boldsymbol{H}$ 中非零矩阵块的比例，并且$0 < \rho ≤ 1$。 0.5 左右的密度并不罕见，接近于零的密度也是可能的。总体而言，分支引起的稀疏性，可以使惯性矩阵方法在分支运动树上，比在相同规模的无分支树上，更高效。

图 6.3. 因式分解 $\boldsymbol{H} = \boldsymbol{L}^T \boldsymbol{L}$ 保持稀疏性。

复合刚体算法本身已经利用了分支引起的稀疏性，只需计算 $\boldsymbol{H}$ 的非零子矩阵。如果我们尝试使用标准分解算法对惯性矩阵进行分解，这相当于将该之视为稠密矩阵，那么计算复杂度将是 $O(n^3)$。因此，我们希望找到一种分解算法，能够充分利用惯性矩阵的稀疏性。事实证明，这是一个容易解决的问题，解决方案是将 $\boldsymbol{H}$ 分解为 $\boldsymbol{L}^T\boldsymbol{L}$ 或 $\boldsymbol{L}^T\boldsymbol{DL}$ 的形式，并设计分解算法以跳过那些零矩阵块。

在稀疏矩阵文献中，分解 $\boldsymbol{H} = \boldsymbol{L}^T\boldsymbol{L}$ 被描述为重新排列的 Cholesky 分解，它相当于对原始矩阵重排列后，执行标准 Cholesky 分解(George and Liu，1981)。同样，分解 $\boldsymbol{H} = \boldsymbol{L}^T\boldsymbol{DL}$ 被描述为重新排列的 $\boldsymbol{LDL}^T$ 分解。这意味着 $\boldsymbol{L}^T\boldsymbol{L}$ 和 $\boldsymbol{L}^T\boldsymbol{DL}$ 分解具有与标准 Cholesky 和 $\boldsymbol{LDL}^T$ 分解相同的数值属性。

$\boldsymbol{L}^T\boldsymbol{L}$ 或 $\boldsymbol{L}^T\boldsymbol{DL}$分解的特性是，将其应用于包含分支引起稀疏性的矩阵时，无需填充 (fill-in) 操作。换句话说，矩阵中的每个分支引发的零元素在整个因式分解过程中都保持为零。(Featherstone (2005) 证明了这一点。)具有此属性的因式分解被认为是最优的(Duff et al., 1986)。一个直接的后果是 $\boldsymbol{H}$ 的稀疏模式保留在结果因子中，因此也是稀疏的。图 6.3 说明了这种效果，图中显示了原始模式和生成的三角因子中的稀疏模式。这种稀疏性也可以在回溯过程中被利用。无填充操作的第二个作用是，在因式分解过程中可以完全忽略分支引起的零块。

表 6.3 $\boldsymbol{L}^T\boldsymbol{L}$ 和 $\boldsymbol{L}^T\boldsymbol{DL}$ 分解算法.
$\boldsymbol{L}^T\boldsymbol{L}$ and $\boldsymbol{L}^T\boldsymbol{DL}$ factorization algorithms

6.5 稀疏分解算法 Sparse Factorization Algorithms

表 6.3 中给出了稀疏分解 $\boldsymbol{L}^T\boldsymbol{L}$ 和 $\boldsymbol{L}^T\boldsymbol{DL}$ 的算法伪代码。它们都以，一个 $n \times n$ 的矩阵 $\boldsymbol{H}$ 和一个有 $n$ 个元素的整数数组 $\lambda$，作为输入。 $\boldsymbol{H}$ 应该是对称且正定的，并且 $\lambda$ 的元素应该满足 $0 ≤ λ(i) < i$。此外，$\boldsymbol{H}$ 具有以下稀疏模式，可被算法最佳利用： 在 $\boldsymbol{H}$ 的每一行 $k$ 上，主对角线下方的非零元素仅出现在 $\lambda(k), \lambda(\lambda(k))$ 等列中。 此规则可以识别出那些被算法认为是非零的元素。所有其他元素均假定为零并被忽略。如果对于所有 $k$ 都有 $\lambda(k) = k − 1$，那么算法将每个元素都视为非零，在这种情况下，它们执行密集矩阵 $\boldsymbol{L}^T\boldsymbol{L}$ 或 $\boldsymbol{L}^T\boldsymbol{DL}$ 分解。

图 6.4. 因式分解过程示意图。

图 6.4 中显示了算法的工作原理。它们都在给定矩阵上进行原位操作，并且从不访问主对角线上方的任何元素。 $\boldsymbol{L}^T\boldsymbol{L}$ 算法计算 $\boldsymbol{L}$ 结果将通过 $\boldsymbol{H}$ 矩阵的的下三角返回。 $\boldsymbol{L}^T\boldsymbol{DL}$ 算法计算 $\boldsymbol{D}$ 和 $\boldsymbol{L}$，通过主对角线返回 $\boldsymbol{D}$，下三角返回 $\boldsymbol{L}$。之所以可以这样做，是因为该算法计算单位下三角矩阵，其对角线元素的值为 1，因此可以用主对角线返回矩阵 $\boldsymbol{D}$。

每个算法都有一个外层的循环，依次从 $n$ 到 1 访问每一行。在分解过程的任何阶段，第 $k + 1$ 到 $n$ 行都完成分解，并包含行 $k + 1$ 到 $n$ 的返回因子。第 1 行到第 $k$ 行可以分为三个区域，如图所示。区域 1 仅包含元素 $H_{kk}$；区域 2 由第 $k$ 行从 1 到 $k − 1$ 的元素构成；区域3由第 1 行到第 $k-1$ 行的三角形区域组成。每个区域需要进行不同的计算，如图所示。$\boldsymbol{L}^T\boldsymbol{L}$ 分解的伪代码在两个单独的循环中执行区域 2 和区域 3 计算；但是 $\boldsymbol{L}^T\boldsymbol{DL}$ 分解的伪代码以一种交错的方式计算，使得它在任何点都不需要记住多个 $H_{ki}'$ 值。当前记住的值存储在局部变量 $a$ 中。

内部循环设计为仅迭代 $\lambda(k), \lambda(\lambda(k)), \cdots$。这就是利用稀疏性的地方。实际上，算法知道零在哪里，并简单地跳过它们。图 6.4 通过显示图 6.3中矩阵 $\boldsymbol{H}$ 分解的前三个步骤来说明计算成本的降低。当 $k = 7$ 时，算法执行两批区域2计算和三批区域3的计算；类似的，$k = 6$ 和 $k = 5$ 时也这样。这远远少于没有零时所需的计算次数。

扩展父数组 Expanding the Parent Array

表 6.4 计算扩展父数组的算法和扩展连接图的示意图
Algorithm for calculating the expanded parent array, and illustration of the expanded connectivity graph

运动树的父数组中包含了 $N_B$ 个元素，但分解算法需要一个 $n$ 维的数组。如果 $n = N_B$ 则无需采取任何措施。否则，必须构造一个扩展的父数组以用于分解过程。

表 6.4 中介绍了执行此操作的算法，并说明了该算法的作用。我们从原始连接图开始，将每个多自由度关节替换为一串儿单自由度的关节。这会生成一个扩展图，其中每条边代表一个关节变量。这个新图被重新编号，使得边 $i$ 代表 $\ddot{\boldsymbol{q}}$ 中的第 $i$ 个变量。扩展的父数组就是根据该图计算的。表 6.4 中显示了运动树的原始连接图和扩展连接图，其中关节 2 具有三个自由度，其他每个关节只有一个自由度。因此，边 2 被三个边组成的链所取代，并且新树被重新编号，以便每条边的编号等于其对应的变量在 $\ddot{\boldsymbol{q}}$ 中的索引。由于关节 2 的变量占据 $\ddot{\boldsymbol{q}}$ 中的第2, 3, 4位，因此新边的编号必须为 2、3 和 4。所以原始图的父数组为 (0, 1, 1, 2, 2, 3);展开图的父数组为 (0, 1, 2, 3, 1, 4, 4, 5)。

表 6.4 中的算法直接从原始数组扩展父数组 $\lambda'$。首先，是将 $\lambda'$ 初始化为数组 $(0, 1, 2, \cdots, n−1)$。这个技巧正确地初始化了 $\lambda'$ 中的 $n − N_B$ 个元素，只留下需要从 $\lambda$ 计算的 $N_B$ 个元素。其次，在一个循环中构造一个映射数组，使得 $map(i) = \sum_{j=1}^i n_j$，其中 $n_j$ 是关节 $j$ 的自由度。关节 $i$ 的变量出现在 $\ddot{\boldsymbol{q}}$ 的 $map(i − 1) + 1$ 到 $map(i)$ 处。最后，由一个循环将两个映射合二为一，存储在 $\lambda(i)$ 中的值转换为 $map(\lambda(i))$，这正是要插入到 $\lambda'$ 中的值；这个值将被插入到 $\lambda'$ 的 $map(i − 1) + 1$ 处。对于表 6.4 中的示例，从零开始索引，映射数组为 $(0, 1, 4, 5, 6, 7, 8)$； $\lambda'$ 中的第二个元素在第一个循环中就被改为 $\lambda'(3)$ 和 $\lambda'(4)$，元素 $\lambda(1) \cdots \lambda(3)$ 被映射为 $\lambda'(1), \lambda'(2), \lambda'(5)$，而$\lambda(4) \cdots \lambda(6)$ 增加了 2 被映射为 $\lambda'(6) \cdots \lambda'(8)$。

表 6.5 乘法和 back-substitution 算法
Multiplication and back-substitution algorithms

计算扩展的父数组一次后，应将其存储在系统模型中供将来使用。

Back-Substitution

$\boldsymbol{H}$ 中的稀疏模式也出现在 $\boldsymbol{L}$ 中，因此可以使用与 $\boldsymbol{H}$ 相同的技术来利用 $\boldsymbol{L}$ 中的稀疏性。表 6.5 给出了计算向量的 $\boldsymbol{Lx}, \boldsymbol{L}^T\boldsymbol{x}, \boldsymbol{L}^{-1}\boldsymbol{x}, \boldsymbol{L}^{-T}\boldsymbol{x}$的算法，它们的输入只有 $\lambda, \boldsymbol{L}, \boldsymbol{x}$。Back-substitution 使用 $\boldsymbol{L}^{-1}\boldsymbol{x}$ 和 $\boldsymbol{L}^{-T}\boldsymbol{x}$。该表还提供了计算 $\boldsymbol{Hx}$ 的算法。这些计算通过让内部循环仅迭代 $\boldsymbol{L}$ 或 $\boldsymbol{H}$ 的非零元素来利用稀疏性。算法假设 $\boldsymbol{L}$ 是一般下三角因子，例如由 $\boldsymbol{L}^T\boldsymbol{L}$ 分解产生的因子。要修改它们以使用单位下三角因子，只需将 $L_{ii}$ 替换为 1。

计算 $\boldsymbol{Lx}, \boldsymbol{L}^T\boldsymbol{x}, \boldsymbol{Hx}$ 的算法将计算结果保存在单独的向量 $\boldsymbol{y}$ 中，并保持 $\boldsymbol{x}$ 不变。 $\boldsymbol{Lx}$ 算法允许 $\boldsymbol{y}$ 和 $\boldsymbol{x}$ 是同一个向量，我们称这种方式为原地计算in situ，但是另外两个向量的计算要求 $\boldsymbol{y} \neq \boldsymbol{x}$。计算 $\boldsymbol{L}^{-1}\boldsymbol{x}, \boldsymbol{L}^{-T}\boldsymbol{x}$ 的算法都是原地计算的。要实现 $\boldsymbol{y} = \boldsymbol{L}^{−1}\boldsymbol{x}$ 或 $\boldsymbol{y} = \boldsymbol{L}^{−T}\boldsymbol{x}$，只需将 $\boldsymbol{x}$ 复制到 $y$，并将算法应用于 $\boldsymbol{y}$。

表 6.6 稀疏算法的计算复杂度
Computational cost of sparse algorithms

计算代价 Computational Cost

表6.6中给出了每种算法的计算复杂度。其中符号$\mathsf{m}, \mathsf{a}, \mathsf{d}, √$ 分别表示浮点乘法、加法、除法以及平方根的计算。变量 $D_1, D_2$ 与 $\boldsymbol{H}$ 的稀疏模式有关:

$$ D_1 = \sum_{k=1}^n \left(d_k - 1\right) \qquad D_2 = \sum_{k=1}^n \frac{d_k\left(d_k - 1\right)}{2} $$

其中，$d_k$ 是从刚体 $k$ 到基座的路径上的节点数，即 $d_k = \left|\kappa(k)\right|$，可以视为刚体 $k$ 在树中的深度。表达式 $d_k − 1$ 和 $d_k(d_ k − 1)/2$ 统计第 $k$ 行进行区域 2 和区域 3 计算的次数。因此$D_1, D_2$ 分别统计了区域2和区域3的计算总数。 $D_1$ 也是主对角线下方非零元素的总数。因此，$\boldsymbol{H}$ 中非零元素的数量为 $n + 2D_1$。如果不存在分支引起的稀疏性，则 $D_1$ 和 $D_2$ 取其最大可能值，即 $$ D_1 = \left(n^2 - n\right) / 2 \qquad D_2 = \left(n^3 - n\right) / 6 $$ 将这些值代入表 6.6 中的公式可生成与标准 Cholesky 和 $\boldsymbol{LDL}^T$ 分解算法相同的计算复杂度。如果 $d$ 表示树的深度，那么 $D_1$ 和 $D_2$ 的边界为: $$ D_1 ≤ n(d - 1) \qquad D_2 ≤ nd(d-1)/2 $$ 这些上界说明在最坏的情况下分解过程的计算复杂度是 $O(nd^2)$。

6.6 背景材料 Additional Notes

Uicker (1965, 1967) 是最早的动力学算法之一。它将 Denavit and Hartenberg (1955) 提出的 $4 \times 4$ 运动学矩阵与 $4 \times 4$ 伪惯性矩阵相结合，并使用拉格朗日技术获得闭环机构的运动方程。大约在同一时间，Hooker and Margulies (1965) 使用矢量(牛顿-欧拉)动力学，及其增广来构建卫星的自由浮动运动树的运动方程。到 20 世纪 70 年代中期，已经有多种计算机程序可用于计算各种刚体系统的正动力学，Paul (1975) 对这些早期工作做了精彩的综述。 ADAMS 中使用的稀疏矩阵公式(Orlandea et al.，1977) 是一个被证明特别成功的早期工作。最早的计算刚体动力学教科书就在此时出现(Wittenburg，1977)。它提出了一种构建面向计算的通用刚体系统模型，并计算其动力学的详细方法。

复合刚体算法最早作为方法 3 出现在 Walker and Orin (1982) 中。它提供了一种计算运动树关节空间惯性矩阵的新方法，该方法比以前的方法要快得多。随后在 Featherstone (1984, 1987), Balafoutis and Patel (1989, 1991); Lilly and Orin (1991); Lilly (1993); McMillan and Orin (1998) 中出现了基于该算法的基本思想的更高效的版本和变体。但是，直到 Featherstone (2005) 人们才认识到分支引起的稀疏现象。

正动力学传播方法的第一个例子出现在 Vereshchagin (1974) 中。这篇文章使用最小约束的高斯原理Gauss' principle of least constraint来建模动力学，并通过动态规划求解。本质上这篇文章的算法与铰接体算法 articulated-body algotithm 相同，但其重要意义直到铰接体算法发表后才被认识到。传播算法的下一个例子出现在 Armstrong (1979) 中，随后 Featherstone (1983) 中描述了铰接体算法的原型，然后在 Featherstone (1984, 1987) 中提出了完整版本。 Brandl et al. (1988); McMillan and Orin (1995); McMillan et al. (1995) 随后提出了更高效的版本。其他一些算法已被证明与铰接体算法几乎相同；例如，Rodriguez (1987); Rodriguez et al., (1991) 的正动力学算法和 Rosenthal (1990) 中的 $O(n)$ 算法。

从表面上看，惯性矩阵方法和传播方法似乎有很大不同。但是，人们发现了它们之间有趣的联系。 Rodriguez 等人利用卡尔曼滤波器理论，提出了关节空间惯性矩阵的两种因式分解：一种就是递归牛顿-欧拉算法，而另一种则则是铰接体算法的逆(Rodriguez，1987；Rodriguez et al.，1991)。 Ascher et al., (1997) 发现了另一个联系。从类似于式(3.18) 的方程开始，他们提出，可以通过将高斯消元法应用于系数矩阵的一种排列来获得关节空间惯性矩阵，而铰接体算法则通过将高斯消元法应用于系数矩阵的另一种排列来获得。

严格来说，惯性矩阵和传播方法只是计算运动树正动力学的三种方法中的两种。第三种方法是将各个刚体的运动方程与约束方程组合起来，形成一个大矩阵方程(如第 3 章所述)。这种方法对于闭环系统非常有用，但是当应用于运动树时，它并不比与惯性矩阵和传播方法更有竞争力。此类算法的示例可以在 Baraff (1996) 中找到。

Chapter6:正动力学:惯性矩阵法Forward Dynamics: Inertia Matrix Methods