Chapter7:正动力学:传播算法
Forward Dynamics: Propagation Methods

正动力学问题留给我们两组未知数：关节加速度和关节关节约束力。通常我们不太可能在任何一个刚体的局部坐标系下求解这些未知数，但是我们可以建立一些约束方程。 传播算法会局部地计算这些参数，然后将它们传播给邻居刚体节点，直到我们最终遇到一个特别的刚体，可以求解出它的动力学参数。 获得这个刚体的解之后，我们就可以计算邻居刚体的动力学参数，如此传播直至获得整个问题的解。原理上，这个过程就像是求解线性方程组，首先三角化系数矩阵， 然后通过反向代入(back-substitution)的方式依次解出各个未知数。实际上，一些传播算法就是按照这种范式来实现的，比如， Ascher et al., 1997; Rosenthal, 1990; Saha, 1997。传播算法要比惯性矩阵算法更复杂，但是它的计算复杂度只有 \(O(N_B)\)，理论上，这是求解正动力学问题的最小复杂度。

铰接式刚体算法articulated-body algotithm是一种典型的传播算法，它是求解运动树正动力学问题的最快算法。 本章的大部分内容，都是在描述该算法及其相关概念，例如铰接式刚体惯性articulated-body inertia。 最后两节对该主题进行了更广泛的介绍，包括扩展其他算法的一些基础方程和技术。

这是一个铰接体运动方程 articulated-body equation of motion。它具有与式(\(\ref{equa_7_1}\))相同的代数形式，但是参数不同。该式中的 \(\boldsymbol{I}^A, \boldsymbol{p}^A\) 分别是刚体 \(B\) 在这个由 \(B, B'\) 和一个关节构成的二刚体系统中的铰接体的惯性和偏置力。

式(\(\ref{equa_7_2}\))这样的方程，描述了一个刚体的加速度相应，同时考虑了一组其他刚体和关节的动态作用。我们称这个刚体为句柄 handle， 这个单词非常专业，但在程序员那里十分常见，就遵循程序员的惯例吧。 我们称包含这样刚体的系统为一个铰接体 articulated body。 因此，句柄就是 \(\boldsymbol{I}^A, \boldsymbol{p}^A\) 所描述的主体。铰接体则是该刚体系统，或者子系统，其动态特性已经包含到 \(\boldsymbol{I}^A, \boldsymbol{p}^A\) 中了。 在图 7.1(b)中，刚体 \(B\) 就是句柄，整个二刚体系统就是铰接体。

基本属性 Basic Properties

铰接体惯性具有如下刚体惯性的属性: ① 它们是对称的、正定的矩阵 ② 它们是空间 \(M^6\) 到 \(F^6\) 的映射 ③ 它们还遵循相同的坐标变换规则。 但是，铰接体惯性是从加速度到力的映射，不是从速度到动量的映射，所以类似 \(\boldsymbol{I}^A \boldsymbol{v}\) 和 \(\frac{1}{2}\boldsymbol{v}^T\boldsymbol{I}^A\boldsymbol{v}\) 的式子是没有意义的，其中 \(\boldsymbol{v}\) 表示速度。一个铰接体可能仅包含一个刚体，此时铰接体和刚体的惯性矩阵和偏置力相同。这为递归计算方法提供了起点。

图 7.2. 铰接体惯性和偏置力的加法

铰接体惯性只依赖于其成员刚体的惯性和成员关节的约束。因此它们是关节变量的函数，没有关于速度变量或各种力的项。在这方面，它们类似于广义惯性矩阵。 速度项、外力(\(\boldsymbol{f}\)除外)等，只会出现在偏置力中。在 3D 空间中，需要 21 个参数来描述一个一般的铰接体惯性，而刚体惯性则只需要 10 个参数。 在 2D 空间中，描述一般的铰接体惯性需要 6 个参数，刚体惯性只需要 4 个。注意 21 和 6 分别是对称 \(6 \times 6\) 和 \(3 \times 3\) 矩阵中的独立变量数量。 一般的，一个铰接体惯性中的表观质量会随着作用力的方向而改变，而且没有表观质心(详细参见例 7.1)。

加法 Addition

铰接体惯性是有加法定义的，其物理含义如图7.2所示。图中描述了两个铰接体 \(A_1, A_2\)，它们被连接在一起构成了一个铰接体 \(A\)。 这个链接通过将刚体 \(B_1, B_2\) 用焊在一起，形成一个新的刚体 \(B\)。如果符号 \(\boldsymbol{I}_i^A, \boldsymbol{p}_i^A, \boldsymbol{I}^A, \boldsymbol{p}^A\) 分别表示\(B_i\) 在 \(A_i\)中、\(B\) 在 \(A\)中，铰接体的惯性和偏置力。那么有 \(\boldsymbol{I}^A = \boldsymbol{I}_1^A + \boldsymbol{I}_2^A\) 和 \(\boldsymbol{p}^A = \boldsymbol{p}_1^A + \boldsymbol{p}_2^A\)。

惯性的逆 Inverse Inertia

式(\(\ref{equa_7_2}\)) 假设句柄有 6 个自由度。如果假设不成立，那么就需要用更一般的公式替代: $$ \begin{equation}\label{equa_7_3} \boldsymbol{a} = \boldsymbol{\mathit{\Phi}}^A \boldsymbol{f} + \boldsymbol{b}^A \end{equation} $$ 其中，\(\boldsymbol{\mathit{\Phi}}^A, \boldsymbol{b}^A\) 分别是句柄的铰接体惯性的逆和偏置加速度。这种形式的方程总是存在的。 如果句柄有 6 个自由度，那么 \(\boldsymbol{\mathit{\Phi}}^A = (\boldsymbol{I}^A)^{-1}\)，\(\boldsymbol{b}^A = -\boldsymbol{\mathit{\Phi}}^A\boldsymbol{p}^A\)。 否则，\(\boldsymbol{\mathit{\Phi}}\) 是一个奇异的矩阵，它的秩等于句柄的运动自由度。铰接体惯性的逆是对称的、半正定的矩阵，它们具有和刚体惯性逆矩阵相同的变换。

多重句柄 Multiple Handles

一个铰接体可能有不只一个句柄。如果我们把句柄从 1 到 \(h\) 编号，并定义向量 \(\boldsymbol{f}_i, \boldsymbol{a}_i\) 分别是句柄 \(i\) 的力和加速度， 那么式(\(\ref{equa_7_3}\))可以扩展为: $$ \begin{equation}\label{equa_7_4} \begin{bmatrix} \boldsymbol{a}_1 \\ \boldsymbol{a}_2 \\ \vdots \\ \boldsymbol{a}_h \end{bmatrix} = \begin{bmatrix} \boldsymbol{\mathit{\Phi}}_{1}^A & \boldsymbol{\mathit{\Phi}}_{12}^A & \cdots & \boldsymbol{\mathit{\Phi}}_{1h}^A \\ \boldsymbol{\mathit{\Phi}}_{21}^A & \boldsymbol{\mathit{\Phi}}_{2}^A & \cdots & \boldsymbol{\mathit{\Phi}}_{2h}^A \\ \vdots & \vdots & \ddots & \vdots \\ \boldsymbol{\mathit{\Phi}}_{h1}^A & \boldsymbol{\mathit{\Phi}}_{h2}^A & \cdots & \boldsymbol{\mathit{\Phi}}_{h}^A \end{bmatrix} \begin{bmatrix} \boldsymbol{f}_1 \\ \boldsymbol{f}_2 \\ \vdots \\ \boldsymbol{f}_h \end{bmatrix} + \begin{bmatrix} \boldsymbol{b}_1^A \\ \boldsymbol{b}_2^A \\ \vdots \\ \boldsymbol{b}_h^A \end{bmatrix} \end{equation} $$ 其中，\(\boldsymbol{\mathit{\Phi}}_i^A, \boldsymbol{b}_i^A\)分别是句柄 \(i\) 的铰接体惯性逆和偏置加速度。\(\boldsymbol{\mathit{\Phi}}\) 描述的是句柄 \(i\) 耦合句柄 \(j\) 的作用力的角速度系数。 该系数矩阵是对称的、半正定的，并且它的秩等于所有句柄的独立运动自由度的和。如果秩是 \(6h\)，那么式(\(\ref{equa_7_4}\))就是可逆的，对其求逆就可以得到式(\(\ref{equa_7_2}\)的多重句柄形式。

7.2 计算铰接体惯性 Calculating Articulated-Body Inertias

计算铰接体惯性主要有投影(projection)和装配(assembly)两种基本策略。投影法把铰接体整体的运动方程投射到句柄的运动空间中。装配法则一步一步地从基础组件开始组装铰接体， 在组装的过程中计算一系列的铰接体惯性。在机器人控制系统中，经常使用投影法计算操作空间惯性operational-space inertias (Khatib, 1987, 1995)。装配法常用于 \(O(n)\) 动力学算法中。

7.2.1 投影法 The Projection Method

铰接体是一种刚体系统。因此可以在广义坐标系下写出其运动方程: $$ \begin{equation}\label{equa_7_5} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} \end{equation} $$ 其中 \(\boldsymbol{C}\) 考虑了作用在系统上，句柄上的力除外，的所有力。令 \(\boldsymbol{v}, \boldsymbol{a}, \boldsymbol{f}\) 分别表示句柄的空间速度和加速度，以及作用在其上的空间力。 如果 \(\boldsymbol{J}\) 是根据 \(\boldsymbol{v} = \boldsymbol{J}\dot{\boldsymbol{q}}\) 将 \(\dot{\boldsymbol{q}}\) 映射到 \(\boldsymbol{v}\) 的雅可比行列式， 那么它定义了如下的 \(\boldsymbol{f}\) 和 \(\boldsymbol{\tau}\)，\(\boldsymbol{a}\) 和 \(\ddot{\boldsymbol{q}}\) 之间的关系: $$ \begin{equation}\label{equa_7_6} \boldsymbol{\tau} = \boldsymbol{J}^T \boldsymbol{f} \end{equation} $$ $$ \begin{equation}\label{equa_7_7} \boldsymbol{a} = \boldsymbol{J} \ddot{\boldsymbol{q}} + \dot{\boldsymbol{J}} \dot{\boldsymbol{q}} \end{equation} $$ 将这两个式子代入式(\(\ref{equa_7_5}\))，有: $$ \begin{equation}\label{equa_7_8} \boldsymbol{a} = \boldsymbol{J} \boldsymbol{H}^{-1} (\boldsymbol{J}^T \boldsymbol{f} - \boldsymbol{C})+ \dot{\boldsymbol{J}} \dot{\boldsymbol{q}} = \mathit{\Phi}^A\boldsymbol{f} + \boldsymbol{b}^A \end{equation} $$ 其中 $$ \begin{equation}\label{equa_7_9} \mathit{\Phi}^A = \boldsymbol{J}\boldsymbol{H}^{-1}\boldsymbol{J}^T \end{equation} $$ $$ \begin{equation}\label{equa_7_10} \boldsymbol{b}^A = \dot{\boldsymbol{J}} \dot{\boldsymbol{q}} - \boldsymbol{J}\boldsymbol{H}^{-1} \boldsymbol{C} \end{equation} $$ 这些是铰接体惯性的逆和偏置加速度的一般方程。它们适用于任何铰接体，并且与广义坐标的选择无关。如果句柄具有完整的六个运动自由度，那么 \(\mathit{\Phi}^A\) 是可逆的，我们可以定义 $$ \begin{equation}\label{equa_7_11} \boldsymbol{f} = \boldsymbol{I}^A \boldsymbol{a} + \boldsymbol{p}^A \end{equation} $$ $$ \begin{equation}\label{equa_7_12} \boldsymbol{I}^A = \left(\boldsymbol{J} \boldsymbol{H}^{-1} \boldsymbol{J}^T\right)^{-1} \end{equation} $$ $$ \begin{equation}\label{equa_7_13} \boldsymbol{p}^A = -\boldsymbol{I}^A \boldsymbol{b}^A \end{equation} $$ 铰接体惯性的许多基本属性都可以从方程(\(\ref{equa_7_12}\))中推导出来。例如，已知 \(\boldsymbol{H}\) 是对称且正定的，所以 \(\boldsymbol{I}^A\) 也必须是对称且正定的； 显然 \(\boldsymbol{I}^A\) 不依赖于速度或力。由式(\(\ref{equa_7_9}\)) 得出的 \(\mathit{\Phi}^A\) 的一些基本属性如下: $$ \begin{equation}\label{equa_7_14} \begin{array}{lcr} \text{rank}(\mathit{\Phi}^A) & = & \text{rank}(\boldsymbol{J}) \\ \text{range}(\mathit{\Phi}^A) & = & \text{range}(\boldsymbol{J}) \\ \text{null}(\mathit{\Phi}^A) & = & \text{null}(\boldsymbol{J}) \\ \end{array} \end{equation} $$

多重句柄 Multiple Handles

投影法可以很容易扩展到多重句柄的铰接体上。假设有 \(h\) 个句柄，令 \(\boldsymbol{a}_i, \boldsymbol{f}_i, \boldsymbol{J}_i\) 分别为句柄 \(i\) 的加速度、力和雅可比矩阵。 如果我们按照下面的形式定义 \(\boldsymbol{a}, \boldsymbol{f}, \boldsymbol{J}\)： $$ \boldsymbol{a} = \begin{bmatrix} \boldsymbol{a}_1 \\ \vdots \\ \boldsymbol{a}_h \end{bmatrix} \qquad \boldsymbol{f} = \begin{bmatrix} \boldsymbol{f}_1 \\ \vdots \\ \boldsymbol{f}_h \end{bmatrix} \qquad \boldsymbol{J} = \begin{bmatrix} \boldsymbol{J}_1 \\ \vdots \\ \boldsymbol{J}_h \end{bmatrix} \qquad $$ 那么式(\(\ref{equa_7_6}\)) 到式(\(\ref{equa_7_14}\)) 都可以直接用于多重句柄的情况。

图 7.4. 由两个小铰接体装配的大铰接体

7.2.2 装配法 The Assembly Method

装配法将一个铰接体看做是更小的铰接体组装而成的，我们通过这些基础部件计算它的惯性和偏置力。因此装配法比较适合通过递归算法实现。装配法本身是通用的， 但是这里我们只考虑一种特殊情况，所有的铰接体都是浮动运动树 floating kinematic tree，即它们都不与固定基座相连。这种情况的特殊之处在于，每个铰接体都有一个句柄， 并且每个句柄都有完整的 6 个自由度，每个装配操作都是将一个句柄通过一个关节连接到另一个句柄上。我们将在 §7.4 和 §7.5 中介绍更一般的情况。

图 7.4 中示意了由一对铰接体，\(A_1\) 和 \(A_2\)，组装称为一个更大的铰接体 \(A\) 的过程。这是将两个句柄 \(B_1, B_2\) 通过一个关节连起来的过程。 在装配之前，\(B_1, B_2\) 的铰接体方程分别为: $$ \begin{equation}\label{equa_7_15} \boldsymbol{f}_1 = \boldsymbol{I}_1^A \boldsymbol{a}_1 + \boldsymbol{p}_1^A \end{equation} $$ $$ \begin{equation}\label{equa_7_16} \boldsymbol{f}_2 = \boldsymbol{I}_2^A \boldsymbol{a}_2 + \boldsymbol{p}_2^A \end{equation} $$ 装配之后，\(B_1\) 变成了新的铰接体的句柄，它的方程为: $$ \begin{equation}\label{equa_7_17} \boldsymbol{f} = \boldsymbol{I}^A \boldsymbol{a} + \boldsymbol{p}^A \end{equation} $$ 我们的目标是把 \(\boldsymbol{I}^A, \boldsymbol{p}^A\) 用 \(\boldsymbol{I}_1^A, \boldsymbol{I}_2^A, \boldsymbol{p}_1^A, \boldsymbol{p}_2^A\) 和新关节引入的运动约束表示出来。

装配的时候，添加的新关节将引入一个新的力 \(\boldsymbol{f}_J\)，他是从 \(B_1\) 传导给 \(B_2\) 的合力。因此力变量有如下的关系: $$ \boldsymbol{f}_1 = \boldsymbol{f} - \boldsymbol{f}_J $$ $$ \boldsymbol{f}_2 = \boldsymbol{f}_J $$ 其中 \(\boldsymbol{f}_J\) 的值是未知的，但是它试驾了运动约束: $$ \boldsymbol{a}_2 - \boldsymbol{a}_1 = \boldsymbol{S} \ddot{\boldsymbol{q}} + \boldsymbol{c} $$ $$ \boldsymbol{S}^T\boldsymbol{f}_J = \boldsymbol{\tau} $$ 其中，\(\ddot{\boldsymbol{q}}, \boldsymbol{\tau}\) 分别是关节的加速度和力，\(\boldsymbol{S}\) 是关节的运动子空间矩阵。 并且有 \(\boldsymbol{c} = \dot{\boldsymbol{S}} \dot{\boldsymbol{q}}\)，如果关节与时间相关则有 \(\boldsymbol{c} = \dot{\boldsymbol{S}} \dot{\boldsymbol{q}} + \dot{\boldsymbol{\delta}}\)。 \(\ddot{\boldsymbol{q}}\) 是未知的，但是 \(\boldsymbol{\tau}, \boldsymbol{S}, \boldsymbol{c}\) 是给定的。结合式(\(\ref{equa_7_16}\))，我们可以计算出 \(\ddot{\boldsymbol{q}}\): $$ \begin{align} \boldsymbol{\tau} & = \boldsymbol{S}^T \boldsymbol{f}_2 \\ & = \boldsymbol{S}^T \left(\boldsymbol{I}_2^A \boldsymbol{a}_2 + \boldsymbol{p}_2^A\right) \\ & = \boldsymbol{S}^T \left(\boldsymbol{I}_2^A \left(\boldsymbol{a}_1 + \boldsymbol{c} + \boldsymbol{S}\ddot{\boldsymbol{q}}\right) + \boldsymbol{p}_2^A\right) \end{align} $$ 所以有: $$ \begin{equation}\label{equa_7_18} \ddot{\boldsymbol{q}} = \left(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\right)^{-1} \left(\boldsymbol{\tau} - \boldsymbol{S}^T(\boldsymbol{I}_2^A(\boldsymbol{a}_1 + \boldsymbol{c}) + \boldsymbol{p}_2^A)\right) \end{equation} $$ 矩阵 \(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\) 是正定的，因此可逆。如此可以求解出 \(\ddot{\boldsymbol{q}}\)，我们就可以写出 \(\boldsymbol{f}\) 与 \(\boldsymbol{a}_1\) 之间的关系: $$ \begin{align} \boldsymbol{f} & = \boldsymbol{f}_1 + \boldsymbol{f}_2 \\ & = \boldsymbol{I}_1^A \boldsymbol{a}_1 + \boldsymbol{I}_2^A \boldsymbol{a}_2 + \boldsymbol{p}_1^A + \boldsymbol{p}_2^A \\ & = \boldsymbol{I}_1^A \boldsymbol{a}_1 + \boldsymbol{I}_2^A (\boldsymbol{a}_1 + \boldsymbol{c} + \boldsymbol{S}\ddot{\boldsymbol{q}}) + \boldsymbol{p}_1^A + \boldsymbol{p}_2^A \\ & = \boldsymbol{I}_1^A \boldsymbol{a}_1 + \boldsymbol{I}_2^A \left(\boldsymbol{a}_1 + \boldsymbol{c} + \boldsymbol{S}\left(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\right)^{-1} \left(\boldsymbol{\tau} - \boldsymbol{S}^T(\boldsymbol{I}_2^A(\boldsymbol{a}_1 + \boldsymbol{c}) + \boldsymbol{p}_2^A)\right) \right) + \boldsymbol{p}_1^A + \boldsymbol{p}_2^A \\ \end{align} $$ 这个式子可以写成式(\(\ref{equa_7_17}\))的形式，因此有 \(\boldsymbol{I}^A, \boldsymbol{p}^A\): $$ \begin{equation}\label{equa_7_19} \boldsymbol{I}^A = \boldsymbol{I}_1^A + \boldsymbol{I}_2^A - \boldsymbol{I}_2^A\boldsymbol{S}\left(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\right)^{-1}\boldsymbol{S}^T\boldsymbol{I}_2^A \end{equation} $$ $$ \begin{equation}\label{equa_7_20} \boldsymbol{p}^A = \boldsymbol{p}_1^A + \boldsymbol{p}_2^A + \boldsymbol{I}_2^A\boldsymbol{c} + \boldsymbol{I}_2^A\boldsymbol{S}\left(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\right)^{-1} \left(\boldsymbol{\tau} - \boldsymbol{S}^T\left(\boldsymbol{I}_2^A \boldsymbol{c} + \boldsymbol{p}_2^A \right)\right) \end{equation} $$ 根据这些式子，对于一个浮动运动树的铰接体，我们计算出其中任何句柄的铰接体惯性和偏置力。

装配子树 Assembling Subtrees

图 7.4 中的三个铰接体分别被标记为\(A_1, A_2, A\) 仅仅是为了区分它们，下角标 1 和 2 没有特别意义。 在实践中，我们通常会定义一组铰接体，\(A_1 \cdots A_{N_B}\) ，其中 \(A_i\) 包含以刚体 \(i\) 为根的子树中的所有刚体，刚体 \(i\) 用作句柄（如图 7.5所示）。 在这种情况下，最有用的装配公式是那些告诉我们如何根据刚体 \(i\) 的子节点，即所有 \(j \in \mu(i)\) 的惯性和偏置力 \(\boldsymbol{I}_j^A, \boldsymbol{p}_j^A\)， 来计算 \(\boldsymbol{I}_i^A, \boldsymbol{p}_i^A\)。 有如下公式: $$ \begin{equation}\label{equa_7_21} \boldsymbol{I}_i^A = \boldsymbol{I}_i + \sum_{j \in \mu(i)} \boldsymbol{I}_j^a \end{equation} $$ $$ \begin{equation}\label{equa_7_22} \boldsymbol{p}_i^A = \boldsymbol{p}_i + \sum_{j \in \mu(i)} \boldsymbol{p}_j^a \end{equation} $$

其中 \(\boldsymbol{I}_i, \boldsymbol{p}_i\) 是刚体 \(i\) 的惯性和偏置力，有:

$$ \begin{equation}\label{equa_7_23} \boldsymbol{I}_j^a = \boldsymbol{I}_j^A - \boldsymbol{I}_j^A\boldsymbol{S}_j\left(\boldsymbol{S}_j^T \boldsymbol{I}_j^A \boldsymbol{S}_j\right)^{-1} \boldsymbol{S}_j^T\boldsymbol{I}_j^A \end{equation} $$ $$ \begin{equation}\label{equa_7_24} \boldsymbol{p}_j^a = \boldsymbol{p}_j^A + \boldsymbol{I}_j^a \boldsymbol{c}_j + \boldsymbol{I}_j^A\boldsymbol{S}_j\left(\boldsymbol{S}_j^T \boldsymbol{I}_j^A \boldsymbol{S}_j\right)^{-1} \left(\boldsymbol{\tau}_j - \boldsymbol{S}_j^T\boldsymbol{p}_j^A\right) \end{equation} $$

这些式子都是对刚体 \(i\) 的子节点应用式(\(\ref{equa_7_19}\), \(\ref{equa_7_20}\)) 得到的。每次套用这俩公式的时候，\(A_1\) 都是刚体 \(i\) 加上它的子树， \(A_2\) 是将要添加的下一个子树。On each application, \(A_1\) consists of body \(i\) plus every child subtree that has already been processed, and \(A_2\) is the next child subtree to be added. 变量 \(\boldsymbol{I}_j^a, \boldsymbol{p}_j^a\) 是 \(\boldsymbol{I}_j^A, \boldsymbol{p}_j^A\) 通过关节 \(j\) 传播的结果。 它们可以看做是一个没有质量massless的句柄的表观惯性和偏置力，该句柄通过关节 \(j\) 与刚体 \(j\) 链接。 在这个例子中，式(\(\ref{equa_7_21}, \ref{equa_7_22}\)) 把所有这些无质量的句柄与刚体 \(i\) 联系起来了，参见图7.2。 \(\boldsymbol{I}_j^a, \boldsymbol{p}_j^a\) 还有一个有意思的特性，它们把通过关节的力the force across a joint表示成父刚体的加速度的函数， 而 \(\boldsymbol{I}_j^A, \boldsymbol{p}_j^A\) 则将之表示为子节点的加速度的函数: $$ \begin{align}\label{equa_7_25} \boldsymbol{f}_j & = \boldsymbol{I}_j^A \boldsymbol{a}_j + \boldsymbol{p}_j^A \\ & = \boldsymbol{I}_j^a \boldsymbol{a}_{\lambda(j)} + \boldsymbol{p}_j^a \end{align} $$ \(\boldsymbol{I}_j^a\) 在提升传播计算的效率上有两个作用。其一，它可以把式(\(\ref{equa_7_20}\))中的关于\(\boldsymbol{c}\)的两项写成式(\(\ref{equa_7_24}\))中的一项。 其二，它具有关系 \(\boldsymbol{I}_j^a \boldsymbol{S}_j = \boldsymbol{0}\)。受到 \(\boldsymbol{S}_j\) 的影响，这一特性意味着 \(\boldsymbol{I}_j^a\) 中有一个甚至更多的行和列都是 0。 比如，如果\(j\)是一个旋转关节，有 \(\boldsymbol{S}_j = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}^T\)，那么 \(\boldsymbol{I}_j^a\) 中的第3行和第3列都是是 0。 利用这些特性，我们在一定程度上降低铰接体惯性的计算量。

其中，\(\boldsymbol{a}_0\) 是基座的加速度，\(\boldsymbol{c}_1 = \dot{\boldsymbol{S}}_1\dot{\boldsymbol{q}}_1\)。关节也对传导的力有约束: $$ \begin{equation}\label{equa_7_28} \boldsymbol{S}_1^T \boldsymbol{f}_1 = \boldsymbol{\tau}_1 \end{equation} $$ 综合这些表达式，我们有: $$ \boldsymbol{S}_1^T \left(\boldsymbol{I}_1^A\left(\boldsymbol{a}_0 + \boldsymbol{c}_1 + \boldsymbol{S}_1 \ddot{\boldsymbol{q}}_1 \right) + \boldsymbol{p}_1^A \right) = \boldsymbol{\tau}_1 $$ 可以求解出 \(\ddot{\boldsymbol{q}}_1\)： $$ \begin{equation}\label{equa_7_29} \ddot{\boldsymbol{q}}_1 = \left(\boldsymbol{S}_1^T \boldsymbol{I}_1^A \boldsymbol{S}_1\right)^{-1} \left(\boldsymbol{\tau}_1 - \boldsymbol{S}_1^T \boldsymbol{I}_1^A\left(\boldsymbol{a}_0 + \boldsymbol{c}_1\right) - \boldsymbol{S}_1^T \boldsymbol{p}_1^A \right) \end{equation} $$ 已知 \(\boldsymbol{I}_1^A, \boldsymbol{p}_1^A\) 我们可以直接计算出 \(\ddot{\boldsymbol{q}}_1\)，不需要知道其他加速度变量。 之所以可以这样做，是因为式(\(\ref{equa_7_26}\))已经考虑了 \(A_1\) 中所有刚体对 \(B_1\) 的加速度响应。一旦求出 \(\ddot{\boldsymbol{q}}_1\)，就可以将其代入式(\(\ref{equa_7_27}\))中， 得到 \(B_1\) 的空间加速度。我们可以将这套路复用到 \(B_1\) 的子节点上。在一般情况下，如果刚体 \(\lambda(i)\) 的加速度已知，那么刚体 \(i\) 和关节 \(i\) 的加速度可以计算如下: $$ \begin{equation}\label{equa_7_30} \ddot{\boldsymbol{q}}_i = \left(\boldsymbol{S}_i^T \boldsymbol{I}_i^A \boldsymbol{S}_i\right)^{-1} \left(\boldsymbol{\tau}_i - \boldsymbol{S}_i^T \boldsymbol{I}_i^A\left(\boldsymbol{a}_{\lambda(i)} + \boldsymbol{c}_i\right) - \boldsymbol{S}_i^T \boldsymbol{p}_i^A \right) \end{equation} $$ $$ \begin{equation}\label{equa_7_31} \boldsymbol{a}_i = \boldsymbol{a}_{\lambda(i)} + \boldsymbol{c}_i + \boldsymbol{S}_i \ddot{\boldsymbol{q}}_i \end{equation} $$ 因此，如果已知铰接体的惯性和偏置力，那么运动树的正动力学就可以利用这两个公式，从 1 到 \(N_B\) 遍历 \(i\)得出。

算法 Algorithm

完全按照上述材料实现的，运动树正动力学的，铰接体算法的计算复杂度是 \(O(N_B)\)。它需要遍历三次动树: ① 向下遍历，从根节点到叶子节点，计算速度和偏置项 ② 向上遍历，计算铰接体惯性和偏置力 ③ 再次向下遍历计算加速度。

Pass 1: 第一次遍历的任务是计算 velocity-product accelerations \(\boldsymbol{c}_i\) 和刚体的偏置力 \(\boldsymbol{p}_i\)。 它们都是刚体速度 \(\boldsymbol{v}_i\) 的函数，所以有必要把速度也一起求出来。第一次遍历需要用到如下的公式: $$ \begin{align}\label{equa_7_32} \boldsymbol{v}_{Ji} & = \boldsymbol{S}_i \dot{\boldsymbol{q}}_i \\ \boldsymbol{c}_{Ji} & = \mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i \\ \boldsymbol{v}_i & = \boldsymbol{v}_{\lambda(i)} + \boldsymbol{v}_{Ji} \qquad (\boldsymbol{v}_0 = \boldsymbol{0}) \\ \boldsymbol{c}_i & = \boldsymbol{c}_{Ji} + \boldsymbol{v}_i \times \boldsymbol{v}_{Ji} \\ \boldsymbol{p}_i & = \boldsymbol{v}_i \times^* \boldsymbol{I}_i \boldsymbol{v}_i - \boldsymbol{f}_i^x \end{align} $$ 这个过程用到的公式与表 5.1 的递归牛顿欧拉算法类似。 如果关节 \(i\) 显式的与时间相关(参见§3.5)， 矢量 \(\boldsymbol{v}_{Ji}\) 和 \(\boldsymbol{c}_{Ji}\) 将分别取值 \(\boldsymbol{S}_i \dot{\boldsymbol{q}}_i + \boldsymbol{\delta}_i\) 和 \(\mathring{\boldsymbol{S}}_i \dot{\boldsymbol{q}}_i + \mathring{\boldsymbol{\delta}}_i\)，但是它们的实现细节由 jcalc 处理了(参见§4.4)。\(\boldsymbol{f}_i^x\) 是作用在刚体 \(i\) 上的外力。

Pass 2: 接下来计算铰接体的惯性和偏置力 \(\boldsymbol{I}_i^A, \boldsymbol{p}_i^A\)。我们通过式(\(\ref{equa_7_21}\))到(\(\ref{equa_7_24}\))的装配公式完成， 重抄如下: $$ \begin{align}\label{equa_7_37} \boldsymbol{I}_i^A & = \boldsymbol{I}_i + \sum_{j \in \mu(i)} \boldsymbol{I}_j^a \\ \boldsymbol{p}_i^A & = \boldsymbol{p}_i + \sum_{j \in \mu(i)} \boldsymbol{p}_j^a \\ \boldsymbol{I}_j^a & = \boldsymbol{I}_j^A - \boldsymbol{I}_j^A\boldsymbol{S}_j\left(\boldsymbol{S}_j^T \boldsymbol{I}_j^A \boldsymbol{S}_j\right)^{-1} \boldsymbol{S}_j^T\boldsymbol{I}_j^A \\ \boldsymbol{p}_j^a & = \boldsymbol{p}_j^A + \boldsymbol{I}_j^a \boldsymbol{c}_j + \boldsymbol{I}_j^A\boldsymbol{S}_j\left(\boldsymbol{S}_j^T \boldsymbol{I}_j^A \boldsymbol{S}_j\right)^{-1} \left(\boldsymbol{\tau}_j - \boldsymbol{S}_j^T\boldsymbol{p}_j^A\right) \end{align} $$ 这些计算是倒着从 \(N_B\) 到 1 遍历 \(i\) 进行的。这意味着并不会对每个 \(j\) 都计算 \(\boldsymbol{I}_j^a, \boldsymbol{p}_j^a\)，只计算那些满足 \(\lambda(j) \neq 0\) 的节点。 如果刚体 \(i\) 没有子节点，那么 \(\boldsymbol{I}_i^A = \boldsymbol{I}_i, \boldsymbol{p}_i^A = \boldsymbol{p}_i\)。

Pass 3: 第三次遍历使用式(\(\ref{equa_7_30}\), \(\ref{equa_7_31}\))计算加速度，重抄如下:

$$ \begin{align}\label{equa_7_41} \ddot{\boldsymbol{q}}_i & = \left(\boldsymbol{S}_i^T \boldsymbol{I}_i^A \boldsymbol{S}_i\right)^{-1} \left(\boldsymbol{\tau}_i - \boldsymbol{S}_i^T \boldsymbol{I}_i^A\left(\boldsymbol{a}_{\lambda(i)} + \boldsymbol{c}_i\right) - \boldsymbol{S}_i^T \boldsymbol{p}_i^A \right) \\ \boldsymbol{a}_i & = \boldsymbol{a}_{\lambda(i)} + \boldsymbol{c}_i + \boldsymbol{S}_i \ddot{\boldsymbol{q}}_i \qquad\qquad (\boldsymbol{a}_0 = -\boldsymbol{a}_g) \end{align} $$

如果将基座的加速度初始化为 \(-\boldsymbol{a}_g\) 而不是 \(\boldsymbol{0}\)，我们就可以模拟虚拟向上加速度的均匀重力场的作用，这要比把重力看做是一个外力更好。

方程(\(\ref{equa_7_37}\), 46) 已经从计算 \(\mu(i)\) 转换为 \(\lambda(i)\) 的计算。因此，\(\boldsymbol{I}_i^A, \boldsymbol{p}_i^A\) 在第一个 for 循环的最后，被初始化为 \(\boldsymbol{I}_i, \boldsymbol{p}_i\)。在第二个 for 循环的 if 语句中完成了其余的计算， (参考§5.3中方程5.20的实现)。

7.4 其他装配方程 Alternative Assembly Formulae

第7.3节中描述了铰接体算法的标准版本，它基于 §7.2.2 中的装配公式。当然可以使用其他公式来代替。 本节介绍一种可以替代装配公式的系统过程，进而得到了铰接体算法的替代版本。在大多数情况下，替代方案比标准算法更复杂并且效率更低。但是，它们可以解决更多的问题。

在 §7.2.2 中我们解决了如下的问题，给定如下 5 个方程 $$ \begin{equation}\label{equa_7_51} \boldsymbol{f}_1 = \boldsymbol{I}_1^A \boldsymbol{a}_1 + \boldsymbol{p}_1^A \end{equation} $$ $$ \begin{equation}\label{equa_7_52} \boldsymbol{f}_2 = \boldsymbol{I}_2^A \boldsymbol{a}_2 + \boldsymbol{p}_2^A \end{equation} $$ $$ \begin{equation}\label{equa_7_53} \boldsymbol{f} = \boldsymbol{f}_1 + \boldsymbol{f}_2 \end{equation} $$ $$ \begin{equation}\label{equa_7_54} \boldsymbol{a}_2 = \boldsymbol{a}_1 + \boldsymbol{c} + \boldsymbol{S}\ddot{\boldsymbol{q}} \end{equation} $$ $$ \begin{equation}\label{equa_7_55} \boldsymbol{S}^T\boldsymbol{f}_2 = \boldsymbol{\tau} \end{equation} $$ 推导系数 \(\boldsymbol{I}^A, \boldsymbol{p}^A\) 的表达式，最终得到目标方程: $$ \begin{equation}\label{equa_7_56} \boldsymbol{f} = \boldsymbol{I}^A \boldsymbol{a}_1 + \boldsymbol{p}^A \end{equation} $$ 通过一些简单规则，我们可以把 5 个方程揉成一个组合方程: $$ \begin{bmatrix} \boldsymbol{1} & -\boldsymbol{1} & -\boldsymbol{1} & \boldsymbol{0} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{1} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{1} & -\boldsymbol{I}_2^A & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{1} & -\boldsymbol{S} \\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{S}^T & \boldsymbol{0} & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \boldsymbol{f} \\ \boldsymbol{f}_1 \\ \boldsymbol{f}_2 \\ \boldsymbol{a}_2 \\ \ddot{\boldsymbol{q}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{0} \\ \boldsymbol{p}_1^A \\ \boldsymbol{p}_2^A \\ \boldsymbol{c} \\ \boldsymbol{\tau} \end{bmatrix} + \begin{bmatrix} \boldsymbol{0} \\ \boldsymbol{I}_1^A \\ \boldsymbol{0} \\ \boldsymbol{1} \\ \boldsymbol{0} \end{bmatrix} \boldsymbol{a}_1 $$ 这些简单规则包括:

1. 目标方程(\(\ref{equa_7_56}\))中右侧的未知数 \(\boldsymbol{a}_1\) 变成了上式中右侧的未知数。
2. 目标方程(\(\ref{equa_7_56}\))中左侧的未知数 \(\boldsymbol{f}\) 变成了上式中组合向量(composite vector)的顶部。
3. 维度不是 6 的未知向量 \(\ddot{\boldsymbol{q}}\) 成为了上式中组合向量(composite vector)的底部。
4. 维度不是 6 的方程 \(\boldsymbol{S}^T\boldsymbol{f}_2 = \boldsymbol{\tau}\) 成为了上式中组合方程(composite equation)的底部。
5. 剩下的方程和未知数可以以任意的顺序插入，使得 \(5 \times 5\) 的组合矩阵(composite matrix)尽可能地接近上三角的形式。

构造了这个方程之后，原来的问题就可以通过高斯消元和反向代入的方式解决。只要系数矩阵变成了上三角的形式，我们就可以立即把 \(\ddot{\boldsymbol{q}}\) 写成 \(\boldsymbol{a}_1\) 的函数。 反向代入的过程最终会得到一个方程把\(\boldsymbol{f}\)当作\(\boldsymbol{a}_1\)的函数。此时，\(\boldsymbol{a}_1\) 的系数就是 \(\boldsymbol{I}^A\) 所需要的表达式。 其余项则是 \(\boldsymbol{p}^A\)。

展开 表7.2 其它装配方程 >>>

表7.2中标题“标准公式 Standard Formulae”下说明了该过程。 在高斯消元阶段结束后，组合公式的第五行展开为: $$ \boldsymbol{S}^T\boldsymbol{I}_2^A \boldsymbol{S} \ddot{\boldsymbol{q}} = \boldsymbol{\tau} - \boldsymbol{S}^T\boldsymbol{p}_2^A - \boldsymbol{S}^T\boldsymbol{I}_2^A\boldsymbol{c} - \boldsymbol{S}^T\boldsymbol{I}_2^A\boldsymbol{a}_1 $$ 可以得到 \(\ddot{\boldsymbol{q}}\)，参见式(\(\ref{equa_7_18}\)) $$ \ddot{\boldsymbol{q}} = \left(\boldsymbol{S}^T \boldsymbol{I}_2^A \boldsymbol{S}\right)^{-1} \left(\boldsymbol{\tau} - \boldsymbol{S}^T\boldsymbol{p}_2^A - \boldsymbol{S}^T\boldsymbol{I}_2^A\boldsymbol{c} - \boldsymbol{S}^T\boldsymbol{I}_2^A\boldsymbol{a}_1)\right) $$ 把 \(\ddot{\boldsymbol{q}}\) 写成 \(\boldsymbol{a}_1\) 的表达式，通过反向迭代，我们就可以将 \(\boldsymbol{a}_2\) 也写成 \(\boldsymbol{a}_1\) 的表达式， 最终得到 \(\boldsymbol{f}\) 的 \(\boldsymbol{a}_1\) 的表达式，从中我们可以推出\(\boldsymbol{I}^A,\boldsymbol{p}^A\)的表达式。揉搓成一个组合方程的要点是使得求解的过程系统化了， 可以应用于不同的方程，我们可以尝试做两点改变:

1. 将式(\(\ref{equa_7_51}\),\(\ref{equa_7_52}\),\(\ref{equa_7_56}\))用它们的逆替换，即\(\boldsymbol{a}_1 = \mathit{\Phi}_1^A \boldsymbol{f}_1 + \boldsymbol{b}_1^A, \boldsymbol{a}_2 = \mathit{\Phi}_2^A \boldsymbol{f}_2 + \boldsymbol{a}_2^A, \boldsymbol{a}_1 = \mathit{\Phi}^A \boldsymbol{f} + \boldsymbol{b}^A\)
2. 将式(62, 63) 替换为等效隐式关节约束方程，即， \(\boldsymbol{T}^T\boldsymbol{a}_2 = \boldsymbol{T}^T(\boldsymbol{a}_1 + \boldsymbol{c}), \boldsymbol{f}_2 = \boldsymbol{T}_a\boldsymbol{\tau} + \boldsymbol{T\lambda}\)

表7.2中给出了通过此方法生成的两个替代公式：variant 1 采用了两项更改； variant 2 仅采用了第二项更改。每一个都可以用作铰接体算法的替代公式。 脚注1: 这两种变体都不会计算\(\ddot{\boldsymbol{q}}\)，但是我们可以通过公式\(\ddot{\boldsymbol{q}} = \boldsymbol{T}_a^T(\boldsymbol{a}_2 - \boldsymbol{a}_1 - \boldsymbol{c})\)根据\(\boldsymbol{a}_1, \boldsymbol{a}_2\)求出\(\ddot{\boldsymbol{q}}\)。如果将主动关节力 \(\boldsymbol{T}_a\boldsymbol{\tau}\) 替换为一对等效的外力，那么基于变体1的算法将更高效。这些等效的外力对各个刚体的偏置加速度有贡献。

这一过程产生了一些有用的矩阵恒等式，比如，\(\boldsymbol{I}^A\) 的变体公式必须等于标准公式，两者都必须是 \(\mathit{\Phi}^A\) 公式的逆。 类似的特性也适用于 \(\boldsymbol{p}^A, \boldsymbol{b}^A\)。

任何适用惯性，而不是惯性的逆，的公式都必须保证惯性是存在的。原作者应该是想表示可以写出惯性的数学表达式。 因此标准公式和变体2的公式仅适用于句柄具有完整的六个自由度的情况。而变体1的公式仅要求 \(\boldsymbol{T}^T(\mathit{\Phi}_1^A + \mathit{\Phi}_2^A)\boldsymbol{T}\)满秩。 其物理意义就是，关节不施加冗余的约束，也就是说不会复制已经存在的约束。变体1可以应用的充分条件是两个句柄中至少有一个具有完整的六个自由度。 因此基于变体1的算法能够计算任何运动树和一些闭环系统的铰接体惯性的逆，而标准算法仅限于浮动运动树。

图 7.6. 装配具有多个句柄的铰接体

7.5 多重句柄 Multiple Handles

到目前为止，我们仅在只有一个句柄的铰接体中应用了装配法，对于运动树已经足够了。但是，如果我们想要组装一般的刚体系统，或者如果希望组装顺序具有更大的灵活性， 那么我们必须能够组装具有多个句柄的铰接体。Featherstone (1999a,b) 描述了一种分而治之(divide-and-conquer)算法，在一个有 \(O(N_B)\) 个处理器的并行计算机上， 能够以 \(O(\log(N_B))\) 的时间复杂度，处理运动树和闭环系统。 现在让我们开发一个具有多个句柄的铰接体的装配公式。

图7.6中画出了两个铰接体 \(A_{12}, A_{34}\)，我们要把他们装配成一个更大的铰接体 \(A\)。刚体 \(A_{12}\) 有两个句柄分别记为 1 和 2， \(A_{34}\) 的两个句柄记为 3 和 4。我们在 2 和 3 之间建立一个关节把两个铰接体装配起来，装配后的铰接体将只有句柄 1 和 4。我们假设 2 和 3 中至少有一个句柄有六个自由度。 在装配之前 \(A_{12}, A_{34}\) 的运动方程为: $$ \begin{equation}\label{equa_7_57} \begin{bmatrix} \boldsymbol{a}_1 \\ \boldsymbol{a}_2 \end{bmatrix} = \begin{bmatrix} \mathit{\Phi}_1 & \mathit{\Phi}_{12} \\ \mathit{\Phi}_{21} & \mathit{\Phi}_2 \end{bmatrix} \begin{bmatrix} \boldsymbol{f}_1 \\ \boldsymbol{f}_2 \end{bmatrix} + \begin{bmatrix} \boldsymbol{b}_1 \\ \boldsymbol{b}_2 \end{bmatrix} \end{equation} $$ $$ \begin{equation}\label{equa_7_58} \begin{bmatrix} \boldsymbol{a}_3 \\ \boldsymbol{a}_4 \end{bmatrix} = \begin{bmatrix} \mathit{\Phi}_3 & \mathit{\Phi}_{34} \\ \mathit{\Phi}_{43} & \mathit{\Phi}_4 \end{bmatrix} \begin{bmatrix} \boldsymbol{f}_3 \\ \boldsymbol{f}_4 \end{bmatrix} + \begin{bmatrix} \boldsymbol{b}_3 \\ \boldsymbol{b}_4 \end{bmatrix} \end{equation} $$ 装配之后 \(A\) 的运动方程为: $$ \begin{equation}\label{equa_7_59} \begin{bmatrix} \boldsymbol{a}_1 \\ \boldsymbol{a}_4 \end{bmatrix} = \begin{bmatrix} \mathit{\Phi}_1^A & \mathit{\Phi}_{14}^A \\ \mathit{\Phi}_{41}^A & \mathit{\Phi}_4^A \end{bmatrix} \begin{bmatrix} \boldsymbol{f}_1 \\ \boldsymbol{f}_4 \end{bmatrix} + \begin{bmatrix} \boldsymbol{b}_1^A \\ \boldsymbol{b}_4^A \end{bmatrix} \end{equation} $$ 我们省略了式(\(\ref{equa_7_57}\),\(\ref{equa_7_58}\))中的上角标来简化符号。在这里我们使用惯性的逆，因为有可能铰接体内的两个句柄加一起并没有完整的 12 个自由度， 即使它们单独都有 6 个自由度。在没有完全自由度的情况下，方程中的系数矩阵将是奇异的。但是，我们假设句柄 2 和 3 至少有一个句柄有六个自由度，因此矩阵 \(\mathit{\Phi}_2\) 和 \(\mathit{\Phi}_3\) 中至少有一个是正定的。这意味着 \(\mathit{\Phi}_2 + \mathit{\Phi}_3\) 是正定的。

装配的时候，关节将给句柄 2 和 3 引入如下的运动约束: $$ \begin{equation}\label{equa_7_60} \boldsymbol{T}^T(\boldsymbol{a}_3 - \boldsymbol{a}_2) = \boldsymbol{T}^T \boldsymbol{c} \end{equation} $$ 其中 \(\boldsymbol{c} = \dot{\boldsymbol{S}}\dot{\boldsymbol{q}}\)。关节还将引入力约束: $$ \begin{equation}\label{equa_7_61} \boldsymbol{f}_3 = \boldsymbol{T\lambda} = - \boldsymbol{f}_2 \end{equation} $$ 在制定方程(\(\ref{equa_7_61}\))时，我们假设关节处的任何主动力，都已经在方程(\(\ref{equa_7_57}\), \(\ref{equa_7_58}\)) 的偏置加速度中考虑到了。 也就是说，主动关节力 \(\boldsymbol{T}_a\boldsymbol{\tau}\) 已被一对大小相等且方向相反的外力所取代，\(\boldsymbol{T}_a\boldsymbol{\tau}\)作用在句柄 3， \(-\boldsymbol{T}_a\boldsymbol{\tau}\)作用在句柄 2 上。并且这两个外力对 \(A_{12}, A_{34}\) 运动的作用已经包含在向量\(\boldsymbol{b}_1 \cdots \boldsymbol{b}_4\) 中。

我们的目标是把式(\(\ref{equa_7_59}\))的系数用式(\(\ref{equa_7_57}, \ref{equa_7_58}, \ref{equa_7_60}, \ref{equa_7_61}\))中的系数表出。 首先需要替换方程(\(\ref{equa_7_60}\)) 中的\(\boldsymbol{a}_3, \boldsymbol{a}_2\)，有: $$ \boldsymbol{T}^T\left( \mathit{\Phi}_3 \boldsymbol{f}_3 + \mathit{\Phi}_{34}\boldsymbol{f}_4 + \boldsymbol{b}_3 - \mathit{\Phi}_{21}\boldsymbol{f}_1 - \mathit{\Phi}_2\boldsymbol{f}_2 - \boldsymbol{b}_2 \right) = \boldsymbol{T}^T\boldsymbol{c} $$ 接下来用式(\(\ref{equa_7_61}\))替换方程中的 \(\boldsymbol{f}_3, \boldsymbol{f}_2\)，有: $$ \boldsymbol{T}^T(\mathit{\Phi}_2 + \mathit{\Phi}_3)\boldsymbol{T\lambda} = \boldsymbol{T}^T(\mathit{\Phi}_{21}\boldsymbol{f}_1 - \mathit{\Phi}_{34} \boldsymbol{f}_4 + \boldsymbol{c} - \boldsymbol{b}_3 + \boldsymbol{b}_2) $$ 由于 \(\mathit{\Phi}_2 + \mathit{\Phi}_3\)是正定的，\(\boldsymbol{\lambda}\)的系数一定是非奇异的，所以我们可以解出\(\boldsymbol{\lambda}\): $$ \begin{align}\label{equa_7_62} \boldsymbol{\lambda} & = \boldsymbol{Y}^{-1}\boldsymbol{T}^T(\mathit{\Phi}_{21}\boldsymbol{f}_1 - \mathit{\Phi}_{34}\boldsymbol{f}_4 + \boldsymbol{\beta}) \\ \boldsymbol{Y} & = \boldsymbol{T}^T(\mathit{\Phi}_2 + \mathit{\Phi}_3)\boldsymbol{T} \\ \boldsymbol{\beta} & = \boldsymbol{c} - \boldsymbol{b}_3 + \boldsymbol{b}_2 \end{align} $$ 最后一步是用式(\(\ref{equa_7_61}, \ref{equa_7_62}\))替代式(\(\ref{equa_7_57}, \ref{equa_7_58}\))中\(\boldsymbol{a}_1, \boldsymbol{a}_4\)表达式中的\(\boldsymbol{f}_2, \boldsymbol{f}_3\)，有: $$ \begin{align} \boldsymbol{a}_1 & = \mathit{\Phi}_1\boldsymbol{f}_1 - \mathit{\Phi}_{12}\boldsymbol{T}\boldsymbol{Y}^{-1}\boldsymbol{T}^T (\mathit{\Phi}_{21}\boldsymbol{f}_1 - \mathit{\Phi}_{34}\boldsymbol{f}_4 + \boldsymbol{\beta}) + \boldsymbol{b}_1 \\ \boldsymbol{a}_4 & = \mathit{\Phi}_4\boldsymbol{f}_4 + \mathit{\Phi}_{43}\boldsymbol{T}\boldsymbol{Y}^{-1}\boldsymbol{T}^T (\mathit{\Phi}_{21}\boldsymbol{f}_1 - \mathit{\Phi}_{34}\boldsymbol{f}_4 + \boldsymbol{\beta}) + \boldsymbol{b}_4 \end{align} $$ 这两个式子仅将 \(\boldsymbol{a}_1, \boldsymbol{a}_4\) 表示为 \(\boldsymbol{f}_1, \boldsymbol{f}_4\) 的函数，因此是方程(\(\ref{equa_7_59}\)) 的组成部分。 方程(\(\ref{equa_7_59}\)) 的系数公式为: $$ \begin{align}\label{equa_7_63} \mathit{\Phi}_1^A & = \mathit{\Phi}_1 - \mathit{\Phi}_{12}\boldsymbol{W}\mathit{\Phi}_{21} \\ \mathit{\Phi}_4^A & = \mathit{\Phi}_4 - \mathit{\Phi}_{43}\boldsymbol{W}\mathit{\Phi}_{34} \\ \mathit{\Phi}_{14}^A & = \mathit{\Phi}_{12}\boldsymbol{W}\mathit{\Phi}_{34} = (\mathit{\Phi}_{41}^A)^T \\ \boldsymbol{b}_1^A & = \boldsymbol{b}_1 - \mathit{\Phi}_{12}\boldsymbol{W\beta} \\ \boldsymbol{b}_4^A & = \boldsymbol{b}_4 - \mathit{\Phi}_{43}\boldsymbol{W\beta} \\ \boldsymbol{W} & = \boldsymbol{TY}^{-1}\boldsymbol{T}^T \end{align} $$

一般化 Generalizations

为了扩展句柄的数量，我们用四个句柄组替换四个单独的句柄。第 1 组包含 \(A_{12}\) 中所有组装过程中不会连接到关节上的句柄， 第 2 组包含所有连接到关节上的句柄。 第 2 组中的每个句柄将通过一个接头连接到第 3 组中的一个句柄； 因此第 2 组和第 3 组必须包含相同数量的句柄。为了允许多个关节连接到同一个刚体上，我们允许一个刚体有多个句柄。 在这种情况下，句柄不应再被视为刚体，而应被视为刚体上的位置。 任何运动树都可以通过一系列装配操作来获得，每次装配只引入一个关节，即每次只去用第 2 组和第 3 组中的一对儿句柄。 但是闭环系统需要再一次装配中使用多个关节。

定义了这些句柄组之后，接下来需要定义\(\boldsymbol{a}_1, \boldsymbol{f}_2, \mathit{\Phi}_{34}\)，构建涉及到这些句柄组的所有向量和矩阵的复合向量和矩阵。 比如说，如果第 1 组中有句柄 \(1a, 1b, 1c, \cdots\)，加速度为 \(\boldsymbol{a}_{1a}, \boldsymbol{a}_{1b}, \cdots\)，那么 \(\boldsymbol{a}_1\) 就是复合向量 \(\begin{bmatrix} \boldsymbol{a}_{1a}^T & \boldsymbol{a}_{1b}^T & \cdots \end{bmatrix}^T\)。注意 \(\boldsymbol{T}\) 将是由各个关节的约束力子空间矩阵构成的对角块矩阵。

给定这些复合向量和矩阵，方程(\(\ref{equa_7_63}\)) 的推导过程与方程(\(\ref{equa_7_57}, \ref{equa_7_62}\) 的方式相同。只是不再能保证 \(\boldsymbol{Y}\) 是可逆的。 Featherstone (1999b) 讨论了如果 \(\boldsymbol{Y}\) 是奇异的时候该怎么办。它还介绍了如何将约束稳定项(constraint stabilization terms)合并到装配公式中， (参见§8.3)。