刚体动力学算法 Chapter 3 刚体系统动力学（1）

Chapter 3 刚体系统动力学

动力学算法的目的是对运动方程进行数值求解，但在我们能够进行求解之前，我们首先需要知道这个方程是什么。本章介绍了一个一般刚体系统的运动方程，并提供了多种从简单方程构建这些方程的方法。这些方法构成了后续章节中出现的各种算法的基础。然而，本章的重点主要是刚体动力学的数学，而不是特定的算法。

刚体系统是由刚体组成的集合，这些刚体可以通过关节连接在一起，并且可能受到各种力的作用。关节的效果是对所连接的两个刚体施加运动约束：在某些方向上允许相对运动，而在其他方向上不允许。更准确地说，关节的效果是将一个约束力引入系统中。这个力具有特殊的性质，我们并不知道它的具体值，但我们知道它对系统产生的效果-它取决于它需要多大的值，以使得结果运动符合运动约束。本章的很大一部分涉及描述运动约束的方法，并将其应用于刚体运动方程。

第3.1节和第3.2节提供了对该主题的高层次介绍：

• 第3.1节以各种标准形式呈现了运动方程；
• 第3.2节解释了如何从描述子系统、单个刚体和运动约束的方程构建这些方程。

接下来的三节更详细地讨论了运动约束的话题：

• 第3.3节展示了如何将其表示为向量子空间；
• 第3.4节介绍了刚体系统中可能出现的约束类型的标准分类；
• 第3.5节提供了关节约束的详细模型。（我们将关节定义为对两个刚体的相对运动施加任何运动约束。）

最后，第3.6节和第3.7节展示了如何获得连接在一起的单个刚体和刚体集合的显式运动方程。这两节讲述了在制定运动方程时使用空间向量的方法。

3.1 运动方程

对于一般刚体系统的运动方程，通常可以写成以下的规范形式：

$$\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{\tau }\text{\hspace{1em}(3.1)}$$

在这个方程中，$\mathbf{q},\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ 分别是广义坐标、速度和加速度的向量，$\mathbf{\tau }$ 是广义力的向量。$\mathbf{H}$ 是广义惯性矩阵，用 $\mathbf{H}(\mathbf{q})$ 表示它依赖于 $\mathbf{q}$。类似地，$\mathbf{C}$ 是广义偏置力，用 $\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$ 表示它同时依赖于 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$。一旦理解了这些依赖关系，可以使用较短的符号 $\mathbf{H}$ 和 $\mathbf{C}$ 来表示。偏置力是能产生零加速度的 $\mathbf{\tau }$ 值。它包括考虑到的科里奥利力、离心力、重力以及作用在系统上除 $\mathbf{\tau }$ 之外的任何其他力。$\mathbf{H}$ 和 $\mathbf{C}$ 共同构成了运动方程的系数，$\mathbf{\tau }$ 和 $\ddot{\mathbf{q}}$ 是变量。除了在运动方程中的角色之外，$\mathbf{H}$ 还有一个重要的性质：系统的动能可以表示为

$$T=\frac{1}{2}{\dot{\mathbf{q}}}^{\mathrm{T}}\mathbf{H}\dot{\mathbf{q}}\text{\hspace{1em}(3.2)}$$

方程3.1 中的每个量都是坐标向量或矩阵。考虑到，至少简要地考虑一下，这些量代表的对象类型。为此，我们用符号 ${\mathbf{M}}^n$ 和 ${\mathbf{F}}^n$ 表示 $n$ 维运动矢量和力矢量空间，其中 $n$ 是由 方程3.1 描述的系统的自由度。现在我们可以说 $\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ 表示 ${\mathbf{M}}^n$ 的元素，而 $\mathbf{\tau }$ 和 $\mathbf{C}$ 表示 ${\mathbf{F}}^n$ 的元素，而 $\mathbf{H}$ 是从 ${\mathbf{M}}^n$ 到 ${\mathbf{F}}^n$ 的映射。然而，$\mathbf{q}$ 并不表示一个向量，而是表示系统配置空间中的一个点，该空间是一个 $n$ 维流形。

根据定义，要满足使得 ${\mathbf{\tau }}^{\mathrm{T}}\dot{\mathbf{q}}$ 是由 $\mathbf{\tau }$ 传递给系统的功率，因此，不能随机选择一组广义力变量。这意味着广义坐标系在 ${\mathbf{M}}^n$ 和 ${\mathbf{F}}^n$ 上实际上是对偶坐标系。这又意味着广义运动和力向量与空间向量具有许多代数性质相似，如第2章中所述。唯一的例外是在广义运动和广义力上没有定义叉积运算符。

严格来说，$\mathbf{H}$ 和 $\mathbf{C}$ 不仅依赖于 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$，还取决于刚体系统本身。因此，更准确的写法应该是 $\mathbf{H}(\text{模型},\mathbf{q})$ 和 $\mathbf{C}(\text{模型},\mathbf{q},\dot{\mathbf{q}})$，其中符号“模型”指的是描述特定刚体系统的数据集，包括组成部分：刚体和关节的数量、它们的连接方式、刚体的惯性参数等。我们称这种描述为系统模型，它是第4章的主题。现在，只需要知道这样一个模型存在，并且诸如 $\mathbf{H}$ 和 $\mathbf{C}$ 这样的量依赖于它。

我们最终的目标是能够计算出方程3.1中出现的一个或多个量的数值。特别是，我们对以下两个计算感兴趣：正向动力学，即给定 $\mathbf{\tau }$ 计算 $\ddot{\mathbf{q}}$，以及逆向动力学，即给定 $\ddot{\mathbf{q}}$ 计算 $\mathbf{\tau }$。我们可以简洁地表示这些计算如下：
$$\ddot{\mathbf{q}}=\mathrm{FD}(\text{ model },\mathbf{q},\dot{\mathbf{q}},\mathbf{\tau })\text{\hspace{1em}(3.3)}$$

和

$$\mathbf{\tau }=\mathrm{ID}(\mathrm{model},\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}),\text{\hspace{1em}(3.4)}$$

其中，$\mathrm{FD} $ 和 $\mathrm{ID} $ 是分别表示正向动力学计算和逆向动力学计算的函数。通过将这些方程与 公式3.1 进行比较，可以清楚地看出，$\mathrm{FD} $ 和 $\mathrm{ID} $ 必须分别计算为 ${\mathbf{H}}^{-1}(\mathbf{\tau }-\mathbf{C})$ 和 $\mathbf{H}\ddot{\mathbf{q}}+\mathbf{C}$。然而，实现它们的算法不一定需要计算 $\mathbf{C}$ 或$\mathbf{H}$ 或 ${\mathbf{H}}^{-1}$ 。因此，公式3.3 和 3.4 的有用属性在于它们清楚地显示了每个计算的输入和输出，但并不暗示任何特定的计算方法。

公式3.1 假设位置变量是速度变量的积分。这并不总是成立的。例如，如果系统包含非完整约束（见第3.4节），那么位置变量将比速度变量多。或者，某些类型的运动更适合由冗余的位置变量集来描述，这再次导致位置变量比速度变量多。即使位置和速度变量的数量相同，后者可能偶尔也可能是前者的导数以外的其他值。无论原因如何，我们可以通过用替代的速度变量向量 $\mathbf{\alpha }$ 替换 $\dot{\mathbf{q}}$ 来简单地改写 公式3.1，得到

$$\mathbf{H}(\mathbf{q})\dot{\mathbf{\alpha }}+\mathbf{C}(\mathbf{q},\mathbf{\alpha })=\mathbf{\tau }\text{\hspace{1em}(3.5)}$$

然后对 公式3.3 和 3.4 进行相应修改。为了仿真，我们仍然需要知道 $\dot{\mathbf{q}}$ 的值，以便可以将其积分得到 $\mathbf{q}$ 。因此， 公式3.5 必须补充一个方程，从 $\mathbf{q}$ 和 $\mathbf{\alpha }$ 计算 $\dot{\mathbf{q}}$ 。这个方程可以写成

$$\dot{\mathbf{q}}=\mathbf{Q}(\mathbf{q})\mathbf{\alpha }\text{\hspace{1em}(3.6)}$$

其中 $\mathbf{Q}$ 是一个依赖于 $\mathbf{q}$ 和系统模型的矩阵。或者，它可以写成

$$\dot{\mathbf{q}}=\mathrm{qdfn}(\mathrm{model},\mathbf{q},\mathbf{\alpha })\text{\hspace{1em}(3.7)}$$

其中qdfn表示从 $\mathbf{q}$ 和 $\mathbf{\alpha }$ 计算 $\dot{\mathbf{q}}$。公式3.6显示了 $\dot{\mathbf{q}}$ 与 $\mathbf{q}$ ， $\dot{\mathbf{q}}$ 和 $\mathbf{\alpha }$ 之间的数学关系的数学结构，它表明 $\dot{\mathbf{q}}$ 线性地依赖于 $\mathbf{\alpha }$ 。而公式3.7则显示了计算的输入和输出，而不暗示任何特定的计算方法。

仍然谈到仿真问题，大多数仿真器都期望给出一组一阶微分方程。我们可以通过定义一个状态变量 $\mathbf{x}={\left[{\mathbf{q}}^{\mathrm{T}}{\mathbf{\alpha }}^{\mathrm{T}}\right]}^{\mathrm{T}}$ 来满足这个要求，并以状态空间形式表示正向动力学：

$$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\mathbf{q}}\\ \dot{\mathbf{\alpha }}\end{array}\right]=\left[\begin{array}{c}\mathbf{Q}\mathbf{\alpha }\\ {\mathbf{H}}^{-1}(\mathbf{\tau }-C)\end{array}\right]\text{\hspace{1em}(3.8)}$$

如果 ${\mathrm{FD}}_x$表示状态空间正向动力学计算函数，则

$$\dot{\mathbf{x}}={\mathrm{FD}}_x(\mathrm{model},\mathbf{x},\mathbf{\tau })=\left[\begin{array}{c}\mathrm{qdfn}(\mathrm{model},\mathbf{q},\mathbf{\alpha })\\ \mathrm{FD}(\mathrm{model},\mathbf{q},\mathbf{\alpha },\mathbf{\tau })\end{array}\right]\text{\hspace{1em}(3.9)}$$

在接下来的章节中，我们将主要研究可以通过公式3.1描述的系统，而不是通过 公式3.5 描述的更一般的系统。这是因为我们将获得的大部分结果可以直接适应更一般类型的系统。大多数动力学算法对于公式3.1和3.5之间的区别是无关紧要的，除非涉及到编程细节的层面。

最后还有一个需要提及的可能性。有时识别和使用一个明确的独立速度变量集是不切实际或不可取的。在这种情况下，仍然使用公式3.1 或 公式3.5 作为运动方程；但现在 $\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ （或 $\mathbf{\alpha }$ 和 $\dot{\mathbf{\alpha }}$ ）受到一组运动约束的限制，所以运动方程必须补充一个第二个方程来描述这些约束。这个主题将在下一节中详细介绍。

3.2 构建运动方程

构建刚体系统的运动方程是一系列数学运算的结果。起点是单个刚体 和/或 子系统的运动方程集合，我们对它们执行的两个主要操作是：

• 整合多个方程形成更大子系统的方程
• 应用额外的运动约束

通过按特定顺序执行这些操作，我们定义了获取所需运动方程的特定过程，由此也定义了计算该方程数值的特定算法（或算法类）。以下是一些常见的过程：

1. 整合所有刚体的方程（将它们的方程合并为一个方程），然后施加所有约束。
2. 获取生成树（spanning tree）的运动方程，然后施加所有剩余的约束。
3. 从单一刚体/子系统开始，添加更多的刚体/子系统，并添加相关约束，重复这个过程。
4. 将子系统配对，施加配对子系统的约束，重复这个过程。

方法1 是许多简单动力学算法的典型方法。它很容易理解，但会产生稀疏矩阵。采用这种方法的算法必须使用稀疏矩阵技术，否则与其他算法相比速度会太慢。方法1的示例可见第3.7节。

方法2适用于闭环系统，并在第8章中详细介绍。闭环系统的生成树在第4章中被定义为一个动力学树，它考虑了原始系统中的所有刚体，但只考虑部分运动约束。方法2的关键是可以使用非常高效的算法来计算生成树的动力学。

方法3大致上是关节-刚体算法的工作原理，不过它使用的是关节-刚体方程而不是完整的运动方程。该算法在第7章中进行了描述。

方法4由分治算法（divide-and-conquer algorithms，Featherstone, 1999a,b）使用。采用这种方法的算法适用于并行计算机上的实现。

聚合运动方程

假设我们有一组 $N$ 个刚体子系统，其中第 $i$ 个子系统的运动方程为 ${\mathbf{H}}_i{\ddot{\mathbf{q}}}_i+{\mathbf{C}}_i={\mathbf{\tau }}_i$。如果我们希望将它们视为单个系统，则该系统的运动方程为
$$\left[\begin{array}{cccc}{\mathbf{H}}_1& 0& \cdots & 0\\ 0& {\mathbf{H}}_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\mathbf{H}}_N\end{array}\right]\left[\begin{array}{c}{\ddot{\mathbf{q}}}_1\\ {\ddot{\mathbf{q}}}_2\\ ⋮\\ {\ddot{\mathbf{q}}}_N\end{array}\right]+\left[\begin{array}{c}{\mathbf{C}}_1\\ {\mathbf{C}}_2\\ ⋮\\ {\mathbf{C}}_N\end{array}\right]=\left[\begin{array}{c}{\mathbf{\tau }}_1\\ {\mathbf{\tau }}_2\\ ⋮\\ {\mathbf{\tau }}_N\end{array}\right]\text{\hspace{1em}(3.10)}$$

请注意，单个刚体也是一个刚体子系统。因此，我们可以从一组 $N$ 个刚体开始，或者甚至是由单个刚体和其他系统任意混合而成的系统。如果子系统 $i$ 恰好是单个刚体，则我们可以使用其空间运动方程 ${\mathbf{I}}_i{\mathbf{a}}_i+{\mathbf{p}}_i={\mathbf{f}}_i$ 代替广义坐标方程（即，我们使用 ${\mathbf{I}}_i$ 代替 ${\mathbf{H}}_i$ 等）。这样做的原因是因为空间运动矢量和空间力矢量是广义运动矢量和力矢量的特例；也就是说，${\mathrm{M}}^6$ 和 ${\mathrm{F}}^6$ 是${\mathbf{M}}^n$ 和 ${\mathbf{F}}^n$的特例，${\mathrm{M}}^6$ 和 ${\mathrm{F}}^6$ 上的普吕克坐标是 ${\mathbf{M}}^n$ 和 ${\mathbf{F}}^n$ 上的广义坐标的特例。因此，我们可以自由地将空间速度与广义速度混合使用，对于加速度和力也是如此。

运动约束

运动约束是对系统的运动变量施加的代数约束。它们可以用两种不同的方式表达，如下所示：
$$\begin{array}{lccc}& \text{ position }& \text{ velocity }& \text{ acceleration }\\ \text{ implicit: }& \mathbf{\varphi }(\mathbf{q})=0& \mathbf{K}\dot{\mathbf{q}}=0& \mathbf{K}\ddot{\mathbf{q}}=\mathbf{k}\\ \text{ explicit: }& \mathbf{q}=\mathbf{\gamma }(\mathbf{y})& \dot{\mathbf{q}}=\mathbf{G}\dot{\mathbf{y}}& \ddot{\mathbf{q}}=\mathbf{G}\ddot{\mathbf{y}}+\mathbf{g}\end{array}\text{\hspace{1em}(3.11)}$$

其中

$$\mathbf{K}=\frac{\partial \mathbf{\varphi }}{\partial \mathbf{q}},\mathbf{k}=-\dot{\mathbf{K}}\dot{\mathbf{q}},\mathbf{G}=\frac{\partial \mathbf{\gamma }}{\partial \mathbf{y}}\text{ and }\mathbf{g}=\dot{\mathbf{G}}\dot{\mathbf{y}}.$$

隐式运动约束是指通过方程或等式隐含地定义的约束。这些约束可能涉及到机器人的几何形状、运动范围或物理限制等。通常，隐式运动约束以一种不直接明确给出的方式存在。例如，一个简单的约束可以是机器人的末端执行器必须保持在某个平面上。这个约束可以通过将末端执行器的位置矢量与该平面的法向量做点积，等于零来隐含地表示。

相比之下，显示运动约束是直接以等式或不等式的形式给出的约束条件。这些约束通常以数学表达式的形式描述，明确地规定了机器人的运动限制。例如，显示运动约束可以是机器人关节的角度不能超过特定的范围，或者末端执行器必须保持在一个特定的空间区域内。

这些不是最一般的方程。最一般的情况在 第3.4节 中讨论。如果 $\mathbf{\varphi }$ 和 $\mathbf{\gamma }$ 都描述相同的约束，则有：
$$\mathbf{\varphi }\circ \mathbf{\gamma }=0,\mathbf{KG}=0\text{ and }\mathbf{Kg}=\mathbf{k}.\text{\hspace{1em}(3.12)}$$

运动约束是由约束力引起的。一个无约束（或较少受约束）的刚体系统具有运动方程

$$\mathbf{H}\ddot{\mathbf{q}}+\mathbf{C}=\mathbf{\tau },\text{\hspace{1em}(3.13)}$$

并且该系统要受到运动约束的影响，则受约束系统的运动方程是

$$\mathbf{H}\ddot{\mathbf{q}}+\mathbf{C}=\mathbf{\tau }+{\mathbf{\tau }}_c,\text{\hspace{1em}(3.14)}$$

其中 ${\mathbf{\tau }}_c$ 是约束力。这个力是未知的，但它具有一个重要的特性，允许我们计算它的值，或者根据需要从方程中消去它：

约束力对与运动约束兼容的每个速度自由度的每个方向都不产生功（Jourdain原理的虚功原理）。(The constraint force delivers zero power along every direction of velocity freedom that is compatible with the motion constraints. (Jourdain’s principle of virtual power.))

虚功原理

如果一个刚体或系统处于平衡状态，那么它所受到的外力对其所做的虚功（做功的力沿着实际运动方向）必须为零。也就是说，在平衡状态下，刚体或系统所受到的力对其所做的实际功等于零。

在机器人运动学中，我们可以应用虚功原理来分析机器人的约束和主动力。以下是虚功原理的一般表达形式：

$$\delta W=0$$

其中，$\delta W$表示外力对刚体或系统所做的虚功，也称为虚位移功。它是各个力在相应的虚位移上的投影之和。

对于机器人而言，可以将虚功原理应用于分析机器人关节的平衡和约束条件。假设机器人具有 $n$ 个关节，则可以对每个关节 $j$ 应用虚功原理，得到虚功原理的方程形式：

$$\delta W_j={\mathbf{F}}_j\cdot \delta {\mathbf{q}}_j=0$$

其中，${\mathbf{F}}_j$ 表示作用在关节 $j$ 上的合力，$\delta {\mathbf{q}}_j$ 表示关节 $j$ 的虚位移。这个方程表明，合力对关节虚位移的投影为零。

应用虚功原理可以得到机器人关节的运动学方程和约束条件。通过对所有关节的虚功原理进行组合，可以获得机器人的整体运动方程，并用于控制和规划机器人的运动。

需要注意的是，虚功原理只能用于分析静力平衡或准静态条件下的运动，即假设刚体或系统的运动速度非常小。

实际上，这个说法意味着 ${\mathbf{\tau }}_c$ 必须满足 ${\mathbf{\tau }}_c\cdot \dot{\mathbf{q}}=0$，不仅对于 $\dot{\mathbf{q}}$ 的一个值，而是对于约束所允许的每个值都成立。如果系统受到隐式运动约束的影响，那么可以证明 ${\mathbf{\tau }}_c$ 的形式为

$${\mathbf{\tau }}_c={\mathbf{K}}^{\mathrm{T}}\mathbf{\lambda }\text{\hspace{1em}(3.15)}$$

其中 $\mathbf{\lambda }$ 是一组未知的力变量；如果系统受到显式运动约束的影响，那么 ${\mathbf{\tau }}_c$ 具有以下性质

$${\mathbf{G}}^{\mathrm{T}}{\mathbf{\tau }}_c=0.\text{\hspace{1em}(3.16)}$$

这些方程很容易验证。如果 ${\mathbf{\tau }}_c={\mathbf{K}}^{\mathrm{T}}\mathbf{\lambda }$，则 ${\mathbf{\tau }}_c\cdot \dot{\mathbf{q}}={\mathbf{\lambda }}^{\mathrm{T}}\mathbf{K}\dot{\mathbf{q}}$，并且只要 $\mathbf{K}\dot{\mathbf{q}}=0$，这个表达式就为零。同样，如果 $\dot{\mathbf{q}}=\mathbf{G}\dot{\mathbf{y}}$，则 $\dot{\mathbf{q}}\cdot {\mathbf{\tau }}_c={\dot{\mathbf{y}}}^{\mathrm{T}}{\mathbf{G}}^{\mathrm{T}}{\mathbf{\tau }}_c$，只要 ${\mathbf{G}}^{\mathrm{T}}{\mathbf{\tau }}_c=0$，这个表达式就为零。$\mathbf{\lambda }$ 的元素可以视为一组Lagrange乘子。

施加隐式运动约束

要将隐性运动约束施加于刚体系统，我们只需将方程3.15代入方程3.14，并将结果与方程3.11中的隐性加速度约束方程组合即可。结果是
$$\left[\begin{array}{cc}\mathbf{H}& {\mathbf{K}}^{\mathrm{T}}\\ \mathbf{K}& 0\end{array}\right]\left[\begin{array}{c}\ddot{\mathbf{q}}\\ -\mathbf{\lambda }\end{array}\right]=\left[\begin{array}{c}\mathbf{\tau }-\mathbf{C}\\ \mathbf{k}\end{array}\right]\text{\hspace{1em}(3.17)}$$

这是一个关于 $n+n_c$ 个未知数 $n+n_c$ 个方程的集合，其中 $n=\dim (\ddot{\mathbf{q}})$ 是无约束系统的自由度，$n_c=\dim (\mathbf{\lambda })=\dim (\mathbf{\varphi })$ 是对系统施加的约束的数量。可以证明系数矩阵的秩是 $n+n_{ic}$，其中 $n_{ic}=\mathrm{rank}(\mathbf{K})$ 是对系统施加的独立约束的数量。因此，如果 $n_{ic}=n_c$，则系数矩阵是非奇异的，可以解出 $\ddot{\mathbf{q}}$ 和 $\mathbf{\lambda }$ 的值。然而，如果 $n_{ic}<n_c$，则仍然可以解出 $\ddot{\mathbf{q}}$，但是 $\mathbf{\lambda }$ 中将有 $n_c-n_{ic}$ 个不确定度。当 $n_{ic}<n_c$ 时，系统被称为超约束。理想情况下，我们总希望 $n_{ic}=n_c$，但在实践中并不总是可能保证这一点。

施加显式运动约束

要将显式运动约束施加于刚体系统，我们将 方程3.14 与 方程3.16 以及 方程3.11 中的显式加速度约束方程组合。得到的方程为
$$\left[\begin{array}{ccc}H& -1& 0\\ -1& 0& G\\ 0& G^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\ddot{q}\\ {\tau }_c\\ \ddot{y}\end{array}\right]=\left[\begin{array}{c}\mathbf{\tau }-\mathbf{C}\\ -\mathbf{g}\\ 0\end{array}\right]\text{\hspace{1em}(3.18)}$$

对这个方程应用高斯消元法的一步操作得到

$$\left[\begin{array}{ccc}\mathbf{H}& -1& 0\\ 0& {\mathbf{H}}^{-1}& \mathbf{G}\\ 0& {\mathbf{G}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\ddot{\mathbf{q}}\\ {\mathbf{\tau }}_c\\ \ddot{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}\mathbf{\tau }-\mathbf{C}\\ {\mathbf{H}}^{-1}(\mathbf{\tau }-\mathbf{C})-\mathbf{g}\\ 0\end{array}\right],$$

从中我们可以提取出

$$\left[\begin{array}{cc}{\mathbf{H}}^{-1}& \mathbf{G}\\ {\mathbf{G}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{\tau }}_c\\ \ddot{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{H}}^{-1}(\mathbf{\tau }-\mathbf{C})-\mathbf{g}\\ 0\end{array}\right]\text{\hspace{1em}(3.19)}$$

对这个方程应用高斯消元法得到

$$\left[\begin{array}{cc}{\mathbf{H}}^{-1}& \mathbf{G}\\ 0& -{\mathbf{G}}^{\mathrm{T}}\mathbf{H}\mathbf{G}\end{array}\right]\left[\begin{array}{c}{\mathbf{\tau }}_c\\ \ddot{\mathbf{y}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{H}}^{-1}(\mathbf{\tau }-\mathbf{C})\mathbf{g}\\ -{\mathbf{G}}^{\mathrm{T}}(\mathbf{\tau }-\mathbf{C}-\mathbf{H}\mathbf{g})\end{array}\right]$$

从中我们得到

$${\mathbf{G}}^{\mathrm{T}}\mathbf{H}\mathbf{G}\ddot{\mathbf{y}}={\mathbf{G}}^{\mathrm{T}}(\mathbf{\tau }-\mathbf{C}-\mathbf{H}\mathbf{g})\text{\hspace{1em}(3.20)}$$

最后，如果我们进行代换 $\mathbf{u}={\mathbf{G}}^{\mathrm{T}}\mathbf{\tau }$，其中 $\mathbf{u}$ 是一组新的广义力，那么我们得到

$${\mathbf{H}}_G\ddot{\mathbf{y}}+{\mathbf{C}}_G=\mathbf{u}\text{\hspace{1em}(3.21)}$$

其中

$${\mathbf{H}}_G={\mathbf{G}}^{\mathrm{T}}\mathbf{H}\mathbf{G}\text{ and }{\mathbf{C}}_G={\mathbf{G}}^{\mathrm{T}}(\mathbf{C}+\mathbf{H}\mathbf{g})$$

因此，我们得到了一个新的受约束系统的运动方程，其代数形式与无约束系统的运动方程（3.13式）相同。

现在需要说明几点。首先，方程（3.19）可以看作是方程（3.17）的对偶，即运动和力的变量位置互换。然而，在实践中，方程（3.17）比方程（3.19）更有用。其次，方程（3.20）可以直接从方程（3.14）得出，首先通过左乘 ${\mathbf{G}}^{\mathrm{T}}$ 消除 ${\mathbf{\tau }}_c$，然后使用方程（3.11）中的显式加速度约束替换 $\ddot{\mathbf{q}}$。这个方法有时被称为投影法(projection method)，因为第一步相当于将运动方程投影到 ${\mathbf{G}}^{\mathrm{T}}$ 的值域空间上。第三，${\mathbf{H}}_G$ 继承了 $\mathbf{H}$ 的对称性和正定性的特性。它还具有约束系统的动能为

$$T=\frac{1}{2}{\dot{\mathbf{y}}}^{\mathrm{T}}{\mathbf{H}}_G\dot{\mathbf{y}}\text{\hspace{1em}(3.22)}$$

最后，我们可以证明 $\mathbf{u}\cdot \dot{\mathbf{y}}$ 是传递给约束系统的功率：

$$\mathbf{u}\cdot \dot{\mathbf{y}}={\left({\mathbf{G}}^{\mathrm{T}}\mathbf{\tau }\right)}^{\mathrm{T}}\dot{\mathbf{y}}={\mathbf{\tau }}^{\mathrm{T}}\mathbf{G}\mathbf{y}=\mathbf{\tau }\cdot \dot{\mathbf{q}}\text{\hspace{1em}(3.23)}$$

因此，$\mathbf{u}$ 符合广义力向量的定义。

3.3 向量子空间

向量子空间是描述和分析运动约束的自然数学工具，因为它们捕捉了约束的基本特性。例如，方程（3.13）中的系统在 ${\mathbf{M}}^n$ 中可以自由运动，因为 $\dot{\mathbf{q}}$ 是 ${\mathbf{M}}^n$ 的元素，并且没有约束。然而，方程（3.14）中的系统只受到子空间 $S\subset {\mathrm{M}}^n$ 的约束。如果约束被隐式描述，那么 $S=\mathrm{null}(\mathbf{K})$；如果约束被显式描述，那么 $S=$ range $(\mathbf{G})$。如果 ${\mathbf{K}}_1$ 和 ${\mathbf{K}}_2$ 都描述相同的约束，则它们必须共享的一点是 $\mathrm{null}\left({\mathbf{K}}_1\right)=\mathrm{null}\left({\mathbf{K}}_2\right)=S$。同样，如果 ${\mathbf{G}}_1$ 和 ${\mathbf{G}}_2$ 描述相同的约束，则它们必须共享的一点是 range $\left({\mathbf{G}}_1\right)=\mathrm{range}\left({\mathbf{G}}_2\right)=S$。因此，子空间 $S$ 而不是某个特定的矩阵捕捉了约束的本质。因此，本节回顾了一些关于向量子空间及其应用的基础知识。

向量子空间是指由一组向量张成的向量集合所形成的线性子空间。这个子空间由所选向量的线性组合所生成，并包含了所有这些线性组合的线性组合。向量子空间具有其特定的维数和性质。

更具体地说，给定一个向量空间V，它的一个子集S中的向量元素可以通过线性组合来生成一个向量子空间。这意味着我们可以从S中选择任意数量的向量，对它们进行标量乘法和向量加法操作，从而得到一个新的向量，该向量也属于S。换句话说，向量子空间是原始向量子集中的所有线性组合所生成的。

向量子空间遵循向量空间的定义，即它满足以下几个条件：

• 包含零向量，即原点
• 对于任意两个向量在子空间中的加法封闭
• 对于任意一个向量与标量的乘法封闭

向量子空间的维数由生成子空间的向量数量决定。如果生成子空间的向量数量为n，则该子空间的维数为n。此外，向量子空间的基向量也是表示该子空间的基础，它们是线性无关的，并且可用于表示子空间中的任何向量。

基础知识

向量空间的子空间是指一个既是子集又是向量空间的集合。因此，子空间是指对于加法和标量乘法封闭的任意子集。设 $V$ 是一个向量空间，Y={y1,y2,…}Y=\left\{\boldsymbol{y}_{1}, \boldsymbol{y}_{2}, \ldots\right\}Y={y1​,y2​,…} 是 $V$ 的任意子集。数量 $\mathrm{span}(Y)$ 表示由 $Y$ 的所有元素的线性组合构成的集合，因此它也是一个子空间。因此，满足 $Y=\mathrm{span}(Y)$ 的子集是一个子空间。如果 $S$ 是 $V$ 的子空间，Y={y1,y2,…}Y=\left\{\boldsymbol{y}_{1}, \boldsymbol{y}_{2}, \ldots\right\}Y={y1​,y2​,…} 是 $S$ 的一组基，则 $S=\mathrm{span}(Y)$，且 $\dim (S)=|Y|$（即子空间 $S$ 的维数等于 $Y$ 中元素的个数）。零维子空间是只包含零向量的集合。由于子空间本身也是一个向量空间，所以子空间可以有子空间，依此类推。

没有特殊的符号表示"是子空间"。相反，我们使用子集的符号。因此，我们用 $Y\subseteq V$ 表示 $Y$ 是 $V$ 的子集，用 $S\subseteq V$ 表示 $S$ 是一个子空间。必须阅读上下文来确定所指的含义。表达式 $S\subset V$ 表示 $S$ 是 $V$ 的子空间，这意味着 $\dim (S)<\dim (V)$。

假设 $S\subseteq V$，并且 $\dim (S)=m$，$\dim (V)=n$。则 $m$ 必须满足 $0\le m\le n$。如果 $m=0$，则 S={0}S=\{\mathbf{0}\}S={0}；如果 $m=n$，则 $S=V$；但是如果 $m$ 取任何其他值，则存在无穷多个具有该维数的子空间。唯一确定一个 $m$ 维子空间所需的参数数量为 $m(n-m)$。

矩阵表示

表示子空间的最简单方法是将其表示为矩阵的范围或零空间。我们主要使用前者。设 $S$ 是 $n$ 维向量空间 $V$ 的一个 $m$ 维子空间，S={s1,…,sm}\mathcal{S}=\left\{\boldsymbol{s}_{1}, \ldots, \boldsymbol{s}_{m}\right\}S={s1​,…,sm​} 是 $S$ 的一组基。则任意向量 $\mathbf{v}\in S$ 可以表示为 $\mathbf{v}={\sum }_{i=1}^m{\mathbf{s}}_i{\alpha }_i$ 的形式，其中 ${\alpha }_i$ 是 $\mathbf{v}$ 在 $\mathcal{S}$ 坐标系下的坐标。如果 ${\mathbf{s}}_i$ 本身是坐标向量，在 $V$ 的某个基下表示，则它们可以组合成一个 $n\times m$ 矩阵 $\mathbf{S}=\left[\begin{array}{lll}{\mathbf{s}}_1& \cdots & {\mathbf{s}}_m\end{array}\right]$，满足 $\mathrm{range}(\mathbf{S})=S$。此时，$\mathbf{v}$ 可以表示为 $\mathbf{v}=\mathbf{S}\mathbf{\alpha }$，其中 $\mathbf{\alpha }={\left[\begin{array}{lll}{\alpha }_1& \cdots & {\alpha }_m\end{array}\right]}^{\mathrm{T}}$。

矩阵 $\mathbf{S}$ 同时定义了一个子空间和一组基。事实上，在 $\mathbf{S}$ 中的 $mn$ 个数中，我们可以说有 $m(n-m)$ 个定义了子空间，$m^2$ 个定义了基。如果 S′={s1′,…,sm′}\mathcal{S}^{\prime}=\left\{\boldsymbol{s}_{1}^{\prime}, \ldots, \boldsymbol{s}_{m}^{\prime}\right\}S′={s1′​,…,sm′​} 是 $S$ 的另一组基，则矩阵 ${\mathbf{S}}^{\prime }=\left[\begin{array}{lll}{\mathbf{s}}_1^{\prime }& \cdots & {\mathbf{s}}_m^{\prime }\end{array}\right]$ 和 $\mathbf{S}$ 定义了相同的子空间，但是不同的基。此时，${\mathbf{S}}^{\prime }$ 可以表示为 ${\mathbf{S}}^{\prime }=\mathbf{S}\mathbf{A}$ 的形式，其中 $\mathbf{A}$ 是从 ${\mathcal{S}}^{\prime }$ 坐标系到 $\mathcal{S}$ 坐标系的坐标变换。（$\mathbf{A}$ 的元素满足 ${\mathbf{s}}_j^{\prime }={\sum }_{i=1}^m{\mathbf{s}}_i{A}_{ij}$。）因此，任何可逆矩阵 $\mathbf{A}$ 和矩阵 $\mathbf{S}\mathbf{A}$ 都描述了与 $\mathbf{S}$ 相同的子空间。

向量空间的零空间（Null Space），也被称为核空间（Kernel），是指线性映射中被映射到零向量的向量所形成的子空间。

给定一个线性映射（或线性变换）$T：V\to W$，其中 $V$ 和 $W$ 是两个向量空间。那么，映射 $T$ 的零空间是所有在 $V$ 中的向量 $v$，使得 $T(v)=0$ 的向量 $v$ 组成的子空间。简而言之，零空间就是被 $T$ 变换到零向量的所有向量的集合。

以矩阵表示的线性映射为例，对于矩阵$A：R^n\to R^m$，其零空间是所有满足$Av=0$的向量 $v$ 的集合。这些向量是矩阵 $A$ 的零特征值对应的特征向量。零空间的维数等于矩阵的列数减去矩阵的秩，可以用来描述线性映射的退化性和约束条件。

向量的分解

对一个向量进行的最基本操作之一是将其分解为两个子空间中的分量。给定向量 $\mathbf{v}\in V$ 和子空间 $S_1,S_2\subseteq V$，我们的目标是将 $\mathbf{v}$ 表示为 $\mathbf{v}={\mathbf{v}}_1+{\mathbf{v}}_2$ 的形式，其中 ${\mathbf{v}}_1\in S_1$，${\mathbf{v}}_2\in S_2$。如果 $\mathbf{v}\in \mathrm{span}\left(S_1\cup S_2\right)$，则可以进行这种分解；如果 S1∩S2={0}S_{1} \cap S_{2}=\{\mathbf{0}\}S1​∩S2​={0}（即 $S_1$ 和 $S_2$ 没有非零元素相同），则此分解是唯一的。如果我们希望对任何 $\mathbf{v}$ 进行可能且唯一的分解，则 $S_1$ 和 $S_2$ 必须满足的条件是：
S1∩S2={0} and dim⁡(S1)+dim⁡(S2)=dim⁡(V)(3.24) S_{1} \cap S_{2}=\{\mathbf{0}\} \quad \text { and } \quad \operatorname{dim}\left(S_{1}\right)+\operatorname{dim}\left(S_{2}\right)=\operatorname{dim}(V) \tag{3.24} S1​∩S2​={0} and dim(S1​)+dim(S2​)=dim(V)(3.24)

这些条件意味着 $\mathrm{span}\left(S_1\cup S_2\right)=V$。如果 $S_1$ 和 $S_2$ 满足式子 3.24，那么我们可以说 $V$ 是 $S_1$ 和 $S_2$ 的 direct sum，记作
$$V=S_1\oplus S_2\text{\hspace{1em}(3.25)}$$
这个等式简单地表示任何 $V$ 中的元素都可以唯一地分解为 $S_1$ 中的一个元素和 $S_2$ 中的一个元素的和。基本上，式子 3.24 和 3.25 表达了同样的意思。

现在让我们进行分解。给定一个向量 $\mathbf{v}\in V$ 和两个表示子空间的矩阵 ${\mathbf{S}}_1$ 和 ${\mathbf{S}}_2$，满足式子 3.25；任务是找到满足下式的 ${\mathbf{\alpha }}_1$ 和 ${\mathbf{\alpha }}_2$：
$$\mathbf{v}={\mathbf{S}}_1{\mathbf{\alpha }}_1+{\mathbf{S}}_2{\mathbf{\alpha }}_2=\left[\begin{array}{ll}{\mathbf{S}}_1& {\mathbf{S}}_2\end{array}\right]\left[\begin{array}{l}{\mathbf{\alpha }}_1\\ {\mathbf{\alpha }}_2\end{array}\right]\text{\hspace{1em}(3.26)}$$
这个问题可以使用以下结果解决：如果 $S_1\oplus S_2=V$，那么 $\left[\begin{array}{ll}{\mathbf{S}}_1& {\mathbf{S}}_2\end{array}\right]$ 定义了 $V$ 上的一组基，并且是一个非奇异矩阵。鉴于 ${\left[\begin{array}{ll}{\mathbf{S}}_1& {\mathbf{S}}_2\end{array}\right]}^{-1}$ 存在，方程式 3.26 的解简单地是
$$\left[\begin{array}{l}{\mathbf{\alpha }}_1\\ {\mathbf{\alpha }}_2\end{array}\right]={\left[\begin{array}{ll}{\mathbf{S}}_1& {\mathbf{S}}_2\end{array}\right]}^{-1}\mathbf{v}\text{\hspace{1em}(3.27)}$$
这个公式很容易推广到将向量分解为多于两个子空间的情况。

正交补(Orthogonal Complements)

如果两个欧几里得向量 $\mathbf{u}$ 和 $\mathbf{v}$ 满足方程 $\mathbf{u}\cdot \mathbf{v}=0$，则它们被称为正交向量。如果 $Y_1$ 和 $Y_2$ 是 ${\mathbb{R}}^n$ 的子集，并且 $Y_1$ 中的每个元素都与 $Y_2$ 中的每个元素正交，则这两个子集被称为正交的，记作 $Y_1\perp Y_2$。与给定子集 $Y\subseteq {\mathbb{R}}^n$ 正交的所有向量的集合被称为它的正交补，记作 $Y^{\perp }$。正交补总是一个子空间。表 3.1 列出了一些正交子集的基本性质。

这种正交性是基于欧几里得内积定义的，因此仅适用于欧几里得向量。然而，可以定义其他形式的正交性，这些正交性基于其他标量积。特别地，如果 $\mathbf{u}\in U$ 和 $\mathbf{v}\in V$，其中 $U=V^{\ast }$，那么可以定义 $\mathbf{u}$ 和 $\mathbf{v}$ 之间的标量积，并且可以说如果 $\mathbf{u}\cdot \mathbf{v}=0$，那么它们

Subsets:	Subspaces:
$Y_1\perp Y_2\Rightarrow Y_2\perp Y_1$	$\dim (S)+\dim \left(S^{\perp }\right)=\dim (U)$
$Y_1\perp Y_2\Rightarrow \mathrm{span}\left(Y_1\right)\perp Y_2$	$S_1\perp S_2\iff {\mathbf{S}}_1^{\mathrm{T}}{\mathbf{S}}_2=0$
$Y_1\perp Y_2\wedge Z\subseteq Y_1\Rightarrow Z\perp Y_2$	$S_1\oplus S_2^{\perp }=U\iff {\left({\mathbf{S}}_1^{\mathrm{T}}{\mathbf{S}}_2\right)}^{-1}$ exists
$Y_1\perp Y_2\Rightarrow Y_1\subseteq Y_2^{\perp }$	$S_1\oplus S_2^{\perp }=U\iff S_1^{\perp }\oplus S_2=V$
$Y^{\perp }=(\mathrm{span}(Y){)}^{\perp }$	Euclidean Subspaces:
${\left(Y^{\perp }\right)}^{\perp }=\mathrm{span}(Y)$	$S\oplus S^{\perp }={\mathrm{E}}^n$

表3.1：正交子集的属性。在一般情况下，$S,S_1,Y,Y_1\subseteq U$ 和 $S_2,Y_2\subseteq V$，其中$U=V^{\ast }$。 在欧几里德情况下，$S,\dots ,Y_2\subseteq {\mathrm{E}}^n$ 是正交的。

通过推广，如果 $Y_1$ 和 $Y_2$ 分别是 $U$ 和 $V$ 的子集，并且 $Y_1$ 的每个元素都与 $Y_2$ 的每个元素正交，则 $Y_1\perp Y_2$。同样，与 $Y\subseteq U$ 正交的 $V$ 的所有元素的集合是 $Y$ 的正交补。

观察到欧几里德和对偶的正交性具有不同且不兼容的数学属性。特别地，如果 $Y_1$ 和 $Y_2$ 是相同向量空间的子集，那么关系$Y_1\perp Y_2$ 只能在欧几里得意义上成立；而如果它们是不同向量空间的子集，则只能在对偶意义上成立。正交补在机器人学文献中具有一定的声名，这是由于将 $6\mathrm{D}$ 向量错误地视为欧几里德向量，并使用欧几里德形式的正交补而不是对偶形式。这个问题在Duffy（1990）中有所讨论。一些作者试图通过使用替代名称，如“‘natural orthogonal complement”或“reciprocal complement”，来与这个错误保持距离，以表示对偶正交补的形式。

正交补在刚体动力学中起到以下作用：如果系统最初可以在 ${\mathbf{M}}^n$ 中自由运动，并且运动约束将运动限制在子空间 $S\subseteq {\mathrm{M}}^n$ 中，则施加该约束在系统上的约束力是 $S^{\perp }\subseteq {\mathrm{F}}^n$ 的元素。

例3.1 假设我们已经给定一个运动子空间 $S\subseteq {\mathrm{M}}^6$ 和一个正定的空间惯性 $\mathbf{I}:{\mathrm{M}}^6\mapsto {\mathrm{F}}^6$ 。利用这些量，我们可以定义两个子空间 $T_a=\mathbf{I}S$ 和 $T_c=S^{\perp }$ ，它们具有性质 $T_a\oplus T_c={\mathrm{F}}^6$ 。（表达式 $\mathbf{I}S$ 是映射 $\mathbf{I}$ 下 $S$ 的像，即集合 {Is∣s∈S}\{\boldsymbol{I} \boldsymbol{s} {|} \boldsymbol{s} \in S\}{Is∣s∈S} 。）我们可以通过以下证明这个性质。根据等式3.24，我们必须证明 $\dim \left(T_a\right)+\dim \left(T_c\right)=6$ ，并且 $T_a\cap T_c$ 不包含非零元素。第一部分很容易：$\dim \left(T_a\right)=\dim (S)$ 和 $\dim \left(T_c\right)=6-\dim (S)$。为了证明第二部分，我们假设存在 $\mathbf{t}\ne 0\in \left(T_a\cap T_c\right)$ 并证明它导致矛盾：如果 $t\in T_a$ ，那么存在一个非零向量 $s\in S$ 使得 $\mathbf{t}=\mathbf{I}\mathbf{s}$ ；但如果 $\mathbf{t}\in T_c$ ，则 ${\mathbf{s}}^{\mathrm{T}}\mathbf{t}=0$ ，意味着 ${\mathbf{s}}^{\mathrm{T}}\mathbf{I}\mathbf{s}=0$ ；但这是不可能的，因为 $\mathbf{I}$ 是正定的。

在建立了 $T_a\oplus T_c={\mathrm{F}}^6$ 之后，我们可以将任何 $\mathbf{f}\in {\mathrm{F}}^6$ 唯一地分解为分量 ${\mathbf{f}}_a\in T_a$ 和 ${\mathbf{f}}_c\in T_c$ 。给定任意张成 $S$ 的矩阵 $\mathbf{S}$，我们可以按如下方式获得 ${\mathbf{f}}_a$ 和 ${\mathbf{f}}_c$：
$${\mathbf{S}}^{\mathrm{T}}\mathbf{f}={\mathbf{S}}^{\mathrm{T}}{\mathbf{f}}_a+{\mathbf{S}}^{\mathrm{T}}{\mathbf{f}}_c={\mathbf{S}}^{\mathrm{T}}{\mathbf{f}}_a,$$

but ${\mathbf{f}}_a=\mathbf{I}\mathbf{S}\mathbf{\alpha }$ for some $\mathbf{\alpha }$, so

$${\mathbf{S}}^{\mathrm{T}}\mathbf{f}={\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\mathbf{\alpha },$$

这意味着

$$\mathbf{\alpha }={\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}\mathbf{f},$$

因此

$${\mathbf{f}}_a=\mathbf{I}\mathbf{S}{\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}\mathbf{f}\text{\hspace{1em}(3.28)}$$

${\mathbf{f}}_c$ 可以通过简单地使用 ${\mathbf{f}}_c=\mathbf{f}-{\mathbf{f}}_a$ 来得到。方程式3.28 中的表达式显然不依赖于选择 $\mathbf{S}$ 的具体方式。我们可以通过将该方程中的每个 $\mathbf{S}$ 替换为 $\mathbf{S}\mathbf{A}$ 来证明这一点，其中 $\mathbf{A}$ 是维度正确的可逆矩阵，并且证明 ’ $\mathbf{A}$ ’ 互相抵消。

这种分解的物理解释如下。如果 $\mathbf{I}$ 是约束在子空间 $S$ 中刚体运动的惯性，并且 $\mathbf{f}$ 是作用在该刚体上的力，则 ${\mathbf{f}}_a$ 是导致加速度的分量，${\mathbf{f}}_c$ 是被约束力所抵消的分量（即约束力将为 $-{\mathbf{f}}_c$）。如果刚体静止，并且除了 $\mathbf{f}$ 和运动约束力之外没有受到其他力的影响，则其加速度将为

$$\mathbf{a}={\mathbf{I}}^{-1}{\mathbf{f}}_a=\mathbf{S}{\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}\mathbf{f}.$$

表达式 $\mathbf{S}{\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}$ 是受约束刚体的逆惯性矩阵（apparent inverse inertia）（参见方程式3.54）。