Chapter8: 闭环系统
Closed Loop Systems

本章讨论有运动环的刚体系统的正逆动力学。运动环的存在使得动力学问题的难度上升了一个层级，需要新的公式，带来了新的问题，系统表现出了新的特性。特别是逆动力学，运动环的存在改变了它的自然特性。有两种主要的策略，用来对闭环系统建模，构建其运动方程:

从一个无约束的刚体开始，将所有的关节约束同时作用在它的上面。
从系统的生成树开始，应用闭环约束。

我们还可以有第三种选择，就是参照§7.5通过装配法来实现，在这里我们并不讨论这种方法。我们将在§8.13 中介绍方法 1，它将生成大的、稀疏矩阵方程。方法 2 是典型闭环系统的最佳选择，也是本章的主题。§8.10还会介绍一些闭环系统的特性， §8.11将介绍闭环系统函数的应用， §8.12介绍逆动力学。

8.1 运动方程 Equations of Motion

我们已经在第三章中介绍了，包括闭环系统在内的，一般刚体系统的运动方程。本节我们根据生成树推导闭环系统的运动方程。其过程可以总结如下:

制定闭环系统生成树的运动方程。
给运动方程添加一些项，用来表示闭环关节施加在生成树上的力。
制定一个运动学方程描述闭环关节作用在生成树上的运动约束。
组合这两个方程。

如果生成树有 $n$ 个自由度，而闭环关节引入了 $n^c$ 个约束，那么我们将需要 $n + n^c$ 个方程来求解系统的 $n + n^c$ 个未知数。对于一个有 $N_B$ 个刚体，$N_J$ 个关节并且有 $N_L = N_J - N_B$ 个运动闭环的刚体系统，我们假设已经建立了其生成树，并且按照规则对各个刚体和关节进行了编号。那么生成树关节数量 $n$ 与闭环约束的数量 $n^c$ 可以用如下式子表示: $$ \begin{equation}\label{equa_8_1} n = \sum_{i=1}^{N_B} n_i \qquad \text{and} \qquad n^c = \sum_{k = N_B + 1}^{N_J} n_k^c \end{equation} $$ 其中，$n_i$ 是树关节 $i$ 允许的自由度，$n_k^c$ 是闭环关节 $k$ 引入的约束数量。我们分别用索引变量 $i,k$ 来遍历生成树关节和闭环关节。更一般的，我们用如下的惯例来表示运动闭环相关的索引，$i$ 和 $j$ 用来遍历树关节和刚体，从 1 到 $N_B$ 取值；$l$用来遍历闭环，从 1 到 $N_L$ 取值； $k$ 表示闭环索引 $l$ 对应的闭环关节。根据我们的标准编码规则， $k,l$ 有关系 $k = l + N_B$，因此 $k$ 是从 $N_B + 1$ 到 $N_J$ 的区间取值。此后，如果$k,l$一起出现，都有 $k = l + N_B$。

生成树的运动方程可以写作: $$ \begin{equation}\label{equa_8_2} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} \end{equation} $$ 其中 $\ddot{\boldsymbol{q}}, \boldsymbol{\tau}$ 分别是生成树关节的加速度和力向量。闭环系统和它的生成树之间的位移差别是，闭环系统包含了闭环关节，而生成树则不包含。因此，闭环系统的运动方程就可以通过给式($\ref{equa_8_2}$)增加闭环关节作用在生成树上的力来得到。闭环系统的运动方程就可以写成: $$ \begin{equation}\label{equa_8_3} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} + \boldsymbol{\tau}^c + \boldsymbol{\tau}^a \end{equation} $$ 其中 $\boldsymbol{\tau}^c, \boldsymbol{\tau}^a$ 分贝考虑了闭环关节产生的约束力和主动力。主动力由闭环关节出的弹簧、阻尼、执行器等部件产生。如果一个关节没有这些力的作用，那么这样关节就是被动的，如果所有的闭环关节都是被动的，有 $\boldsymbol{\tau}^a = \boldsymbol{0}$。此时动力学计算软件可以省略 $\boldsymbol{\tau}^a$ 的计算。通常认为 $\boldsymbol{\tau}^a$ 是已知的，但是 $\boldsymbol{\tau}^c$ 是未知的，它可以写成一个关于 $n^c$ 个约束力变量的函数。因此，式($\ref{equa_8_3}$)描述的系统有 $n$ 个方程，包含了 $n + n^c$ 个未知数。因此我们还需要补充另外 $n^c$ 个方程才能求解。这些方程可以根据运动学约束写出。

闭环关节给生成树引入了一系列的运动学约束，它们可以写成一个矩阵方程的形式: $$ \begin{equation}\label{equa_8_4} \boldsymbol{K}\ddot{\boldsymbol{q}} = \boldsymbol{k} \end{equation} $$ 其中 $\boldsymbol{K}$ 是一个 $n^c \times n$ 的矩阵。我们说这个方程在加速度层面 acceleration level 上描述约束，因为它已经微分了很多次出现了加速度。 on the grounds that it has been differentiated a sufficient number of times for the acceleration variables to appear. 我们将在 §8.2 中推导 $\boldsymbol{K},\boldsymbol{k}$ 的表达式。因为 $\boldsymbol{\tau}^c$ 是引入的约束力，它们可以写成如下的形式: $$ \begin{equation}\label{equa_8_5} \boldsymbol{\tau}^c = \boldsymbol{K}^T \boldsymbol{\lambda} \end{equation} $$ 其中 $\boldsymbol{\lambda}$ 是一个有 $n^c$ 未知约束力变量的向量。我们将在§8.4 中介绍它。方程 ($\ref{equa_8_3}, \ref{equa_8_4}, \ref{equa_8_5}$) 现在可以集成到一个矩阵方程中，得到完整的闭环系统运动方程 $$ \begin{equation}\label{equa_8_6} \begin{bmatrix} \boldsymbol{H} & \boldsymbol{K}^T \\ \boldsymbol{K} & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{q}} \\ -\boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a \\ \boldsymbol{k} \end{bmatrix} \end{equation} $$ 这是一个有 $n + n^c$ 个关于 $n + n^c$ 个未知数的方程组。其系数矩阵是对称的，但不是正定的。如果该矩阵满秩，我们就可以从这个方程组中解出 $\ddot{\boldsymbol{q}}, \boldsymbol{\lambda}$。如果不满秩，那么 $\boldsymbol{\lambda}$ 中的一些元素将是不确定的，但是我们仍可以解出 $\ddot{\boldsymbol{q}}$。系数矩阵的秩是 $n + \text{rank}(\boldsymbol{K})$。

8.2 闭环约束方程 Loop Constraint Equations

现在我们从关节 $k$ 的速度 $\boldsymbol{v}_{Jk}$ 开始推导式($\ref{equa_8_4}$)中的闭环约束方程的系数公式。该速度可以写成: $$ \begin{equation}\label{equa_8_7} \boldsymbol{v}_{Jk} = \boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)} \end{equation} $$ 其中 $p(k), s(k)$ 分别是关节 $k$ 的前驱和后继刚体。一般情况下，关节 $k$ 引入的约束具有如下的形式: $$ \boldsymbol{T}_k^T \boldsymbol{v}_{Jk} = \boldsymbol{T}_k^T \boldsymbol{\sigma}_k $$ 其中$\boldsymbol{T}_k, \boldsymbol{\sigma}_k$分别表示关节$k$的约束力子空间和偏置速度，参见式(3.38)。为了简单起见，我们假设所有的闭环关节的 $\boldsymbol{\sigma}_k = \boldsymbol{0}$，所以约束方程可以简化为 $$ \begin{equation}\label{equa_8_8} \boldsymbol{T}_k^T \boldsymbol{v}_{Jk} = \boldsymbol{0} \end{equation} $$ 结合式($\ref{equa_8_7}, \ref{equa_8_8}$ 有速度约束方程: $$ \begin{equation}\label{equa_8_9} \boldsymbol{T}_k^T (\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)})= \boldsymbol{0} \end{equation} $$ 对其微分可以得到一个加速度约束方程: $$ \begin{equation}\label{equa_8_10} \boldsymbol{T}_k^T (\boldsymbol{a}_{s(k)} - \boldsymbol{a}_{p(k)}) + \dot{\boldsymbol{T}}_k^T (\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)}) = \boldsymbol{0} \end{equation} $$ 运动树中任何刚体的速度都可以写成$\dot{\boldsymbol{q}}$的函数 $$ \begin{equation}\label{equa_8_11} \boldsymbol{v}_i = \boldsymbol{J}_i \dot{\boldsymbol{q}} \end{equation} $$ 其中 $\boldsymbol{v}_i$ 是刚体 $i$ 的速度，$\boldsymbol{J}_i$ 是刚体 $i$ 的雅可比矩阵(参见例4.2)。 $\boldsymbol{J}_i$ 可以展开如下: $$ \begin{equation}\label{equa_8_12} \boldsymbol{J}_i = \begin{bmatrix} \epsilon_{i1}\boldsymbol{S}_1 & \epsilon_{i2}\boldsymbol{S}_2 & \cdots & \epsilon_{iN_B}\boldsymbol{S}_{N_B} \end{bmatrix} \end{equation} $$ 其中，所有支撑刚体 $i$ 的关节 $j$ 的系数 $\epsilon_{ij}$ 都取 1，即关节位于从基座到刚体的路径上。其它情况下 $\epsilon_{ij} = 0$，可以表示为: $$ \epsilon_{ij} = \begin{cases} 1 & \text{if } j \in \kappa(i) \\ 0 & \text{otherwise} \end{cases} $$ 同样，运动树中任何刚体的加速度可以用式($\ref{equa_8_11}$)的微分中的 $\ddot{\boldsymbol{q}}$ 表示: $$ \begin{equation}\label{equa_8_13} \boldsymbol{a}_i = \boldsymbol{J}_i \ddot{\boldsymbol{q}} + \dot{\boldsymbol{J}}_i \dot{\boldsymbol{q}} = \boldsymbol{J}_i \ddot{\boldsymbol{q}} + \boldsymbol{a}_i^{vp} \end{equation} $$ $\boldsymbol{a}_i^{vp}$ 是刚体$i$的速度积(velocity-product)加速度。如果所有树关节的加速度变量都是零，那么刚体 $i$ 的加速度就是$\boldsymbol{a}_i^{vp}$。它可以在计算 $\boldsymbol{C}$ 的同时得到，详细参加§8.6。结合式($\ref{equa_8_10}, \ref{equa_8_13}$)有: $$ \boldsymbol{T}_k^T (\boldsymbol{J}_{s(k)} - \boldsymbol{J}_{p(k)}) \ddot{\boldsymbol{q}} + \boldsymbol{T}_k^T (\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}) + \dot{\boldsymbol{T}}_k^T (\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)}) = \boldsymbol{0} $$ 我们可以简化为: $$ \begin{equation}\label{equa_8_14} \boldsymbol{T}_k^T (\boldsymbol{J}_{s(k)} - \boldsymbol{J}_{p(k)}) \ddot{\boldsymbol{q}} = \boldsymbol{k}_l \end{equation} $$ 其中 $$ \begin{equation}\label{equa_8_15} \boldsymbol{K}_l = - \boldsymbol{T}_k^T (\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}) - \dot{\boldsymbol{T}}_k^T (\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)}) \end{equation} $$ 式($\ref{equa_8_14}$)是关节 $k$ 给生成树引入的加速度约束，对于每一个运动闭环都有这样一个方程。把这些方程收集起来，有: $$ \begin{equation}\label{equa_8_16} \begin{bmatrix} \boldsymbol{T}_{N_B + 1}^T (\boldsymbol{J}_{s(N_B + 1)} - \boldsymbol{J}_{p(N_B + 1)}) \\ \vdots \\ \boldsymbol{T}_{N_J}^T (\boldsymbol{J}_{s(N_J)} - \boldsymbol{J}_{p(N_J)}) \\ \end{bmatrix} \ddot{\boldsymbol{q}} = \begin{bmatrix} \boldsymbol{k}_1 \\ \vdots \\ \boldsymbol{k}_{N_L} \end{bmatrix} \end{equation} $$ 对比式($\ref{equa_8_4}$)有: $$ \begin{equation}\label{equa_8_17} \boldsymbol{K} = \begin{bmatrix} \boldsymbol{T}_{N_B + 1}^T (\boldsymbol{J}_{s(N_B + 1)} - \boldsymbol{J}_{p(N_B + 1)}) \\ \vdots \\ \boldsymbol{T}_{N_J}^T (\boldsymbol{J}_{s(N_J)} - \boldsymbol{J}_{p(N_J)}) \\ \end{bmatrix} \qquad \text{and} \qquad \boldsymbol{k} = \begin{bmatrix} \boldsymbol{k}_1 \\ \vdots \\ \boldsymbol{k}_{N_L} \end{bmatrix} \end{equation} $$ 表达式 $\boldsymbol{J}_{s(k)} - \boldsymbol{J}_{p(k)}$ 定义了一个闭环雅可比loop Jacobian，它是一个将 $\dot{\boldsymbol{q}}$ 映射到闭环关节的速度上。maps $\dot{\boldsymbol{q}}$ to velocity across a loop-closing joint. 记 $\boldsymbol{J}_{Ll}$ 为闭环 $l$ 的闭环雅可比，有: $$ \begin{equation}\label{equa_8_18} \begin{array}{rcl} \boldsymbol{J}_{Ll} & = & \boldsymbol{J}_{s(k)} - \boldsymbol{J}_{p(k)} \\ & = & \begin{bmatrix}\varepsilon_{l1} \boldsymbol{S}_1 & \varepsilon_{l2}\boldsymbol{S}_2 & \cdots & \varepsilon_{lN_B}\boldsymbol{S}_{N_B} \end{bmatrix} \end{array} \end{equation} $$ 其中 $\varepsilon_{lj} = \epsilon_{s(k)j} - \epsilon_{p(k)j}$。这些数字标识了每个闭环的树关节。如果我们将闭环 $l$ 的根定义为编号最大的刚体，它是 $p(k)$ 和 $s(k)$ 的共同祖先，那么闭环 $l$ 涉及的树关节，就是两个子集的并集：将根连接到 $p(k)$ 的关节，以及将根连接到 $s(k)$ 的关节。第一个子集中的每个关节 $\varepsilon_{lj}$ 的值为 -1，第二个子集中的每个关节 $\varepsilon_{lj}$ 的值为 +1，其他所有关节 $\varepsilon_{lj}$ 的值为 0。使用式($\ref{equa_8_18}$)，我们可以写出 $\boldsymbol{K}$ 的另一种表达式: $$ \begin{equation}\label{equa_8_19} \boldsymbol{K} = \begin{bmatrix} \boldsymbol{K}_{11} & \cdots & \boldsymbol{K}_{1N_B} \\ \vdots & & \vdots \\ \boldsymbol{K}_{N_L 1} & \cdots & \boldsymbol{K}_{N_L N_B} \end{bmatrix} \end{equation} $$ 其中 $$ \begin{equation}\label{equa_8_20} \boldsymbol{K}_{lj} = \varepsilon_{lj} \boldsymbol{T}_k^T \boldsymbol{S}_j \end{equation} $$

8.3 约束稳定性 Constraint Stabilization

式($\ref{equa_8_16}$)理论上是正确的，但是存在数值积分所以并不稳定。问题是，理论上我们希望式($\ref{equa_8_16}$)表现的像是如下形式的微分方程: $$ \ddot{e} = 0 $$ 其中 $e$ 表示闭环闭包(loop-closure)位置误差，但实际上它的的表现是: $$ \ddot{e} = noise $$ 其中 noise 是代表数值误差的小幅度信号，例如数值积分过程中的截断误差。因此，使用方程($\ref{equa_8_16}$)可以确保加速度误差保持较小的值，但是积分过程中存在误差不断累积，就不能保证速度和位置的误差仍然很小。这个问题可以通过给式($\ref{equa_8_4}$)增加一个稳定项 $\boldsymbol{k}_{stab}$，有 $$ \begin{equation}\label{equa_8_21} \boldsymbol{K}\ddot{\boldsymbol{q}} = \boldsymbol{k} + \boldsymbol{k}_{stab} \end{equation} $$ 增加 $\boldsymbol{k}_{stab}$ 的目的是改变数值积分下约束方程的表现，使得它的行为类似如下的形式: $$ \ddot{e} + 2\alpha \dot{e} + \beta^2 e = noise $$ 其中 $\alpha > 0, \beta \neq 0$。这个方法是 Baumgarte 提出的，有时被称为 Baumgarte stabilization (Baumgarte, 1972)。稳定常数通常按照如下的方式选取: $$ \alpha = \beta = 1 / T_{stab} $$ 其中 $T_{stab}$是位置和速度误差衰减的时间常数。$\boldsymbol{k}_{stab}$ 定义如下: $$ \begin{equation}\label{equa_8_22} \boldsymbol{k}_{stab} = -2\alpha \begin{bmatrix} \boldsymbol{T}_{N_B + 1}^T \boldsymbol{v}_{JN_B+1} \\ \vdots \\ \boldsymbol{T}_{N_J}^T \boldsymbol{v}_{JN_J} \end{bmatrix} - \beta^2 \begin{bmatrix} \boldsymbol{\delta}_1 \\ \vdots \\ \boldsymbol{\delta}_{N_L} \end{bmatrix} \end{equation} $$ 其中 $$ \begin{equation}\label{equa_8_23} \boldsymbol{\delta}_l = \text{jcalc}(\text{jtype}(k), {}^{s(k),k}\boldsymbol{X}_{p(k),k}) \end{equation} $$ $\boldsymbol{\delta}_l$ 是一个指标，它描述的是生成树的位置打破由闭环关节 $k$ 引入的约束的数量。 is a measure of the degree to which the position of the spanning tree violates the constraint imposed by loop joint $k$. 计算它的式子是 $\boldsymbol{\delta}_l = \boldsymbol{T}_k^T\boldsymbol{d}_l$，其中$\boldsymbol{d}_l$是描述闭环 $l$ 周围位置误差的空间向量。这个向量只依赖于关节类型以及关节前驱相对于后继的变换。关节前驱相对于后继的变换可以计算如下: $$ \begin{equation}\label{equa_8_24} \begin{array}{rcl} {}^{s(k),k}\boldsymbol{X}_{p(k),k} & = & {}^{s(k),k}\boldsymbol{X}_{s(k)} \cdot {}^{s(k)}\boldsymbol{X}_{p(k)} \cdot {}^{p(k)}\boldsymbol{X}_{p(k),k} \\ & = & \boldsymbol{X}_{S}(l) \cdot {}^{s(k)}\boldsymbol{X}_{p(k)} \cdot \boldsymbol{X}_{P}(l)^{-1} \end{array} \end{equation} $$ 其中 $\boldsymbol{X}_P, \boldsymbol{X}_S$分别是存储在系统模型中的闭环关节的前驱和后继帧变换的数组。Baumgarte稳定化的一个问题在于缺少一个硬性且快速的规则来选择 $T_{stab}$。一个较为合理的策略是，问移动系统每秒需要多少帧才不会错过任何重要的事情，然后选择一个与帧采样周期接近或稍短的 $T_{stab}$。例如，如果系统是大型工业机器人，那么 $T_{stab} = 1$ 就太大了，$T_{stab} = 0.1$比较合理，$T_{stab} = 0.01$有点小，但仍然合理。个人理解，这里的工业机器人的控制频率应该在 10~50 Hz左右。如果 $T_{stab}$ 选择得太小，则微分方程会变得不必要的僵硬。$T_{stab}$ 的精确值并不重要，将其放大为 2 倍也只会产生很小的差异。

人们倾向于选择尽可能小的 $T_{stab}$，以便位置和速度误差尽快衰减，因为我们相信这将最大限度地提高模拟的准确性。实际上这是一个糟糕的策略。约束稳定的目的是实现稳定性，而不是精度。如果模拟不够准确，那么改进它的最佳方法是使用更好的积分方法和更短的积分时间步长。

计算$\boldsymbol{\delta}$

令 ${}^s\boldsymbol{X}_p$为闭环关节前驱帧到后继帧的坐标变换，它由生成树的位置变量决定。一般来说，这种变换不会完全符合闭环关节施加的运动约束。因此，我们将该矩阵分解如下: $$ \begin{equation}\label{equa_8_25} {}^s\boldsymbol{X}_p = \boldsymbol{X}_{err} \boldsymbol{X}_J \end{equation} $$

其中 $\boldsymbol{X}_J$完全符合关节约束，$\boldsymbol{X}_{err}$表示约束误差，并假设误差很小。这个分解必须通过符号运算求解，我们可以利用关节允许的运动类型的信息求解，以便获得由 ${}^s\boldsymbol{X}_p$ 表示的 $\boldsymbol{X}_{err}$。针对不同的关节类型，这个表达式也都不一样。若 $\boldsymbol{X}_{err}$ 表示一个小位移，则有一个运动向量 $\boldsymbol{d} \in M^6$，满足: $$ \begin{equation}\label{equa_8_26} \boldsymbol{1} - \boldsymbol{d}{\times} \simeq \boldsymbol{X}_{err} \end{equation} $$ 该向量大约是后继帧应在的位置到其实际位置的位移。获得 $\boldsymbol{d}$ 后，$\boldsymbol{\delta}$ 由下式给出 $$ \begin{equation}\label{equa_8_27} \boldsymbol{\delta} = \boldsymbol{T}^T\boldsymbol{d} \end{equation} $$ 表 8.1 中列举了一些常见的关节类型的 $\boldsymbol{\delta}$ 的计算公式。请注意，零自由度关节可以用作通用闭环关节，因为任何闭环系统都可以通过添加无质量体和零自由度关节来修改，这样每个闭环关节都是零自由度关节。

表 8.1 计算 $\boldsymbol{\delta} = \text{jcalc}(type, \boldsymbol{X})$.
Formulae for $\boldsymbol{\delta} = \text{jcalc}(type, \boldsymbol{X})$

Example 8.1: 表 8.1 中圆柱形关节的计算公式可以按照如下的方式得到。通过结合方程($\ref{equa_8_25}, \ref{equa_8_26}$)，并选择适合圆柱关节的$\boldsymbol{X}_J$ 和 $\boldsymbol{d}$ 表达式，可以得到如下的近似方程: $$ {}^s\boldsymbol{X}_p \simeq \begin{bmatrix} 1 & 0 & -d_2 & 0 & 0 & 0 \\ 0 & 1 & d_1 & 0 & 0 & 0 \\ d_2 & -d_1 & 1 & 0 & 0 & 0 \\ 0 & 0 & -d_5 & 1 & 0 & -d_2 \\ 0 & 0 & d_4 & 0 & 1 & d_1 \\ d_5 & -d_4 & 0 & d_2 & -d_1 & 1 \end{bmatrix} \begin{bmatrix} c & s & 0 & 0 & 0 & 0 \\ -s & c & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & z & 0 & c & s & 0 \\ -z & 0 & 0 & -s & c & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} $$ 其中，$c = \cos(\theta), s = \sin(\theta)$，关节的位置变量$\theta, z$分别是圆柱关节的角位移和线位移。请注意，这里我们设定 $d_3 = d_6 = 0$ 因为它们是 $\boldsymbol{d}$ 在关节运动自由度方向上的坐标。如果我们执行乘法并检查结果，我们会发现 $d_1 = ({}^sX_p)_{23}, d_2 = -({}^sX_p)_{13}$ 等，最后有 $\boldsymbol{\delta} = \begin{bmatrix} d_1 & d_2 & d_4 & d_5 \end{bmatrix}^T$。

8.4 闭环关节力 Loop Joint Forces

令 $\boldsymbol{f}_k$ 表示闭环系统通过闭环关节 $k$ 传导的力。关节 $k$ 对于生成树的作用就是，施加 $\boldsymbol{f}_k$ 到刚体 $s(k)$ 上，同时在刚体 $p(k)$ 上施加力 $-\boldsymbol{f}_k$。现在，如果有一个空间力 $\boldsymbol{f}$ 中用到一个运动树的第 $i$ 个刚体上，那么它对于树的作用与关节空间的力 $\boldsymbol{\tau}$ 一样。 $$ \boldsymbol{\tau} = \boldsymbol{J}_i^T \boldsymbol{f} $$ 因此关节 $k$ 对于生成树的作用等效于一个关节空间力: $$ \boldsymbol{\tau} = \left(\boldsymbol{J}_{s(k)}^T - \boldsymbol{J}_{p(k)}^T\right) \boldsymbol{f}_k = \boldsymbol{J}_{Ll}^T \boldsymbol{f}_k $$ 现在我们将 $\boldsymbol{f}_k$ 分解成主动力 $\boldsymbol{f}_k^a$和约束力 $\boldsymbol{f}_k^c$，并分别考虑它们的作用。如果 $\boldsymbol{\tau}^a$ 表示生成树中所有主动力作用在闭环关节上的 net effect，那么 $\boldsymbol{\tau}^a$ 可以写作: $$ \begin{equation}\label{equa_8_28} \boldsymbol{\tau}^a = \sum_{l=1}^{N_L} \boldsymbol{J}_{Ll}^T \boldsymbol{f}_k^a \end{equation} $$ 类似的，$\boldsymbol{\tau}^c$ 表示作用在闭环关节上的所有约束力的 net effect: $$ \begin{equation}\label{equa_8_29} \boldsymbol{\tau}^c = \sum_{l=1}^{N_L} \boldsymbol{J}_{Ll}^T \boldsymbol{f}_k^c \end{equation} $$ 而关节$k$的约束力也可以表示为如下形式: $$ \begin{equation}\label{equa_8_30} \boldsymbol{f}_k^c = \boldsymbol{T}_k \boldsymbol{\lambda}_k \end{equation} $$ 其中$\boldsymbol{\lambda}_k$是一个$n_k^c$个位置约束力变量的向量(参考式3.34)，所以 $\boldsymbol{\tau}^c$ 也可以写成: $$ \begin{equation}\label{equa_8_31} \boldsymbol{\tau}^c = \sum_{l=1}^{N_L} \boldsymbol{J}_{Ll}^T \boldsymbol{T}_k \boldsymbol{\lambda}_k \end{equation} $$ 将这个方程与式($\ref{equa_8_17}$)中 $\boldsymbol{K}$ 的定义式结合，有: $$ \begin{equation}\label{equa_8_32} \boldsymbol{\tau}^c = \boldsymbol{K}^T\boldsymbol{\lambda} \end{equation} $$ 其中 $$ \boldsymbol{\lambda} = \begin{bmatrix} \boldsymbol{\lambda}_{N_B + 1}^T & \cdots & \boldsymbol{\lambda}_{N_J}^T \end{bmatrix}^T $$ 正如之前提到的，我们假设主动力式已知的。将 $\boldsymbol{\tau}^a$ 纳入运动方程的最好办法是修改算法，从计算 $\boldsymbol{C}$ 变成计算$\boldsymbol{C} - \boldsymbol{\tau}^a$。我们将在§8.6 中详细介绍该算法。

8.5 运动方程的求解 Solving the Equations of Motion

现在我们已经写出了运动方程中的每一项，接下来求解树关节的加速度。受闭环力作用的生成树运动方程为: $$ \begin{equation}\label{equa_8_34} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} + \boldsymbol{K}^T\boldsymbol{\lambda} + \boldsymbol{\tau}^a \end{equation} $$ 闭环约束方程: $$ \begin{equation}\label{equa_8_35} \boldsymbol{K}\ddot{\boldsymbol{q}} = \boldsymbol{k} + \boldsymbol{k}_{stab} \end{equation} $$ 这两个方程可以合并成: $$ \begin{equation}\label{equa_8_36} \begin{bmatrix} \boldsymbol{H} & \boldsymbol{K}^T \\ \boldsymbol{K} & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{q}} \\ -\boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a \\ \boldsymbol{k} + \boldsymbol{k}_{stab} \end{bmatrix} \end{equation} $$ 其中，$\boldsymbol{H}$ 是一个 $n \times n$ 的、对称的、正定矩阵。$\boldsymbol{K}$ 是一个 $n^c \times n$ 的矩阵，它可能不满秩。式($\ref{equa_8_36}$)的系数矩阵也是对称的，其尺寸是 $(n + n^c) \times (n + n^c)$。但它不是正定的，因为它的秩是$n+r$，其中$r = \text{rank}(\boldsymbol{K})$，所以只要$r < n^c$，该系数矩阵就是奇异的。

如果 $r = n^c$，那么式($\ref{equa_8_36}$) 可以唯一确定 $\ddot{\boldsymbol{q}}, \boldsymbol{\lambda}$。如果 $r < n^c$，那么闭环关节引入的 $n^c -r$ 个约束线性依赖于其它 $r$ 个约束。其后果就是，$\lambda$ 中的一些元素是不确定的，但是$\ddot{\boldsymbol{q}}$仍然可以由式($\ref{equa_8_36}$)唯一确定。$r < n^c$还会产生一个问题，式($\ref{equa_8_35}$) 会有些轻微的不一致，即 $\boldsymbol{k} + \boldsymbol{k}_{stab}$ 可能比 $\text{range}(\boldsymbol{K})$ 略大一点。这是计算 $\boldsymbol{K}, \boldsymbol{k}, \boldsymbol{k}_{stab}$ 时的数值误差，以及位置和速度的闭环约束不能完全满足，导致的。式($\ref{equa_8_35}$) 存在轻微不一致的情况是，我们可以用最小二乘的技术求解它。

我们有很多种方法求解上述方程获得 $\ddot{\boldsymbol{q}}$。这里我们主要考虑如下三种方法:

直接求解式($\ref{equa_8_36}$)。
先通过式($\ref{equa_8_36}$)解出 $\boldsymbol{\lambda}$，在用它来求解 $\ddot{\boldsymbol{q}}$。
求解式($\ref{equa_8_35}$)，用独立的加速度变量的向量表示$\ddot{\boldsymbol{q}}$，然后求解方程($\ref{equa_8_24}$)。

方法1:直接求解式($\ref{equa_8_36}$)

如果方程($\ref{equa_8_36}$)的系数矩阵是非奇异的，那么我们就可以通过一般线性方程组求解器，或者针对对称的不定矩阵的求解器，直接求解该方程。这是最简单的求解方法，但不是最高效的，因此该方法只使用与那些计算效率没那么重要的场合。如果 $r = n^c$，或者在并入式($\ref{equa_8_36}$)之前， $n^c - r$的线性相关行可以从式($\ref{equa_8_35}$)中约简掉，那么系数矩阵就是非奇异的。需要将 $n^c$ 约简到 $n^c = r$，约简的方法可以结合方法 2 和 3 一起使用。

方法2:先解出$\boldsymbol{\lambda}$

通过把式($\ref{equa_8_36}$)中第二行减去第一行的$\boldsymbol{KH}^{-1}$倍，我们就得到了一下形式的关于 $\boldsymbol{\lambda}$ 的方程: $$ \begin{equation}\label{equa_8_37} \boldsymbol{A\lambda} = \boldsymbol{b} \end{equation} $$ 其中 $$ \begin{equation}\label{equa_8_38} \boldsymbol{A} = \boldsymbol{K}\boldsymbol{H}^{-1}\boldsymbol{K}^T \end{equation} $$ $$ \begin{equation}\label{equa_8_39} \boldsymbol{b} = \boldsymbol{k} + \boldsymbol{k}_{stab} - \boldsymbol{K}\boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a) \end{equation} $$ 系数矩阵 $\boldsymbol{A}$ 是一个对称的半正定矩阵。它的尺寸是 $n^c \times n^c$，它的秩是 $r$。如果 $r = n^c$，那么 $\boldsymbol{A}$ 是可逆的，式($\ref{equa_8_37}$) 有唯一的解: $$ \begin{equation}\label{equa_8_40} \boldsymbol{\lambda} = \boldsymbol{A}^{-1}\boldsymbol{b} \end{equation} $$ 如果 $r < n^c$ 那么 $\boldsymbol{A}$ 是奇异的，并且方程($\ref{equa_8_37}$)有无穷多个解。此时方程的通解为: $$ \begin{equation}\label{equa_8_41} \boldsymbol{\lambda} = \boldsymbol{A}^+ \boldsymbol{b} + \left(\boldsymbol{1} - \boldsymbol{A}^+ \boldsymbol{A}\right)\boldsymbol{z} \end{equation} $$ 其中 $\boldsymbol{A}^+$ 是 $\boldsymbol{A}$ 的伪逆，$\boldsymbol{z}$ 是任意向量，算子 $\boldsymbol{1} - \boldsymbol{A}^+ \boldsymbol{A}$ 将向量投射到 $\boldsymbol{A}$ 的零空间中。如果式($\ref{equa_8_37}$)是一致的，那么式($\ref{equa_8_41}$)描述了一组精确的解，否则，它描述了一组最小二乘解。当且仅当式($\ref{equa_8_35}$)一致时，式($\ref{equa_8_37}$)才是一致的。求解出 $\boldsymbol{\lambda}$，最后一步就是将其代入方程($\ref{equa_8_34}$)中，求解所得方程就可以得到$\ddot{\boldsymbol{q}}$。注意 $\boldsymbol{z}$ 对 $\ddot{\boldsymbol{q}}$ 没有影响，因为: $$ \boldsymbol{K}^T(\boldsymbol{1} - \boldsymbol{A}^+\boldsymbol{A}) = \boldsymbol{0} $$ 因此，我们可以自由选择$\boldsymbol{z}$的取值，以方便计算，比如说取 $\boldsymbol{z} = \boldsymbol{0}$。下面的计算过程将以相对高效的方式计算 $\ddot{\boldsymbol{q}}$。它使用了§6.5 节中描述的 $\boldsymbol{L}^T\boldsymbol{L}$ 分解和反向替换算法，因此利用了 $\boldsymbol{H}$ 中可能存在的分支引起的稀疏性。它可以轻松修改为使用 $\boldsymbol{L}^T\boldsymbol{DL}$ 分解。

将$\boldsymbol{H}$分解为$\boldsymbol{H} = \boldsymbol{L}^T\boldsymbol{L}$。
计算 $\boldsymbol{\tau}' = \boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a$。
利用反向代入back-substitution，计算 $\boldsymbol{Y} = \boldsymbol{L}^{-1} \boldsymbol{K}^T$，和 $\boldsymbol{z} = \boldsymbol{L}^{-T}\boldsymbol{\tau}'$。
计算 $\boldsymbol{A} = \boldsymbol{Y}^T\boldsymbol{Y}$ 和 $\boldsymbol{b} = \boldsymbol{k} + \boldsymbol{k}_{stab} - \boldsymbol{Y}^T\boldsymbol{z}$。
求解 $\boldsymbol{A\lambda} = \boldsymbol{b}$ 获得 $\boldsymbol{\lambda}$。
求解 $\boldsymbol{H}\ddot{\boldsymbol{q}} = \boldsymbol{\tau}' + \boldsymbol{K}^T\boldsymbol{\lambda}$ 得到 $\ddot{\boldsymbol{q}}$。

方法3:先求解式($\ref{equa_8_35}$)

假设我们有一个一般线性方程 $\boldsymbol{Ax} = \boldsymbol{b}$，其中 $\boldsymbol{A}$ 是秩为 $r$ 的 $m \times n$ 矩阵，并且 $\boldsymbol{b} \in range(\boldsymbol{A})$(即方程是一致的)。我们需要解出 $\boldsymbol{x}$。如果 $r = n$，则只有一个解 $\boldsymbol{x} = \boldsymbol{A}^+ \boldsymbol{b}$。如果 $r < n$，则将有无数个解 $\boldsymbol{x} = \boldsymbol{Cy} + \boldsymbol{d}$。在这个方程中，$\boldsymbol{y}$ 是一个$n-r$维的未知数的向量， $\boldsymbol{d}$ 是满足 $\boldsymbol{Ad} = \boldsymbol{b}$ 的任意一个向量，$\boldsymbol{C}$ 是满足属性 $\boldsymbol{AC} = \boldsymbol{0}$ 的满秩矩阵。每个 $\boldsymbol{x}$ 都有一个唯一的 $\boldsymbol{y}$ 与之对应，反之亦然。(这哥俩是一一对应的)。本质就是我们在线代中说的通解+特解

我们将该式应用于式($\ref{equa_8_35}$)。该方程有一个秩为 $r$ 的 $n^c \times n$ 的系数矩阵。如果 $r = n$ 那么该方程有一个唯一的解， $\ddot{\boldsymbol{q}} = \boldsymbol{K}^+(\boldsymbol{k} + \boldsymbol{k}_{stab})$。但是如果 $r < n$ 那么将有无数个解，其通解形式为: $$ \begin{equation}\label{equa_8_42} \ddot{\boldsymbol{q}} = \boldsymbol{G}\ddot{\boldsymbol{y}} + \boldsymbol{g} \end{equation} $$ 在这个方程中，$\boldsymbol{G}$ 是一个 $n \times (n - r)$ 满秩矩阵有属性 $$ \begin{equation}\label{equa_8_43} \boldsymbol{KG} = \boldsymbol{0} \end{equation} $$ $\ddot{\boldsymbol{y}}$ 是一个 $n - r$ 为的独立加速度变量的向量，$\boldsymbol{g}$ 是一个满足下式的向量: $$ \begin{equation}\label{equa_8_44} \boldsymbol{Kg} = \boldsymbol{k} + \boldsymbol{k}_{stab} \end{equation} $$ 通常，$\ddot{\boldsymbol{y}}$ 被选为 $\ddot{\boldsymbol{q}}$ 元素的子集。因此，它可以被认为是闭环系统的独立关节加速度变量的向量。我们将在§8.8中介绍计算 $\boldsymbol{G}, \boldsymbol{g}$ 的算法。现在我们有一个方程把 $\ddot{\boldsymbol{q}}$ 写成未知数 $\ddot{\boldsymbol{y}}$ 的函数。接下来是要找到 $\ddot{\boldsymbol{y}}$ 的表达式。将方程($\ref{equa_8_34}$)左乘 $\boldsymbol{G}^T$ 可以消除 $\boldsymbol{\lambda}$，有: $$ \boldsymbol{G}^T\boldsymbol{H}\ddot{\boldsymbol{q}} = \boldsymbol{G}^T(\boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a) $$ 代入式($\ref{equa_8_42}$)，(参考式(3.20))有: $$ \begin{equation}\label{equa_8_45} \boldsymbol{G}^T\boldsymbol{HG}\ddot{\boldsymbol{y}} = \boldsymbol{G}^T(\boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a - \boldsymbol{Hg}) \end{equation} $$ 该方程可被视为闭环系统的运动方程，它以独立关节加速度变量 $\ddot{\boldsymbol{y}}$ 表示。矩阵 $\boldsymbol{G}^T\boldsymbol{HG}$ 是对称的、正定的，因此方程($\ref{equa_8_45}$) 可以直接求解 $\ddot{\boldsymbol{y}}$，有 $$ \begin{equation}\label{equa_8_46} \ddot{\boldsymbol{y}} = (\boldsymbol{G}^T\boldsymbol{HG})^{-1} \boldsymbol{G}^T (\boldsymbol{\tau} - \boldsymbol{C} + \boldsymbol{\tau}^a - \boldsymbol{Hg}) \end{equation} $$

表 8.2 计算 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 的公式
Equations for calculating $\boldsymbol{C} - \boldsymbol{\tau}^a$

8.6 求解 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 的算法

处理 $\boldsymbol{\tau}^a$ 的最佳策略是选择生成树，使得每个闭环关节都是被动的。在这种情况下，对于每个闭环关节都有 $\boldsymbol{f}_k^a = \boldsymbol{0}$ 并且 $\boldsymbol{\tau}^a = \boldsymbol{0}$。接下来我们将要修改代码，用 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 代替 $\boldsymbol{C}$ 的计算。

表 8.2 中显示了递归牛顿-欧拉算法的基本方程(详见表5.1)以及用于计算 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 的方程。将原始方程与修改后的方程进行比较，可以看出以下差异。

项$\boldsymbol{S}_i\ddot{\boldsymbol{q}}$ 被抛弃了，因为计算 $\boldsymbol{C}$ 时加速度变量被设置为0了。
变量$\boldsymbol{a}_i, \boldsymbol{\tau}_i$被修改为 $\boldsymbol{a}_i^{vp}, \boldsymbol{C}_i = \boldsymbol{C} - \boldsymbol{\tau}_a'$。这只是符号的改变。脚注: 严格的说 $\boldsymbol{a}_i^{vp}$ 的值是 $\dot{\boldsymbol{J}}_i\dot{\boldsymbol{q}} - \boldsymbol{a}_g$，而不是式($\ref{equa_8_13}$)中定义的$\dot{\boldsymbol{J}}_i\dot{\boldsymbol{q}}$。但是常偏置项$\boldsymbol{a}_g$ 已经被式($\ref{equa_8_15}$)的减法干掉了。
计算关节力时增加了一项，用于在减去一般外力的同时减去主动闭环关节力。

表 8.3 计算 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 的算法
Algorithm for calculating $\boldsymbol{C} - \boldsymbol{\tau}^a$

从生成树的角度来看，主动闭环关节力确实是外力，因为它们来自刚体系统中不属于生成树的部分。系数 $e_{ik}$ 确保仅从 $\boldsymbol{f}_{s(k)}$ 中减去 $\boldsymbol{f}_ k^a$，并且从 $\boldsymbol{f}_{p(k)}$ 中减去 $-\boldsymbol{f}_k^a$。

表 8.3 根据表 8.2中描述的公式，给出了计算 $\boldsymbol{C} - \boldsymbol{\tau}^a$ 的算法的伪代码。这些计算都是在刚体坐标系下进行的。算法中的第一个和第三个循环与表5.1中的代码差不多，第二个循环是新增的。

第一个循环的最后一行将每个向量 $\boldsymbol{f}_i$ 都写成 $\boldsymbol{f}_i^B - \boldsymbol{f}_i^x$ 的形式。第二个循环的目标则是，每个 $\boldsymbol{f}_i$中的项 $\sum_{k = N_B + 1}^{N_J} e_{ik}\boldsymbol{f}_k^a$ 减去，供第三个循环使用。

第二个循环的第二行，使用 jcalc 计算 $\boldsymbol{T}_k^a$，用局部变量 $\boldsymbol{T}^a$ 保存。这里有一个假设，$\boldsymbol{T}_k^a$ 是一个常数，所以 jcalc 只需要知道关节 $k$ 的类型就可以给出正确的值。如果我们不做这个假设，那么就需要计算出关节 $k$ 从前驱坐标系到后继坐标系的变换，以便可以将其作为参数提供给 jcalc。

表达式 $\boldsymbol{T}^a\boldsymbol{\tau}_k$ 给出了在关节$k$的后继坐标系下的 $\boldsymbol{f}_k^a$，它是 $\boldsymbol{T}^a$所支持的坐标系。第二个循环中的第三行计算了在后继坐标系下的 $\boldsymbol{f}_k^a$。接下来的两行从 $\boldsymbol{f}_{s(k)}$ 中将其减去，并加上了 $\boldsymbol{f}_{p(k)}$。请注意，在执行加法之前，$\boldsymbol{f}^a$ 必须从 $s(k)$ 坐标系变换到 $p(k)$ 坐标系下。

表 8.4 计算 $\boldsymbol{K}, \boldsymbol{k}, \boldsymbol{k}_{stab}$ 的算法
Algorithm for calculating $\boldsymbol{K}, \boldsymbol{k}, \boldsymbol{k}_{stab}$

8.7 求解 $\boldsymbol{K}, \boldsymbol{k}$ 的算法

表 8.4 显示了通过方程($\ref{equa_8_19}, \ref{equa_8_20}$) 计算 $\boldsymbol{K}$，通过方程($\ref{equa_8_15}$) 计算 $\boldsymbol{k}$，以及通过方程($\ref{equa_8_22}, \ref{equa_8_23}$) 计算 $\boldsymbol{k}_{tab}$ 的算法的伪代码。这些计算涉及到最初在各种不同坐标系中获得的向量和矩阵。因此，在使用这些量之前，有必要对它们进行坐标变换，统一坐标系。该算法中使用的策略是将所有这些量都转换到基座的坐标系下。这是最简单的策略，但不一定是最好的。

局部变量 $\boldsymbol{X}_p, \boldsymbol{X}_s$ 提供了从基座坐标系分别到关节 $k$ 的前驱和后继坐标系的普吕克变换。$\boldsymbol{X}_p = {}^{p(k),k}\boldsymbol{X}_0$, $\boldsymbol{X}_s = {}^{s(k),k}\boldsymbol{X}_0$。表达式 $\boldsymbol{X}_s\boldsymbol{X}_p^{-1}$ 则是从关节前驱坐标系到后继坐标系的变换，它是式($\ref{equa_8_23}$)中 jcalc 的参数。调用 jcalc 需要闭环关节$k$的位置误差 $\boldsymbol{\delta}$ 和约束力子空间矩阵 $\boldsymbol{T}$。完成该函数的调用之后，紧接着就把 $\boldsymbol{T}$ 从后继坐标系变换到基座坐标系下。但是这里并没有对于 $\boldsymbol{\delta}$ 做任何变换，是因为它表示的是一个形式为 $\boldsymbol{T}^T\boldsymbol{d}$ 的量，其中 $\boldsymbol{d} \in M^6$ 是一个无穷小的位移。

该算法假设每个闭环关节都有 $\mathring{\boldsymbol{T}} = 0$ 和 $\boldsymbol{\sigma} = \dot{\boldsymbol{\sigma}} = \boldsymbol{0}$。如果没有该假设，jcalc 就需要更多的参数并且返回更多的数据。假设 $\mathring{\boldsymbol{T}} = \boldsymbol{0}$ 意味着 $\dot{\boldsymbol{T}} = \boldsymbol{v}_s \times^* \boldsymbol{T}$，这允许我们对方程($\ref{equa_8_15}$) 的右侧做如下的简化，本算法使用的正是这个表达式:

$$ \begin{equation}\label{equa_8_47} \begin{array}{rl} \boldsymbol{k}_l & = - \boldsymbol{T}_k^T \left(\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}\right) - \dot{\boldsymbol{T}}_k^T\left(\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)}\right) \\ & = - \boldsymbol{T}_k^T \left(\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}\right) - \left(\boldsymbol{v}_{s(k)} \times^* \boldsymbol{T}_k\right)^T\left(\boldsymbol{v}_{s(k)} - \boldsymbol{v}_{p(k)}\right) \\ & = - \boldsymbol{T}_k^T \left(\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}\right) + \boldsymbol{T}_k^T\boldsymbol{v}_{s(k)} \times \left(\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp}\right) \\ & = - \boldsymbol{T}_k^T \left(\boldsymbol{a}_{s(k)}^{vp} - \boldsymbol{a}_{p(k)}^{vp} + \boldsymbol{v}_{s(k)} \times \boldsymbol{v}_{p(k)}\right) \end{array} \end{equation} $$

$\boldsymbol{K}$ 的计算方法是首先将整个矩阵初始化为零，然后计算其中非零子矩阵。对于每个闭环 $l$ 位于闭环路径上的每个树节点 $i$ 其子矩阵 $\boldsymbol{K}_{li}$ 均不为零。这些变量在一个 while 循环中通过索引 $i$ 和 $j$ 计算的。它们的初值被分别置为 $p(k)$ 和 $s(k)$，然后朝着根节点也就是基座回溯到 $p(k)$ 和 $s(k)$ 的第一个共同祖先节点。此时，闭环 $l$ 的每个节点都已经被考察了，并且 $i = j$，这是循环的终止条件。

表 8.4中的算法可能涉及一些重复的计算。比如，如果关节 $i$ 出现在不只一个环路中，那么 $\boldsymbol{S}_i$ 将被不止一次的转换到基座坐标系下。类似的，如果刚体 $i$ 是多个闭环关节的后继或者前驱，那么 $\boldsymbol{v}_i$ 和 $\boldsymbol{a}_i^{vp}$ 将多次变换到基座坐标系下，并且多次计算相关的变换 $\boldsymbol{X}_p, \boldsymbol{X}_s$。这些重复计算对整体的效率影响较小，但是如果需要极至的速度，可以修改算法避免此类重复。

舍入误差问题 The Rounding-Error Problem

该算法使用基座的坐标系有两方面的收益，① 使算法保持简单，② 尽可能的减少所需的变换矩阵的数量。但这是要付出代价的，大多数浮点计算都涉及舍入误差，有些误差会随着距离被放大。some of these errors are sensitive to the distance between the frame in which the calculations are performed and the locations of the objects to which those calculations apply. 因此，如果基座到运动环的距离太远，那么计算时就会损失精度。对于大多数刚体系统来说，这并不是什么问题。但对于浮动基座的系统，如果它距离其固定基座原点很远，比如飞机或者航天器，这就可能会是一个问题。在这种情况下，我们可以在浮动基座坐标系下计算 $\boldsymbol{K}, \boldsymbol{k}, \boldsymbol{k}_{stab}$，而不是固定基坐标。

8.8 求解 $\boldsymbol{G}, \boldsymbol{g}$ 的算法

本节我们讨论如何求解线性方程($\ref{equa_8_48}$)，获得形如($\ref{equa_8_49}$)形式的解 $$ \begin{equation}\label{equa_8_48} \boldsymbol{Kx} = \boldsymbol{k} \end{equation} $$ $$ \begin{equation}\label{equa_8_49} \boldsymbol{x} = \boldsymbol{Gy} + \boldsymbol{g} \end{equation} $$ 该方法适用于求解方程($\ref{equa_8_35}$)来获得方程($\ref{equa_8_42}$)。在方程($\ref{equa_8_48}, \ref{equa_8_49}$)中，$\boldsymbol{K}$ 是一个秩为 $r$ 的 $m \times n$ 的矩阵， $\boldsymbol{k} \in \text{range}(\boldsymbol{K})$ (这样能保证式($\ref{equa_8_48}$)是一致的)，$\boldsymbol{G}$ 是一个$n \times (n-r)$的满秩矩阵。式($\ref{equa_8_49}$)产生式($\ref{equa_8_48}$)的所有可能的解。实际上式($\ref{equa_8_49}$)定义了一个从 $\boldsymbol{y}$ 到式($\ref{equa_8_48}$) 的解集合的一一映射关系。如果 $r = n$ 那么式($\ref{equa_8_49}$)可以简化为 $\boldsymbol{x} = \boldsymbol{g}$，并且 $\boldsymbol{g}$ 是唯一的。如果 $r < n$，那么 $\boldsymbol{G}, \boldsymbol{g}$ 就不唯一，我们需要寻找任何一对能够解决问题的值。

假设 $\text{rank}(\boldsymbol{K}) = r$，那么 $\boldsymbol{K}$ 一定包含一个 $r \times r$ 的可逆矩阵。因此一定存在一个 $\boldsymbol{K}$ 的排列，使得该子矩阵位于其左上角。因此我们可以写出: $$ \boldsymbol{K} = \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{K}_{11} & \boldsymbol{K}_{12} \\ \boldsymbol{K}_{21} & \boldsymbol{K}_{22} \end{bmatrix} \boldsymbol{Q}_2 $$ 其中 $\boldsymbol{K}_{11}$ 是一个 $r \times r$ 的可逆矩阵，并且 $\boldsymbol{Q}_1$ 和 $\boldsymbol{Q}_2$ 是排列矩阵。将他们应用于整个式($\ref{equa_8_48}$)有 $$ \begin{equation}\label{equa_8_50} \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{K}_{11} & \boldsymbol{K}_{12} \\ \boldsymbol{K}_{21} & \boldsymbol{K}_{22} \end{bmatrix} \begin{bmatrix} \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \end{bmatrix} = \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{k}_1 \\ \boldsymbol{k}_2 \end{bmatrix} \end{equation} $$ 其中 $$ \begin{bmatrix} \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \end{bmatrix} = \boldsymbol{Q}_2\boldsymbol{x} \qquad \text{and} \qquad \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{k}_1 \\ \boldsymbol{k}_2 \end{bmatrix} = \boldsymbol{k} $$ 假设 $\text{rank}(\boldsymbol{K}) = r$ 并且式($\ref{equa_8_48}$)是一致的，那么方程($\ref{equa_8_50}$)的最后一行(bottom row)是第一行(top row)的线性倍(linear multiple)。所以一定存在一个矩阵 $\boldsymbol{Y}$ 使得 $\boldsymbol{K}_{21} = \boldsymbol{Y}\boldsymbol{K}_{11}, \boldsymbol{K}_{22} = \boldsymbol{Y}\boldsymbol{K}_{12}, \boldsymbol{k}_2 = \boldsymbol{Y}\boldsymbol{k}_1$。我们可以将式($\ref{equa_8_50}$)可以改写为: $$ \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{K}_{11} & \boldsymbol{K}_{12} \\ \boldsymbol{Y}\boldsymbol{K}_{11} & \boldsymbol{Y}\boldsymbol{K}_{12} \end{bmatrix} \begin{bmatrix} \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \end{bmatrix} = \boldsymbol{Q}_1 \begin{bmatrix} \boldsymbol{k}_1 \\ \boldsymbol{Y}\boldsymbol{k}_1 \end{bmatrix} $$ 据此，我们可以得到方程的一种可能的解: $$ \begin{bmatrix} \boldsymbol{x}_1 \\ \boldsymbol{x}_2 \end{bmatrix} = \begin{bmatrix} -\boldsymbol{K}_{11}^{-1} \boldsymbol{K}_{12} \\ \boldsymbol{1} \end{bmatrix} \boldsymbol{x}_2 + \begin{bmatrix}\boldsymbol{K}_{11}^{-1} \boldsymbol{k}_1 \\ \boldsymbol{0} \end{bmatrix} $$ 因此，我们可以得到原始问题的一个解: $$ \begin{equation}\label{equa_8_51} \boldsymbol{G} = \boldsymbol{Q}_2^T \begin{bmatrix} -\boldsymbol{K}_{11}^{-1} \boldsymbol{K}_{12} \\ \boldsymbol{1} \end{bmatrix} \qquad \text{and} \qquad \boldsymbol{g} = \boldsymbol{Q}_2^T \begin{bmatrix} \boldsymbol{K}_{11}^{-1} \boldsymbol{k}_1 \\ \boldsymbol{0} \end{bmatrix} \end{equation} $$ 这个特殊的解使得 $\boldsymbol{y}$ 与 $\boldsymbol{x}_2$ 相等，这意味着 $\boldsymbol{y}$ 是 $\boldsymbol{x}$ 元素的子集。通过以下的修改，方程($\ref{equa_8_51}$) 可以使用标准高斯消除算法或 LU 分解来计算:

必须使用完整主元(complete pivoting)，同时进行行交换和列交换。必须保留列交换的记录，因为它决定了 $\boldsymbol{Q}_2$ 的取值。
如果未将 $r$ 作为参数提供，那么在处理完最后一个独立行之后必须进行一次秩检查。
一旦处理完最后一个独立行，分解过程就会终止。此时，$\boldsymbol{K}$ 左上角的 $r \times r$ 子矩阵包含上三角阵 $\boldsymbol{U}$，满足 $\boldsymbol{LU} = \boldsymbol{K}_{11}$。 $\boldsymbol{K}$ 右上角的 $r \times (n − r)$ 子矩阵包含 $\boldsymbol{L}^{-1}\boldsymbol{K}_{12}$。 $\boldsymbol{k}$ 的前 $r$ 行包含 $\boldsymbol{L}^{-1}\boldsymbol{k}_1$。因此，前 $r$ 行的回代会产生 $\boldsymbol{K}_{11}^{-1}\boldsymbol{K}_{12}$ 和 $\boldsymbol{K}_{11}^{-1}\boldsymbol{k}_1$。

我们可以给这个基本算法添加各种功能。例如，可以提供一个掩码来标识 $\boldsymbol{K}$ 的哪些列对应的关节变量，不参与任何运动闭环。这些列仅包含零，因此因式分解过程不会改变。另一种可能性是允许用户指定他自己选择的自变量。这消除了完整主元的需要，可以用部分主元(partial pivoting)替换。

图 8.1. 矩阵 $\boldsymbol{K}, \boldsymbol{G}$ 中的簇和稀疏模式

8.9 利用 $\boldsymbol{K}, \boldsymbol{G}$ 的稀疏性 Exploiting Sparsity in $\boldsymbol{K}, \boldsymbol{G}$

显然，$\boldsymbol{K}, \boldsymbol{G}$ 可能包含许多零。例如，方程($\ref{equa_8_20}$) 意味着 $\boldsymbol{K}$ 的每个使得$\varepsilon_{lj} = 0$ 的子矩阵 $\boldsymbol{K}_{lj}$ 都为零。方程($\ref{equa_8_51}$) 意味着 $\boldsymbol{G}$ 至少包含 $(n−r)(n−r −1)$ 个零，这是 $(n − r) \times (n − r)$ 单位矩阵中零的数量。很大程度上，这两个矩阵的稀疏性可以归因于一个事实——它们都是块对角的。

为了揭示块对角线结构，我们首先将运动闭环组织成一组最小闭环簇 minimal loop clusters。闭环簇是任何具有以下属性的闭环集合： ① 簇内的闭环不与其外部的任何闭环耦合 ② 最小闭环簇不能分为两个更小的簇。如果两个运动闭环具有共同的关节，则它们之间存在耦合。给定一组最小闭环簇，可以设计生成树的编码规则，使得每个簇内的树节点编号连续。正是这种编号方案揭示了 $\boldsymbol{K}, \boldsymbol{G}$ 的块对角结构。

图 8.1 给出了一个示例。该图描述了包含四个运动环路的闭环系统的连接图。它们被从1到4编号(图中未显示环路编号)，并分别由闭环关节21到24封闭。其中环路 1 与环路 2 耦合，除此之外系统中没有其他耦合。因此，我们可以标识三个最小闭环簇，簇1包含闭环1和闭环2，簇2仅包含闭环3，簇3仅包含闭环4。标识出这三个簇后，我们可以设计一个编号规则，使得树关节在每个簇内连续编号。在这个例子中，蔟1中的关节编号为3到7，簇2中的关节编号为10到12，蔟3 中的关节编号为19和20。其余关节不位于任何运动回路的路径上，因此不属于任何簇。可以按照与常规编号规则一致的任何方式对它们进行编号。

在这个编码规则中，$\boldsymbol{K}$ 则是一个带有间隙的不规则块对角结构。每个蔟对应一个块，矩阵块一般都是矩形的感觉是个废话，每个块的大小和形状都可能与其他块不同，并且块之间还可能存在不同大小的水平间隙。这些间隙是由不属于任何蔟的树关节引起的。 $\boldsymbol{K}$ 的这种稀疏模式使得 $\boldsymbol{G}$ 也是一种不规则的块对角结构，只是没有间隙。$\boldsymbol{K}$ 中的每个块$\boldsymbol{K}_i$ 都会在$\boldsymbol{G}$中产生一个矩阵块。 $\boldsymbol{K}$ 的每个间隙都会对应 $\boldsymbol{G}$ 中的一个单位矩阵块。为了便于识别，连接图中的关节已经被分为六组，标记为 $a$ 到 $f$，并对 $\boldsymbol{K}$ 的列和 $\boldsymbol{G}$ 的行做了相同的划分。

块对角线的稀疏性可用于计算 $\boldsymbol{KH}^{-1}\boldsymbol{K}^T$ 和 $\boldsymbol{G}^T\boldsymbol{HG}$，以及根据 $\boldsymbol{K}$ 计算 $\boldsymbol{G}$。特别的，每次可以从 $\boldsymbol{K}$ 中计算出 $\boldsymbol{G}$ 的一个块。比如 $\boldsymbol{x} = \boldsymbol{G}_i\boldsymbol{y} + \boldsymbol{g}_i$ 是 $\boldsymbol{K}_i\boldsymbol{x} = \boldsymbol{k}_i$ 的解，每个 $\boldsymbol{G}_i$ 和 $\boldsymbol{g}_i$ 都可以用 §8.8 中的算法直接从 $\boldsymbol{K}_i$ 和 $\boldsymbol{k}_i$ 计算出来。

8.10 闭环系统的一些特性 Some Properties of Closed-Loop Systems

闭环系统比运动树表现出更多的属性和行为，这可能给动力学算法带来新的困难。本节介绍与动力学相关的闭环系统的一些特殊属性。

机动性(Mobility): 机动性是刚体系统的运动自由度。运动树的机动性就是 $n$，而闭环系统的机动性由以下通用公式给出: $$ \begin{equation}\label{equa_8_52} mobility = n - r \end{equation} $$ 如果 $\boldsymbol{y}$ 是刚体系统中的一个独立坐标向量，那么 $\boldsymbol{y}$ 的维度就是 $n -r$。如果一个系统具有属性 $r = n^c$，我们就说他是适当约束(properly constrainted)的。对于这样的系统，其机动性可以表示为: $$ \begin{equation}\label{equa_8_53} mobility = n_{tot} - 6 N_L \end{equation} $$

图 8.2. 可变机动性(a) 和配置歧义性(b)

可变机动性(Variable Mobility): 运动树的机动性是固定的，但是闭环系统的机动性与 $r$ 有关，而 $r$ 可以随着配置而改变。图 8.2(a) 中给出了一种可变机动性的例子。只要角度 $\theta$ 不为零，该机构的机动性就是 1。但如果 $\theta = 0$，那么两个臂能够独立运动，并且机动性为2。此外，中间的图所示的配置是这两种运动状态之间的边界，在这一瞬时，该机构的机动性为3。就$n,r$而言，我们始终有$n = 5$，但左图中$r = 4$，右图中$r = 3$，中图中$r = 2$。

配置歧义性(Configuration Ambiguity): 如果 $\boldsymbol{q}, \boldsymbol{y}$ 分别是运动树和闭环系统的独立位置变量的向量，那么 $\boldsymbol{q}$ 始终标识着运动树的唯一配置，但 $\boldsymbol{y}$ 不一定标识着闭环系统的唯一配置。图 8.2(b) 中给出了一个简单的例子，即使知道了自变量 $\theta$ 的值，也不足以唯一地确定系统的配置。有两种方法可以解决这个问题:

使用 $\boldsymbol{q}$ 替代 $\boldsymbol{y}$，$\boldsymbol{q}$ 是生成树的位置向量；
建立一个从 $\boldsymbol{y}$ 的每个值到唯一的一个 $\boldsymbol{q}$ 的映射表。

第一个选择时之前的章节中使用的，而第二个是§8.11的主题。对于图 8.2(b)所示的系统，如果我们实现已经知道不会达到其中某一项配置，就可以用方法2来解决。否则，我们必须尝试不同的独立变量，否则需要使用方法1来解决。

过约束(Overconstraint): 如果 $r < n^c$ 那么我们称该闭环系统是过约束的。这样的系统有 $n^c -r$ 个冗余约束，意味着运动方程中将存在 $n^c -r$ 个不确定约束力。相比于适当约束的系统，它们有过度的机动性(excess mobility)，其机动性公式为: $$ \begin{equation}\label{equa_8_55} mobility = n_{tot} - 6 N_L + (n^c -r) \end{equation} $$ 过约束的系统实际上很常见。例如，任何包含平面运动环的系统都将是过约束的。图 8.2(b) 所示的平面闭环包含四个旋转关节，因此该系统有 $n = 3$, $n^c = 5$，但机动性是1，这意味着$r = 2$。

过约束系统的动力学比适当约束系统的动力学更难计算。因此如果可能的话，我们会尽量将前者转换为后者。该转换是通过用约束较少的关节替换原来的闭环关节来实现的，从而使$n^c$的值减小。例如，如果将平面运动环路中的闭环旋转关节替换为 sphere-in-cylinder 关节，它仅有两个约束，$n^c$ 将从 5 减少到 2，这意味着 $n^c = r$

8.11 闭环闭包函数 Loop Closure Functions

令，$\boldsymbol{q}$ 为给定闭环系统的生成树的位置变量向量，$\boldsymbol{y}$ 为同一系统的独立位置变量向量。$\boldsymbol{y}$ 通常是 $\boldsymbol{q}$ 元素的子集，但这不是必需的。我们假设 $\boldsymbol{y}$ 唯一定义 $\boldsymbol{q}$。不是说好的 $\boldsymbol{y}$ 是 $\boldsymbol{q}$ 的子集吗？怎么又是唯一定义的呢... 为了使这个假设成立，可能有必要对 $\boldsymbol{y}$ 的值施加一些限制。因此，我们定义 $\boldsymbol{y}$ 的可接受值的集合 $C ⊆ \mathsf{R}^{(n−r)}$，并假设 $\boldsymbol{y} \in C$。给定这些假设，存在一个函数 $\gamma$，对于所有 $\boldsymbol{y} \in C$，都有如下关系(参考式3.11): $$ \begin{equation}\label{equa_8_56} \boldsymbol{q} = \gamma(\boldsymbol{y}) \end{equation} $$

对上式微分有: $$ \begin{equation}\label{equa_8_57} \dot{\boldsymbol{q}} = \boldsymbol{G}\dot{\boldsymbol{y}} \end{equation} $$ $$ \begin{equation}\label{equa_8_58} \boldsymbol{G} = \frac{\partial \gamma}{\partial \boldsymbol{y}} \end{equation} $$ 再次微分有: $$ \begin{equation}\label{equa_8_59} \ddot{\boldsymbol{q}} = \boldsymbol{G}\ddot{\boldsymbol{y}} + \boldsymbol{g} \end{equation} $$ $$ \begin{equation}\label{equa_8_60} \boldsymbol{g} = \dot{\boldsymbol{G}}\dot{\boldsymbol{y}} \end{equation} $$ 式($\ref{equa_8_59}$)的形式与式($\ref{equa_8_42}$)相同，当闭环-闭包的误差为0时，这两个方程是相同的。我们现在有一种计算 $\boldsymbol{G}, \boldsymbol{g}$ 的新方法，它们是根据独立变量的向量计算的。该方法可概括如下：

定义 $\boldsymbol{y}$，并确定 $C$
给定$\boldsymbol{y} \in C$，找到函数 $\gamma$ 的表达式，将$\boldsymbol{y}$ 唯一地映射到 $\boldsymbol{q}$ 上。
对该表达式进行符号微分，获得 $\boldsymbol{G}, \boldsymbol{g}$ 的表达式。
码代码实现这些表达式

该方法不如§8.5中描述的方法那么通用，主要是因为步骤 2 并不总是可行。它也不太方便，因为除了系统模型之外，它还要求用户提供 $\boldsymbol{G}, \boldsymbol{g}$ 的表达式或代码。尽管如此，它适用于各种实际的闭环系统，并且用这种方法计算 $\boldsymbol{G}, \boldsymbol{g}$ 比使用§8.7和§8.8中的算法的计算代价小很多。

该方法的另一个优点是不会发生 loop-closure 错误。这是因为我们没有对 $\ddot{\boldsymbol{q}}$ 积分得到 $\dot{\boldsymbol{q}}, \boldsymbol{q}$，而是对 $\ddot{\boldsymbol{y}}$ 积分得到 $\dot{\boldsymbol{y}}, \boldsymbol{y}$。这样，向量 $\boldsymbol{q}, \dot{\boldsymbol{q}}$ 是使用方程($\ref{equa_8_56}, \ref{equa_8_57}$) 从 $\boldsymbol{y}, \dot{\boldsymbol{y}}$ 计算出来的，这意味着它们的值自动满足闭环约束。因此不需要约束稳定项。

一个可行的例子(A Worked Example)

图 8.3中显示了一个包含4个刚体、5个关节和1个运动闭环的刚体系统。该系统是一个平面机构，其机动性为2。刚体$B_1,B_4$形成双连杆臂(two-link arm)，而$B_2,B_3$分别是线性执行器的气缸和活塞。关节3是平动关节，其余均是旋转关节。关节1、2、3、5位于运动闭环的链路上，其中关节5是闭环关节。因此树关节变量的向量为 $\boldsymbol{q} = \begin{bmatrix} q_1 & q_2 & q_3 & q_4 \end{bmatrix}^T$。其中 $q_3$ 是距离，其余是角度。在这个例子中，我们选择$q_1,q_4$为独立变量，也就是说，我们定义独立变量向量 $\boldsymbol{y} = \begin{bmatrix} y_1 & y_2 \end{bmatrix}^T$，使得 $y_1 = q_1, y_2 = q_4$。我们的主要任务是将$q_2,q_3$表示为$y_1$的函数。

图 8.3. 一个简单闭环系统的 $\gamma, \boldsymbol{G}, \boldsymbol{g}$

运动闭环的几何形式非常简单，只是一个三角形。因此可以使用标准三角公式将 $q_2,q_3$ 表示为 $y_1$ 的函数，$q_2,q_3$ 在三角形上标记为 $\theta_2, d$。为了使映射唯一，我们引入约束 $d > 0$，并使用四象限版本的反正切函数就是atan2来计算 $\theta_2$。如果 $l_1 = l_2$ 那么我们要求 $\theta_1 \neq 0$。在这种情况下，$C$ 必须排除所有具有 $y_1 = 0$ 的向量 $\boldsymbol{y}$。当运动环具有显式的解算公式时，则称之为有一个封闭解closed-form solution。比如本例。

图 8.3 还给出了 $\gamma, \boldsymbol{G}, \boldsymbol{g}$ 的明确公式。可以看出$\boldsymbol{G}$是块对角的，它有一个$3\times 1$的块和一个$1\times 1$的块。请注意，相比于§8.7和§8.8中的向量和矩阵方程，该图中的方程都是标量方程。因此，通过此处的方程计算 $\boldsymbol{G}, \boldsymbol{g}$ 的成本要低得多。

总之，该方法给用户带来了额外的工作，必须提供代码来计算 $\gamma, \boldsymbol{G}, \boldsymbol{g}$。但作为回报，这部分动力学计算的效率可能会得到很大的提高。该方法适用于具有简单、封闭解的运动闭环的系统，例如挖掘机臂、大型工业机器人、并联机器人和 Stuart-Gough 平台（飞行模拟器）。

8.12 逆动力学 Inverse Dynamics

逆动力学是计算产生给定加速度所需的力的问题。在运动树中，$\boldsymbol{\tau}$ 和 $\ddot{\boldsymbol{q}}$ 之间存在一一映射的关系，并且根据 $\ddot{\boldsymbol{q}}$ 计算 $\boldsymbol{\tau}$ 非常简单。但在闭环系统中，$\ddot{\boldsymbol{q}}$ 的给定值必须符合闭环约束，并且能够产生相同加速度的$\boldsymbol{\tau}$值有无穷多个。因此，运动闭环的存在，显著地提高逆动力学问题的难度。

现在我们来制定闭环系统的逆动力学方程。我们从生成树的运动方程开始: $$ \begin{equation}\label{equa_8_61} \boldsymbol{\tau} = \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} - \boldsymbol{\tau}^a - \boldsymbol{K}^T \boldsymbol{\lambda} \end{equation} $$ 在该方程中，$\boldsymbol{\tau}$ 是作用在树关节的未知力的矢量。任何已知的力，例如那些由弹簧和阻尼器产生的力，均假定包含在 $\boldsymbol{C}$ 中。我们还假定闭环关节处不存在未知的主动力。因此，如果 $\boldsymbol{\tau}^a$ 出现，那么它完全由已知力组成。

方程($\ref{equa_8_61}$) 中包含两个未知数，$\boldsymbol{\tau}, \boldsymbol{\lambda}$。由于我们只对 $\boldsymbol{\tau}$ 感兴趣，接下来就要消除 $\boldsymbol{\lambda}$。这是通过将方程乘以 $\boldsymbol{G}^T$ 来完成的，有: $$ \boldsymbol{G}^T\boldsymbol{\tau} = \boldsymbol{G}^T(\boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} - \boldsymbol{\tau}^a) $$ 这个方程可以写成: $$ \begin{equation}\label{equa_8_62} \boldsymbol{G}^T\boldsymbol{\tau} = \boldsymbol{G}^T\boldsymbol{\tau}_{ID} \end{equation} $$ 其中 $\boldsymbol{\tau}_{ID}$ 是通过对生成树应用逆动力学函数计算出的力，即: $$ \begin{array}{rl} \boldsymbol{\tau}_{ID} & = \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} - \boldsymbol{\tau}^a \\ & = \text{ID}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}) \end{array} $$ 在这种情况下，$\text{ID}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}})$ 是§8.6中描述的算法的输出，其中加入了关节加速度项。需要注意的是，$\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}$ 必须全部符合闭环约束。方程($\ref{equa_8_62}$) 是闭环系统的基本逆动力学方程。由于 $\boldsymbol{G}^T$ 是一个 $(n − r) \times n$ 矩阵，该方程对 $n$ 维未知向量引入了 $n − r$ 个约束，从而留下 $r$ 个自由度。换句话说，有 $\infty^r$ 个不同的 $\boldsymbol{\tau}$ 值会产生相同的加速度。如果我们想要唯一解，那么我们必须引入更多约束，或应用一个优化标准(optimality criterion)。

主动力(Actuator Forces)

逆动力学的典型用途是计算执行器的力矢量以产生给定的加速度。并非每个关节都由执行器提供动力，因此让我们分别将 $p, \boldsymbol{u}$ 定义为树中驱动自由度的数量，以及执行器输出力的 $p$ 维向量。 $\boldsymbol{u}, \boldsymbol{\tau}$ 的关系通过以下方程表示:

$$ \boldsymbol{\tau} = \boldsymbol{Q} \begin{bmatrix} \boldsymbol{u} \\ \boldsymbol{0} \end{bmatrix} $$

其中 $\boldsymbol{Q}$ 是一个 $n \times n$ 的排列矩阵。该方程简单地把$\boldsymbol{\tau}$的元素写成 $\boldsymbol{u}$ 的 $p$ 元素的排列，和 $n − p$ 个零。 $\boldsymbol{\tau}$ 的零值元素对应着未被驱动的自由度。我们还定义两个矩阵 $\boldsymbol{G}_u, \boldsymbol{G}_0$，它们分别包含$\boldsymbol{G}$中与驱动自由度和未驱动自由度相对应的行。这些矩阵与 $\boldsymbol{G}$ 的关系可以写成: $$ \boldsymbol{G} = \boldsymbol{Q}\begin{bmatrix} \boldsymbol{G}_u \\ \boldsymbol{G}_0 \end{bmatrix} $$ 根据这些定义，有: $$ \boldsymbol{G}^T\boldsymbol{\tau} = \begin{bmatrix} \boldsymbol{G}_u^T & \boldsymbol{G}_0^T \end{bmatrix} \boldsymbol{Q}^T \boldsymbol{Q} \begin{bmatrix} \boldsymbol{u} \\ \boldsymbol{0} \end{bmatrix} = \begin{bmatrix} \boldsymbol{G}_u^T & \boldsymbol{G}_0^T \end{bmatrix} \begin{bmatrix} \boldsymbol{u} \\ \boldsymbol{0} \end{bmatrix} = \boldsymbol{G}_u^T \boldsymbol{u} $$ 代入式($\ref{equa_8_62}$)，有: $$ \begin{equation}\label{equa_8_63} \boldsymbol{G}_y^T \boldsymbol{u} = \boldsymbol{G}^T \boldsymbol{\tau}_{ID} \end{equation} $$ 这是执行器力的逆动力学方程。在求解该方程时，需要考虑如下三种情况，其中情况1和情况2并不是互斥的。

$\text{rank}(\boldsymbol{G}_u) < n - r$。此时，系统是欠驱动的(underactuated)，它的自由度，比执行器可以控制的自由度多。
$p > \text{rank}(\boldsymbol{G}_u)$。此时，系统是过驱动的(redundantly actuated)，有无穷多个不同的 $\boldsymbol{u}$ 都可以产生相同的加速度。
$p = \text{rank}(\boldsymbol{G}_u) = n - r$。此时，系统是合适驱动的(properly actuated)，$\boldsymbol{G}_u$ 是可逆的，并且式($\ref{equa_8_63}$)有唯一的解 $\boldsymbol{u} = \boldsymbol{G}_u^{-T}\boldsymbol{G}^T\boldsymbol{\tau}_{ID}$

Example 8.2: 如图 8.3中所示的机构。该机构有两个自由度，因此需要两个执行器。因此，$\boldsymbol{G}_u$ 是$2 \times 2$ 的矩阵，包含了$\boldsymbol{G}$ 的两行，并且这两行的选择必须保证 $\boldsymbol{G}_u$ 可逆。给定图中所示的 $G$ 的公式， $\boldsymbol{G}_u$ 必定包含 $\boldsymbol{G}$ 的第 4 行和其他三行中的任意一行。这意味着关节 4 以及其他三个关节中的任意一个必定存在执行器。由于这个机制很简单，这个结果可以通过检查得到。

我们假设关节 3 和 4 是受驱动的，意味着 $\boldsymbol{G}_u = \begin{bmatrix} u_2 & 0 \\ 0 & 1 \end{bmatrix}$。该系统的逆动力学公式可以写作: $$ \begin{array}{rl} \boldsymbol{u} & = \boldsymbol{G}_u^{-T} \boldsymbol{G}^T \boldsymbol{\tau}_{ID} \\ & = \begin{bmatrix} u_2 & 0 \\ 0 & 1 \end{bmatrix}^{-1} \begin{bmatrix} 1 & u_1 & u_2 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \boldsymbol{\tau}_{ID} \\ & = \begin{bmatrix} 1/u_2 & u_1/u_2 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \boldsymbol{\tau}_{ID} \end{array} $$ 其中 $$ \boldsymbol{\tau}_{ID} = \text{ID}(\gamma(\boldsymbol{y}), \boldsymbol{G}\dot{\boldsymbol{y}}, \boldsymbol{G}\ddot{\boldsymbol{y}} + \boldsymbol{g}) $$ 请注意，$\boldsymbol{u}$ 并不是该系统的广义力向量，因为表达式 $\boldsymbol{u}^T\dot{\boldsymbol{y}}$ 不等于 $\boldsymbol{u}$ 传递的功率。这种差异是由 $\dot{q}_1$ 是 $\dot{\boldsymbol{y}}$ 中的第一个元素，而 $\tau_3$ 是 $\boldsymbol{u}$ 中的第一个元素造成的。如果您希望 $\boldsymbol{u}$ 成为广义力向量，那么 $\boldsymbol{u}$ 的元素必须与 $\boldsymbol{\tau}$ 的子集相同，就像 $\dot{\boldsymbol{y}}$ 是 $\dot{\boldsymbol{q}}$ 的子集一样。

8.13 稀疏矩阵法 Sparse Matrix Method

到目前为止，我们研究的每个算法都是通过将一组闭环约束应用于生成树来工作的。另一种策略是放弃生成树，并直接处理系统中各个刚体和关节。这种方法将产生具有大型稀疏矩阵的方程，任何基于这些方程的算法都必须利用其稀疏性，才能具有竞争力。此类算法往往比基于生成树的算法效率较低，但在特殊情况下它们可能会更高效，正如我们将在下面看到的那样。

一组 $N_B$ 个刚体的运动方程，受到一组 $N_J$ 个关节施加的运动约束，有: $$ \begin{equation}\label{equa_8_64} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{PT} \\ \boldsymbol{T}^T\boldsymbol{P}^T & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \boldsymbol{a} \\ -\boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{P}\boldsymbol{T}^a\boldsymbol{\tau} - \boldsymbol{v} \times^* \boldsymbol{Iv} + \boldsymbol{f}^x \\ -\dot{\boldsymbol{T}}^T \boldsymbol{P}^T \boldsymbol{v} \end{bmatrix} \end{equation} $$ 这个方程是从§3.7节中推导出来的。 $\boldsymbol{v}, \boldsymbol{a}, \boldsymbol{f}^x$ 分别是刚体的速度、加速度和外力的矢量。$\boldsymbol{\lambda}$ 是关节约束力变量的向量， $\boldsymbol{T}^a\boldsymbol{\tau}$ 是通过关节传递的主动力的矢量，$\boldsymbol{P}^T$ 是从刚体到关节速度的映射矩阵，即 $\boldsymbol{P}^T\boldsymbol{v}$ 是关节速度的向量。 $\boldsymbol{I}, \boldsymbol{T}$ 分别是刚体惯性和关节约束力子空间的复合矩阵。$\boldsymbol{a}, \boldsymbol{\lambda}$ 是未知数。系数矩阵的大小为 $6N_B + n_{tot}^c$ 的平方，其中 $n_{tot}^c$ 是关节施加的约束总数。

我们的主要兴趣在于系数矩阵的稀疏模式，让我们来检查它的组成部分。$\boldsymbol{I}, \boldsymbol{T}$ 都是块对角矩阵，定义为 $$ \boldsymbol{I} = \text{diag}(\boldsymbol{I}_1, \cdots, \boldsymbol{I}_{N_B}) \qquad \text{and} \qquad \boldsymbol{T} = \text{diag}(\boldsymbol{T}_1, \cdots, \boldsymbol{T}_{N_J}) $$ $\boldsymbol{P}$ 也是稀疏的，但不是块对角的，它的定义为: $$ \boldsymbol{P}_{ij} = \begin{cases} \boldsymbol{1}_{6 \times 6} & \text{if } i = s(j) \\ - \boldsymbol{1}_{6 \times 6} & \text{if } i = p(j) \\ \boldsymbol{0}_{6 \times 6} & \text{otherwise} \\ \end{cases} $$ 因此 $\boldsymbol{P}$ 是一个 $6N_B \times 6N_J$ 的矩阵，由为 $6 \times 6$ 块的 $N_B \times N_J$ 矩阵构成。它的每一列都有两个非零块，但直接连接到底座的关节对应的列除外，这些列只有一个非零块。根据这些定义，系数矩阵在主对角线上包含 $N_B$ 个非零块，并且最多有 $4N_J$ 个非对角线非零块。后者的确切位置由连通性决定。

图 8.4. 由梯状连接图产生的稀疏模式

方程($\ref{equa_8_64}$)是稀疏矩阵算术软件库中需要求解的经典方程，使用此类库可能是最好的解决方法。稀疏矩阵库通常会提供以下的接口:

一个分析矩阵稀疏模式的函数，用于预处理，以便分析出最佳分解顺序，以及在分解过程中，哪些零值元素将被非零值填充。
一个用于紧凑存储矩阵中所有非零值，以及因式分解期间将更改的所有零值条目，的数据结构
一个按照预处理步骤中确定的顺序，执行因式分解的函数。
a function to perform back-substitution using the resulting factors

如果一个 $n \times n$ 的矩阵仅包含 $O(n)$ 个非零元素，那么有时可以仅通过 $O(n)$ 的算术运算来分解该矩阵。如果系统是运动树，那么式($\ref{equa_8_64}$的$O(n)$分解总是可能的。脚注: 这是Baraff算法的基础(Baraff, 1996) 当然，也有许多闭环系统可以进行 $O(n)$ 因式分解，其中包括一些无法通过任何使用生成树的算法在 $O(n)$ 中求解的系统。在这种情况下，对于足够大的 $n$，稀疏矩阵算法的性能将优于生成树算法。

图 8.4中显示了一个通过稀疏矩阵方法实现 $O(n)$ 动力学的闭环系统的示例。它的连接图是一种有时称为梯子(ladder)的图。它可以通过在右侧添加更多节点来无限扩展。该图还显示了其系数矩阵，它是一种对称排列的稀疏模式。选择这种排列是为了使生成的矩阵具有固定的带宽，即，每个非零块出现在主对角线，或者两侧最近的三个对角线之一上。具有此属性的矩阵始终可以通过 $O(n)$ 运算进行因式分解。

为了解释这种排列，图中的节点均按照它们在矩阵中出现的顺序进行编号，并且各关节已被赋予反映其连接性的数字对，$ij$是从刚体 $i$ 连接到刚体 $j$ 的关节。排列矩阵中的第1行是原始矩阵中包含$\boldsymbol{I}_1$ 的行(刚体编号如图所示)。第2行包含$\boldsymbol{I}_2$，第3行标记为12，是原始矩阵中包含 $\boldsymbol{T}^T\boldsymbol{P}^T$ 并与关节12有关的行，等等。注意看，标有刚体编号的每一行在主对角线上都包含一个非零块，标有数字对$ij$ 的每一行仅在 $i$ 列和 $j$ 列中包含非零块。

如果我们将生成树算法应用于该系统，那么我们将遇到以下问题：无论我们选择什么生成树，关节空间惯性矩阵 $\boldsymbol{H}$ 中总会有 $O(n^2)$ 个非零元素，因此不可能仅通过 $O(n)$ 操作来计算动力学。

最后，有必要强调一些细节。首先，如果要在动力学模拟器中使用该算法，则需要约束稳定。必要的额外项尚未包含在方程($\ref{equa_8_64}$)。其次，我们假设系数矩阵满秩，也就是说，我们假设闭环系统受到适当约束(properly constrained)。如果矩阵是奇异的，那么稀疏分解可能不起作用。

Chapter8: 闭环系统
Closed Loop Systems

8.1 运动方程 Equations of Motion

8.2 闭环约束方程 Loop Constraint Equations

8.3 约束稳定性 Constraint Stabilization

计算\(\boldsymbol{\delta}\)

8.4 闭环关节力 Loop Joint Forces

8.5 运动方程的求解 Solving the Equations of Motion

方法1:直接求解式(\(\ref{equa_8_36}\))

方法2:先解出\(\boldsymbol{\lambda}\)

方法3:先求解式(\(\ref{equa_8_35}\))

8.6 求解 \(\boldsymbol{C} - \boldsymbol{\tau}^a\) 的算法