刚体动力学是物理仿真的基石，为机器人控制、动画生成和虚拟环境构建提供了数学框架。本章深入探讨刚体运动的数学描述，从经典的牛顿-欧拉方程出发，逐步引入现代几何力学的观点。我们将特别关注旋转表示的数值稳定性问题、惯性张量的物理意义，以及李群框架下的优雅表述。这些理论不仅是理解后续章节的基础，更是设计高效、稳定仿真系统的关键。
刚体动力学的核心在于理解六个自由度的运动：三个平动自由度和三个转动自由度。牛顿处理了平动部分，而欧拉的天才在于他认识到转动需要完全不同的数学框架。这两组方程的耦合构成了刚体动力学的基础，也是所有现代物理引擎的出发点。
刚体的质心运动遵循牛顿第二定律，其在惯性坐标系中的表述为：
\[m\ddot{\mathbf{r}}_c = \mathbf{F}_{ext}\]其中，$m$ 是刚体质量，$\mathbf{r}c$ 是质心位置向量，$\mathbf{F}{ext}$ 是作用在刚体上的外力合力。这个方程描述了刚体的平动，与质点动力学完全一致。
这个看似简单的方程隐含了一个深刻的物理原理：无论刚体内部的质量如何分布，其质心的运动总是可以当作一个质点来处理。这种分离使得我们可以独立地处理平动和转动，大大简化了问题的复杂度。在实际仿真中，这意味着我们可以先更新质心位置，再处理姿态变化，这种解耦是许多高效算法的基础。
值得注意的是，$\mathbf{F}_{ext}$ 包含了所有外力的矢量和，包括重力、接触力、摩擦力等。在多体系统中，这些力可能来自约束、关节或碰撞。正确计算这些力的合力及其作用点，是确保仿真物理真实性的关键。特别是在处理分布力（如流体阻力）时，需要通过积分将其归算到质心上。
刚体绕质心的旋转运动由欧拉方程描述。在体坐标系（body frame）中，角动量的变化率为：
\[\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega}) = \boldsymbol{\tau}_{ext}\]这里需要特别注意坐标系的选择。体坐标系随刚体一起运动，使得惯性张量 $\mathbf{I}$ 保持常数矩阵。角速度 $\boldsymbol{\omega}$ 和外力矩 $\boldsymbol{\tau}_{ext}$ 都在体坐标系中表示。
交叉积项 $\boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega})$ 源于科里奥利效应，是非线性动力学的关键来源。在数值仿真中，这一项的正确处理直接影响系统的能量守恒性和长期稳定性。这个非线性项的物理意义深刻：它代表了旋转坐标系中的惯性力效应。当刚体同时绕多个轴旋转时，不同方向的角动量会相互耦合，产生进动和章动等复杂现象。
理解这个方程的一个直观方式是考虑旋转的网球拍。当你试图让它绕中间轴稳定旋转时，会发现它总是会翻转——这正是欧拉方程中非线性项导致的不稳定性。这种不稳定性不是数值误差，而是物理系统的固有特性，被称为中间轴定理或网球拍定理。
在实际应用中，欧拉方程的求解面临几个挑战。首先，非线性项使得解析解只在特殊情况下存在（如欧拉陀螺、拉格朗日陀螺）。其次，数值积分时需要在世界坐标系和体坐标系之间频繁转换，每次转换都可能引入数值误差。第三，对于细长或扁平的物体，惯性张量的条件数很大，导致数值刚性问题。
将平动和转动结合，刚体的完整状态由13个变量描述：
这种状态表示看似冗余（13个变量描述6个自由度），但在数值实现中却是最实用的。位置和线速度的分离允许使用高阶积分方法，而四元数避免了欧拉角的奇异性问题。角速度虽然不是姿态的直接时间导数，但它具有明确的物理意义，便于施加约束和计算动能。
状态演化方程为： \(\begin{aligned} \dot{\mathbf{r}} &= \mathbf{v} \\ \dot{\mathbf{q}} &= \frac{1}{2}\mathbf{q} \otimes \boldsymbol{\omega} \\ \dot{\mathbf{v}} &= \frac{1}{m}\mathbf{F}_{ext} \\ \dot{\boldsymbol{\omega}} &= \mathbf{I}^{-1}(\boldsymbol{\tau}_{ext} - \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega})) \end{aligned}\)
这个方程组构成了一个一阶常微分方程系统，可以用标准的ODE求解器处理。然而，它的特殊结构需要特别注意。前两个方程是运动学方程，描述了位置和姿态如何随速度变化。后两个是动力学方程，描述了速度如何受力和力矩影响。这种运动学-动力学分离在某些积分方案中可以被利用来提高效率。
特别值得注意的是四元数更新方程。这里的 $\boldsymbol{\omega}$ 需要先扩展为纯四元数 $(0, \omega_x, \omega_y, \omega_z)$，然后进行四元数乘法。这个操作在每个时间步都要执行，因此其效率直接影响整体性能。许多物理引擎会预计算相关矩阵来加速这个过程。
在实际系统中，刚体很少是完全自由的。关节、接触和其他约束限制了运动空间。这些约束可以通过两种方式处理：拉格朗日乘子法（保持笛卡尔坐标）或约简坐标法（使用广义坐标）。
拉格朗日乘子法在笛卡尔空间工作，约束通过额外的约束力来维持： \(\begin{aligned} m\ddot{\mathbf{r}} &= \mathbf{F}_{ext} + \mathbf{J}^T\lambda \\ \mathbf{J}\ddot{\mathbf{r}} &= \mathbf{c} \end{aligned}\)
其中 $\mathbf{J}$ 是约束雅可比矩阵，$\lambda$ 是拉格朗日乘子（物理上对应约束力），$\mathbf{c}$ 是约束加速度。这种方法的优点是概念清晰，容易处理变化的约束（如碰撞），缺点是需要求解一个混合的微分-代数方程组（DAE）。
约简坐标法使用最小数量的广义坐标 $\mathbf{q}$ 来参数化允许的运动。例如，一个铰链关节只需要一个角度参数。运动方程变为： \(\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}\)
这里 $\mathbf{M}$ 是广义质量矩阵，$\mathbf{C}$ 包含科里奥利和离心力项，$\mathbf{g}$ 是重力项，$\boldsymbol{\tau}$ 是广义力。这种形式自动满足约束，数值上更稳定，但推导复杂，难以处理接触等单边约束。
旋转的数学表示是刚体仿真中最微妙的部分之一。三维旋转群 $SO(3)$ 的拓扑结构（它同胚于三维实射影空间 $\mathbb{RP}^3$）使得不存在全局无奇异的三参数表示。这个基本的拓扑障碍迫使我们在不同的表示方法之间做出权衡。四元数以增加一个参数为代价，换取了全局无奇异性和计算效率，成为现代物理引擎的标准选择。
| 四元数 $\mathbf{q} = (q_w, q_x, q_y, q_z)$ 提供了无奇异性的旋转表示。单位四元数满足 $ | \mathbf{q} | = 1$，形成三维旋转群 $SO(3)$ 的双覆盖。这个双覆盖意味着每个旋转对应两个四元数：$\mathbf{q}$ 和 $-\mathbf{q}$，这在某些应用中需要特别注意，例如在插值时可能需要选择”短路径”。 |
从几何角度理解，单位四元数可以解释为绕轴 $\mathbf{n}$ 旋转角度 $\theta$ 的表示： \(\mathbf{q} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2}(n_x\mathbf{i} + n_y\mathbf{j} + n_z\mathbf{k})\)
这个表示揭示了四元数的一个关键优势：旋转的合成对应于四元数乘法，而旋转角度的线性插值可以通过四元数的球面线性插值（SLERP）实现。这使得四元数特别适合动画和轨迹规划。
四元数乘法定义为： \(\mathbf{q}_1 \otimes \mathbf{q}_2 = \begin{pmatrix} q_{1w}q_{2w} - \mathbf{q}_{1v} \cdot \mathbf{q}_{2v} \\ q_{1w}\mathbf{q}_{2v} + q_{2w}\mathbf{q}_{1v} + \mathbf{q}_{1v} \times \mathbf{q}_{2v} \end{pmatrix}\)
其中 $\mathbf{q}_v = (q_x, q_y, q_z)^T$ 是四元数的向量部分。
这个乘法规则不是任意的，而是源于四元数作为实数域上的四维赋范可除代数的唯一性（Frobenius定理）。从计算角度看，四元数乘法需要16次乘法和12次加法，相比旋转矩阵的27次乘法和18次加法，效率提升明显。更重要的是，四元数乘法的数值稳定性远优于连续的旋转矩阵乘法，后者会逐渐失去正交性。
四元数的时间导数与角速度的关系是仿真中的核心公式：
\[\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \boldsymbol{\omega}_{local}\]这个公式的推导基于旋转的微小变化。考虑时间 $dt$ 内的微小旋转 $d\theta = |\boldsymbol{\omega}|dt$，对应的四元数增量为： \(d\mathbf{q} \approx (1, \frac{1}{2}\boldsymbol{\omega}dt)\)
将这个增量应用到当前四元数上，取极限就得到了上述微分方程。这个推导隐含了一个重要假设：角速度在 $dt$ 时间内保持恒定。对于高速旋转或大时间步长，这个假设可能不成立，需要使用更精确的积分方法。
或等价地，使用矩阵形式： \(\dot{\mathbf{q}} = \frac{1}{2}\mathbf{\Omega}(\boldsymbol{\omega})\mathbf{q}\)
其中： \(\mathbf{\Omega}(\boldsymbol{\omega}) = \begin{pmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{pmatrix}\)
这个矩阵形式在批量处理多个刚体时特别有用，可以利用SIMD指令或GPU并行计算。注意 $\mathbf{\Omega}$ 是反对称矩阵，这保证了四元数范数的守恒（至少在连续情况下）。
在长时间仿真中，数值误差会导致四元数偏离单位球面。必须定期进行归一化：
\[\mathbf{q}_{normalized} = \frac{\mathbf{q}}{|\mathbf{q}|}\]| 归一化的频率是一个需要权衡的问题。过于频繁的归一化会掩盖数值积分的问题，使得难以调试；过于稀疏的归一化可能导致旋转矩阵退化。实践中，通常监控归一化误差 $\epsilon = | \mathbf{q} | ^2 - 1 | $，当 $\epsilon > 10^{-6}$ 时进行归一化。 |
更精细的方法是使用约束稳定化技术，在积分过程中施加软约束：
\[\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes \boldsymbol{\omega} + \lambda(1 - |\mathbf{q}|^2)\mathbf{q}\]其中 $\lambda > 0$ 是稳定化参数，典型值为 $0.1 \sim 1.0$。这种方法的优点是平滑地将四元数拉回单位球面，避免了硬归一化可能引入的不连续性。从控制理论角度看，这相当于添加了一个比例反馈项，使得单位球面成为吸引流形。
虽然四元数是内部表示的最佳选择，但在与外部系统交互时，经常需要转换到其他表示形式。
四元数到旋转矩阵： \(\mathbf{R} = \begin{pmatrix} 1-2(q_y^2+q_z^2) & 2(q_xq_y-q_wq_z) & 2(q_xq_z+q_wq_y) \\ 2(q_xq_y+q_wq_z) & 1-2(q_x^2+q_z^2) & 2(q_yq_z-q_wq_x) \\ 2(q_xq_z-q_wq_y) & 2(q_yq_z+q_wq_x) & 1-2(q_x^2+q_y^2) \end{pmatrix}\)
| 这个转换是最常用的，因为许多图形API和线性代数库都使用矩阵。注意这个公式假设四元数已归一化，否则需要除以 $ | \mathbf{q} | ^2$。 |
旋转矩阵到四元数： 转换的关键是利用矩阵的迹和对角元素。首先计算： \(q_w = \frac{1}{2}\sqrt{1 + R_{11} + R_{22} + R_{33}}\)
然后根据 $q_w$ 的大小选择数值稳定的公式计算其他分量。当 $q_w$ 接近零时（对应180度旋转），需要特殊处理以避免数值不稳定。
四元数与轴角表示： 轴角表示 $(\mathbf{n}, \theta)$ 与四元数的关系直接： \(\mathbf{q} = (\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\mathbf{n})\)
反向转换时需要注意 $\theta = 0$ 和 $\theta = \pi$ 的特殊情况。当 $\theta$ 很小时，$\mathbf{n}$ 的计算会不稳定，此时可以使用泰勒展开： \(\mathbf{n} \approx \frac{2\mathbf{q}_v}{\theta} \approx \mathbf{q}_v(1 + \frac{\theta^2}{12} + ...)\)
惯性张量 $\mathbf{I}$ 描述了刚体质量分布对旋转的影响。在连续体情况下：
\[I_{ij} = \int_V \rho(\mathbf{r})(r^2\delta_{ij} - r_ir_j)dV\]对于离散质点系统： \(I_{ij} = \sum_k m_k(r_k^2\delta_{ij} - r_{ki}r_{kj})\)
惯性张量是对称正定矩阵，其特征值表示主转动惯量，特征向量定义主轴方向。
当需要计算相对于非质心点的惯性张量时，使用平行轴定理：
\[\mathbf{I}_{new} = \mathbf{I}_{cm} + m[\mathbf{d}^T\mathbf{d}\mathbf{E}_3 - \mathbf{d}\mathbf{d}^T]\]其中 $\mathbf{d}$ 是从质心到新参考点的位移向量，$\mathbf{E}_3$ 是3×3单位矩阵。
多个刚体组合时，总惯性张量通过叠加原理计算：
\[\mathbf{I}_{total} = \sum_i (\mathbf{R}_i\mathbf{I}_i\mathbf{R}_i^T + m_i[\mathbf{d}_i^T\mathbf{d}_i\mathbf{E}_3 - \mathbf{d}_i\mathbf{d}_i^T])\]其中 $\mathbf{R}_i$ 是第i个刚体的旋转矩阵，$\mathbf{d}_i$ 是其质心相对于组合体质心的位置。
刚体的位姿可以用特殊欧几里得群 $SE(3)$ 的元素表示：
\[\mathbf{T} = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{pmatrix} \in SE(3)\]其中 $\mathbf{R} \in SO(3)$ 是旋转矩阵，$\mathbf{t} \in \mathbb{R}^3$ 是平移向量。
$SE(3)$ 的李代数 $\mathfrak{se}(3)$ 由速度扭转（twist）组成：
\[\boldsymbol{\xi}^{\wedge} = \begin{pmatrix} [\boldsymbol{\omega}]_{\times} & \mathbf{v} \\ \mathbf{0}^T & 0 \end{pmatrix} \in \mathfrak{se}(3)\]其中 $[\boldsymbol{\omega}]_{\times}$ 是角速度的反对称矩阵表示：
\[[\boldsymbol{\omega}]_{\times} = \begin{pmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{pmatrix}\]从李代数到李群的指数映射提供了从速度到有限运动的转换：
\[\mathbf{T}(t) = \exp(t\boldsymbol{\xi}^{\wedge})\mathbf{T}_0\]对于纯旋转（$\mathbf{v} = \mathbf{0}$），Rodrigues公式给出闭式解：
\[\exp([\boldsymbol{\omega}]_{\times}) = \mathbf{E}_3 + \frac{\sin\theta}{\theta}[\boldsymbol{\omega}]_{\times} + \frac{1-\cos\theta}{\theta^2}[\boldsymbol{\omega}]_{\times}^2\]| 其中 $\theta = | \boldsymbol{\omega} | $。 |
伴随表示描述了扭转和力旋量在不同坐标系间的变换：
\[\text{Ad}_{\mathbf{T}} = \begin{pmatrix} \mathbf{R} & \mathbf{0} \\ [\mathbf{t}]_{\times}\mathbf{R} & \mathbf{R} \end{pmatrix}\]这在多体系统动力学中用于传递速度和力： \(\boldsymbol{\xi}_b = \text{Ad}_{\mathbf{T}_{ab}}\boldsymbol{\xi}_a\)
在强化学习训练中，刚体动力学的仿真精度直接影响策略的质量。关键考虑因素包括：
积分步长选择：过大的步长导致能量漂移和不稳定，过小的步长增加计算成本。典型的折中是1-5ms。
接触建模精度：简化的接触模型可能导致策略过度依赖仿真artifacts，在真实世界中失效。
惯性参数准确性：质量和惯性张量的误差会导致仿真与现实的动力学响应差异，影响sim-to-real转移。
大规模并行训练需要高效的刚体仿真：
向量化计算：利用SIMD指令同时处理多个刚体的状态更新。
稀疏性利用：多体系统中，大部分刚体对之间没有相互作用，可以利用稀疏矩阵技术。
自适应时间步长：在运动平缓时使用较大步长，在碰撞等剧烈变化时细化。
将刚体动力学公式化为可微分形式，允许通过梯度下降直接优化控制策略：
\[\frac{\partial \mathcal{L}}{\partial \mathbf{u}} = \frac{\partial \mathcal{L}}{\partial \mathbf{x}_T} \prod_{t=1}^{T} \frac{\partial \mathbf{f}}{\partial \mathbf{u}_t}\]其中 $\mathcal{L}$ 是损失函数，$\mathbf{f}$ 是动力学转移函数。
莱昂哈德·欧拉（Leonhard Euler, 1707-1783）在1750年的开创性工作《发现刚体运动的新原理》中，首次建立了刚体旋转的完整数学理论。他的主要贡献包括：
欧拉角的引入：通过三个连续旋转描述任意姿态，虽然存在万向锁问题，但在特定应用中仍然重要。
主轴定理：证明了任何刚体都存在三个相互垂直的主轴，沿这些轴的转动惯量达到极值。
欧拉方程的推导：从角动量守恒出发，推导出描述刚体自由旋转的非线性微分方程组。
欧拉的工作奠定了分析力学的基础，其方法论影响了后续两个世纪的物理学发展。特别是他强调的”用数学语言精确描述物理现象”的思想，至今仍是计算物理学的核心理念。
刚体动力学可以在相空间 $T^*Q$ 上表述，其中 $Q$ 是位形空间。对于自由刚体，$Q = SE(3)$，相空间具有自然的辛结构：
\[\omega = d\mathbf{p} \wedge d\mathbf{q}\]辛结构保证了哈密顿流的体积守恒（刘维尔定理）和能量守恒。
物理量的演化由泊松括号描述：
\[\frac{df}{dt} = \{f, H\}\]其中哈密顿量为： \(H = \frac{1}{2m}\mathbf{p}^T\mathbf{p} + \frac{1}{2}\boldsymbol{\omega}^T\mathbf{I}\boldsymbol{\omega} + V(\mathbf{q})\)
诺特定理告诉我们，系统的对称性对应守恒量。例如，旋转不变性导致角动量守恒。
利用对称性可以降低系统维度。动量映射 $\mathbf{J}: T^Q \rightarrow \mathfrak{g}^$ 将相空间点映射到李代数的对偶空间：
\[\mathbf{J}(\mathbf{q}, \mathbf{p}) = \text{Ad}^*_{\mathbf{q}^{-1}}\mathbf{p}\]在动量映射的水平集上进行约化，得到约化相空间上的动力学。
保持几何结构的数值积分器具有优越的长期行为：
这些方法在长时间仿真和高精度要求的场合具有明显优势。
本章建立了刚体动力学的完整数学框架，核心要点包括：
牛顿-欧拉方程提供了刚体运动的基本描述，分离了平动和转动自由度。
四元数表示解决了欧拉角的万向锁问题，提供了数值稳定的姿态参数化。
惯性张量刻画了质量分布对旋转的影响，主轴分析简化了动力学计算。
李群框架提供了优雅的几何描述，统一了运动学和动力学。
仿真考虑包括数值稳定性、计算效率和可微分性，直接影响强化学习的效果。
关键公式回顾：
习题 1.1 证明反对称矩阵的指数映射总是得到正交矩阵，即若 $\mathbf{A}^T = -\mathbf{A}$，则 $\exp(\mathbf{A}) \in SO(3)$。
提示：利用 $(\exp(\mathbf{A}))^T\exp(\mathbf{A})$ 并展开级数。
习题 1.2 一个均匀密度的长方体，尺寸为 $a \times b \times c$，质量为 $m$。计算其关于质心的惯性张量。
提示：利用对称性，非对角元素为零。
习题 1.3 推导四元数乘法的矩阵表示，即找到矩阵 $\mathbf{L}(\mathbf{q}_1)$ 使得 $\mathbf{q}_1 \otimes \mathbf{q}_2 = \mathbf{L}(\mathbf{q}_1)\mathbf{q}_2$。
提示：将四元数乘法公式展开并整理成矩阵形式。
习题 1.4 考虑一个自由旋转的刚体（无外力矩），其三个主转动惯量满足 $I_1 > I_2 > I_3$。分析围绕中间轴（$I_2$）旋转的稳定性。
提示：线性化欧拉方程并分析特征值。
习题 1.5 设计一个数值方案，在积分四元数时自动保持单位约束，要求二阶精度。
提示：考虑投影方法或拉格朗日乘子法。
习题 1.6 推导平面三连杆机械臂的李代数表示，并给出从关节速度到末端执行器速度的雅可比矩阵。
提示：使用指数坐标和伴随表示。
习题 1.7 证明刚体的动能可以写成 $T = \frac{1}{2}\boldsymbol{\xi}^T\mathbf{M}\boldsymbol{\xi}$，其中 $\boldsymbol{\xi}$ 是速度扭转，$\mathbf{M}$ 是广义质量矩阵。给出 $\mathbf{M}$ 的具体形式。
提示：分别考虑平动和转动动能。
最常见的错误是混淆世界坐标系和体坐标系。记住：
| 建议：每10-100步归一化一次，或当 $ | \mathbf{q} | ^2 - 1 | > \epsilon$ 时 |
下一章：第2章：数值积分方法