刚体动力学算法第一章 简介（Chapter 1 Introduction）

第一章 简介（Chapter 1 Introduction）

刚体动力学是一门古老的学科，但被计算机赋予了新的活力和变革。今天，我们可以在计算机游戏、动画和虚拟现实软件、模拟器、运动控制系统以及各种工程设计和分析工具中找到动力学计算。在每种情况下，计算机都会计算与物理系统的刚体近似运动相关的力、加速度等。

本书的主要目的是展示一系列在计算机上执行各种动力学计算的有效算法。每个算法都有详细的描述，以便读者可以理解它是如何工作的以及为什么它是高效的；并解释了基本概念，例如递归公式、分支诱导稀疏性和关节型刚体惯性。

刚体动力学通常使用 3D 向量来表达。然而，本书的主题是动力学算法，这个主题最好使用 $6\mathrm{D}$ 向量来表达。因此，我们采用基于空间向量的 $6\mathrm{D}$ 表示法，这将在第 2 章中进行解释。与 3D 向量相比，空间表示法大大减少了代数量，简化了描述和解释动力学算法的任务，并且简化了在计算机上实现算法的过程。

本章对本书的主题进行了简短介绍。它介绍了一些关于动力学算法的内容，一些关于空间向量的内容，并解释了本书的组织方式。

1.1 动力学算法

刚体动力学定义了运动方程，来描述作用在物理系统上的力和产生的加速度的关系。动力学算法则是计算这些物理量数值解的方法。我们主要关注两种类型的动力学：

正向动力学（forward dynamics）: 给定外力，计算刚体系统的加速度响应
逆向动力学（inverse dynamics）: 给定加速度，计算必须施加到刚体系统的力

正向动力学主要用于仿真模拟。
逆动力学有多种用途，例如：运动控制系统、轨迹规划、机械设计以及作为正向动力学计算的组成部分。刚体系统的运动方程可以写成以下规范形式：
$$\mathbf{\tau }=\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}).\text{\hspace{1em}(1.1)}$$
在此方程中， $\mathbf{q}$， $\dot{\mathbf{q}}$ 和 $\ddot{\mathbf{q}}$ 分别是位置、速度和加速度变量的向量，$\mathbf{\tau }$ 是外力的向量。$\mathbf{H}$ 是由 $\mathbf{q}$ 确定的惯量矩阵，记为 $\mathbf{H}(\mathbf{q})$ 。$\mathbf{C}$ 是力项的向量，它考虑了科里奥利力、离心力、重力以及作用在系统上（除了 $\mathbf{\tau }$ 中的力）之外的任何其他力。写作 $\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$ 以表示它是 $\mathbf{q}$ 和 $\dot{\mathbf{q}}$ 相关的函数。 $\mathbf{H}$ 和 $\mathbf{C}$ 是运动方程的系数矩阵（coefficients ），$\mathbf{\tau }$ 和 $\ddot{\mathbf{q}}$ 是变量。通常情况下，其中一个变量是给定的，另一个变量是未知的。

虽然习惯上写成 $\mathbf{H}(\mathbf{q})$ 和 $\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$，但写成 $\mathbf{H}$ (model, $\mathbf{q})$ 和 $\mathbf{C}(\mathrm{model},\mathbf{q},\dot{\mathbf{q}})$ 会更准确，以表明 $\mathbf{H}$ 和 $\mathbf{C}$ 取决于它们所应用的特定刚体系统。model 符号是指描述特定刚体系统的组成部分的数据集合：主体和关节的数量、它们连接在一起的方式以及与相关的每个参数的值。每个组件（惯性参数、几何参数等 ）。

系统模型(system model): 系统模型描述了系统本身，数学模型(mathematical model)则描述了系统表现的某些层面。

在计算机上，模型将是“系统模型”类型的变量，其值将是包含特定刚体系统的系统模型的数据结构。公式 (1.1) 是数学模型。

将正向和逆向动力学计算封装到一对函数 $\mathrm{FD} $ 和 $\mathrm{ID}$ 中。这些函数满足
$$\ddot{\mathbf{q}}=\mathrm{FD}(\text{ model },\mathbf{q},\dot{\mathbf{q}},\mathbf{\tau })\text{\hspace{1em}(1.2)}$$
和
$$\mathbf{\tau }=\mathrm{ID}(\mathrm{model},\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}).\text{\hspace{1em}(1.2)}$$
将这些方程与方程(1.1)进行比较。 显然，$\mathrm{FD}$ 和 $\mathrm{ID}$ 必须分别计算为${\mathbf{H}}^{-1}(\mathbf{\tau }-\mathbf{C})$ 和 $\mathbf{H}\ddot{\mathbf{q}}+\mathbf{C}$。然而，这些方程的优点是它们清楚地显示了每次计算的输入和输出以及系统模型在这两种情况下都是输入。因此，预计实现 $\mathrm{FD}$ 和 $\mathrm{ID}$ 的算法将适用于一类刚体系统，并将使用系统模型中的数据来计算该模型所描述的特定刚体系统的动力学。我们将使用基于模型的算法这个名称来指代像这样工作的算法。

这种方法的一大优点是可以编写、测试、记录一段计算机代码等，以计算广泛类别中任何刚体系统的动力学。感兴趣的两个主要类别称为运动树和闭环系统。粗略地说，运动树是任何不包含运动学环的刚体系统，闭环系统是任何不是运动树的刚体系统。计算运动树的动力学比计算运动树的动力学要容易得多对于闭环系统。基于模型的算法可以根据其两个主要属性进行分类：它们执行什么计算，以及它们适用于什么类型的系统。本书的算法主体包括运动树和闭环系统的正向和逆向动力学算法。

1.2 空间向量（ Spatial Vectors）

三维空间中的刚体有六个运动自由度，我们通常用三维矢量来表示其动力学。因此，要说明一个刚体的运动方程，我们实际上必须说明两个矢量方程：
$$\mathbf{f}=m{\mathbf{a}}_C\text{ and }{\mathbf{n}}_C=\mathbf{I}\dot{\mathbf{\omega }}+\mathbf{\omega }\times \mathbf{I}\mathbf{\omega }.\text{\hspace{1em}(1.4)}$$
第一个方程表示施加在刚体上的力与其质量中心的线加速度之间的关系。
第二个方程表示施加在刚体上的力矩（以其质量中心为参考系）与其角加速度之间的关系。

在空间矢量符号中，我们使用的 6D 矢量表示刚体运动的线性和角度 。因此，线加速度和角加速度组合成空间加速度矢量，力和力矩组合成空间力矢量，以此类推。使用空间符号，刚体的运动方程可以写成
$$\mathbf{f}=\mathbf{I}\mathbf{a}+\mathbf{v}\times {}^{\ast }\mathbf{I}\mathbf{v},\text{\hspace{1em}(1.5)}$$
其中，$\mathbf{f}$ 是施加在刚体上的力，$\mathbf{v}$ 和 $\mathbf{a}$ 是刚体的空间线速度和空间加速度，$\mathbf{I}$ 是刚体的空间惯性张量。符号 $ \times{ }^*$ 表示空间向量的叉积。该方程的一个明显特征是与 公式1.4 有明显命名冲突。我们可以通过空间符号(e.g. $\hat{f}$) 上方加个 hats 来解决这个问题。

除了运动方程之外，空间符号还提供了更多简化。例如，如果两个刚体 连接在一起形成一个刚体，那么新刚体的空间惯性由简单的公式给出
$${\mathbf{I}}_{\text{new }}={\mathbf{I}}_1+{\mathbf{I}}_2,$$
其中， ${\mathbf{I}}_1$ 和 ${\mathbf{I}}_2$ 是两个刚体原来的惯性。。这个等式取代了三维矢量法中的三 个等式：一个用于计算新质量，一个用于计算新质心，一个用于计算绕新质心的新转动惯量。所有这些简化的总体效果是，与标准三维矢量符号相比，空间符号通常可将代数量减少至少 4 倍。With the barrier of algebra out of the way，分析师 就可以更简洁地陈述问题，用更少的步骤解决问题，并获得更紧凑的解决 方案。

空间矢量的另一个好处是简化了计算机代码的编写过程。由此产生的代 码更短，更易于读取、编写和调试，但效率并不比使用三维矢量的软件低 。例如，某些编程语言允许使用以下语句实现公式 1.5
$$\mathrm{f}=\mathrm{I}\ast \mathrm{a}+\mathrm{v}\cdot \mathrm{cross}(\mathrm{I}\ast \mathrm{v});$$
其中每个变量的类型已向编译器声明。因此，编译器知道 $\mathrm{I}$ 和 $\mathrm{v}$ 分别 表示刚体惯性和空间运动矢量，因此可以将表达式 $\mathrm{I}\ast \mathrm{v}$ 编译成代码， 调用空间算术库中的一个例程，该例程旨在高效地执行这一特定的乘法 运算。

1.3 单位和符号（Units and Notation）

角度假定以弧度为单位。除此之外，本书中的等式与单位的选择无关， 只要求所选的单位制一致。在明确提到单位的少数地方，使用的是国际单位制单位。 数学符号一般遵循 ISO 准则。因此，变量用斜体表示；常量和函数用罗马字母表示；向量和矩阵用粗斜体表示。向量用小写字母表示( 公式1.1 中的向量 $C$ 是唯一的例外)，矩阵用大写字母表示。符号简表见第 265 页，有些符号在索引中有条目。 这里有几个细节值得一提。符号 $0$ 和 $1$ 分别表示零矩阵和同一矩阵 ，$0$ 也表示零向量。上标 ${}^{-\mathrm{T}}$ 表示逆的转置，因此 ${\mathbf{A}}^{-\mathrm{T}}$ 表示 ${\left({\mathbf{A}}^{-1}\right)}^{\mathrm{T}}$ 。

像 $\mathbf{a}\times$ 这样的表达式 表示将 $\mathbf{b}$ 映射到 $\mathbf{a}\times \mathbf{b}$ 的运算符。如果一个量被描述为数组，那么它就是一个有编号的列表。如果 $\lambda$ 是一 个数组，那么 $\lambda$ 中的元素 $i $ 就被写成 $\lambda (i)$。如果一个符号有一个前导上标（如 ${}^A\mathbf{v}$ ），则该上标表示一个坐标系。坐标系也可以用下标表示。 如第 2 章所述，空间矢量有时用帽子标记，坐标矢量则用下划线标记。

1.4 读者指南（Readers’ Guide）

本书内容可分为三个部分：准备、主要算法和附加主题。第 2、3 和 4 章是准备部分，第 5 至 8 章介绍了主要算法。有足够背景知识的读者可 以直接跳到第 5 章。第 9 章及以后各章假定读者对前面的材料有基本了 解，但在其他方面自成体系。

第 2 章介绍了空间向量代数的基本原理；第 3 章解释了如何计算和 分析刚体系统的运动方程。这两章是数学含量最高的两章，它们所涉及 的主题比后面算法所需的最低深度还要深。第 4 章介绍了系统模型的各 个组成部分。后面章节中使用的许多量都是在这里定义的。

第 5、6 和 7 章依次介绍了三种最著名的运 动 树算法：用于逆动力学的 递归牛顿-欧拉算法，以及用于正动力学的复合刚体算法和铰接体算法。第 5 章还解释了递归表述的概念，这也是这些算法高效的原因。然后，第 8 章介绍了几种计算闭环系统动力学的技术。这四章大致按照复杂度递增的 顺序排列（即算法逐渐变得复杂）。

第 9 章、第 10 章和第 11 章涉及各种额外主题。第 9 章考虑了更多算 法，包括正演动力学和反演动力学的混合算法以及浮基系统的算法。第 10 章考虑了数值精度和计算效率问题；第 11 章研究了受接触和撞击影响的刚 体系统动力学。

读者会发现文中的例子分布不均。在某些情况下，它们说明了正文中涉 及的概念。在另一些情况下，它们描述的观点更容易通过示例的形式来解 释。读者还会发现，每种主要算法都以方程组和伪代码程序的形式呈现。 在许多情况下，两者是并列的。伪代码的目的是不言自明，并易于转化为 真正的计算机代码。