刚体模拟1-无约束刚体动力学 Rigid Body Simulation I—Unconstrained Rigid Body Dynamics

介绍

• 这部分课程笔记涉及刚体动力学问题。为了帮助您开始模拟刚体运动，我们提供了实现这些笔记中讨论的大多数概念的代码片段。这部分课程笔记分为两部分。第一部分涵盖了在其允许运动中完全不受约束的刚体运动；即不关心刚体之间碰撞的模拟。给定作用在刚体上的任何外力，我们将展示如何模拟刚体响应这些力的运动。这些笔记中的数学推导是相当非正式和直观的。

• 笔记的第二部分解决了当我们将物体视为实体时出现的受限运动问题，并且需要禁止相互渗透。我们通过计算接触体之间的适当接触力来强制执行这些非穿透约束。给定这些接触力的值，模拟过程与无约束情况完全相同：我们只需将所有力施加到物体上，让模拟展开，就好像物体的运动完全不受约束一样。如果我们正确计算了接触力，则物体的最终运动将不会相互渗透。这些接触力的计算是整个模拟过程中要求最高的部分。[1]

Part I. 无约束刚体动力学 Unconstrained Rigid Body Dynamics

1. 模拟基础 Simulation Basics

• 这部分课程笔记旨在全面实施刚体运动。在本节中，我们将展示模拟刚体运动的基本结构。在第 2 节中，我们将定义实现刚体模拟器所需的术语、概念和方程。在此之后，我们将给出一些代码来实际实现我们需要的方程。我们将使用的一些概念和方程的推导将留给附录 A。

• 此时您唯一需要熟悉的是求解常微分方程的基本概念（但不是数值细节）。如果你不熟悉这个主题，那么你很幸运：只需回到这些课程笔记的开头，阅读“微分方程基础”部分。您可能还想阅读下一节关于“粒子动力学”的内容，尽管我们将在这里重复其中的一些材料。

• 模拟刚体的运动与模拟粒子的运动几乎相同，所以让我们从粒子模拟开始。我们模拟粒子的方式如下。我们让函数 x(t) 表示粒子在时间 t 在世界空间（模拟期间所有粒子或物体占据的空间）中的位置。函数 v(t) = ˙x(t) =(d /dt) x(t) 给出了粒子在时间 t 的速度。粒子在时间 t 的状态是粒子的位置和速度。我们通过定义系统的状态向量 Y(t) 来概括这个概念：对于单个粒子，
  

• 当我们谈论实际实现时，我们必须将 Y(t) “展平”为一个数组。对于单个粒子，Y(t) 可以描述为一个包含六个数字的数组：通常，我们会让 数组的前三个元素代表 x(t)，后三个元素代表 v(t)。 稍后，当我们讨论包含矩阵和向量的状态向量 Y(t) 时，会执行相同类型的操作以将 Y(t) 展平为一个数组。 当然，我们还必须反转这个过程，将一组数字转换回状态向量 Y(t)。 不过，这一切都归结为非常简单的簿记，因此，从今以后，我们将假设我们知道如何将任何类型的状态向量 Y(t) 转换为（具有适当长度的）数组，反之亦然。 （对于涉及粒子的简单示例，请查看这些笔记的“粒子系统动力学”部分。）

• 对于具有 n 个粒子的系统，我们将 Y(t) 扩大为
  
  其中 xi(t) 和 vi(t) 是第 i 个粒子的位置和速度。 使用 n 个粒子并不比使用一个粒子更难，所以我们现在让 Y(t) 成为单个粒子的状态向量（稍后我们将使用它，单个刚体）。

• 要真正模拟粒子的运动，我们还需要知道一件事——在时间 t 作用在粒子上的力。 我们将 F(t) 定义为在时间 t 作用在我们的粒子上的力。 函数 F(t) 是作用在粒子上的所有力的总和：重力、风、弹簧力等。如果粒子的质量为 m，那么 Y 随时间的变化由下式给出
  

• 给定 Y(t) 的任何值，等式 (1-3) 描述了 Y(t) 如何在时间 t 瞬时变化。模拟从 Y(0) 的一些初始条件开始，（即 x(0) 和 v( 0)) 然后使用数值方程求解器跟踪 Y 随时间的变化或“流动”，只要我们感兴趣。如果我们只想知道一秒后粒子的位置，我们要求求解器计算 Y (1)，假设时间单位是秒。 如果我们要为粒子的运动设置动画，我们需要计算 Y(1/30)、Y(2/30) 等等。

• 求解器使用的数值方法相对于我们的实际实现而言相对不重要。 让我们看看我们如何使用类似 C++ 的语言实际与数值求解器进行交互。 假设我们可以访问一个数值求解器，我们通常将其写为一个名为 ode 的函数。 通常，ode 具有以下规范：

typedef void（*dydt_func）（double t, double y[], double ydot[]）；

void ode(double y0[], double yend[], int len, double t0, double t1, dydt_func dydt);

• 我们将初始状态向量作为数组 y0 传递给 ode。求解器 ode 对 y0 的固有结构一无所知。由于求解器可以处理任意维度的问题，我们还必须传递 y0 的长度 len。 （对于一个有 n 个粒子的系统，我们显然有 len = 6n。）我们还将模拟的开始和结束时间 t0 和 t1 传递给求解器。求解器的目标是计算 t1 时刻的状态向量并将其返回到数组yend 中。

• 我们还将函数 dydt 传递给 ode。给定一个编码状态向量 Y(t) 和时间 t 的数组 y，dydt 必须计算(d/dt)Y(t), 存入ydot中返回。（我们必须将 t 传递给 dydt 的原因是我们的系统中可能有随时间变化的力。在这种情况下，dydt 必须知道“现在是什么时间”才能确定这些力的值。）在追踪Y(t) 从 t0 到 t1 的流动，求解器 ode 可以随意调用 dydt。鉴于我们有这样一个例程 ode，我们唯一需要做的工作就是编写例程 dydt，我们将把它作为参数提供给 ode。

• 模拟刚体遵循与模拟粒子完全相同的模式。唯一的区别是刚体的状态向量 Y(t) 包含更多信息，而导出的 dtY(t) 稍微复杂一些。但是，我们将使用完全相同的范例来使用求解器 ode 跟踪刚体的运动，我们将提供函数 dydt。

2. 刚体概念 Rigid Body Concepts

• 本节的目标是为刚体开发方程（1-3）的类似物。 我们开发的最终微分方程在 2.11 节中给出。 不过，为了做到这一点，我们需要先定义很多概念和关系。 附录 A 中有一些较长的推导。在下一节中，我们将展示如何编写数值求解器 ode 所需的函数 dydt 来计算本节开发的导数 (d /dt)Y(t)。

2.1 位置和方向 Position and Orientation

• 一个粒子在时间 t 在空间中的位置可以描述为一个向量 x(t)，它描述了粒子从原点的平移。刚体更复杂，除了平移它们，我们还可以旋转它们。为了在世界空间中定位刚体，我们将使用向量 x(t)，它描述了刚体的平移。我们还必须描述身体的旋转，我们将（现在）用 3 × 3 旋转矩阵 R(t) 来描述。我们将 x(t) 和 R(t) 称为刚体的空间变量。
  

• 与粒子不同，刚体占据一定体积的空间并具有特定的形状。因为刚体只能进行旋转和平移，所以我们将刚体的形状定义为一个固定不变的空间，称为体空间。给定身体空间中身体的几何描述，我们使用 x(t) 和 R(t) 将身体空间描述转换为世界空间（图 1）。为了简化我们将使用的一些方程，我们将要求我们对身体空间中刚体的描述使得身体的质心位于原点 (0, 0, 0)。我们稍后会更精确地定义质心，但现在，质心可以被认为是刚体中位于刚体几何中心的一点。在描述身体的形状时，我们要求这个几何中心位于身体空间的 (0, 0, 0) 处。如果我们同意 R(t) 指定了身体围绕质心的旋转，那么身体空间中的固定向量 r 将在时间 t 旋转到世界空间向量 R(t)r。同样，如果 p0 是刚体上的任意点，在身体空间中，那么 p0 的世界空间位置 p(t) 是首先围绕原点旋转 p0 然后平移它的结果：
  

• 由于物体的质心位于原点，因此质心的世界空间位置始终由 x(t) 直接给出。 这让我们可以通过说 x(t) 是时间 t 时世界空间中质心的位置来赋予 x(t) 一个非常物理的含义。 我们还可以为 R(t) 赋予物理意义。 考虑身体空间中的 x 轴，即向量 (1, 0, 0)。 在时间 t，这个向量有方向
  

• 在世界空间。 如果我们将 R(t) 的分量写为
  
  这是 R(t) 的第一列。 R(t) 的物理意义是，当在时间 t 变换到世界空间时，R(t) 的第一列给出了刚体的 x 轴指向的方向。 同样，R(t) 的第二列和第三列，
  
  是时间 t 时刚体在世界空间中的 y 轴和 z 轴的方向（图 2）。
  

2.2 线速度 Linear Velocity

• 为简单起见，我们将 x(t) 和 R(t) 称为物体在时间 t 的位置和方向。 接下来我们需要做的是定义位置和方向如何随时间变化。 这意味着我们需要 x˙(t) 和 R˙(t) 的表达式。 由于 x(t) 是质心在世界空间中的位置，x˙(t) 是质心在世界空间中的速度。 我们将线速度 v(t) 定义为：
  
  如果我们想象身体的方向是固定的，那么物体所能承受的唯一运动就是纯粹的平移。 量 v(t) 给出了这种平移的速度。

2.3 角速度 Angular Velocity

• 除了平移，刚体也可以旋转。 然而，想象一下我们冻结了空间中质心的位置。 因此，身体点的任何运动都必须是由于身体围绕通过质心的某个轴旋转。 （否则质心本身会移动）。 我们可以将该自旋描述为向量 ω(t)。 ω(t) 的方向给出了身体旋转的轴的方向（图 3）。 ω(t) 的大小 |ω(t)| 表示身体旋转的速度。 |ω(t)| 具有转数/时间的维度； 因此，如果角速度保持恒定，|ω(t)| 与物体在给定时间段内旋转的角度有关。 量 ω(t) 称为角速度。
  

• 对于线速度，x(t) 和 v(t) 通过 v(t) = (d/dt) x(t) 相关联。 R(t) 和 ω(t) 有什么关系？（显然，R˙(t) 不可能是 ω(t)，因为 R(t) 是一个矩阵，而 ω(t) 是一个向量。）。 为了回答这个问题，让我们提醒自己 R(t) 的物理意义。 我们知道 R(t) 的列告诉我们在时间 t 变换后的 x、y 和 z 体轴的方向。 这意味着 R˙(t) 的列必须描述 x、y 和 z 轴被变换的速度。 为了发现 ω(t) 和 R(t) 之间的关系，让我们研究一下刚体中任意向量的变化与角速度 ω(t) 的关系。

• 图 4 显示了具有角速度 ω(t) 的刚体。 考虑在世界空间中指定的时间 t 的向量 r(t)。 假设我们认为这个向量固定在身体上； 也就是说，r(t) 与刚体一起在世界空间中移动。 由于 r(t) 是一个方向，它与任何平移效应无关； 特别是，r˙(t) 与 v(t) 无关。 为了研究 r˙(t)，我们将 r(t) 分解为向量 a 和 b，其中 a 平行于 ω(t)，b 垂直于 ω(t)。 假设刚体要保持一个恒定的角速度，所以 r(t) 的尖端画出一个以 ω(t) 轴为中心的圆
  （图 4）。 这个圆的半径是|b|。 由于向量 r(t) 的尖端沿该圆瞬时移动，因此 r(t) 的瞬时变化垂直于 b 和 ω(t)。 由于 r(t) 的尖端在半径为 b 的圆中移动，因此 r(t) 的瞬时速度的大小为 |b||ω(t)|。 由于 b 和 ω(t) 是垂直的，因此它们的叉积具有大小
  
  综上所述，我们可以写出 r˙(t) = ω(t) × (b)。 然而，由于 r(t) = a + b 并且 a 平行于 ω(t)，所以我们有 ω(t) × a = 0，因此
  
  因此，我们可以简单地将向量的变化率表示为

现在让我们把所有这些放在一起。 在时间 t，我们知道刚体的 x 轴方向在世界空间是 R(t) 的第一列，即

在t时刻，R(t) 第一列的导数就是这个向量的变化率：使用我们刚刚发现的叉积规则，这个变化是

R(t) 的其他两列显然也是如此。 这意味着我们可以写出

不过，这种表达方式太麻烦了，不能随身携带。 为了简化事情，我们将使用以下技巧。 如果 a 和 b 是 3 向量，则 a × b 是向量

给定向量 a，让我们定义 a* 为矩阵

那么[2]
([2]这看起来有点太“神奇”了。 是不是有人意外发现了这个身份？ 这是一种碰巧有效的关系吗？ 这个结构可以通过考虑所谓的无穷小旋转来推导出来。 感兴趣的读者可能希望阅读 Goldstein[10] 的第 4.8 章，以获得更完整的 a∗ 矩阵推导)

使用“*”符号，我们可以更简单地将 R˙(t) 重写为

根据矩阵乘法的规则，我们可以将其分解为

这是一个矩阵-矩阵乘法。 但由于右边的矩阵是 R(t) 本身，我们得到简单的

最后，这给出了我们想要的 R˙(t) 和 ω(t) 之间的关系。 请注意向量的 r˙(t) = ω(t) × r(t) 和旋转矩阵的 R˙(t) = ω(t)∗R(t) 之间的对应关系。

2.4 物体质量 Mass of a Body

• 为了计算出一些推导，我们需要（从概念上）对刚体的体积进行一些积分。 为了使这些推导更简单，我们将暂时想象一个刚体是由大量的小粒子组成的。 粒子的索引从 1 到 N。第 i 个粒子的质量为 mi，每个粒子在体空间中都有一个（恒定）位置 r0i。 因此，第 i 个粒子在时间 t 时在世界空间中的位置，记为 ri(t)，由以下公式给出

物体的总质量 M 是总和

（此后，总和被假定为从 1 到 N 与索引变量 i 的总和。）

2.5 粒子的速度 Velocity of a Particle

• 第 i 个粒子的速度 r˙i(t) 由方程 (2-13) 微分得到：利用关系式 R˙(t) = ω∗R(t)，我们得到

我们可以将其重写为

使用方程 (2-13) 中 ri(t) 的定义。 回想一下“∗”算子的定义，对于任何向量 a，ω(t)∗a = ω(t) × a。 使用它，我们可以简单地写

请注意，这将刚体上一个点的速度分成两个分量（图 5）：一个线性分量 v(t) 和一个角分量 ω × (ri(t) - x(t))。

2.6 质心 Center of Mass

• 我们对质心的定义将使我们能够同样将物体的动力学分为线性和角度分量。 世界空间中物体的质心定义为

其中 M 是物体的质量（即单个质量 mi 的总和）。 当我们说我们使用质心坐标系时，我们的意思是在身体空间中，

请注意，这也意味着 Σmir0i = 0。

• 我们将 x(t) 称为时间 t 的质心位置。 这是真的？ 是的：由于第 i 个粒子在时间 t 的位置 ri(t) = R(t)r0i + x(t)，所以在时间 t 的质心是

此外，关系

也非常有用。

2.7 力和扭矩 Force and Torque

• 当我们想象由于某种外部影响（例如重力、风、接触力）而作用在刚体上的力时，我们会想象该力作用在刚体的特定粒子上。 （请记住，我们的粒子模型只是概念性的。我们可以在身体上或身体内部的任何几何位置上作用力，因为我们总是可以想象在那个确切位置恰好有一个粒子。）。 力作用于粒子的位置定义了力作用的位置。 我们将让 Fi(t) 表示在时间 t 作用在第 i 个粒子上的外力的总力。 此外，我们将作用在第 i 个粒子上的外部扭矩 τi(t) 定义为

• 扭矩与力的不同之处在于，粒子上的扭矩取决于粒子相对于质心 x(t) 的位置 ri(t)。 我们可以直观地将 τi(t) 的方向视为身体由于 Fi(t) 而旋转的轴，如果质心被牢牢固定在适当的位置（图 6）。

• 作用在物体上的总外力 F(t) 是 Fi(t) 的总和：

而总外部扭矩类似地定义为

请注意，F(t) 没有传达各种力作用在身体上的位置的信息； 然而，τ(t) 确实告诉我们一些关于力 Fi(t) 在身体上的分布。

2.8 线性动量 Linear Momentum

• 质量为 m 且速度为 v 的粒子的线性动量 p 定义为

刚体的总线性动量 P(t) 是每个粒子的质量和速度的乘积之和：

由方程（2-17）可知，第 i 个粒子的速度 r˙i(t) 为 r˙i(t) = v(t) + ω(t) × (ri(t) − x(t))。 因此，物体的总线性动量为

因为我们使用的是质心坐标系，我们可以应用方程（2-20）并获得

这给了我们一个很好的结果，即刚体的总线性动量与物体只是一个质量为 M 和速度为 v(t) 的粒子相同。 因此，我们在 P(t) 和 v(t) 之间有一个简单的转换：P(t) = Mv(t) 和 v(t) = P(t)/M。 由于 M 是一个常数，

线性动量的概念让我们可以非常简单地表达总力 F(t) 对刚体的影响。 附录 A 导出关系

• 这表示线性动量的变化等于作用在物体上的总力。 请注意，P(t) 没有告诉我们物体的旋转速度，这很好，因为 F(t) 也没有传达物体旋转速度的变化！

• 由于 P(t) 和 v(t) 之间的关系很简单，我们将使用 P(t) 作为刚体的状态变量，而不是 v(t)。 我们当然可以让 v(t) 成为状态变量，并使用关系

然而，使用 P(t) 而不是 v(t) 作为状态变量将更符合我们处理角速度和加速度的方式。

2.9 角动量 Angular Momentum

• 虽然线性动量的概念非常直观（P(t) = Mv(t)），但角动量的概念（对于刚体）却不是。人们甚至对刚体的角动量感到困扰的唯一原因是，它可以让你写出比你坚持角速度更简单的方程。考虑到这一点，最好不要担心将直观的物理解释附加到角动量上——总而言之，这是一个最不直观的概念。角动量最终简化了方程，因为它本质上是守恒的，而角速度则不是：如果你有一个漂浮在空间中的物体，没有扭矩作用在它上面，那么物体的角动量是恒定的。但是，对于物体的角速度而言，情况并非如此：即使物体的角动量是恒定的，物体的角速度也可能不是！因此，即使没有力作用在物体上，物体的角速度也会发生变化。正因为如此，最终选择角动量作为状态变量而不是角速度变得更简单。

• 对于线性动量，我们有关系 P(t) = Mv(t)。类似地，我们通过方程 L(t) = I(t)ω(t) 定义刚体的总角动量 L(t)，其中 I(t) 是一个 3 × 3 矩阵（技术上是二阶矩阵）张量）称为惯性张量，我们将对其进行描述。惯性张量 I(t) 描述了物体中的质量如何相对于物体的质心分布。张量 I(t) 取决于物体的方向，但不取决于物体的平移。请注意，对于角动量和线动量情况，动量是速度的线性函数——只是在角度情况下，比例因子是一个矩阵，而在线性情况下它只是一个标量。还要注意 L(t) 与任何平移效应无关，而 P(t) 与任何旋转效应无关。

• L(t) 与总扭矩 τ(t) 之间的关系非常简单：附录 A 推导

类似于关系 P˙(t) = F(t)。

2.10 惯性张量 The Inertia Tensor

• 惯性张量 I(t) 是角动量 L(t) 和角速度 ω(t) 之间的比例因子。 在给定的时间 t，通过定义 r0i = ri(t) - x(t)，让 r0i 是第 i 个粒子从 x(t) 的位移。 张量 I(t) 用 r0i 表示为对称矩阵

• 对于实际实现，我们用世界空间中物体体积上的积分代替有限和。 质量项 mi 由密度函数代替。 乍一看，似乎我们需要评估这些积分以在方向 R(t) 发生变化时找到 I(t)。 在模拟过程中这样做会非常昂贵，除非身体的形状非常简单（例如，球体或立方体）以至于可以象征性地评估积分。
• 幸运的是，通过使用身体空间坐标，我们可以根据身体空间坐标中的预先计算积分来廉价地计算任何方向 R(t) 的惯性张量。 （该积分通常在模拟开始之前计算，应被视为描述身体物理特性的输入参数之一。）使用以下事实： ，我们可以将 I(t) 重写为差

取 ri' 与自身的外积相乘，即

令 1令 1 表示 3 × 3 单位矩阵，我们可以简单地将 I(t) 表示为 表示 3 × 3 单位矩阵，我们可以简单地将 I(t) 表示为

这有什么帮助？
因为 ri(t) = R(t)r0i + x(t) 其中 r0i 是一个常数，所以 r0i = R(t)r0i。那么，由于 R(t)R(t)T = 1，

由于 r0Ti r0i 是一个标量，我们可以通过编写重新排列事物

如果我们将 Ibody 定义为矩阵

然后从前面的方程我们有

• 由于 Ibody 是在体空间中指定的，因此它在模拟过程中是恒定的。 因此，通过在模拟开始之前预先计算一个物体的 Ibody，我们可以很容易地从 Ibody 和方向矩阵 R(t) 计算 I(t)。 第 5.1 节根据身体空间中身体体积的积分推导了矩形物体的身体空间惯性张量。

此外，I(t) 的倒数由公式给出

因为，对于旋转矩阵，R(t)T = R(t)−1 和 (R(t)T)T = R(t)。 显然，(Ibody)-1 在模拟过程中也是一个常数

2.11 刚体运动方程 Rigid Body Equations of Motion

• 最后，我们涵盖了定义状态向量 Y(t) 所需的所有概念！ 对于刚体，我们将 Y(t) 定义为

• 因此，刚体的状态是它的位置和方向（描述空间信息），以及它的线性和角动量（描述速度信息）。 身体的质量 M 和身体空间惯性张量 Ibody 是常数，我们假设我们知道模拟何时开始。 在任何给定时间，计算辅助量 I(t)、ω(t) 和 v(t)

导数 (d/dt)Y(t) 是

下一节给出了计算 (d/dt)Y(t) 的函数 dydt 的实现

• 最后一点：与其将身体的方向表示为 Y(t) 中的矩阵 R(t)，不如使用四元数。 第 4 节讨论了使用四元数代替旋转矩阵。 简而言之，四元数是一种四元素向量，可用于表示旋转。 如果我们将 Y(t) 中的 R(t) 替换为四元数 q(t)，我们可以将 R(t) 视为直接从 q(t) 计算的辅助变量，正如 ω(t) 是从 L(t) 计算出来的。第 4 节推导出一个类似于 R˙(t) = ω(t)∗R(t) 的公式，它用 q(t) 和 ω(t) 表示 q˙(t)。

3. 计算 (d/dt) Y(t)

• 让我们考虑一下刚体的函数 dydt 的实现。 代码是用 C++ 编写的，我们假设我们有称为矩阵和三元组的数据类型（类），它们分别实现了 3 × 3 矩阵和 3 空间中的点。 使用这些数据类型，我们将通过结构表示刚体

struct RigidBody{
  /* Constant quantities*/
  double mass;            /* mass M*/
  matirx Ibody;            /* Ibdoy*/
  matirx Ibodyinv;        /* Ibody-1 (inverse of Ibody)*/

  /* State variables*/
  triple x;            /* x(t)*/
  matix Ibody;        /* R(t) */
  triple P;          /* P(t) */
  triple L;         /* L(t) */
};