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

3.4 约束分类

运动约束可以根据其对刚体系统施加的约束性质进行分类。在每种情况下，约束都由系统的运动变量的代数方程描述，但方程的形式各不相同。如图3.1所示，有一个标准的分类层次结构。

首先区分等式约束和不等式约束。前者源于两个刚体之间的永久物理接触；后者则在刚体既可以接触又可以分离时产生。不等式约束可用于描述碰撞、弹跳和失去接触等现象。它们是第11章的主题，在这里不再进一步考虑。

接下来区分完整约束和非完整约束。前者是对位置变量的约束，通常源于滑动接触（sliding contact）。后者是对速度变量的约束，通常源于滚动接触（ rolling contact）。非完整约束函数${\varphi }_{nh}$在速度变量上是线性的，因此可以表达为以下形式
$${\varphi }_{nh}(\mathbf{q},\dot{\mathbf{q}},t)={\varphi }_{nh}^0+\sum _{i=1}^n{\varphi }_{nh}^i{\dot{q}}_i\text{\hspace{1em}(3.29)}$$

系数${\varphi }_{nh}^i$是位置变量和时间的函数。为了对比，一个完整约束函数${\varphi }_h$的时间导数为

$$\frac{\mathrm{d}}{\mathrm{d}t}{\varphi }_h(\mathbf{q},t)=\frac{\partial {\varphi }_h}{\partial t}+\sum _{i=1}^n\frac{\partial {\varphi }_h}{\partial q_i}{\dot{q}}_i\text{\hspace{1em}(3.30)}$$

它具有相同的代数形式。因此，在速度和加速度层面上，完整约束和非完整约束之间没有区别。这两个方程的主要区别在于 方程3.29 是不可积的；也就是说，${\varphi }_{nh}$ 不是任何函数的导数，否则它将仅仅是通过其导数表示的完整约束。这个性质的结果是，含有非完整约束的系统具有比速度自由度更多的位置自由度，因此需要更多的位置变量而非速度变量。如果刚体系统至少包含一个非完整约束，那么它被称为非完整系统；否则，它被称为完整系统。因此，方程3.1 只能描述完整系统，而方程3.5可以描述完整系统和非完整系统。

${}^2$鉴于约束方程在所有时间均成立，而不仅在单个时刻成立，因此方程$\dot{\varphi }=0$和$\varphi =0$描述了同样的约束。

最后，还需区分的是固连约束（scleronomic constraint）与时变约束（rheonomic constraint）。固连约束只是位置变量的函数，而时变约束既与位置变量又与时间有关。固连约束产生于两个刚体之间的滑动接触。它们是典型机械系统中最简单且最常见的约束类型。时变约束可以看作是某些滑动自由度被强制作为时间的预定函数而变化的固连约束。时变约束的作用是引入运动激励；即预定运动。

图3.2展示了一些运动约束的示例。图(a)和(b)分别显示了一个圆柱接头和一个棱柱接头，它们都是固连约束。圆柱接头允许刚体 $B_2$ 在相对于 $B_1$ 的平移和旋转中滑动，而棱柱接头只允许相对滑动运动。因此，圆柱接头具有两个自由度的运动，而棱柱接头仅允许一个自由度的运动。如果我们将圆柱接头的一个自由度设为时间的预定函数，它就变成了一个一自由度的时变约束。如果对棱柱接头也进行同样的处理，它将成为一个零自由度的时变约束（实际上非常有用）。

运动约束几乎总是涉及两个刚体之间的关系。然而，也可能设计涉及多于两个刚体的约束，并且不能简化为一组两体约束。图3.2©给出了一个例子。该图展示了一个二维刚体系统，其中三个相同的圆形刚体受到约束，必须保持在一个无质量、零直径、非弹性的绳子的边界内。假设绳子保持紧绷，这三个刚体可以以满足方程 $l_1+l_2+l_3+2\pi r=p$ 的任何方式自由运动，其中 $p$ 和 $r$ 分别为绳子的周长和刚体的半径。

图3.2(d)显示了一个球体静止在一个平面表面上。如果这个球体被约束以无滑动地滚动，它会有三个瞬时运动自由度：可以沿两个方向滚动，也可以围绕接触点旋转。然而，通过适当的操作序列，可以使球体到达平面上的任意点，并具有任意方向。因此，它有五个位置自由度。这是非完整约束的特征。如果球体可以滑动，那么它就是一个固连约束。

从图3.2(a)和(b)中刚体的形状可以看出，它们不能被拉开；但是没有物理上的限制约束绳子松弛，或者球体与平面失去接触。如果我们对这些系统即将受到的力有足够的了解，可以确保这种情况不会发生，那么就可以将这些约束建模为等式约束；否则，必须将其建模为不等式约束。

3.5 关节约束

我们定义关节为两个刚体之间的运动约束。因此，一个关节可以对一对刚体的相对速度施加从零到六个约束。我们称这两个刚体为前驱和后继，我们说关节连接从前驱到后继。关节速度 ${\mathbf{v}}_{\mathrm{J}}$ 定义为后继相对于前驱的速度：

$${\mathbf{v}}_{\mathrm{J}}={\mathbf{v}}_s-{\mathbf{v}}_p\text{\hspace{1em}(3.31)}$$

其中 ${\mathbf{v}}_p$ 和 ${\mathbf{v}}_s$ 分别是前驱和后继刚体的速度。关节约束的一般形式是

$${\mathbf{v}}_{\mathrm{J}}=\mathbf{S}(\mathbf{q},t)\dot{\mathbf{q}}+\mathbf{\sigma }(\mathbf{q},t)\text{\hspace{1em}(3.32)}$$

其中 $\mathbf{S}$ 是关节的运动子空间矩阵，$\mathbf{\sigma }$ 是偏置速度，$\mathbf{q}$ 和 $\dot{\mathbf{q}}$ 是关节的位置和速度变量，$t$ 是时间。如果位置变量不是速度变量的积分，则必须使用不同的符号表示 $\dot{\mathbf{q}}$ 或 $\mathbf{q}$。偏置速度是当 $\dot{\mathbf{q}}=0$ 时 ${\mathbf{v}}_{\mathrm{J}}$ 的值，只有在关节明确依赖于时间时（即它是一个流变或时间相关的非完整约束），它才具有非零值。如果关节不依赖于时间，则方程3.32简化为

$${\mathbf{v}}_{\mathrm{J}}=\mathbf{S}(\mathbf{q})\dot{\mathbf{q}}\text{\hspace{1em}(3.33)}$$

方程3.32 的效果是将速度 ${\mathbf{v}}_{\mathrm{J}}-\mathbf{\sigma }$ 限制在子空间 $S\subseteq {\mathbb{R}}^6$ 中。如果关节允许 $n_f$ 个相对运动自由度，则 $\dim (S)=n_f$，$S$ 是一个 $6\times n_f$ 的矩阵。（${}^3$关节所施加的约束数为 $n_c=6-n_f$，约束力必须位于 $n_c$ 维子空间 $S^{\perp }\subseteq {\mathbb{R}}^6$ 中。）

如果 $n_f=0$，则 $S$ 是一个 $6\times 0$ 的矩阵。这样的量遵循矩阵代数的基本规则，再加上一个额外的规则：$m\times 0$ 的矩阵与 $0\times n$ 的矩阵的乘积是一个 $m\times n$ 的零矩阵。因此，$6\times 0$ 矩阵 $\mathbf{S}$ 与 $0\times 1$ 矢量 $\dot{\mathbf{q}}$ 的乘积是一个 $6\times 1$ 的零向量。设 ${\mathbf{f}}_{\mathrm{J}}$ 是通过关节传递的力。我们将其定义为从前驱传递到后继的力；因此，作用在后继上的力为 $f_{\mathrm{J}}$，作用在前驱上的力为 $-{\mathbf{f}}_{\mathrm{J}}$。这个力可以看作是主动力和约束力的总和。前者来自关节上（或两个刚体之间）的执行器、弹簧、阻尼器或任何其他力元件，后者是施加运动约束的力。在这一点上，我们有一个选择。约束力必须是子空间 $S^{\perp }$ 的元素，但对于主动力有两种处理方式：它可以被视为已知的空间力，或者作为映射到 ${\mathbb{R}}^6$ 的子空间的广义力。我们在这里选择后者。

设 $T$ 和 $T_a$ 分别是约束力和主动力的子空间。根据定义，$T=S^{\perp }$，但 $T_a$ 可以是满足 $T\oplus T_a={\mathbb{R}}^6$ 的任何子空间。我们现在可以将 $f_{\mathrm{J}}$ 表示为

$${\mathbf{f}}_{\mathrm{J}}={\mathbf{T}}_a\mathbf{\tau }+\mathbf{T}\mathbf{\lambda }\text{\hspace{1em}(3.34)}$$

其中 ${\mathbf{T}}_a\mathbf{\tau }$ 是主动力，$\mathbf{T}\mathbf{\lambda }$ 是约束力，${\mathbf{T}}_a$ 和 $\mathbf{T}$ 满足
$${\mathbf{T}}_a^{\mathrm{T}}\mathbf{S}=1\text{\hspace{1em}(3.35)}$$

且

$${\mathbf{T}}^{\mathrm{T}}\mathbf{S}=0\text{\hspace{1em}(3.36)}$$

$\mathbf{\tau }$ 是对应于 $\dot{\mathbf{q}}$ 的广义力向量，$\mathbf{\lambda }$ 是约束力变量的向量。根据 ${\mathbf{T}}_a$ 和 $\mathbf{T}$ 的性质，我们可以立即推断出：

$${\mathbf{S}}^{\mathrm{T}}{\mathbf{f}}_{\mathrm{J}}=\mathbf{\tau }\text{\hspace{1em}(3.37)}$$

和

$${\mathbf{T}}^{\mathrm{T}}{\mathbf{v}}_{\mathrm{J}}={\mathbf{T}}^{\mathrm{T}}\mathbf{\sigma }\text{\hspace{1em}(3.38)}$$

方程3.37是从通过关节传递的空间力中获取关节的广义力的公式；方程3.38是速度约束方程的隐式形式。

方程3.35的理由如下。关节传递的总功率为 ${\mathbf{f}}_{\mathrm{J}}\cdot {\mathbf{v}}_{\mathrm{J}}$，但这来自于两个独立的功率源：$\mathbf{\sigma }$ 和 $\mathbf{\tau }$。归因于 $\mathbf{\sigma }$ 的功率为 ${\mathbf{f}}_{\mathrm{J}}\cdot \mathbf{\sigma }$，因此归因于 $\mathbf{\tau }$ 的功率必须为 ${\mathbf{f}}_{\mathrm{J}}\cdot \mathbf{S}\dot{\mathbf{q}}$。现在，这第二个功率也必须等于 $\mathbf{\tau }\cdot \dot{\mathbf{q}}$，因为它们是关节处的广义力和速度向量，因此我们有以下功率平衡方程：

$$\mathbf{\tau }\cdot \dot{\mathbf{q}}={\mathbf{f}}_{\mathrm{J}}\cdot \mathbf{S}\dot{\mathbf{q}}={\mathbf{\tau }}^{\mathrm{T}}{\mathbf{T}}_a^{\mathrm{T}}\mathbf{S}\dot{\mathbf{q}}\text{\hspace{1em}(3.39)}$$

但是这个方程对所有的 $\mathbf{\tau }$ 和 $\dot{\mathbf{q}}$ 必须成立，所以我们必须有 ${\mathbf{T}}_a^{\mathrm{T}}\mathbf{S}=1$。关节的加速度约束方程可以直接从方程3.32和3.38通过求导得到。方程3.32的导数为
$$\begin{array}{rl}{\mathbf{a}}_{\mathrm{J}}& =\mathbf{S}\ddot{\mathbf{q}}+\dot{\mathbf{S}}\dot{\mathbf{q}}+\dot{\mathbf{\sigma }}\\ & =\mathbf{S}\ddot{\mathbf{q}}+\left(\stackrel{\circ }{S}+{\mathbf{v}}_s\times \mathbf{S}\right)\dot{\mathbf{q}}+\left(\stackrel{\circ }{\sigma }+{\mathbf{v}}_s\times \mathbf{\sigma }\right)\\ & =\mathbf{S}\ddot{\mathbf{q}}+\stackrel{\circ }{S}\dot{\mathbf{q}}+\stackrel{\circ }{\sigma }+{\mathbf{v}}_s\times (\mathbf{S}\dot{\mathbf{q}}+\mathbf{\sigma })\\ & =\mathbf{S}\ddot{\mathbf{q}}+{\mathbf{c}}_{\mathrm{J}}+{\mathbf{v}}_s\times {\mathbf{v}}_{\mathrm{J}}\end{array}\text{\hspace{1em}(3.40)}$$

其中，$\stackrel{\circ }{S}$ 和 $\stackrel{\circ }{\sigma }$ 是 $\mathbf{S}$ 和 $\mathbf{\sigma }$ 的显式导数，它们在一个后继刚体运动的坐标系中计算，且

$${\mathbf{c}}_{\mathrm{J}}=\stackrel{\circ }{S}\dot{\mathbf{q}}+\stackrel{\circ }{\sigma }\text{\hspace{1em}(3.41)}$$

（实际上，${\mathbf{c}}_{\mathrm{J}}={\stackrel{\circ }{\mathrm{v}}}_{\mathrm{J}}$）。如果 $\mathbf{S}$ 和 $\mathbf{\sigma }$ 是 $\mathbf{q}$ 和 $t$ 的函数，如方程式 3.32 所示，那么 $\stackrel{\circ }{S}$ 和 $\stackrel{\circ }{\sigma }$ 的一般公式为
$$\stackrel{\circ }{S}=\frac{\partial \mathbf{S}}{\partial t}+\sum _{i=1}^{n_p}\frac{\partial \mathbf{S}}{\partial q_i}{\dot{q}}_i\text{\hspace{1em}(3.42)}$$

且

$$\stackrel{\circ }{\sigma }=\frac{\partial \mathbf{\sigma }}{\partial t}+\sum _{i=1}^{n_p}\frac{\partial \mathbf{\sigma }}{\partial q_i}{\dot{q}}_i\text{\hspace{1em}(3.43)}$$

在继续之前，读者应该记住上述相对复杂的方程适用于一般情况，在实践中几乎从不需要。这部分是因为大多数常见关节类型具有 $\stackrel{\circ }{S}=0$ 和 $\mathbf{\sigma }=0$（暗示 ${\mathbf{c}}_{\mathrm{J}}=0$）的性质，也部分是因为当我们在关节中遇到显式的时间依赖性（即 $\mathbf{\sigma }\ne 0$）的罕见情况时，几乎总能通过第9章的混合动力学算法来实现时间相关项，而不是使用 $\mathbf{\sigma }$。

最后，隐式加速度约束方程可以通过对方程3.38求导数或将方程3.40乘以 ${\mathbf{T}}^{\mathrm{T}}$ 来得到。两种方法得到的结果相同，但后者更简单：

$$\begin{array}{rl}{\mathbf{T}}^{\mathrm{T}}{\mathbf{a}}_{\mathrm{J}}& ={\mathbf{T}}^{\mathrm{T}}\left(\mathbf{S}\ddot{\mathbf{q}}+{\mathbf{c}}_{\mathrm{J}}+{\mathbf{v}}_s\times {\mathbf{v}}_{\mathrm{J}}\right)\\ & ={\mathbf{T}}^{\mathrm{T}}\left({\mathbf{c}}_{\mathrm{J}}+{\mathbf{v}}_s\times {\mathbf{v}}_{\mathrm{J}}\right).\end{array}\text{\hspace{1em}(3.44)}$$

例3.2 我们来看如何定义主动力子空间。图3.3显示了一个系统，它由一个移动的刚体 $B$ 和一个通过齿轮和传动装置驱动该关节的电机 $M$ 组成，关节连接着固定的基座。在坐标系中也显示了$x$轴和$y$轴，其中$z$轴与关节的旋转轴对齐。用这些坐标表示，关节的运动和约束力子空间矩阵为
$$\mathbf{S}=\left[\begin{array}{c}0\\ 0\\ 1\\ 0\\ 0\\ 0\end{array}\right]\text{ and }\mathbf{T}=\left[\begin{array}{ccccc}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{array}\right]$$

严格来说，$\mathbf{S}$ 和 $\mathbf{T}$ 的取值不止这些，因为任何两个张成正确子空间的矩阵都可以。

主动力通过齿轮接触点传递到刚体 $B$，它是一个在 $y$ 方向上大小为 $f$ 的线性力。接触点的坐标为 $(-r,0,0)$，其中 $r$ 是齿轮的半径，所以主动力的 Plücker 坐标为 $\mathbf{f}={\left[\begin{array}{llllll}0& 0& -rf& 0& f& 0\end{array}\right]}^{\mathrm{T}}$。现在，在这个例子中 $\mathbf{\tau }$ 只是一个标量，所以如果 $\mathbf{f}={\mathbf{T}}_a\mathbf{\tau }$，那么 ${\mathbf{T}}_a$ 就是 $\mathbf{f}$ 的标量倍数。根据式3.35，${\mathbf{T}}_a$ 必须满足 ${\mathbf{T}}_a^{\mathrm{T}}\mathbf{S}=1$，所以解可以简单地表示为

$${\mathbf{T}}_a=\mathbf{f}/\left({\mathbf{f}}^{\mathrm{T}}\mathbf{S}\right)=\left[\begin{array}{c}0\\ 0\\ 1\\ 0\\ -1/r\\ 0\end{array}\right]$$

注意到 ${\mathbf{T}}_a$ 和 $\left({\mathbf{T}}_a\right)$ 的值都依赖于 $r$。这种依赖关系对运动方程没有影响，但它确实影响了约束力的值。从物理上讲，约束力来自关节的轴承。因此，如果任务是模拟系统的运动，那么 $r$ 是无关紧要的；但如果任务是为关节选择适当的轴承，那么必须考虑 $r$ 的值。

3.6 刚体的约束动力学

考虑图3.4所示的刚体。该刚体通过一个具有运动子空间 $S\subseteq {\mathrm{M}}^6$ 的关节连接到一个固定基座。关节对刚体的运动施加 $n_c$ 个约束，使其具有 $n_f$ 个自由度，其中 $n_f=\dim (S)$，$n_c=6-n_f=\dim \left(S^{\perp }\right)$。这个刚体上有三个力：一个施加力 $\mathbf{f}$，一个约束力 ${\mathbf{f}}_c$，一个重力 ${\mathbf{f}}_g$（未显示）。因此，该刚体的运动方程为
$$\mathbf{f}+{\mathbf{f}}_c+{\mathbf{f}}_g=\mathbf{I}\mathbf{a}+\mathbf{v}\times {}^{\ast }\mathbf{I}\mathbf{v}$$

其中，$\mathbf{I}$，$\mathbf{a}$ 和 $\mathbf{v}$ 分别表示刚体的惯性、加速度和速度。我们对单独的项 ${\mathbf{f}}_g$ 和 $\mathbf{v}{\times }^{\ast }\mathbf{I}\mathbf{v}$ 不感兴趣，因此将它们合并为一个偏置力项 $\mathbf{p}=\mathbf{v}{\times }^{\ast }\mathbf{I}\mathbf{v}-{\mathbf{f}}_g$ 。受约束刚体的运动方程可以写成如下形式：

$$\mathbf{f}+{\mathbf{f}}_c=\mathbf{I}\mathbf{a}+\mathbf{p}\text{\hspace{1em}(3.45)}$$

其中满足约束条件：

$$\mathbf{v}\in S\text{ and }{\mathbf{f}}_c\in S^{\perp }\text{\hspace{1em}(3.46)}$$

我们寻求这个方程组的解，其中加速度为施加力的函数。需要注意的是，${\mathbf{f}}_c$ 是一个未知量，并且必须在解的过程中进行求解 和/或 消除。

方法1

我们引入一个 $6\times n_f$ 的矩阵 $\mathbf{S}$，它张成了子空间 $S$。利用这个矩阵，方程3.46 中的约束方程可以写成如下形式：

$$\mathbf{v}=\mathbf{S}\dot{q}\text{\hspace{1em}(3.47)}$$

和

$${\mathbf{S}}^{\mathrm{T}}{\mathbf{f}}_c=0\text{\hspace{1em}(3.48)}$$

加速度约束为

$$\mathbf{a}=(\mathbf{S}\ddot{\mathbf{q}}+\dot{\mathbf{S}}\dot{\mathbf{q}})\text{\hspace{1em}(3.49)}$$

其中 $\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ 分别是关节速度和加速度变量的向量。方程3.48 暗示了我们可以通过两边同时乘以 $S^{\mathrm{T}}$ 来消除 方程3.45 中的 ${\mathbf{f}}_c$，得到

$${\mathbf{S}}^{\mathrm{T}}\mathbf{f}={\mathbf{S}}^{\mathrm{T}}(\mathbf{I}\mathbf{a}+\mathbf{p})\text{\hspace{1em}(3.50)}$$

将 方程3.49 代入这个方程可以得到

$${\mathbf{S}}^{\mathrm{T}}\mathbf{f}={\mathbf{S}}^{\mathrm{T}}(\mathbf{I}(\mathbf{S}\ddot{\mathbf{q}}+\dot{\mathbf{S}}\dot{\mathbf{q}})+\mathbf{p})$$

可以按照如下方式求解 $\ddot{\mathbf{q}}$：

$$\ddot{\mathbf{q}}={\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}(\mathbf{f}-\mathbf{I}\dot{\mathbf{S}}\dot{\mathbf{q}}-\mathbf{p})\text{\hspace{1em}(3.51)}$$

表达式 ${\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}$ 是一个 $n_f\times n_f$ 的对称且正定矩阵，因此可以保证其可逆性。找到 $\ddot{\mathbf{q}}$ 的表达式后，最后一步是将方程3.51代回方程3.49，得到

$$\mathbf{a}=\mathbf{S}{\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}{\mathbf{S}}^{\mathrm{T}}(\mathbf{f}-\mathbf{I}\dot{\mathbf{S}}\dot{\mathbf{q}}-\mathbf{p})+\dot{\mathbf{S}}\dot{\mathbf{q}}\text{\hspace{1em}(3.52)}$$

这个方程可以写成如下形式：

$$a=\mathbf{\Phi }\mathbf{f}+\mathbf{b}\text{\hspace{1em}(3.53)}$$

其中

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

和

$$\mathbf{b}=\dot{\mathbf{S}}\dot{\mathbf{q}}-\mathbf{\Phi }(\mathbf{I}\dot{\mathbf{S}}\dot{\mathbf{q}}+\mathbf{p})\text{\hspace{1em}(3.55)}$$

$\mathbf{\Phi }$ 是约束刚体的表观逆惯性矩阵（ apparent inverse inertia），$\mathbf{b}$ 是其偏置加速度，即在 $\mathbf{f}=0$ 时的加速度。表观逆惯性矩阵描述了加速度对施加力变化的响应方式。它是一个对称、正半定矩阵，具有以下特性：$\mathrm{range}(\mathbf{\Phi })=S,\mathrm{null}(\mathbf{\Phi })=S^{\perp }$ 和 $\mathrm{rank}(\mathbf{\Phi })=n_f$。

方法2

解决 方程3.45 和 方程3.46 的另一种方法是引入一个 $6\times n_c$ 的矩阵 $\mathbf{T}$，它张成了子空间 $S^{\perp }$。利用这个矩阵，约束方程可以写成如下形式：

$${\mathbf{T}}^{\mathrm{T}}\mathbf{v}=0\text{\hspace{1em}(3.56)}$$

和

$${\mathbf{f}}_c=\mathbf{T}\mathbf{\lambda }\text{\hspace{1em}(3.57)}$$

其中 $\mathbf{\lambda }$ 是约束力变量的向量，加速度约束为

$${\mathbf{T}}^{\mathrm{T}}\mathbf{a}+{\dot{\mathbf{T}}}^{\mathrm{T}}\mathbf{v}=0\text{\hspace{1em}(3.58)}$$

将 方程3.57 代入 方程3.45 中得到

$$\mathbf{f}+\mathbf{T}\mathbf{\lambda }=\mathbf{I}\mathbf{a}+\mathbf{p}\text{\hspace{1em}(3.59)}$$

然后可以将方程3.58和3.59合并为一个方程，

$$\left[\begin{array}{cc}\mathbf{I}& \mathbf{T}\\ {\mathbf{T}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ -\mathbf{\lambda }\end{array}\right]=\left[\begin{array}{c}\mathbf{f}-\mathbf{p}\\ -{\dot{\mathbf{T}}}^{\mathrm{T}}\mathbf{v}\end{array}\right]\text{\hspace{1em}(3.60)}$$

可以求解 $\mathbf{a}$ 和 $\mathbf{\lambda }$ 。这个方程中的系数矩阵是对称且非奇异的，但不一定是正定矩阵。

方法3

第三种可能性是推导出用广义坐标表示的运动方程。向量 $\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ 可以用作约束刚体的广义速度和加速度变量，因此 方程3.51 已经给出了广义加速度作为施加的空间力 $\mathbf{f}$ 的函数。设 $\mathbf{\tau }$ 表示作用在刚体上的广义力。要使 $\mathbf{\tau }$ 等效于 $\mathbf{f}$，两个力必须对 $S$ 中的任何速度都提供相同的功率。$\mathbf{\tau }$ 提供的功率为 ${\mathbf{\tau }}^{\mathrm{T}}\dot{\mathbf{q}}$，而 $\mathbf{f}$ 提供的功率为 ${\mathbf{f}}^{\mathrm{T}}\mathbf{v}$ ，因此我们有以下关系：
$${\mathbf{\tau }}^{\mathrm{T}}\dot{\mathbf{q}}={\mathbf{f}}^{\mathrm{T}}\mathbf{v}={\mathbf{f}}^{\mathrm{T}}\mathbf{S}\dot{\mathbf{q}}$$

但这个方程必须对所有的$\dot{\mathbf{q}}$都成立，因此有

$$\mathbf{\tau }={\mathbf{S}}^{\mathrm{T}}\mathbf{f}\text{\hspace{1em}(3.61)}$$

将这个方程代入方程3.51中得到

$$\ddot{\mathbf{q}}={\left({\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\right)}^{-1}\left(\mathbf{\tau }-{\mathbf{S}}^{\mathrm{T}}(\mathbf{I}\dot{\mathbf{S}}\dot{\mathbf{q}}+\mathbf{p})\right),$$

这是约束刚体的运动方程，通过广义坐标表示。它可以写成标准形式

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

其中

$$\mathbf{H}={\mathbf{S}}^{\mathrm{T}}\mathbf{I}\mathbf{S}\text{\hspace{1em}(3.63)}$$

是广义惯性矩阵，而

$$\mathbf{C}={\mathbf{S}}^{\mathrm{T}}(\mathbf{I}\dot{\mathbf{S}}\dot{\mathbf{q}}+\mathbf{p})\text{\hspace{1em}(3.64)}$$

是广义偏置力。方程3.61 意味着从 $\mathbf{f}$ 到 $\mathbf{\tau }$ 的转换会导致少量的信息丢失。无限多个不同的 $\mathbf{f}$ 值映射到相同的 $\mathbf{\tau }$ 值，从而产生相同的加速度，但每个$\mathbf{f}$ 向量的值不同。如果只知道 $\mathbf{\tau }$，那么可以计算出 $\mathbf{f}+{\mathbf{f}}_c$ 的值，但无法得到各个向量的值。如果目标是进行运动模拟，则丢失的信息无关紧要；但如果目标是设计一个机械系统，则丢失的信息与计算关节轴承上的动态负载力等任务相关。

3.7 多体系统动力学

本章的最后部分将为包含多个刚体和关节的系统推导出一个运动方程。这个分析展示了 第3.5节 和 第3.6节 中与关节相关的内容与 第3.2节 中的广义约束之间的联系，说明了如何从前者构建后者。

假设我们有一个包含以下元素的刚体系统：

• 一个固定基座，我们将其视为 0 号刚体；
• $N_B$ 个移动刚体，它们被编号为 1 到 $N_B$；
• 以及 $N_J$ 个关节，它们被编号为 1 到 $N_J$。
• 对于每个关节 $j$，我们定义 $p(j)$ 和 $s(j)$ 为该关节的前驱刚体和后继刚体的编号。
• 这组 $2N_J$ 个数字定义了系统的连接性。在以下分析中，我们对连接性不加限制。

在制定运动方程时，我们将遵循 第3.6节 中的方法2，但由于当前系统复杂性更高，可能需要额外的步骤。

步骤1：刚体的运动方程

我们假设只有重力 ${\mathbf{f}}_{gi}$ 和连接在刚体 $i$ 上的关节力对刚体 $i$ 起作用。那么刚体 $i$ 的运动方程可以写成：

$${\mathbf{f}}_i={\mathbf{I}}_i{\mathbf{a}}_i+{\mathbf{p}}_i$$

其中，${\mathbf{f}}_i$ 是作用在刚体 $i$ 上的关节力的总和，${\mathbf{p}}_i={\mathbf{v}}_i{\times }^{\ast }{\mathbf{I}}_i{\mathbf{v}}_i-{\mathbf{f}}_{gi}$ 是偏置力。所有 $N_B$ 个刚体的方程可以组合成一个单一的方程：

$$\left[\begin{array}{c}{\mathbf{f}}_1\\ {\mathbf{f}}_2\\ ⋮\\ {\mathbf{f}}_{N_B}\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{I}}_1& 0& \cdots & 0\\ 0& {\mathbf{I}}_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\mathbf{I}}_{N_B}\end{array}\right]\left[\begin{array}{c}{\mathbf{a}}_1\\ {\mathbf{a}}_2\\ ⋮\\ {\mathbf{a}}_{N_B}\end{array}\right]+\left[\begin{array}{c}{\mathbf{p}}_1\\ {\mathbf{p}}_2\\ ⋮\\ {\mathbf{p}}_{N_B}\end{array}\right]\text{\hspace{1em}(3.65)}$$

我们可以更简洁地表示这个方程为：

$$\mathbf{f}=\mathbf{I}\mathbf{a}+\mathbf{p}\text{\hspace{1em}(3.66)}$$

其中，$\mathbf{f}$、$\mathbf{a}$ 和 $\mathbf{p}$ 是 $6N_B$ 维向量，$\mathbf{I}$ 是一个 $6N_B\times 6N_B$ 的分块对角矩阵。为了完整起见，我们还定义$\mathbf{v}={\left[\begin{array}{ll}{\mathbf{v}}_1^{\mathrm{T}}& {\mathbf{v}}_2^{\mathrm{T}}\cdots {\mathbf{v}}_{N_B}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$，其中 ${\mathbf{v}}_i$ 是刚体 $i$ 的速度，因为在关节约束方程中会用到这个向量。

步骤2：由刚体运动求得关节运动

方程3.66 中的向量 $\mathbf{a}$ 是刚体加速度的向量；但是关节所施加的约束是以关节速度和加速度来表达的。因此，我们需要一个以刚体运动来表达关节运动的公式。设 ${\mathbf{v}}_{\mathrm{J}j}$ 和 ${\mathbf{a}}_{\mathrm{J}j}$ 分别表示关节 $j$ 处的速度和加速度，${\mathbf{v}}_{\mathrm{J}}$ 和 ${\mathbf{a}}_{\mathrm{J}}$ 是由 ${\mathbf{v}}_{\mathrm{J}1}\cdots {\mathbf{v}}_{\mathrm{J}{N}_J}$和${\mathbf{a}}_{\mathrm{J}1}\cdots {\mathbf{a}}_{\mathrm{J}{N}_J}$ 连接而成的 $6N_J$ 维向量。根据定义，${\mathbf{v}}_{\mathrm{J}j}$ 和 ${\mathbf{a}}_{\mathrm{J}j}$ 是关节的下一刚体相对于前一刚体的速度和加速度，因此有：

$${\mathbf{v}}_{\mathrm{J}j}={\mathbf{v}}_{s(j)}-{\mathbf{v}}_{p(j)}$$

以及

$${\mathbf{a}}_{\mathrm{J}j}={\mathbf{a}}_{s(j)}-{\mathbf{a}}_{p(j)}$$

这些方程可以汇集在一起并表示为：
$${\mathbf{v}}_{\mathrm{J}}=\left[\begin{array}{c}{\mathbf{v}}_{\mathrm{J}1}\\ ⋮\\ {\mathbf{v}}_{\mathrm{J}{N}_J}\end{array}\right]=\left[\begin{array}{c}{\mathbf{v}}_{s(1)}-{\mathbf{v}}_{p(1)}\\ ⋮\\ {\mathbf{v}}_{s\left(N_J\right)}-{\mathbf{v}}_{p\left(N_J\right)}\end{array}\right]={\mathbf{P}}^{\mathrm{T}}\mathbf{v}\text{\hspace{1em}(3.67)}$$

且

$${\mathbf{a}}_{\mathrm{J}}=\left[\begin{array}{c}{\mathbf{a}}_{\mathrm{J}1}\\ ⋮\\ {\mathbf{a}}_{\mathrm{J}{N}_J}\end{array}\right]=\left[\begin{array}{c}{\mathbf{a}}_{s(1)}-{\mathbf{a}}_{p(1)}\\ ⋮\\ {\mathbf{a}}_{s\left(N_J\right)}-{\mathbf{a}}_{p\left(N_J\right)}\end{array}\right]={\mathbf{P}}^{\mathrm{T}}\mathbf{a}\text{\hspace{1em}(3.68)}$$

其中

$${\mathbf{P}}_{ij}=\{\begin{array}{cc}{1}_{6\times 6}& \text{ if }i=s(j)\\ -1_{6\times 6}& \text{ if }i=p(j)\\ 0_{6\times 6}& \text{ otherwise. }\end{array}\text{\hspace{1em}(3.69)}$$

矩阵 $\mathbf{P}$ 是一个 $6N_B\times 6N_J$ 的矩阵，按照每个刚体和关节的对应关系组织为一个 $N_B\times N_J$ 的分块矩阵，其中每个分块是一个 $6\times 6$ 的矩阵。它将收集到的刚体速度和加速度的向量$(\mathbf{v}$ 和 $\mathbf{a})$映射到收集到的关节速度和加速度的向量$({\mathbf{v}}_{\mathrm{J}}$ 和 ${\mathbf{a}}_{\mathrm{J}})$。$\mathbf{P}$ 的每一行对应一个刚体，每一列对应一个关节。（请注意，我们使用 $i$ 作为刚体编号索引，$j$ 作为关节编号索引。）每一列最多包含两个非零分块：在对应于关节的后继刚体的行中为 $1$，在对应于关节的前驱刚体的行中为 $-1$。如果一个关节连接两个运动刚体，则它的列中将恰好有两个非零分块，正如上述描述的那样。

${}^4$ 严格来说，我们指的是分块行，列也是类似的。然而，如果关节连接或来自固定基座（刚体0），那么它的列将只包含一个非零分块：在与其连接到/来自的运动刚体的行中适当的位置为 $1$ 或 $-1$。每一行 $i$ 都包含一个$1$，表示具有刚体$i$作为它的后继的每个关节，以及一个$-1$，表示具有刚体$i$作为它的前驱的每个关节。

步骤3：由关节力求得刚体力

设 ${\mathbf{f}}_{\mathrm{J}j}$ 是通过关节 $j$ 传输的力，${\mathbf{f}}_{\mathrm{J}}$ 是通过连接关节的向量 $f_{\mathrm{J}1}\cdots f_{\mathrm{J}{N}_J}$ 所形成的 $6N_J$ 维向量。根据定义，${\mathbf{f}}_{\mathrm{J}j}$ 是通过关节$j$从刚体 $p(j)$ 传输到刚体 $s(j)$ 的力。如果我们定义集合 $s^{-1}(i)$ 和 $p^{-1}(i)$ 为具有刚体 $i$ 作为其后继或前驱的所有关节的集合，那么向量 ${\mathbf{f}}_i$（之前定义为作用在刚体 $i$ 上的所有关节力的总和）可以按照以下方式表示：

$${\mathbf{f}}_i=\sum _{j\in s^{-1}(i)}{\mathbf{f}}_{\mathrm{J}j}-\sum _{j\in p^{-1}(i)}{\mathbf{f}}_{\mathrm{J}j}。$$

然而，将这个表达式与$\mathbf{P}$的第$i$行的描述进行对比，可以清楚地看出我们也可以将${\mathbf{f}}_i$表示为：

$${\mathbf{f}}_i=\sum _{j=1}^{N_J}{\mathbf{P}}_{ij}{\mathbf{f}}_{\mathrm{J}j}$$

因此，
$$\mathbf{f}=\mathbf{P}{\mathbf{f}}_{\mathrm{J}}\text{\hspace{1em}(3.70)}$$

步骤4：运动约束

设 ${\mathbf{T}}_j$ 是一个 $6\times n_{cj}$ 的矩阵，它构成了关节 $j$ 的运动子空间 $S_j^{\perp }$，其中 $n_{cj}$ 表示关节 $j$ 所施加的约束数量。那么，关节 $j$ 的速度约束方程可以写成：

$${\mathbf{T}}_j^{\mathrm{T}}{\mathbf{v}}_{\mathrm{J}j}=0,$$

加速度约束方程为：

$${\mathbf{T}}_j^{\mathrm{T}}{\mathbf{a}}_{\mathrm{J}j}+{\dot{\mathbf{T}}}_j^{\mathrm{T}}{\mathbf{v}}_{\mathrm{J}j}=0.$$

将所有的加速度约束方程集合起来得到：

$${\mathbf{T}}^{\mathrm{T}}{\mathbf{a}}_{\mathrm{J}}+{\dot{\mathbf{T}}}^{\mathrm{T}}{\mathbf{v}}_{\mathrm{J}}=0\text{\hspace{1em}(3.71)}$$

其中 $\mathbf{T}$ 是一个 $6N_J\times n_c$ 的分块对角矩阵，其第 $j$ 个对角块为 ${\mathbf{T}}_j$ 。$n_c$ 是由所有关节施加的约束总数，可以表示为 $n_c={\sum }_{j=1}^{N_J}{n}_{cj}$。将 方程3.71 与 方程3.67 和 方程3.68 相结合，得到对刚体加速度的以下约束：

$${\mathbf{T}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}\mathbf{a}+{\dot{\mathbf{T}}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}\mathbf{v}=0\text{\hspace{1em}(3.72)}$$

步骤5：约束力

${\mathbf{f}}_{\mathrm{J}j}$ 是主动力和约束力的叠加，可以表示为：

$${\mathbf{f}}_{\mathrm{J}j}={\mathbf{T}}_{aj}{\mathbf{\tau }}_j+{\mathbf{T}}_j{\mathbf{\lambda }}_j$$

（参见方程3.34）。在这个方程中，${\mathbf{T}}_{aj}$ 是一个 $6\times n_{fj}$ 的矩阵，它构成了关节 $j$ 的主动力子空间；${\mathbf{\tau }}_j$ 是一个 $n_{fj}$ 维向量，表示关节 $j$ 处的广义力；${\mathbf{\lambda }}_j$是一个 $n_{cj}$ 维向量，表示约束力变量；$n_{fj}=6-n_{cj}$表示关节$j$允许的运动自由度数量。将所有关节的方程结合起来，我们有：

$${\mathbf{f}}_{\mathrm{J}}={\mathbf{T}}_a\mathbf{\tau }+\mathbf{T}\mathbf{\lambda }\text{\hspace{1em}(3.73)}$$

其中 $\mathbf{\tau }$ 和 $\mathbf{\lambda }$ 分别是由 ${\mathbf{\tau }}_1\cdots {\mathbf{\tau }}_{N_J}$ 和 ${\mathbf{\lambda }}_1\cdots {\mathbf{\lambda }}_{N_J}$ 拼接而成的向量，${\mathbf{T}}_a$ 是一个分块对角矩阵，其第 $j$ 个对角块为 ${\mathbf{T}}_{aj}$。

步骤6：最终方程

将 方程3.70 和 方程3.73 代入 方程3.66，得到以下运动方程：

$$\mathbf{P}\left({\mathbf{T}}_a\mathbf{\tau }+\mathbf{T}\mathbf{\lambda }\right)=\mathbf{I}\mathbf{a}+\mathbf{p}$$

将这个方程与方程3.72结合，得到：

$$\left[\begin{array}{cc}\mathbf{I}& \mathbf{P}\mathbf{T}\\ {\mathbf{T}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ -\mathbf{\lambda }\end{array}\right]=\left[\begin{array}{c}\mathbf{P}{\mathbf{T}}_a\mathbf{\tau }-\mathbf{p}\\ -{\dot{\mathbf{T}}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}\mathbf{v}\end{array}\right]\text{\hspace{1em}(3.74)}$$

这是原始刚体系统的运动方程。未知数为 $\mathbf{a}$ 和 $\mathbf{\lambda }$，此方程可以唯一地求解出 $\mathbf{a}$。如果系数矩阵是非奇异的，那么 $\mathbf{\lambda }$ 也是唯一确定的。通过将这里的方程与第3.2节中的方程进行比较，可以看出 ${\mathbf{T}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}$ 和 $-{\dot{\mathbf{T}}}^{\mathrm{T}}{\mathbf{P}}^{\mathrm{T}}\mathbf{v}$ 在这里相当于 方程3.11 中的 $\mathbf{K}$ 和 $\mathbf{k}$，$\mathbf{I},\mathbf{a}$ 和 $\mathbf{p}$ 在这里相当于方程3.14中的 $\mathbf{H},\ddot{\mathbf{q}}$ 和 $\mathbf{C}$，$\mathbf{P}{\mathbf{T}}_a\mathbf{\tau }$ 和 $\mathbf{P}\mathbf{T}\mathbf{\lambda }$ 在这里相当于 方程3.14 中的 $\mathbf{\tau }$ 和 ${\mathbf{\tau }}_c$，整个 方程3.74 相当于 方程3.17。

讨论

方程3.74 是动力学算法的可行基础，但在设计算法的过程中还有一些细节需要解决。例如，速度和加速度变量明显是各个刚体速度和加速度向量的普吕克坐标，但在哪个坐标系中？文章中没有提及位置变量，也没有说明如何计算 $\mathbf{I},\mathbf{T},{\mathbf{T}}_a\mathbf{\tau }$ 或 ${\dot{\mathbf{T}}}^{\mathrm{T}}\mathbf{v}$。另一个问题是如何求解 方程3.74。系数矩阵很大，但也是稀疏的，使用稀疏分解过程将极大地提高效率。然而，如果 方程3.72 中的约束不是线性无关的，那么系数矩阵将是奇异的，在这种情况下，需要使用能处理奇异矩阵的求解过程。最后，还有约束稳定化的问题：由于数值舍入和截断误差，在模拟过程中可能会导致位置和速度约束中积累错误。这个问题在 第8.3节 中进行了讨论。