Chapter3: 刚体系统的动力学
Dynamics of Rigid Body Systems

动力学主要是计算运动方程的数值解，首先，在计算之前，我们需要先知道方程长啥样。本章介绍一般刚体系统的运动方程，并从简单的开始，展示构建这些公式的各种方法。这些方法是后续章节中出现的各种算法的基础。本章主要关注刚体动力学的数学表述，而不是特定的算法。

一个刚体系统，是由若干个关节连接在一起的刚体构成的，它们可能受到各种力的作用。关节的作用是对其连接的两个物体施加运动约束：允许某些方向上的相对运动，但在其他方向上不允许。更确切的说法是，关节给系统引入了约束力。我们并不知道这种力的大小，但是我们很清楚它确实对系统产生了作用，它的实际大小使得运动约束得以满足。本章大部分内容都是在讲描述运动约束的方法，以及如何将之应用到刚体的运动方程中。

3.1 节和3.2 节对本章的主题进行一些高维度的介绍，其中，3.1节介绍了运动方程的各种标准形式， 3.2 节解释了它们是如何从描述子系统、单个刚体和运动约束的方程中构造的。接下来三节，描述运动约束的一些细节。 3.3 节描述了如何把它们表示成向量子空间，3.4 节对刚体系统中可能出现的约束类型进行了标准分类， 3.5 节描述了关节约束的详细模型，按照我们的定义，只要在两个刚体之间建立了运动学约束就是关节。最后，3.6 节和3.7 节介绍如何获取单个刚体，以及有关节连接在一起的刚体集合的，显式运动方程。最后两节体现了空间矢量在构建运动方程中的应用。

3.1 运动方程

通常，刚体系统的运动方程可以写成如下的形式: $$ \begin{equation}\label{equa_3_1} \boldsymbol{H}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) = \boldsymbol{\tau} \end{equation} $$ 在该方程中，$\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}$是广义的位置、速度、加速度向量，$\boldsymbol{\tau}$ 是广义力。 $\boldsymbol{H}$ 是广义惯性矩阵，我们将其写成$\boldsymbol{H}(\boldsymbol{q})$的形式是要强调它与$\boldsymbol{q}$相关。类似的，$\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})$是依赖于$\boldsymbol{q}, \dot{\boldsymbol{q}}$的广义偏置力。在没有歧义的情况下，我们会直接使用$\boldsymbol{H,C}$。偏置力因作用力而改变，它们将使得总体的加速度为 0。The bias force is simply the value of $\boldsymbol{\tau}$ that will produce zero acceleration. 它涵盖了科里奥利力、离心力、重力以及作用在系统上的任何其他不是 $\boldsymbol{\tau}$ 力。总的来说，$\boldsymbol{H,C}$是运动方程的系数，$\boldsymbol{\tau}, \ddot{\boldsymbol{q}}$是其中的变量。此外，$\boldsymbol{H}$还有一个重要的特性，用于描述系统的能量: $$ \begin{equation}\label{equa_3_2} T = \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{H} \dot{\boldsymbol{q}} \end{equation} $$

式($\ref{equa_3_1}$)中的每个量都是坐标向量或者矩阵。记 $n$ 维的运动和力向量空间分别为 $M^n$ 和 $F^n$，其中 $n$ 是式($\ref{equa_3_1}$)中描述的系统自由度。现在我们可以说$\dot{\boldsymbol{q}}$ 和$\ddot{\boldsymbol{q}}$是空间$M^n$中的元素，$\boldsymbol{\tau}$ 和 $\boldsymbol{C}$ 是空间$F^n$的元素，矩阵$\boldsymbol{H}$将空间$M^n$映射到空间$F^n$上。然而，$\boldsymbol{q}$却不是一个向量，它描述的是系统配置空间中的一个点，这个配置空间常被称为 $n$ 维流形(n-dimensional manifold)。

根据定义，有一组广义力的变量是不能随便选择的，必须能够以 $\boldsymbol{\tau}^T \dot{\boldsymbol{q}}$ 的形式描述 $\boldsymbol{\tau}$ 作用到系统上的功率。这意味着，广义坐标系统下空间$M^n$ 和 $F^n$ 实际上是对偶的。因此，广义运动和力向量适用第2章中介绍的空间向量的很多性质。最大的意外是，叉乘运算符并不是定义在广义运动和力上的。

严格来说，$\boldsymbol{H}$ 和 $\boldsymbol{C}$ 不仅仅依赖于 $\boldsymbol{q}$ 和 $\dot{\boldsymbol{q}}$，还与刚体系统本身有关。因此，写成 $\boldsymbol{H}(model, \boldsymbol{q})$ 和 $\boldsymbol{C}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}$ 的形式更准确一些。其中 $model$ 是指刚体系统的一些特性，比如，刚体和关节的数量，刚体之间的连接方式，刚体的惯性参数等。我们称之为系统模型system model，将在第4章中介绍。

我们的终极目标是计算出式($\ref{equa_3_1}$)中的一个或多个变量的数值解。我们主要研究的就是两种计算:正动力学 forward dynamics，给定 $\boldsymbol{\tau}$ 计算 $\ddot{\boldsymbol{q}}$，和逆动力学 inverse dynamics，给定 $\ddot{\boldsymbol{q}}$ 计算 $\boldsymbol{\tau}$。我们可以将他们写成如下的形式: $$ \begin{equation}\label{equa_3_3} \ddot{\boldsymbol{q}} = \text{FD}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{\tau}) \end{equation} $$ $$ \begin{equation}\label{equa_3_4} \boldsymbol{\tau} = \text{ID}(model, \boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}) \end{equation} $$ 其中，FD 和 ID 表示前向和逆动力学的函数式。根据式($\ref{equa_3_1}$)，FD 和 ID 分别是 $\boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C})$ 和 $\boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C}$。但是算法的实现不需要计算$\boldsymbol{C}, \boldsymbol{H}, \boldsymbol{H}^{-1}$。因此，式($\ref{equa_3_3}$)和式($\ref{equa_3_4}$)的优势在于它清晰的描述了输入和输出，但是并没有给出具体的计算方法。

式($\ref{equa_3_1}$)中存在一个假设，位置变量是速度变量的积分。但实际情况并不总是如此。比如说，如果一个系统里存在非完整(nonholonomic)约束(详见§3.4) 那么它的位置量要比速度量多。有时为了描述一些特殊类型的运动，会引入一些冗余的位置量，这也将道之位置量比速度量多。即是位置和速度量的数量相同，速度量也可能不是位置的微分 it may occasionally be useful for the latter to be something other than the derivatives of the former。不管什么原因，我们都可以将式($\ref{equa_3_1}$)中的 $\dot{\boldsymbol{q}}$替换成任何速度变量$\boldsymbol{\alpha}$: $$ \begin{equation}\label{equa_3_5} \boldsymbol{H}(\boldsymbol{q})\dot{\boldsymbol{\alpha}} + \boldsymbol{C}(\boldsymbol{q}, \boldsymbol{\alpha}) = \boldsymbol{\tau} \end{equation} $$ 出于仿真的目的，我们仍然需要知道$\dot{\boldsymbol{q}}$的值，才能够通过对其积分获得 $\boldsymbol{q}$。因此，式($\ref{equa_3_5}$)必须提供一个从$\boldsymbol{q}$和$\boldsymbol{\alpha}$ 计算 $\dot{\boldsymbol{q}}$的公式，其形式为: $$ \begin{equation}\label{equa_3_6} \dot{\boldsymbol{q}} = \boldsymbol{Q}(\boldsymbol{q})\boldsymbol{\alpha} \end{equation} $$ 其中，$\boldsymbol{Q}$是一个关于$\boldsymbol{q}$和系统模型的矩阵。有时我们还将上式记为: $$ \begin{equation}\label{equa_3_7} \dot{\boldsymbol{q}} = \text{qdfn}(model, \boldsymbol{q}, \boldsymbol{\alpha}) \end{equation} $$ 其中，qdfn 表示从 $\boldsymbol{q}$ 和 $\boldsymbol{\alpha}$ 计算得到 $\dot{\boldsymbol{q}}$的函数。式($\ref{equa_3_6}$) 揭露了$\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{\alpha}$ 之间关系的数学结构。而式($\ref{equa_3_7}$) 只描述了输入和输出，并没有提供任何特定的计算方法。

对于仿真而言，大多数模拟器都希望获得一组一阶微分方程。我们可以通过定义一个状态变量 $\boldsymbol{x} = \begin{bmatrix} \boldsymbol{q}^T & \boldsymbol{\alpha}^T \end{bmatrix}^T$，并在状态空间中表达正动力学，来达到这点: $$ \begin{equation}\label{equa_3_8} \dot{\boldsymbol{x}} = \begin{bmatrix} \dot{\boldsymbol{q}} \\ \dot{\boldsymbol{\alpha}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{Q\alpha} \\ \boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C}) \end{bmatrix} \end{equation} $$ 若 $\text{FD}_x$ 表示状态空间的正动力学公式，则: $$ \begin{equation}\label{equa_3_9} \dot{\boldsymbol{x}} = \text{FD}_x(model, \boldsymbol{x}, \boldsymbol{\tau}) = \begin{bmatrix} \text{qdfn}(model, \boldsymbol{q}, \boldsymbol{\alpha}) \\ \text{FD}(model, \boldsymbol{q}, \boldsymbol{\alpha}, \boldsymbol{\tau}) \end{bmatrix} \end{equation} $$ 接下来的章节中，我们主要研究通过式($\ref{equa_3_1}$)描述的系统，而不是式($\ref{equa_3_5}$)所描述的更一般系统。因为大多数情况下，我们从简单的系统中获得结论，都可以直接推广到更一般的系统中。大多数动力学算法是不关心式($\ref{equa_3_1}$)和式($\ref{equa_3_5}$)之间的差别的，但是编程的时候就需要关心它。

最后还有一点需要注意。有时辨识和处理一组显式的独立速度变量是不切实际、不可取的。在这种情况下，我们仍然用式($\ref{equa_3_1}$)或者式($\ref{equa_3_5}$)描述运动方程，但是此时$\dot{\boldsymbol{q}},\ddot{\boldsymbol{q}}$ (或者$\boldsymbol{\alpha}, \dot{\boldsymbol{\alpha}}$) 受限与一组运动约束，因此必须补充第二组方程来描述这些约束，才能完整写出运动方程。我们将在下一节中介绍这一主题。

3.2 构造运动方程

我们通过一组数学运算获得刚体系统的运动方程。从单个刚体和/或子系统的运动方程的集合开始，我们对它们执行的两个主要操作是:

将方程收集在一起，形成一个更大的子系统的方程；
应用其他运动约束。

按照特定顺序执行这些操作，我们定义了一个特定的程序，来获得所需的运动方程。因此也定义了一个特定的算法(或者式一类算法)，供计算机计算该方程的数值解。下面是一些常用的套路:

收集所有的刚体(比如说，把他们的方程整理到一个方程中)，然后应用所有的约束。
获取生成树(spanning tree)的运动方程，然后应用所有剩余的约束。
从单个刚体或者子系统开始，添加一个刚体或者子系统，并应用与新增刚体或子系统相关的约束，重复。
成对地组合子系统，并在每个子系统中分别应用约束，重复。

很多简单动力学算法使用的就是方法 1。它很好理解，但是容易产生大而稀疏的矩阵。使用这种方法的算法必须使用稀疏矩阵技术，否则相比于其他方法它将跑的很慢。 3.7节中将给出一个这样的示例。方法 2 是闭环系统的标准方法，我们将在第8章中详细讨论它。第4章中将闭环系统的生成树(spanning tree)定义为一棵运动学树(kinematic tree)，它考虑了原始系统中所有的刚体和部分运动约束。方法 2 的优点是有大量高效的算法可以用来计算生成树的动力学。方法 3 大致是关节体(articulated-body)算法的工作方式，不同之处是关节体算法使用的是关节体方程而不是完整的运动方程，我们将在第7章中介绍该算法。最后，方法 4 常见于分治算法(divide-and conquer, Featherstone, 1999a,b)。这类算法通常可以通过并行计算加速。

收集运动方程(collecting equations of motion)

假设我们有一组 $N$ 个刚体构成的子系统，其中第 $i$ 个系统的运动方程为 $\boldsymbol{H}_i \ddot{\boldsymbol{q}}_i + \boldsymbol{C}_i = \boldsymbol{\tau}_i$。如果我们想把他们看做是一个系统，那么需要将他们的运动方程写成如下的形式: $$ \begin{equation}\label{equa_3_10} \begin{bmatrix} \boldsymbol{H}_1 & \boldsymbol{0} & \cdots & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{H}_2 & \cdots & \boldsymbol{0} \\ \vdots & \vdots & \ddots & \vdots \\ \boldsymbol{0} & \boldsymbol{0} & \cdots & \boldsymbol{H}_N \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{q}}_1 \\ \ddot{\boldsymbol{q}}_2 \\ \vdots \\ \ddot{\boldsymbol{q}}_N \end{bmatrix} + \begin{bmatrix} \boldsymbol{C}_1 \\ \boldsymbol{C}_2 \\ \vdots \\ \boldsymbol{C}_N \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau}_1 \\ \boldsymbol{\tau}_2 \\ \vdots \\ \boldsymbol{\tau}_N \end{bmatrix} \end{equation} $$ 需要注意，单个刚体也是一个刚体子系统。因此，我们可以从一组 $N$ 个刚体，或者单个刚体于其他系统的任意组合开始。如果子系统 $i$ 恰好是一个刚体，那么我们可以用他的空间运动方程(spatial equation of motion) $\boldsymbol{I}_i \boldsymbol{a}_i + \boldsymbol{p}_i = \boldsymbol{f}_i$ 代替广义坐标运动方程(generalized-coordinates equation of motion)，比如用$\boldsymbol{I}_i$ 代替 $\boldsymbol{H}_i$等。之所以可以这样做，是因为空间运动和力向量是广义运动和力向量的空间形式。因为，$M^6$ 和 $F^6$是空间 $M^n$ 和 $F^n$的特例， $M^6$和$F^6$的普吕克坐标是$M^n$和$F^n$的广义坐标系的特殊形式。因此，我们可以自由的使用空间速度替换广义速度，类似的操作也适用于加速度和力。

运动约束(motion constraints)

运动约束是系统中运动变量的几何约束，它可以以如下的两种形式表示: $$ \begin{equation}\label{equa_3_11} \begin{array}{c} & \text{position} & \text{velocity} & \text{acceleration} \\ \text{implicit}: & \boldsymbol{\phi}(\boldsymbol{q}) = \boldsymbol{0} & \boldsymbol{K}\dot{\boldsymbol{q}} = \boldsymbol{0} & \boldsymbol{K}\ddot{\boldsymbol{q}} = \boldsymbol{k} \\ \text{explicit}: & \boldsymbol{q} = \boldsymbol{\gamma}(\boldsymbol{y}) & \dot{\boldsymbol{q}} = \boldsymbol{G}\dot{\boldsymbol{y}} & \ddot{\boldsymbol{q}} = \boldsymbol{G}\ddot{\boldsymbol{y}} + \boldsymbol{g} \\ \end{array} \end{equation} $$ 其中， $$ \boldsymbol{K} = \frac{\partial \boldsymbol{\phi}}{\partial \boldsymbol{q}}, \qquad \boldsymbol{k} = - \dot{\boldsymbol{K}}\dot{\boldsymbol{q}}, \qquad \boldsymbol{G} = \frac{\partial \boldsymbol{\gamma}}{\partial \boldsymbol{y}}, \qquad \boldsymbol{g} = \dot{\boldsymbol{G}}\dot{\boldsymbol{y}} $$ 它们并不是最一般的形式，3.4节将介绍一般情况。如果 $\boldsymbol{\phi}$ 和 $\boldsymbol{\gamma}$ 描述的是相同的约束，那么有 $$ \begin{equation}\label{equa_3_12} \boldsymbol{\phi} \circ \boldsymbol{\gamma} = \boldsymbol{0}, \qquad \boldsymbol{KG} = \boldsymbol{0}, \qquad \boldsymbol{Kg} = \boldsymbol{k} \end{equation} $$

运动约束是由约束力产生的。对于一个无约束(或者欠约束)的刚体系统，有运动方程: $$ \begin{equation}\label{equa_3_13} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} \end{equation} $$ 如果这个系统受到运动约束，那么有受约束的运动方程: $$ \begin{equation}\label{equa_3_14} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} + \boldsymbol{\tau}_c \end{equation} $$ 其中，$\boldsymbol{\tau}_c$是约束力。这个力是未知的，但是它有一个重要的特性，使得我们可以计算出它的值，或者从方程中约去它:

约束力沿着任何与运动约束兼容的速度自由度的方向上的做功都是零(Jourdain's虚功原理)。
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.)

这意味着，$\boldsymbol{\tau}_c$必须满足$\boldsymbol{\tau}_c \cdot \dot{\boldsymbol{q}} = 0$，不仅仅是对$\dot{\boldsymbol{q}}$中的某个值成立，而且是对于约束允许的任何值都成立。如果是隐式运动约束，有 $$ \begin{equation}\label{equa_3_15} \boldsymbol{\tau}_c = \boldsymbol{K}^T\boldsymbol{\lambda} \end{equation} $$ 其中，$\boldsymbol{\lambda}$是一个未知力的向量。如果是显式运动约束，那么有 $$ \begin{equation}\label{equa_3_16} \boldsymbol{G}^T\boldsymbol{\tau}_c = \boldsymbol{0} \end{equation} $$ 这点很容易证明。如果 $\boldsymbol{\tau}_c = \boldsymbol{K}^T\boldsymbol{\lambda}$ 那么 $\boldsymbol{\tau}_c \cdot \dot{\boldsymbol{q}} = \boldsymbol{\lambda}^T \boldsymbol{K} \dot{\boldsymbol{q}}$ 对于每个使得$\boldsymbol{K}\dot{\boldsymbol{q}} = \boldsymbol{0}$成立的$\dot{\boldsymbol{q}}$都能使该式为零。如果$\dot{\boldsymbol{q}} = \boldsymbol{G}\dot{\boldsymbol{y}}$，那么$\dot{\boldsymbol{q}} \cdot \boldsymbol{\tau}_c = \dot{\boldsymbol{y}}^T \boldsymbol{G}^T \boldsymbol{\tau}_c$。任何满足$\boldsymbol{G}^T\boldsymbol{\tau}_c = \boldsymbol{0}$的$\dot{\boldsymbol{y}}$都能使该式为零。$\boldsymbol{\lambda}$的可以看做是拉格朗日乘子的集合。

隐式运动约束的应用(applying implicit motion constraints)

在刚体系统中应用隐式的运动约束，直接将式($\ref{equa_3_15}$)代入式($\ref{equa_3_14}$)，结合式($\ref{equa_3_11}$)中描述的隐式加速度约束公式，有: $$ \begin{equation}\label{equa_3_17} \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{k} \end{bmatrix} \end{equation} $$ 上式一共有$n + n_c$个方程组，$n + n_c$ 个未知数，其中 $n = \text{dim}(\ddot{\boldsymbol{q}})$ 表示非约束系统的自由度数， $n_c = \text{dim}(\boldsymbol{\lambda}) = \text{dim}(\boldsymbol{\phi})$是作用在系统上的约束数量。上式的系数矩阵的秩是 $n + n_{ic}$，其中，$n_{ic} = \text{rank}(\boldsymbol{K})$ 是作用在系统上的独立约束的数量。因此，如果$n_{ic} = n_c$，那么系数矩阵就是非奇异的，我们可以从式($\ref{equa_3_17}$)中解出$\ddot{\boldsymbol{q}}$和$\boldsymbol{\lambda}$。即使 $n_{ic} < n_c$ 我们仍然有可能从中解出 $\ddot{\boldsymbol{q}}$，但是 $\boldsymbol{\lambda}$会有 $n_c - n_{ic}$ 度的不确定性。我们说 $n_{ic} < n_c$ 这样的系统是过约束的(overconstrained)。理想情况是有$n_{ic} = n_c$，但是在实践中总是不能保证这点。

显式运动约束的应用(applying explicit motion constraints)

组合式($\ref{equa_3_14}$)和式($\ref{equa_3_16}$)以及式($\ref{equa_3_11}$)中描述的显式加速度约束方程，可以将显式运动约束应用到刚体系统中: $$ \begin{equation}\label{equa_3_18} \begin{bmatrix} \boldsymbol{H} & -\boldsymbol{1} & \boldsymbol{0} \\ -\boldsymbol{1} & \boldsymbol{0} & \boldsymbol{G} \\ \boldsymbol{0} & \boldsymbol{G}^T & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{q}} \\ \boldsymbol{\tau}_c \\ \ddot{\boldsymbol{y}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \boldsymbol{C} \\ -\boldsymbol{g} \\ \boldsymbol{0} \end{bmatrix} \end{equation} $$ 对上式进行一下高斯消元，有 $$ \begin{bmatrix} \boldsymbol{H} & -\boldsymbol{1} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{H}^{-1} & \boldsymbol{G} \\ \boldsymbol{0} & \boldsymbol{G}^T & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \ddot{\boldsymbol{q}} \\ \boldsymbol{\tau}_c \\ \ddot{\boldsymbol{y}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \boldsymbol{C} \\ \boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C}) -\boldsymbol{g} \\ \boldsymbol{0} \end{bmatrix} $$ 从中，我们可以提取出 $$ \begin{equation}\label{equa_3_19} \begin{bmatrix} \boldsymbol{H}^{-1} & \boldsymbol{G} \\ \boldsymbol{G}^T & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \boldsymbol{\tau}_c \\ \ddot{\boldsymbol{y}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C}) -\boldsymbol{g} \\ \boldsymbol{0} \end{bmatrix} \end{equation} $$ 对其进行高斯消元，有 $$ \begin{bmatrix} \boldsymbol{H}^{-1} & \boldsymbol{G} \\ \boldsymbol{0} & -\boldsymbol{G}^T \boldsymbol{HG} \end{bmatrix} \begin{bmatrix} \boldsymbol{\tau}_c \\ \ddot{\boldsymbol{y}} \end{bmatrix} = \begin{bmatrix} \boldsymbol{H}^{-1}(\boldsymbol{\tau} - \boldsymbol{C}) - \boldsymbol{g} \\ - \boldsymbol{G}^T (\boldsymbol{\tau} - \boldsymbol{C} - \boldsymbol{Hg}) \end{bmatrix} $$ 因此，有 $$ \begin{equation}\label{equa_3_20} \boldsymbol{G}^T \boldsymbol{HG} \ddot{\boldsymbol{y}} = \boldsymbol{G}^T (\boldsymbol{\tau} - \boldsymbol{C} - \boldsymbol{Hg}) \end{equation} $$ 令，$\boldsymbol{u} = \boldsymbol{G}^T\boldsymbol{\tau}$表示新的广义力，$\boldsymbol{H}_G = \boldsymbol{G}^T\boldsymbol{HG}, \boldsymbol{C}_G = \boldsymbol{G}^T(\boldsymbol{C} + \boldsymbol{Hg})$，有: $$ \begin{equation}\label{equa_3_21} \boldsymbol{H}_G \ddot{\boldsymbol{y}} + \boldsymbol{C}_G = \boldsymbol{u} \end{equation} $$ 至此，我们得到了带约束系统的运动方程，它的形式与式($\ref{equa_3_13}$)中描述的非约束系统的运动方程一样。

有几点需要强调一下。首先，式($\ref{equa_3_19}$)可以看做式($\ref{equa_3_17}$)的对偶，因为运动和力变量的位置发生了变化。但是式($\ref{equa_3_17}$)比($\ref{equa_3_19}$)更实用。其次，对式($\ref{equa_3_14}$)两侧左乘$\boldsymbol{G}^T$可以直接得到式($\ref{equa_3_20}$)，消除$\boldsymbol{\tau}_c$，用式($\ref{equa_3_11}$)中的显式加速度约束替代$\ddot{\boldsymbol{q}}$。这种方法的相当于将运动方程投影到$\boldsymbol{G}^T$的值空间(range space)中，因此有时被称为投影法(projection method)。再次，$\boldsymbol{H}_G$ 继承了 $\boldsymbol{H}$ 的对称性和正定性。此外它还可以用于描述受约束系统的动能: $$ \begin{equation}\label{equa_3_22} T = \frac{1}{2} \dot{\boldsymbol{y}}^T \boldsymbol{H}_G \dot{\boldsymbol{y}} \end{equation} $$ 最后，$\boldsymbol{u} \cdot \dot{\boldsymbol{y}}$ 是约束系统的功率: $$ \begin{equation}\label{equa_3_23} \boldsymbol{u} \cdot \dot{\boldsymbol{y}} = \left( \boldsymbol{G}^T \boldsymbol{\tau} \right)^T \dot{\boldsymbol{y}} = \boldsymbol{\tau}^T \boldsymbol{G} \dot{\boldsymbol{y}} = \boldsymbol{\tau} \cdot \dot{\boldsymbol{q}} \end{equation} $$ 因此，$\boldsymbol{u}$ 实际并不满足广义力的定义。

3.3 矢量子空间

矢量子空间自然是描述和分析运动约束的数学工具，因为它抓住了约束的本质。比如说，式($\ref{equa_3_13}$)中描述的系统可以在空间 $M^n$ 中自由移动，因为 $\dot{\boldsymbol{q}}$ 是 $M^n$ 中的元素并且无约束。但是式 ($\ref{equa_3_14}$) 中的系统是有约束的，只能在一个子空间 $S \subset M^n$ 中运动。如果隐式的描述了运动约束，那么 $S = \text{null}(\boldsymbol{K})$，如果是显式地描述，则 $S = \text{range}(\boldsymbol{G})$。如果 $\boldsymbol{K}_1, \boldsymbol{K}_2$描述的相同的约束，那么它们的运动子空间就是一个，即 $\text{null}(\boldsymbol{K}_1) = \text{null}(\boldsymbol{K}_2) = S$。类似的，如果 $\boldsymbol{G}_1, \boldsymbol{G}_2$ 描述的是相同约束，那么有 $\text{range}(\boldsymbol{G}_1) = \text{range}(\boldsymbol{G}_2) = S$。因此，捕捉约束本质的是子空间 S，而不是其它什么特殊的矩阵。本节，我们介绍一下矢量子空间的一些基础性质以及如何使用它们。

基础(Basics)

矢量空间的子空间是其一个子集，只要该子集中的元素对加法和数乘封闭，就是一个子空间。令 $V$ 表示一个矢量空间，$Y = \left\{\boldsymbol{y}_1, \boldsymbol{y}_2, \cdots \right\}$ 表示 $V$ 中的任何一个子集。$Y$ 中所有元素的线性组合构成的集合 $\text{span}(Y)$ 是一个子空间。如果 $S$ 是 $V$ 的子空间，并且 $Y = \left\{\boldsymbol{y}_1, \boldsymbol{y}_2, \cdots \right\}$ 是 $S$ 的一组基，那么 $S = \text{span}(Y)$ 并且 $S$ 空间的维度与集合 $Y$ 中的元素相同，$\text{dim}(S) = |Y|$。零维子空间是仅包含零向量的集合。子空间本身也是向量空间，它也可能具有子空间。

我们没有定义特别的符号用来表示"is a subspace of"，而是采用了子集的符号。我们用 $Y \subseteq V$ 表示 $Y$ 是 $V$ 的子集，也用 $S \subseteq V$ 表示 $S$ 是其一个子空间。我们必须通过上下文确认实际的含义。表达式 $S \subset V$ 表示 $S$ 是 $V$ 的一个真子空间，意味着 $\text{dim}(S) < \text{dim}(V)$。

假设 $S \subseteq V$，并且 $\text{dim}(S) = m, \text{dim}(V) = n$，那么有 $0 ≤ m ≤ n$。如果 $m = 0$，那么 $S = \{\boldsymbol{0}\}$，如果 $m = n$，则 $S = V$。如果 $m$ 取其他任何值，就存在无限多个维度为$m$的子空间。能够唯一标识n维矢量空间的m维子空间所需要的参数数量是 $m(n-m)$。

矩阵形式(Matrix Representation)

使用值空间或者零空间的矩阵形式表示子空间是最简单的方式。我们主要使用值空间。记 $n$ 维矢量空间 $V$ 的一个 $m$ 维子空间为 $S$，其基向量为 $S = \{ \boldsymbol{s}_1, \cdots, \boldsymbol{s}_m \}$。任意矢量 $\boldsymbol{v} \in S$ 都可以写成 $\boldsymbol{v} = \sum_{i=1}^m \boldsymbol{s}_i \alpha_i$ 的形式，其中 $\alpha_i$ 是 $\boldsymbol{v}$ 在空间 $S$ 中的坐标。如果 $\boldsymbol{s}_i$ 本身是由 $V$ 的基向量的坐标表示的，那么我们就可以得到一个 $n \times m$ 的矩阵， $\boldsymbol{S} = \begin{bmatrix} \boldsymbol{s}_1 & \cdots & \boldsymbol{s}_m\end{bmatrix}$，它有属性 $\text{range}(\boldsymbol{S}) = S$。 $\boldsymbol{v}$ 就可以表示为 $\boldsymbol{v} = \boldsymbol{S\alpha}$，其中 $\boldsymbol{\alpha} = \begin{bmatrix} \alpha_1 & \cdots & \alpha_m \end{bmatrix}^T$。

矩阵 $\boldsymbol{S}$ 定义了一个子空间和它的基向量。实际上 $\boldsymbol{S}$ 有 $mn$ 个元素，我们可以说其中的 $m(n -m)$ 个元素定义了子空间，$m^2$ 个元素定义了它的基。如果 $S' = \{s_1', \cdots, s_m'\}$ 是空间 $S$ 的另一组基，那么矩阵 $\boldsymbol{S}' = \begin{bmatrix} \boldsymbol{s}_1' & \cdots & \boldsymbol{s}_m' \end{bmatrix}$ 定义的子空间与 $\boldsymbol{S}$ 相同，只是基不同。此时，$\boldsymbol{S}'$ 可以写成 $\boldsymbol{S}' = \boldsymbol{SA}$，其中 $\boldsymbol{A}$ 是从 $S'$ 坐标系转换到 $S$ 的坐标变换。 $\boldsymbol{A}$ 中的元素满足 $\boldsymbol{s}_j' = \sum_{i=1}^m \boldsymbol{s}_i A_{ij}$。因此，如果矩阵 $\boldsymbol{A}$ 可逆，那么任何矩阵 $\boldsymbol{SA}$ 描述的都是和 $\boldsymbol{S}$ 相同的子空间。

矢量分解(Decomposition of Vectors)

我们对矢量的最基本操作之一，就是将其分解成两个子空间的部分。给定一个矢量 $\boldsymbol{v} \in V$ 和两个子空间 $S_1, S_2 \subseteq V$，我们将把 $\boldsymbol{v}$ 写成 $\boldsymbol{v} = \boldsymbol{v}_1 + \boldsymbol{v}_2$ 的形式，其中，$\boldsymbol{v}_1 \in S_1, \boldsymbol{v}_2 \in S_2$。如果 $\boldsymbol{v} \in \text{span}(S_1 \cup S_2)$，那么就存在该分解。如果 $S_1 \cap S_2 = \{\boldsymbol{0}\}$，即 $S_1, S_2$ 的交集只有零向量，那么分解就是唯一的。如果对任何 $\boldsymbol{v}$ 都唯一存在该分解，那么 $S_1, S_2$ 就必须满足: $$ \begin{equation}\label{equa_3_24} S_1 \cap S_2 = \{ \boldsymbol{0} \} \qquad \text{and} \qquad \text{dim}(S_1) + \text{dim}(S_2) = \text{dim}(V) \end{equation} $$ 其中第二个条件意味着 $\text{span}(S_1 \cup S_2) = V$。如果 $S_1, S_2$ 满足式($\ref{equa_3_24}$)，那么我们可以说 $V$ 是 $S_1, S_2$ 的直和(direct sum)，可以写成: $$ \begin{equation}\label{equa_3_25} V = S_1 \oplus S_2 \end{equation} $$ 这个方程是一种简写形式，表示 $V$ 中的任何元素都可以写成一个 $S_1$ 的元素和一个 $S_2$ 的元素之和。本质上式($\ref{equa_3_24}$)与式($\ref{equa_3_25}$)描述的是同一件事情。现在让我们来按一下分解过程。给定一个矢量 $\boldsymbol{v} \in V$ 以及两个满足式($\ref{equa_3_25}$)的子空间矩阵 $\boldsymbol{S}_1, \boldsymbol{S}_2$，矢量分解的任务是找到向量 $\boldsymbol{\alpha}_1, \boldsymbol{\alpha}_2$ 满足: $$ \begin{equation}\label{equa_3_26} \boldsymbol{v} = \boldsymbol{S}_1 \boldsymbol{\alpha}_1 + \boldsymbol{S}_2 \boldsymbol{\alpha}_2 = \begin{bmatrix} \boldsymbol{S}_1 & \boldsymbol{S}_2 \end{bmatrix} \begin{bmatrix} \boldsymbol{\alpha}_1 \\ \boldsymbol{\alpha}_2 \end{bmatrix} \end{equation} $$ 我们可以通过如下方法求解该问题。若 $S_1 \oplus S_2 = V$ 则 $\begin{bmatrix} \boldsymbol{S}_1 & \boldsymbol{S}_2 \end{bmatrix}$ 定义了 $V$ 的一组基。因此它是一个非奇异的矩阵，存在逆 $\begin{bmatrix} S_1 & S_2 \end{bmatrix}^{-1}$，式($\ref{equa_3_26}$)的解为: $$ \begin{equation}\label{equa_3_27} \begin{bmatrix} \boldsymbol{\alpha}_1 \\ \boldsymbol{\alpha}_2 \end{bmatrix} = \begin{bmatrix} \boldsymbol{S}_1 & \boldsymbol{S}_2 \end{bmatrix}^{-1} \boldsymbol{v} \end{equation} $$ 该式可以很容易扩展到两个以上子空间的矢量分解上。

正交补(Orthogonal Complements)

如果两个欧式矢量 $\boldsymbol{u}, \boldsymbol{v}$ 满足方程 $\boldsymbol{u} \cdot \boldsymbol{v} = \boldsymbol{0}$，那么我们说这两个矢量是正交的。如果 $Y_1, Y_2$ 是 $E^n$ 的子集，并且 $Y_1$ 中的所有元素都与 $Y_2$ 中的所有元素正交，那么我们说这两个子集是正交的，记作 $Y_1 \perp Y_2$。给定子集 $Y \subseteq E^n$，所有与之正交的矢量的集合，被称为$Y$的正交补，记为$Y^{\perp}$。正交补总是一个子空间，表 3.1中列举了正交子集的一些基本性质。

表 3.1 正交子集的性质。一般情况下，$S, S_1, Y, Y_1 \subseteq U$，并且 $S_2, Y_2 \subseteq V$，其中 $U = V^*$。写成欧几里得的形式，$S, \cdots, Y_2 \subseteq E^n$。
Properties of orthogonal subsets. In the general case, $S, S_1, Y, Y_1 \subseteq U$ and $S_2, Y_2 \subseteq V$ where $U = V^*$. In the Euclidean case, $S, \cdots, Y_2 \subseteq E^n$

这种形式的正交特性是建立在欧式内积上的，因此只适用于欧式矢量。当然，我们也可以基于标量积，定义新的正交特性。特别的，如果 $\boldsymbol{u} \in U, \boldsymbol{v} \in V$，其中 $U = V^*$，那么$\boldsymbol{u}, \boldsymbol{v}$之间就存在标量积，如果 $\boldsymbol{u} \cdot \boldsymbol{v} = \boldsymbol{0}$，我们也可以说这两个向量是正交的。如果 $Y_1, Y_2$ 分别是 $U,V$ 的子集，并且 $Y_1$ 中的所有元素都与 $Y_2$ 中的所有元素正交，那么有 $Y_1 \perp Y_2$。类似的，$V$ 中所有与 $Y \subseteq U$ 正交的元素构成的集合，被称为 $Y$ 的正交补。

欧几里得形式和对偶形式的正交性，具有不同且不兼容的数学属性。特别的，在欧式空间中，只有 $Y_1, Y_2$ 是相同矢量空间的子空间时，关系 $Y_1 \perp Y_2$ 才成立，而在对偶空间中，如果他们是不同矢量空间的子集也是成立的。错误的将6D的矢量写成欧几里得的形式，而不是对偶的形式的做法，在机器人相关文献中被广为诟病。 Duffy (1990) 讨论了这个问题。一些研究人员试图通过使用“自然正交补(natural orthogonal complement)”或“倒数补(reciprocal complement)”等替代名称来表示对偶形式的正交补，从而避免这种错误。

正交补在刚体动力学中扮演着如下的角色，系统一开始可以在 $M^n$ 空间中自由移动，运动约束将限定其在子空间 $S \subseteq M^n$ 中运动，那么对这个系统施加的约束力就是 $S$ 的正交补空间 $S^{\perp} \subseteq F^n$。

Example 3.1: 给定一个运动子空间，$S \subseteq M^6$，及其空间惯量 $\boldsymbol{I}:M^6 \mapsto F^6$，空间惯量矩阵是正定的。由此，我们可以定义两个子空间 $T_a = \boldsymbol{I}S$ 和 $T_c = S^{\perp}$，它们存在关系 $T_a \oplus T_c = F^6$。 $\boldsymbol{I}S$ 是 $S$ 经过映射 $\boldsymbol{I}$ 的像，即，集合 $\{ \boldsymbol{Is} | \boldsymbol{s} \in S \}$。其证明如下:

根据式($\ref{equa_3_24}$)，我们需证明 $\text{dim}(T_a) + \text{dim}(T_c) = 6$，并且 $T_a \cap T_c$ 为不包含除零以外的元素。我们很容易证明第一点，$\text{dim}(T_a) = \text{dim}(S)$ 并且 $\text{dim}(T_c) = 6 - \text{dim}(S)$。我们用反证法来证明第二点，首先假设 $\boldsymbol{t} \neq 0 \in (T_a \cap T_c)$。如果 $\boldsymbol{t} \in T_a$ 那么存在一个非零的矢量 $\boldsymbol{s} \in S$，使得 $\boldsymbol{t} = \boldsymbol{Is}$。但是如果 $\boldsymbol{t} \in T_c$ 那么有 $\boldsymbol{s}^T \boldsymbol{t} = 0$，意味着 $\boldsymbol{s}^T\boldsymbol{Is} = 0$。这是不可能的，因为 $\boldsymbol{I}$ 是正定的。

因为 $T_a \oplus T_c = F^6$，我们将任意矢量 $\boldsymbol{f} \in F^6$ 唯一地分解为 $\boldsymbol{f}_a \in T_a, \boldsymbol{f}_c \in T_c$。给定任何矩阵 $\boldsymbol{S}$ 张成 $S$ 空间，我们都可以写出 $\boldsymbol{f}_a, \boldsymbol{f}_c$ 满足如下关系: $$ \boldsymbol{S}^T \boldsymbol{f} = \boldsymbol{S}^T \boldsymbol{f}_a + \boldsymbol{S}^T \boldsymbol{f}_c = \boldsymbol{S}^T \boldsymbol{f}_a $$ 一定存在矢量 $\boldsymbol{\alpha}$，使得 $\boldsymbol{f}_a = \boldsymbol{IS\alpha}$，因此: $$ \boldsymbol{S}^T\boldsymbol{f} = \boldsymbol{S}^T\boldsymbol{IS\alpha} \qquad \Longrightarrow \qquad \boldsymbol{\alpha} = (\boldsymbol{S}^T\boldsymbol{IS\alpha})^{-1} \boldsymbol{S}^T\boldsymbol{f} $$ 因此 $$ \begin{equation}\label{equa_3_28} \boldsymbol{f}_a = \boldsymbol{IS}(\boldsymbol{S}^T\boldsymbol{IS\alpha})^{-1} \boldsymbol{S}^T\boldsymbol{f} \end{equation} $$ $\boldsymbol{f}_c$ 可以直接通过 $\boldsymbol{f}_c = \boldsymbol{f} - \boldsymbol{f}_a$ 求出。显然方程$\ref{equa_3_28}$ 中的表达式必须独立于 S 的特定选择。我们可以通过用 SA 替换该方程中 S 的每个实例来证明这一点，其中 A 是正确大小的任何可逆矩阵，它将在计算过程中约去。

这种分解的物理意义在于，如果 $\boldsymbol{I}$ 是一个刚体的惯性，该刚体被约束在子空间 $S$ 中运动，并且 $\boldsymbol{f}$ 是作用在刚体上的力，那么，$\boldsymbol{f}_a$ 就是 $\boldsymbol{f}$ 中产生加速度的分量，而 $\boldsymbol{f}_c$ 则是与约束力相反的分量，或者说约束力就是 $-\boldsymbol{f}_c$。如果刚体出于 rest 状态，除了 $\boldsymbol{f}$ 和运动约束力之外，不再受到其他力，那么它的加速度可以表示为: $$ \boldsymbol{a} = \boldsymbol{I}^{-1}\boldsymbol{f}_a = \boldsymbol{S}(\boldsymbol{S}^T\boldsymbol{IS\alpha})^{-1} \boldsymbol{S}^T\boldsymbol{f} $$ 表达式 $\boldsymbol{S}(\boldsymbol{S}^T\boldsymbol{IS\alpha})^{-1} \boldsymbol{S}^T$ 是受约束的刚体的惯量的逆(详细参见式$\ref{equa_3_54}$)。

图 3.1. 运动约束的分类。

3.4 约束的分类

我们可以按照作用在刚体系统上的约束性质对运动约束进行分类。对于每种情况，约束都由系统运动变量的代数方程来描述，但方程的形式有所不同。图 3.1中列举了一种经典的分类结构。

首先我们将其分为平等约束和不平等约束(equality and inequality constraints)。前者是由成对的刚体之间的永久物理接触产生的，而后者描述的是刚体之间可以自由接触和分离的情形。不等式约束可以用来描述碰撞、弹跳和失去接触等现象。它们是第 11 章的主题，这里不再进一步讨论。

接下来我们将其分为完整约束和非完整约束(holonomic and nonholonomic constraints)。前者是对位置变量的约束，通常由滑动接触引起。后者是对速度变量的约束，通常由滚动接触引起。非完整约束的方程 $\phi_{nh}$，在速度变量上是线性的，因此可以写成如下的形式: $$ \begin{equation}\label{equa_3_29} \phi_{nh}(\boldsymbol{q}, \dot{\boldsymbol{q}}, t) = \phi_{nh}^0 + \sum_{i=1}^n \phi_{nh}^i \dot{q}_i \end{equation} $$ 其中，系数 $\phi_{nh}^i$是关于位置变量和时间的函数。完整约束 $\phi_h$ 关于时间的微分为: $$ \begin{equation}\label{equa_3_30} \frac{\text{d}}{\text{d} t} \phi_h (\boldsymbol{q}, t) = \frac{\partial \phi_h}{\partial t} + \sum_{i=1}^n \frac{\partial \phi_h}{\partial q_i} \dot{q}_i \end{equation} $$ 它们具有相同的代数形式，因此在速度和加速度层面上，完整约束和非完整约束之间没有区别。这两个方程之间的最大区别在于式($\ref{equa_3_29}$)是不可积的。那是因为 $\phi_{nh}$ 不是任何函数的导数，否则就可以通过他的导数表示一个完整约束了。脚注2: 假设约束方程始终成立，而不仅仅是在单个时刻成立，那么两个方程 $\dot{\phi} = 0$ 和 $\phi = 0$ 描述的是相同约束。这一特性将导致包含非完整约束的系统的位置比速度的自由度大，因此位置变量的个数也比速度变量的多。如果一个刚体系统包含了至少一个非完整约束，则称之为非完整系统，否则为完整系统。因此，式($\ref{equa_3_1}$) 只能描述一个完整系统，而式($\ref{equa_3_5}$)两种系统都能描述。

最后的分类标准是定常约束和非定常约束(scleronomic and rheonomic constraints)，前者仅是位置变量的函数，后者同时也是时间的函数。定常约束是由固定在一起的两个物体表面之间的滑动接触产生的。它们都是经典机械系统中最简单且最常见的约束类型。非定常约束可以看做是定常约束的特例，它的一个或者多个滑动自由度被迫按照规定的时间函数变化。非定常约束的作用是给系统引入运动激励。

图 3.2. 运动约束示例。

图 3.2中列举了一些运动约束。其中图(a),(b) 分别是圆柱形关节和棱柱形关节。它们都是定常约束。圆柱形关节允许刚体 $B_2$ 相对于 $B_1$ 滑动和旋转，而棱柱形关节仅允许相对滑动。因此圆柱形关节有两个运动自由度，而棱柱形只有一个。如果我们将援助关节的一个自由度设定为关于时间的函数，那么它就变成了单自由度的非定常约束。如果对棱柱形关节做相同的限定，那么它将变成一个零自由度的非定常约束，这点在现实中很有用。

运动约束几乎总是两个物体之间的关系。但是，它也可能涉及到两个以上的物体，并且无法将其简化为一组两体约束(two-body constraint)。图 3.2(c)中就给了这么一个例子。该图显示了一个二维的刚体系统，其中有三个相同的圆形刚体被约束在一个无质量(massless), 零直径(zero-diameter)，非弹性(inelastic) 的圈儿中。假设这个圈儿很紧，这三个刚体可以以任何满足方程 $l_1 + l_2 + l_3 + 2\pi r = p$ 的形式运动，其中 $p,r$ 分别是圈儿的周长和刚体的半径。

图 3.2(d)中画的是一个球放在平面上。如果这个球只能滚动不能滑动，那么它有三个瞬时的运动自由度: 可以在两个方向上滚动，并且可以绕着与平面的接触点转动。适当的调整操作顺序，球体可以以任何方向到达平面上的任何点。因此它有五个位置自由度。这就是非完整约束的特点。如果球体可以自由滑动，那么它就是定常约束。

从图3.2(a)和(b)中物体的形状可以清楚看到，它们是不能被拉开的，但是没有任何物理方法防止图3.2(c)中的圈儿松弛，或者使图3.2(d)中的球体与平面失去接触。如果我们对这系统的力有足够的了解，可以确保这样的事件不会发生，那么就可以将这些约束建模成平等约束，否则就必须建模成不平等约束。

3.5 关节约束

我们把两个刚体之间的运动约束定义为一个关节(joint)。一个关节可以约束一对刚体的0到6个自由度的相对速度。我们把两个刚体称为前驱(predecessor)和后继(successor)，表示关节建立的是从前驱到后继的约束。关节速度 $\boldsymbol{v}_J$ 表示后继相对于前驱的速度: $$ \begin{equation}\label{equa_3_31} \boldsymbol{v}_J = \boldsymbol{v}_s - \boldsymbol{v}_p \end{equation} $$ 其中，$\boldsymbol{v}_p$ 和 $\boldsymbol{v}_s$ 分别是前驱和后继刚体的速度。关节约束的一般形式为: $$ \begin{equation}\label{equa_3_32} \boldsymbol{v}_J = \boldsymbol{S}(\boldsymbol{q}, t)\dot{\boldsymbol{q}} + \boldsymbol{\sigma}(\boldsymbol{q}, t) \end{equation} $$ 其中 $\boldsymbol{S}$ 是关节运动子空间矩阵，$\boldsymbol{\sigma}$ 是附加速度(bias velocity)，$\boldsymbol{q}$ 和 $\dot{\boldsymbol{q}}$ 分别是关节位置和速度变量，$t$则是时间。如果位置变量并不是速度变量的积分，那么我们就需要分别用不同的符号来表示 $\dot{\boldsymbol{q}}$ 或者 $\boldsymbol{q}$。当 $\dot{\boldsymbol{q}} = \boldsymbol{0}$ 时， $\boldsymbol{v}_J$ 的取值就是附加速度。此时如果关节显式地依赖时间，即，如果它是非定常约束或者式时间相关的非完整约束，附加速度将为非零值。如果关节约束与时间无关，那么式($\ref{equa_3_32}$)可以简化为: $$ \begin{equation}\label{equa_3_33} \boldsymbol{v}_J = \boldsymbol{S}(\boldsymbol{q})\dot{\boldsymbol{q}} \end{equation} $$ 式($\ref{equa_3_32}$)的作用是讲速度 $\boldsymbol{v}_J - \boldsymbol{\sigma}$ 限定到子空间 $S \subseteq M^6$ 中。如果允许存在 $n_f$ 个相对运动自由度，那么 $\text{dim}(S) = n_f$ 并且 $\boldsymbol{S}$ 是一个 $6 \times n_f$ 的矩阵。脚注3. 如果 $n_f = 0$ 那么 $\boldsymbol{S}$ 是一个 $6 \times 0$ 的矩阵。它除了满足矩阵代数的基本规则外，还有提个附加规则: 一个 $m \times 0$ 的矩阵和一个 $0 \times n$ 的矩阵相乘得到的是一个 $m \times n$ 的全零矩阵。因此， $6 \times 0$ 的矩阵 $\boldsymbol{S}$ 与一个 $0 \times 1$ 的向量 $\dot{\boldsymbol{q}}$ 是一个 $6 \times 1$ 的零向量。关节提供的约束数量为 $n_c = 6 - n_f$，并且约束力一定在 $n_c$ 维的子空间 $S^{\perp} \subseteq F^6$ 中。

我们用符号 $\boldsymbol{f}_J$ 表示通过关节传递的力。它描述的是从前驱传递到后继的力，所以作用到后继上的力为 $\boldsymbol{f}_J$ 并且作用到前驱上的力为 $-\boldsymbol{f}_J$。这个力可以看做是一个主动力和一个约束力的和。前者来自执行器(actuators)、弹簧(springs)、阻尼(dampers)，或者其他作用到关节上的力。而后者是运动约束所产生的力。此时，我们有一个选择。约束力必须是子空间 $S^{\perp}$ 的元素。但是有两种方式来处理主动力，它可以被看做一个已知的空间力，也可以是映射到 $F_6$ 子空间的广义力。这里我们选择后者。

令 $T, T_a$ 分别是约束和主动力子空间。根据定义有 $T = S^{\perp}$，$T_a$ 可以是使得 $T \oplus T_a = F^6$ 的任何子空间。那么我们可以将 $\boldsymbol{f}_J$ 写成如下的形式: $$ \begin{equation}\label{equa_3_34} \boldsymbol{f}_J = \boldsymbol{T}_a \boldsymbol{\tau} + \boldsymbol{T \lambda} \end{equation} $$ 其中，$\boldsymbol{T}_a\boldsymbol{\tau}$ 是主动力，$\boldsymbol{T\lambda}$ 是约束力，$\boldsymbol{T}_a$ 和 $\boldsymbol{T}$ 满足: $$ \begin{equation}\label{equa_3_35} \boldsymbol{T}_a^T \boldsymbol{S} = \boldsymbol{1} \end{equation} $$ $$ \begin{equation}\label{equa_3_36} \boldsymbol{T}^T \boldsymbol{S} = \boldsymbol{0} \end{equation} $$ $\tau$ 则是关于 $\dot{\boldsymbol{q}}$ 的广义力向量，并且 $\boldsymbol{\lambda}$ 是约束力变量的向量。根据$\boldsymbol{T}_a$ 和 $\boldsymbol{T}$ 的性质，我们可以立即推断出 $$ \begin{equation}\label{equa_3_37} \boldsymbol{S}^T \boldsymbol{f}_J = \boldsymbol{\tau} \end{equation} $$ $$ \begin{equation}\label{equa_3_38} \boldsymbol{T}^T \boldsymbol{v}_J = \boldsymbol{T}^T \boldsymbol{\sigma} \end{equation} $$ 方程($\ref{equa_3_37}$) 是通过关节传递的空间力获得关节处广义力的公式，方程($\ref{equa_3_38}$)是速度约束方程的隐式形式。公式($\ref{equa_3_35}$)成立的理由如下。关节传递的总功率为$\boldsymbol{f}_J \cdot \boldsymbol{v}_J$，它有两个独立的做功源 $\boldsymbol{\sigma}$ 和 $\boldsymbol{\tau}$。 $\boldsymbol{\sigma}$ 的功率为 $\boldsymbol{f}_J\cdot \boldsymbol{\sigma}$，$\boldsymbol{\tau}$ 的功率则是 $\boldsymbol{f}_J \cdot \boldsymbol{S}\dot{\boldsymbol{q}}$。第二个功率一定也等于 $\boldsymbol{\tau} \cdot \dot{\boldsymbol{q}}$，因为它们是关节的广义力和速度向量，所以我们有如下的功率守恒方程: $$ \begin{equation}\label{equa_3_39} \boldsymbol{\tau} \cdot \dot{\boldsymbol{q}} = \boldsymbol{f}_J \cdot \boldsymbol{S} \dot{\boldsymbol{q}} = \boldsymbol{\tau}^T \boldsymbol{T}_a^T \boldsymbol{S} \dot{\boldsymbol{q}} \end{equation} $$ 如果这个方程对于所有 $\boldsymbol{\tau}$ 和 $\dot{\boldsymbol{q}}$ 都成立，我们必须有 $\boldsymbol{T}_a^T \boldsymbol{S} = \boldsymbol{1}$。

直接对式($\ref{equa_3_32}$ 和 $\ref{equa_3_38}$ 微分就可以得到关节的加速度约束公式。对式($\ref{equa_3_32}$)求导: $$ \begin{align}\label{equa_3_40} \boldsymbol{a}_J & = \boldsymbol{S}\ddot{\boldsymbol{q}} + \dot{\boldsymbol{S}}\dot{\boldsymbol{q}} + \dot{\boldsymbol{\sigma}} \\ & = \boldsymbol{S}\ddot{\boldsymbol{q}} + (\mathring{\boldsymbol{S}} + \boldsymbol{v}_s \times \boldsymbol{S})\dot{\boldsymbol{q}} + (\mathring{\boldsymbol{\sigma}} + \boldsymbol{v}_s \times \boldsymbol{\sigma}) \\ & = \boldsymbol{S}\ddot{\boldsymbol{q}} + \mathring{\boldsymbol{S}}\dot{\boldsymbol{q}} + \mathring{\boldsymbol{\sigma}} + \boldsymbol{v}_s \times (\boldsymbol{S}\dot{\boldsymbol{q}} + \boldsymbol{\sigma}) \\ & = \boldsymbol{S}\ddot{\boldsymbol{q}} + \boldsymbol{c}_J + \boldsymbol{v}_s \times \boldsymbol{v}_J \end{align} $$ 其中$\mathring{\boldsymbol{S}}$ 和 $\mathring{\boldsymbol{\sigma}}$ 是 $\boldsymbol{S, \sigma}$在坐标系统中的表观导数(apparent derivative)，它与后继刚体一起运动。而， $$ \begin{equation}\label{equa_3_41} \boldsymbol{c}_J = \mathring{\boldsymbol{S}} \dot{\boldsymbol{q}} + \mathring{\boldsymbol{\sigma}} \end{equation} $$ 实际上，$\boldsymbol{c}_J = \mathring{\boldsymbol{v}}_J$。如果$\boldsymbol{S}$和$\boldsymbol{\sigma}$是$\boldsymbol{q}$和$t$的函数，那么$\mathring{\boldsymbol{S}}$和$\mathring{\boldsymbol{\sigma}}$的一般形式为: $$ \begin{equation}\label{equa_3_42} \mathring{\boldsymbol{S}} = \frac{\partial \boldsymbol{S}}{\partial t} + \sum_{i = 1}^{n_p} \frac{\partial \boldsymbol{S}}{\partial q_i} \dot{q}_i \end{equation} $$ $$ \begin{equation}\label{equa_3_43} \mathring{\boldsymbol{\sigma}} = \frac{\partial \boldsymbol{\sigma}}{\partial t} + \sum_{i=1}^{n_p} \frac{\partial \boldsymbol{\sigma}}{\partial q_i} \dot{q}_i \end{equation} $$ 其中$n_p$是关节位置变量的数量。在继续之前，读者应当意识到，上述的那些应用于一般情况的复杂方程在实际中几乎用不到。部分原因是，大多数关节类型都有性质 $\mathring{\boldsymbol{S}} = \boldsymbol{0}$ 和 $\boldsymbol{\sigma} = \boldsymbol{0}$，意味着 $\boldsymbol{c}_J = \boldsymbol{0}$。再就是，关节约束很少显式地依赖时间，我们几乎总是可以通过第9章的混合动力学算法而不是使用$\boldsymbol{\sigma}$来实现时间关联项。

最后，隐式加速度约束方程可以通过对方程($\ref{equa_3_38}$)求导或将式($\ref{equa_3_40}$)乘以 $\boldsymbol{T}^T$ 来获得。两者的结果相同，但后者推导更容易： $$ \begin{align}\label{equa_3_44} \boldsymbol{T}^T\boldsymbol{a}_J & = \boldsymbol{T}^T(\boldsymbol{S}\ddot{\boldsymbol{q}} + \boldsymbol{c}_J + \boldsymbol{v}_s \times \boldsymbol{v}_J) \\ & = \boldsymbol{T}^T(\boldsymbol{c}_J + \boldsymbol{v}_s \times \boldsymbol{v}_J) \end{align} $$

图 3.3. 定义一个主动力子空间。

Example 3.2: 让我们研究一下如何定义主动力子空间。如图 3.3所示的一个系统，其中有一个移动刚体$B$，通过旋转关节连接到固定底座，由一个电机$M$，通过齿轮组驱动该关节运动。图中还显示了坐标系的$x$和$y$轴，$z$轴与关节的转轴重合。在该坐标系下，关节的运动和约束力子空间矩阵是:

$$ \boldsymbol{S} = \begin{bmatrix} 0 \\ 0 \\ 1 \\ 0 \\ 0 \\ 0 \end{bmatrix} \qquad \text{和} \qquad \boldsymbol{T} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} $$

严格来说，这并不是$\boldsymbol{S}$和$\boldsymbol{T}$的唯一可能值，任何能够正确联系这两个子空间的矩阵都是可以的。电机的驱动力通过两个齿轮的接触点传导到刚体$B$，它是$y$方向上大小为$f$的线性力。接触点的坐标为$(−r, 0, 0)$，其中$r$是齿轮的半径，因此主动空间力的普吕克坐标为$\boldsymbol{f} = \begin{bmatrix} 0 & 0 & −rf & 0 & f & 0 \end{bmatrix}^T$。在这个例子中$\boldsymbol{\tau}$只是一个标量，所以如果$\boldsymbol{f} = \boldsymbol{T}_a\boldsymbol{\tau}$，那么$\boldsymbol{T}_a$就只是$\boldsymbol{f}$的标量倍数。根据方程($\ref{equa_3_35}$)有，$\boldsymbol{T}_a$必须满足$\boldsymbol{T}_a^T\boldsymbol{S} = \boldsymbol{1}$，所以有: $$ \boldsymbol{T}_a = \boldsymbol{f} / (\boldsymbol{f}^T\boldsymbol{S}) = \begin{bmatrix} 0 \\ 0 \\ 1 \\ 0 \\ -1/r \\ 0 \end{bmatrix} $$

显然，$\boldsymbol{T}_a$ 和 $\text{range}(\boldsymbol{T}_a)$ 都依赖于 $r$。这个依赖关系对于运动方程没有什么影响，但是会影响约束力的值。物理上，约束力来自于关节的轴承。如果我们只是模拟系统的运动，那么 $r$ 就无关紧要，但是如果要通过仿真结果进行轴承的选型，我们就必须考虑 $r$ 的取值。

图 3.4. 一个受约束的刚体。

3.6 带约束的刚体动力学

如图 3.4所示的刚体，它通过一个关节与基座向量，关节的运动子空间为 $S \subseteq M^6$。该关节给刚体施加了 $n_c$ 个约束，还剩下 $n_f = \text{dim}(S)$ 个自由度，有关系 $n_c = 6 - n_f = \text{dim}(S^{\perp})$。有三个力作用在刚体上，一个作用力$\boldsymbol{f}$、一个约束力$\boldsymbol{f}_c$、以及一个重力$\boldsymbol{f}_g$(重力没有在图中画出)。因此该刚体的运动方程为: $$ \boldsymbol{f} + \boldsymbol{f}_c + \boldsymbol{f}_g = \boldsymbol{Ia} + \boldsymbol{v} \times^* \boldsymbol{Iv} $$ 其中，$\boldsymbol{I}, \boldsymbol{a}, \boldsymbol{v}$ 分别是刚体的惯量、加速度和速度。我们并不特别关心单项的 $\boldsymbol{f}_g$ 和 $\boldsymbol{v} \times^* \boldsymbol{Iv}$，所以将它们整理成偏置力(bias-force)$\boldsymbol{p} = \boldsymbol{v} \times^* \boldsymbol{Iv} - \boldsymbol{f}_g$。因此，受约束的刚体的运动方程可以写成: $$ \begin{equation}\label{equa_3_45} \boldsymbol{f} + \boldsymbol{f}_c = \boldsymbol{Ia} + \boldsymbol{p} \end{equation} $$ 约束为 $$ \begin{equation}\label{equa_3_46} \boldsymbol{v} \in S \qquad \text{和} \qquad \boldsymbol{f}_c \in S^{\perp} \end{equation} $$ 我们要求解的问题是，得到加速度关于所施加力的函数。注意，$\boldsymbol{f}_c$是个未知量，必须作为求解过程的一部分将其估计出来，或者想办法消除掉它。

方法1

我们引入一个 $6 \times n_f$ 的矩阵 $\boldsymbol{S}$ 张成子空间 $S$。那么式($\ref{equa_3_46}$)就可以写成如下的形式: $$ \begin{equation}\label{equa_3_47} \boldsymbol{v} = \boldsymbol{S} \dot{\boldsymbol{q}} \end{equation} $$ $$ \begin{equation}\label{equa_3_48} \boldsymbol{S}^T \boldsymbol{f}_c = \boldsymbol{0} \end{equation} $$ 加速度约束为: $$ \begin{equation}\label{equa_3_49} \boldsymbol{a} = \boldsymbol{S}\ddot{\boldsymbol{q}} + \dot{\boldsymbol{S}}\dot{\boldsymbol{q}} \end{equation} $$ 其中，$\dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}$分别是关节的速度和加速度向量。根据式($\ref{equa_3_48}$)，我们可以从式($\ref{equa_3_45}$)中约去 $\boldsymbol{f}_c$。对其两边同乘以 $\boldsymbol{S}^T$，有: $$ \begin{equation}\label{equa_3_50} \boldsymbol{S}^T\boldsymbol{f} = \boldsymbol{S}^T(\boldsymbol{Ia} + \boldsymbol{p}) \end{equation} $$ 将式($\ref{equa_3_49}$)代入上式有 $$ \boldsymbol{S}^T\boldsymbol{f} = \boldsymbol{S}^T(\boldsymbol{I}(\boldsymbol{S}\ddot{\boldsymbol{q}} + \dot{\boldsymbol{S}}\dot{\boldsymbol{q}}) + \boldsymbol{p}) $$ 可以从中提取出 $\ddot{\boldsymbol{q}}$ $$ \begin{equation}\label{equa_3_51} \ddot{\boldsymbol{q}} = (\boldsymbol{S}^T\boldsymbol{IS})^{-1}\boldsymbol{S}^T(\boldsymbol{f} - \boldsymbol{I}\dot{\boldsymbol{S}}\dot{\boldsymbol{q}} - \boldsymbol{p}) \end{equation} $$ 表达式$\boldsymbol{S}^T\boldsymbol{IS}$是一个$n_f \times n_f$的对称、正定矩阵，因此它一定是可逆的。写出$\ddot{\boldsymbol{q}}$的表达式，将其代入式($\ref{equa_3_49}$)中 $$ \begin{equation}\label{equa_3_52} \boldsymbol{a} = \boldsymbol{S}(\boldsymbol{S}^T\boldsymbol{IS})^{-1}\boldsymbol{S}^T(\boldsymbol{f} - \boldsymbol{I}\dot{\boldsymbol{S}}\dot{\boldsymbol{q}} - \boldsymbol{p}) + \dot{\boldsymbol{S}}\dot{\boldsymbol{q}} \end{equation} $$ 可以整理如下: $$ \begin{equation}\label{equa_3_53} \boldsymbol{a} = \boldsymbol{\Phi f} + \boldsymbol{b} \end{equation} $$ $$ \begin{equation}\label{equa_3_54} \boldsymbol{\Phi} = \boldsymbol{S}(\boldsymbol{S}^T\boldsymbol{IS})^{-1}\boldsymbol{S}^T \end{equation} $$ $$ \begin{equation}\label{equa_3_55} \boldsymbol{b} = \dot{\boldsymbol{S}}\dot{\boldsymbol{q}} - \boldsymbol{\Phi}(\boldsymbol{I}\dot{\boldsymbol{S}}\dot{\boldsymbol{q}} + \boldsymbol{p})\end{equation} $$ $\boldsymbol{\Phi}$ 是受约束刚体的惯性的广义逆(apparent inverse inertia)，$\boldsymbol{b}$ 是其偏置加速度(bias acceleration)，它是$\boldsymbol{f} = \boldsymbol{0}$ 时的加速度。刚体惯性的广义逆描述了加速度是如何随着作用力变化的。它是一个对称的半正定矩阵，具有以下特性：$\text{range}(\boldsymbol{\Phi}) = S$, $\text{null}(\boldsymbol{\Phi}) = S^{\perp}$，$\text{rank}(\boldsymbol{\Phi}) = n_f$。

方法2

另一种求解式($\ref{equa_3_45}$)和式($\ref{equa_3_46}$)的方法是，引入一个$6 \times n_c$的矩阵 $\boldsymbol{T}$，张成空间$S^{\perp}$。那么约束返程就可以写成如下的形式: $$ \begin{equation}\label{equa_3_56} \boldsymbol{T}^T\boldsymbol{v} = \boldsymbol{0} \end{equation} $$ $$ \begin{equation}\label{equa_3_57} \boldsymbol{f}_c = \boldsymbol{T \lambda} \end{equation} $$ 其中，$\lambda$是约束力变量的向量，加速度约束为 $$ \begin{equation}\label{equa_3_58} \boldsymbol{T}^T\boldsymbol{a} + \dot{\boldsymbol{T}^T}\boldsymbol{v} = \boldsymbol{0} \end{equation} $$ 将式($\ref{equa_3_57}$)代入式($\ref{equa_3_45}$)有 $$ \begin{equation}\label{equa_3_59} \boldsymbol{f} + \boldsymbol{T\lambda} = \boldsymbol{Ia} + \boldsymbol{p}\end{equation} $$ 式($\ref{equa_3_58}$)和式($\ref{equa_3_59}$)可以组合成一个方程: $$ \begin{equation}\label{equa_3_60} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{T} \\ \boldsymbol{T}^T & \boldsymbol{0} \end{bmatrix} \begin{bmatrix} \boldsymbol{a} \\ -\boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{f} - \boldsymbol{p} \\ -\dot{\boldsymbol{T}^T}\boldsymbol{v} \end{bmatrix} \end{equation} $$ 求解方程组，即可得到$\boldsymbol{a}$和$\boldsymbol{\lambda}$。该方程的系数矩阵是一个对称的非奇异矩阵，但不是正定的。

方法3

在第3种方法中，我们有可能导出广义坐标形式的运动方程。向量$\dot{\boldsymbol{q}},\ddot{\boldsymbol{q}}$表示受约束刚体的广义速度和加速度变量。因此式($\ref{equa_3_51}$)已经给出了广义加速度与施加的空间力$\boldsymbol{f}$之间的函数关系。令$\boldsymbol{\tau}$表示作用在刚体上的广义力，为了让$\boldsymbol{\tau}$等价于$\boldsymbol{f}$，对于空间$S$中的任何速度，这两个力的功率都必须相同。$\boldsymbol{\tau}$的功率为$\boldsymbol{\tau}^T\dot{\boldsymbol{q}}$， $\boldsymbol{f}$的功率为$\boldsymbol{f}^T \boldsymbol{v}$，所以我们有: $$ \boldsymbol{\tau}^T\dot{\boldsymbol{q}} = \boldsymbol{f}^T \boldsymbol{v} = \boldsymbol{f}^T \boldsymbol{S} \dot{\boldsymbol{q}} $$ 上式要对所有的 $\dot{\boldsymbol{q}}$ 成立，所以有 $$ \begin{equation}\label{equa_3_61} \boldsymbol{\tau} = \boldsymbol{S}^T\boldsymbol{f} \end{equation} $$ 代入式($\ref{equa_3_51}$)有 $$ \ddot{\boldsymbol{q}} = (\boldsymbol{S}^T\boldsymbol{IS})^{-1}(\boldsymbol{\tau} - \boldsymbol{S}^T(\boldsymbol{I}\dot{\boldsymbol{S}}\dot{\boldsymbol{q}} + \boldsymbol{p})) $$ 这是广义坐标形式的受约束刚体的运动方程，可以写成标准形式: $$ \begin{equation}\label{equa_3_62} \boldsymbol{H}\ddot{\boldsymbol{q}} + \boldsymbol{C} = \boldsymbol{\tau} \end{equation} $$ 其中$\boldsymbol{H}$是广义惯性矩阵 $$ \begin{equation}\label{equa_3_63} \boldsymbol{H} = \boldsymbol{S}^T\boldsymbol{IS} \end{equation} $$ $\boldsymbol{C}$是广义偏置力 $$ \begin{equation}\label{equa_3_64} \boldsymbol{C} = \boldsymbol{S}^T(\boldsymbol{I}\dot{\boldsymbol{S}}\dot{\boldsymbol{q}} + \boldsymbol{p}) \end{equation} $$

公式($\ref{equa_3_61}$)意味着从$\boldsymbol{f}$到$\boldsymbol{\tau}$的变换过程中会有少量信息损失。有无穷多个不同的$\boldsymbol{f}$值映射到相同的$\boldsymbol{\tau}$，因此产生相同的加速度，但每个值都会产生不同的$\boldsymbol{f}_c$。如果仅知道$\boldsymbol{\tau}$，则可以计算出$\boldsymbol{f} + \boldsymbol{f}_c$值，但不能计算出各个向量的值。如果仅仅是为了模拟运动，那么丢失的这点信息是无关紧要的。但如果仿真的目标是设计机械系统，那么丢失的信息，与计算关节轴承上的动载荷力等任务相关。

3.7 多刚体系统的动力学

我们通过建立包含多个刚体和关节的系统的运动方程来结束本章。整个构建过程中，展示了第 3.5 节和第 3.6 节中与关节相关的材料，以及它们与第 3.2 节中的广义约束之间的联系。

假设我们有一个刚体系统，包含如下的元素：一个固定的基座，我们将其视为零号刚体；$N_B$个移动的刚体，编号为从 1 到 $N_B$；以及$N_J$个关节，编号为从 1 到 $N_J$。对于每个关节$j$，我们用 $p(j)$和$s(j)$表示该关节的前驱和后继的刚体索引。这组 $2N_J$ 编号定义了系统的链接关系。为了接下来的分析方便，我们对连接关系不做任何限制。

我们将采用第 3.6 节中的方法 2，来推导运动方程。但由于当前系统更加复杂，因此有必要额外增加一些步骤。

Step 1: 刚体运动方程

我们假设作用在刚体$i$上的力只有重力$\boldsymbol{f}_{gi}$和关节的力。那么刚体$i$的运动方程可以写成: $$ \boldsymbol{f}_i = \boldsymbol{I}_i \boldsymbol{a}_i + \boldsymbol{p}_i $$ 其中$\boldsymbol{f}_i$是作用在刚体$i$上的关节力之和，并且$\boldsymbol{p}_i = \boldsymbol{v}_i \times^* \boldsymbol{I}_i\boldsymbol{v}_i - \boldsymbol{f}_{gi}$是其偏置力。那么所有 $N_B$ 个刚体的方程可以写成如下形式: $$ \begin{equation}\label{equa_3_65} \begin{bmatrix} \boldsymbol{f}_1 \\ \boldsymbol{f}_2 \\ \vdots \\ \boldsymbol{f}_{N_B} \end{bmatrix} = \begin{bmatrix} \boldsymbol{I}_1 & \boldsymbol{0} & \cdots & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{I}_2 & \cdots & \boldsymbol{0} \\ \vdots & \vdots & \ddots & \vdots \\ \boldsymbol{0} & \boldsymbol{0} & \cdots & \boldsymbol{I}_{N_B} \end{bmatrix}\begin{bmatrix} \boldsymbol{a}_1 \\ \boldsymbol{a}_2 \\ \vdots \\ \boldsymbol{a}_{N_B} \end{bmatrix} + \begin{bmatrix} \boldsymbol{p}_1 \\ \boldsymbol{p}_2 \\ \vdots \\ \boldsymbol{p}_{N_B} \end{bmatrix} \end{equation} $$ 可以将其表示为更紧凑的形式: $$ \begin{equation}\label{equa_3_66} \boldsymbol{f} = \boldsymbol{Ia} + \boldsymbol{p} \end{equation} $$ 其中，$\boldsymbol{f},\boldsymbol{a},\boldsymbol{p}$是$6N_B$维的向量，$\boldsymbol{I}$是一个$6N_B \times 6N_B$的对角块矩阵。因为关节约束方程中可能用到刚体的速度，为了形式上的完整，我们还定义了$\boldsymbol{v} = \begin{bmatrix} \boldsymbol{v}_1^T & \boldsymbol{v}_2^T & \cdots & \boldsymbol{v}_{N_B}^T \end{bmatrix}^T$，其中$\boldsymbol{v}_i$是刚体$i$的速度。

Step 2: 从刚体方程获得关节方程

式($\ref{equa_3_66}$)是刚体加速度向量，但是关节施加的约束是以关节的速度和加速度来表示的。因此，我们需要一个公式用刚体的运动来表示关节的运动。令 $\boldsymbol{v}_{Jj}$ 和 $\boldsymbol{a}_{Jj}$分别表示关节$j$传递的速度和加速度，$\boldsymbol{v}_J$ 和 $\boldsymbol{a}_J$ 分别是 $\boldsymbol{v}_{J1} \cdots \boldsymbol{v}_{JN_j}$ 和 $\boldsymbol{a}_{J1} \cdots \boldsymbol{a}_{JN_j}$ 组合而成的 $6N_J$ 维的向量。根据定义$\boldsymbol{v}_{Jj}$和$\boldsymbol{a}_{Jj}$是关节的后继刚体相对于前驱的速度和加速度，有: $$ \boldsymbol{v}_{Jj} = \boldsymbol{v}_{s(j)} - \boldsymbol{v}_{p(j)} $$ $$ \boldsymbol{a}_{Jj} = \boldsymbol{a}_{s(j)} - \boldsymbol{a}_{p(j)} $$ 这些方程可以收集到一起写成如下的形式: $$ \begin{equation}\label{equa_3_67} \boldsymbol{v}_J = \begin{bmatrix} \boldsymbol{v}_{J1} \\ \vdots \\ \boldsymbol{v}_{JN_J} \end{bmatrix} = \begin{bmatrix} \boldsymbol{v}_{s(1)} - \boldsymbol{v}_{p(1)} \\ \vdots \\ \boldsymbol{v}_{s(N_J)} - \boldsymbol{v}_{p(N_J)} \\ \end{bmatrix} = \boldsymbol{P}^T\boldsymbol{v} \end{equation} $$ $$ \begin{equation}\label{equa_3_68} \boldsymbol{a}_J = \begin{bmatrix} \boldsymbol{a}_{J1} \\ \vdots \\ \boldsymbol{a}_{JN_J} \end{bmatrix} = \begin{bmatrix} \boldsymbol{a}_{s(1)} - \boldsymbol{a}_{p(1)} \\ \vdots \\ \boldsymbol{a}_{s(N_J)} - \boldsymbol{a}_{p(N_J)} \\ \end{bmatrix} = \boldsymbol{P}^T\boldsymbol{a} \end{equation} $$ 其中 $$ \begin{equation}\label{equa_3_69} \boldsymbol{p}_{ij} = \begin{cases} \boldsymbol{1}_{6\times 6} & \text{if} \quad i = s(j) \\ -\boldsymbol{1}_{6 \times 6} & \text{if} \quad i = p(j) \\ \boldsymbol{0}_{6\times 6} & \text{otherwise} \end{cases} \end{equation} $$ $\boldsymbol{P}$ 是一个 $6N_B \times 6N_J$ 的矩阵，排列成 $N_B \times N_J$ 的 $6 \times 6$矩阵块的阵列。它将刚体的速度$\boldsymbol{v}$和加速度$\boldsymbol{a}$映射到关节的速度$\boldsymbol{v}_J$和加速度$\boldsymbol{a}_J$。 $\boldsymbol{P}$中的每一行对应着一个刚体，每一列对应一个关节。严格来说，这里的行是指矩阵块的行，列是矩阵块的列。我们用角标$i$表示刚体的索引，$j$表示关节的索引。每一列最多包含两个非零的矩阵块，$\boldsymbol{1}_{6\times 6}$ 对应着关节的后继刚体， $-\boldsymbol{1}_{6\times 6}$ 对应着前驱刚体。如果关节连接了两个运动的刚体，那么它的列中将恰好有两个这样的非零块。但是如果一个刚体连接到了基座上(刚体0)，那么它的列将只有一个非零块。根据拓扑关系，刚体是连接到(to)基座还是从基座连接(from)，对应保留了 $\boldsymbol{1}_{6\times 6}$ 或者$-\boldsymbol{1}_{6\times 6}$。每一行$i$中的矩阵块$\boldsymbol{1}_{6\times 6}$都表示它是关节中的后继， $-\boldsymbol{1}_{6\times 6}$都是关节的前驱。这一段话真啰嗦。

Step 3: 从关节力获得刚体力

令$\boldsymbol{f}_{Jj}$表示关节$j$发的力，$\boldsymbol{f}_J$是一个$6N_J$维的向量，集成了向量$\boldsymbol{f}_{J1} \cdots \boldsymbol{f}_{JN_J}$。 $\boldsymbol{f}_{Jj}$ 是刚体 $p(j)$ 通过关节$j$ 传递到刚体 $s(j)$的力。如果我们定义集合$s^{-1}(i)$和$p^{-1}(i)$分别表示所有以刚体$i$为后继或前驱的关节，那么向量$\boldsymbol{f}_i$(所有关节作用在刚体$i$上的力向量之和)可以写成如下的形式， $$ \boldsymbol{f}_i = \sum_{j \in s^{-1}(i)} \boldsymbol{f}_{Jj} - \sum_{j \in p^{-1}(i)} \boldsymbol{f}_{Jj} $$ 将该表达式与矩阵 $\boldsymbol{P}$ 的第$i$行对比，我们可以将$\boldsymbol{f}_i$写成如下形式: $$ \boldsymbol{f}_i = \sum_{j = 1}^{N_J} \boldsymbol{P}_{ij} \boldsymbol{f}_{Jj} $$ 因此有: $$ \begin{equation}\label{equa_3_70} \boldsymbol{f} = \boldsymbol{P} \boldsymbol{f}_{J} \end{equation} $$

Step 4: 运动约束

令$\boldsymbol{T}_j$为张成子空间$S_j^{\perp}$的$6 \times n_{cj}$的矩阵，$S_j$ 是关节$j$的运动子空间，$n_{cj}$是关节的约束数量。那么关节$j$的速度约束方程可以写成: $$ \boldsymbol{T}_j^T \boldsymbol{v}_{Jj} = \boldsymbol{0} $$ 加速度约束是: $$ \boldsymbol{T}_j^T \boldsymbol{a}_{Jj} + \dot{\boldsymbol{T}_j^T}\boldsymbol{v}_{Jj} = \boldsymbol{0} $$ 集成到一起，得到整个系统的加速度约束: $$ \begin{equation}\label{equa_3_71} \boldsymbol{T}^T \boldsymbol{a}_{J} + \dot{\boldsymbol{T}^T}\boldsymbol{v}_{J} = \boldsymbol{0} \end{equation} $$ 其中，$\boldsymbol{T}$是一个$6N_J \times n_c$的对角块矩阵，$T_j$是其第$j$个对角块。$n_c = \sum_{j=1}^{N_J} n_{cj}$是关节提供的所有约束数量。联立式($\ref{equa_3_71}$),($\ref{equa_3_67}$),($\ref{equa_3_68}$)可以写出刚体的加速度约束: $$ \begin{equation}\label{equa_3_72} \boldsymbol{T}^T\boldsymbol{P}^T \boldsymbol{a} + \dot{\boldsymbol{T}^T}\boldsymbol{P}^T\boldsymbol{v} = \boldsymbol{0} \end{equation} $$

Step 5: 约束力

$\boldsymbol{f}_{Jj}$是主动力和约束力的和，参见式($\ref{equa_3_34}$)，可以写成如下的形式: $$ \boldsymbol{f}_{Jj} = \boldsymbol{T}_{aj}\boldsymbol{\tau}_j + \boldsymbol{T}_j\boldsymbol{\lambda}_j $$ 在该方程中，$\boldsymbol{T}_{aj}$是一个$6 \times n_{fj}$ 的矩阵张成了关节$j$的主动力子空间，$\boldsymbol{\tau}_j$是$n_{fj}$为的广义力向量， $\boldsymbol{\lambda}_j$是一个$n_{cj}$维的向量表示约束力，并且有$n_{fj} = 6 - n_{cj}$表示关节$j$允许的运动自由度数量。将各个关节的方程集成起来，有: $$ \begin{equation}\label{equa_3_73} \boldsymbol{f}_J = \boldsymbol{T}_a \boldsymbol{\tau} + \boldsymbol{T\lambda} \end{equation} $$ 其中，$\boldsymbol{\tau}$和$\boldsymbol{\lambda}$分别表示$\boldsymbol{\tau}_1 \cdots \boldsymbol{\tau}_{N_J}$和$\boldsymbol{\lambda}_1 \cdots \boldsymbol{\lambda}_{N_J}$。 $\boldsymbol{T}_a$是对角块矩阵，$T_{aj}$表示第$j$个对角块。

Step 6: 最终的方程

将式($\ref{equa_3_70}$)($\ref{equa_3_73}$)代入式($\ref{equa_3_66}$)就得到如下方程: $$ \boldsymbol{P}(\boldsymbol{T}_a\boldsymbol{\tau} + \boldsymbol{T\lambda}) = \boldsymbol{Ia} + \boldsymbol{p} $$ 与式($\ref{equa_3_72}$)联立，有: $$ \begin{equation}\label{equa_3_74} \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{p} \\ - \dot{\boldsymbol{T}^T}\boldsymbol{P}^T\boldsymbol{v} \end{bmatrix} \end{equation} $$ 这是原始刚体系统的运动方程。有两个未知数$\boldsymbol{a}$和$\boldsymbol{\lambda}$，从方程中可以唯一的解出$\boldsymbol{a}$。如果系数矩阵是非奇异的，那么$\boldsymbol{\lambda}$也是唯一的。与3.2 节中的方程对比可以看出，此处的 $\boldsymbol{T}^T\boldsymbol{P}^T$和$−\dot{\boldsymbol{T}^T}\boldsymbol{P}^T\boldsymbol{v}$ 等价于方程($\ref{equa_3_11}$)的 $\boldsymbol{K}$ 和 $\boldsymbol{k}$。这里的$\boldsymbol{I,a,p}$ 等价于式($\ref{equa_3_14}$)中的$\boldsymbol{H}, \ddot{\boldsymbol{q}}, \boldsymbol{C}$。 $\boldsymbol{P}\boldsymbol{T}_a\boldsymbol{\tau}$和$\boldsymbol{PT\lambda}$等价于式($\ref{equa_3_14}$)中的$\boldsymbol{\tau}$ 和$\boldsymbol{\tau}_c$。并且式($\ref{equa_3_74}$)整体上等价于式($\ref{equa_3_17}$)。

讨论(Discussion)

方程($\ref{equa_3_74}$)是动力学算法的基础，但在设计算法的过程中还有几个细节需要解决。例如，速度和加速度变量是各个刚体的速度和加速度矢量的普吕克坐标，但是在什么坐标系中呢？也没有提到位置变量，以及如何计算$\boldsymbol{I},\boldsymbol{T},\boldsymbol{T}_a\boldsymbol{\tau}, \dot{\boldsymbol{T}^T}\boldsymbol{v}$。另一个问题是如何求解方程($\ref{equa_3_74}$)，这个系数矩阵很大，也很稀疏，使用稀疏分解过程会大大提高效率。此外，如果方程($\ref{equa_3_72}$)中的约束不全是线性无关的，那么系数矩阵将式奇异的，在这种情况下需要设计适用于奇异矩阵的求解过程。最后，还有一个约束稳定性的问题，由于式($\ref{equa_3_72}$)在加速度级别上描述运动约束，那么计算位置和速度约束时，就可能因为数值舍入误差和截断，产生累积误差。我们将在8.3节中详细讨论这个问题。

Chapter3: 刚体系统的动力学Dynamics of Rigid Body Systems