计算刚体动力学

约束、自由度和广义坐标

考虑由$n_b$个刚体构成的多体系统，选取$n$个广义坐标来描述每一个刚体的位姿。 这些广义坐标记为$\mathbf{q}=[q_1,q_2,\dots ,q_n{]}^{\mathrm{\top }}$。 在无约束的情况下，系统的自由度是$6n_b$。 然而，它们之间未必是独立的，假设它们受到$n_c<n$个形如

$$\mathbf{C}(q_1,q_2,\dots ,q_n,t)=\mathbf{C}(\mathbf{q},t)=\mathbf{0}$$

的独立方程的约束，于是系统的自由度还剩下$(n-n_c)$个。

Note

为什么$n$不一定等于$6n_b$？这取决于所选取的广义坐标，例如当使用四元数来描述刚体的方向时，每个刚体的位姿就有7个广义坐标，但还需要一个四元数模为1的约束方程。

形如$\mathbf{C}(\mathbf{q},t)=\mathbf{0}$的约束被称为是完整(holonomic)约束。 如果不显含时间$t$，则称为稳恒(scleronomic)约束，反之则为非稳恒(rheonomic)约束。 在一些系统中，约束不仅与广义坐标$\mathbf{q}$有关，还与广义速度$\dot{\mathbf{q}}$有关，且无法通过积分消除掉$\dot{\mathbf{q}}$，这类约束被称为非完整(nonholonomic)约束。一个例子就是与地面点接触、无滑动的轮子，它可以滚去任何地方，但是速度方向却被限制。

如没有特殊说明，下面的内容只针对完整约束。

虚位移与广义坐标分解

虚位移$\delta \mathbf{q}$表示系统在特定时刻下、约束所允许的广义坐标发生的无穷小变化量。 对于完整约束，有

$${\mathbf{C}}_{\mathbf{q}}\delta \mathbf{q}=\mathbf{0}$$

其中

$${\mathbf{C}}_{\mathbf{q}}=\frac{\partial \mathbf{C}(\mathbf{q},t)}{\partial \mathbf{q}}$$

是一个$n_c\times n$的矩阵，称为系统雅可比矩阵。

如果约束方程线性无关，则${\mathbf{C}}_{\mathbf{q}}$满秩，此时可以将广义坐标分解为

$$\mathbf{q}=\left[\begin{array}{c}{\mathbf{q}}_i^{\mathrm{\top }}\\ {\mathbf{q}}_d^{\mathrm{\top }}\end{array}\right]$$

其中${\mathbf{q}}_i$是$n-n_c$维的独立广义坐标分量，${\mathbf{q}}_d$是$n_c$维的非独立广义坐标分量。 同时将${\mathbf{C}}_{\mathbf{q}}$分解，得到

$${\mathbf{C}}_{{\mathbf{q}}_i}\delta {\mathbf{q}}_i+{\mathbf{C}}_{{\mathbf{q}}_d}\delta {\mathbf{q}}_d=\mathbf{0}$$

虚功原理与广义力

假设系统受到若干外力，作用在系统中某些点上的力记为

$${\mathbf{F}}_k$$

其作用点的位置为

$${\mathbf{r}}_k={\mathbf{r}}_k(\mathbf{q},t)$$

在虚位移 $\delta \mathbf{q}$ 下，该点产生的虚位移为

$$\delta {\mathbf{r}}_k=\frac{\partial {\mathbf{r}}_k}{\partial \mathbf{q}}\delta \mathbf{q}$$

因此该力所做的虚功为

$$\delta W_k={\mathbf{F}}_k^{\mathrm{\top }}\delta {\mathbf{r}}_k$$

所有外力的虚功之和可以写成

$$\delta W=\sum _k{\mathbf{F}}_k^{\mathrm{\top }}\frac{\partial {\mathbf{r}}_k}{\partial \mathbf{q}}\delta \mathbf{q}={\mathbf{Q}}^{\mathrm{\top }}\delta \mathbf{q}$$

其中

$$\mathbf{Q}=\sum _k{\left(\frac{\partial {\mathbf{r}}_k}{\partial \mathbf{q}}\right)}^{\mathrm{\top }}{\mathbf{F}}_k$$

称为系统的广义力(generalized force)。

因此，实际作用在刚体不同位置、不同方向上的力，都可以通过运动学雅可比映射为统一的广义力 $\mathbf{Q}$。

对于理想约束，约束力对所有满足约束的虚位移均不做虚功：

$$\delta W_c=0$$

即

$${\mathbf{Q}}_c^{\mathrm{\top }}\delta \mathbf{q}=0$$

而允许虚位移满足

$${\mathbf{C}}_{\mathbf{q}}\delta \mathbf{q}=0$$

因此约束广义力必然位于 ${\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}$ 的列空间中，可以使用拉格朗日乘子表示。

第二类拉格朗日方程

对于使用独立广义坐标描述的系统，可以通过系统的动能

$$T=T(\mathbf{q},\dot{\mathbf{q}},t)$$

建立第二类拉格朗日方程：

$$\frac{d}{dt}\left(\frac{\partial T}{\partial \dot{\mathbf{q}}}\right)-\frac{\partial T}{\partial \mathbf{q}}=\mathbf{Q}$$

其中 $\mathbf{Q}$ 为作用在系统上的总广义力。

这里使用的是动能 $T$ 而不是拉格朗日量

$$L=T-V$$

因为势能力也可以直接写成广义力。例如对于势能 $P(\mathbf{q})$，

$${\mathbf{Q}}_P=-\frac{\partial P}{\partial \mathbf{q}}$$

因此

$$\frac{d}{dt}\left(\frac{\partial T}{\partial \dot{\mathbf{q}}}\right)-\frac{\partial T}{\partial \mathbf{q}}={\mathbf{Q}}_P+{\mathbf{Q}}_{\mathrm{其}\mathrm{他}}$$

与使用 $L=T-P$ 的形式完全等价。

这种写法的优点是，可以将重力、弹簧力、阻尼力、电机驱动力、接触力等统一写入广义力 $\mathbf{Q}$，更适合通用多体动力学程序。

将拉格朗日方程进一步整理，一般可以得到

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}={\mathbf{Q}}^{\ast }(\mathbf{q},\dot{\mathbf{q}},t)$$

其中 $\mathbf{M}$ 为系统质量矩阵，${\mathbf{Q}}^{\ast }$ 包括外部广义力以及由速度等产生的动力学项。

增广建模与嵌入建模

对于存在约束

$$\mathbf{C}(\mathbf{q},t)=0$$

的系统，有两种基本的动力学建模方法。

嵌入建模(embedding formulation)

将广义坐标分为独立坐标和非独立坐标：

$$\mathbf{q}=\left[\begin{array}{c}{\mathbf{q}}_i\\ {\mathbf{q}}_d\end{array}\right]$$

由

$${\mathbf{C}}_{{\mathbf{q}}_i}\delta {\mathbf{q}}_i+{\mathbf{C}}_{{\mathbf{q}}_d}\delta {\mathbf{q}}_d=0$$

如果 ${\mathbf{C}}_{{\mathbf{q}}_d}$ 可逆，则有

$$\delta {\mathbf{q}}_d=-{\mathbf{C}}_{{\mathbf{q}}_d}^{-1}{\mathbf{C}}_{{\mathbf{q}}_i}\delta {\mathbf{q}}_i$$

因此所有允许虚位移都可以仅由独立坐标的虚位移表示。

通过这种方式，可以将非独立坐标从动力学方程中消去，最终得到仅关于独立坐标的微分方程。

嵌入建模的优点是最终动力学方程维数较小，只包含真正的系统自由度；

缺点是需要持续确定哪些坐标是独立坐标、哪些是非独立坐标，并且在复杂闭环系统中坐标分割可能比较困难。

增广建模(augmented formulation)

另一种方法是不消去非独立广义坐标，而是保留完整的冗余坐标 $\mathbf{q}$，同时引入拉格朗日乘子 $\mathit{\lambda }$ 表示约束反力。

Info

拉格朗日乘子 $\mathit{\lambda }$ 本身的数值依赖于约束方程的定义和尺度。

例如将约束

$$C(\mathbf{q})=0$$

改写为

$$2C(\mathbf{q})=0$$

虽然物理约束完全相同，但对应的 $\lambda$ 会发生变化。

真正具有直接物理意义的是广义约束力

$${\mathbf{Q}}_{\lambda }={\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}\mathit{\lambda }$$

此时动力学可以写成

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}-\mathbf{Q}+{\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}\mathit{\lambda }=0$$

即

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+{\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}\mathit{\lambda }=\mathbf{Q}$$

这种方法不需要显式消去约束坐标，因此特别适合通用多体系统建模。

代价是系统中除了微分变量 $\mathbf{q}$ 外，还出现了代数变量 $\mathit{\lambda }$，因此最终得到的是微分-代数方程(DAE)。

刚体系统的约束动力学方程（KKT方程）

刚体的位姿与速度表示

对于空间中的一个刚体，可以使用参考点的位置

$$\mathbf{r}\in {\mathbb{R}}^3$$

描述平移运动。

姿态可以使用 Euler parameters，即单位四元数

$$\mathit{\theta }=[{\theta }_0,{\theta }_1,{\theta }_2,{\theta }_3{]}^{\mathrm{\top }}$$

表示，并满足归一化约束

$${\mathit{\theta }}^{\mathrm{\top }}\mathit{\theta }=1$$

因此如果使用位置与 Euler parameters 作为冗余广义坐标，则一个空间刚体可以写成

$${\mathbf{q}}^b=\left[\begin{array}{c}\mathbf{r}\\ \mathit{\theta }\end{array}\right]$$

共有 7 个广义坐标，但由于四元数归一化约束只有 6 个实际自由度。

刚体的角速度 $\mathit{\omega }$ 与 Euler parameters 的导数之间存在关系

$$\mathit{\omega }=\mathbf{G}(\mathit{\theta })\dot{\mathit{\theta }}$$

其中 $\mathbf{G}$ 是由当前姿态决定的矩阵。

因此，刚体的平动速度由 $\dot{\mathbf{r}}$ 描述，转动速度则由 $\mathit{\omega }$ 描述。

Note

用于表示刚体姿态的参数有多种选择，例如欧拉角、方向余弦、四元数等。 对于不同的姿态参数，都可以构造相应的矩阵 $\mathbf{G}$，使角速度写成

$$\mathit{\omega }=\mathbf{G}(\mathit{\theta })\dot{\mathit{\theta }}$$

但不同姿态参数对应的 $\mathbf{G}$ 的形式和维数可能不同。

对于下面给出的 Euler parameters （四元数）情况，该矩阵对应刚体系下的角速度表示。

$$\mathbf{G}(\theta )=2\left[\begin{array}{cccc}-{\theta }_1& {\theta }_0& {\theta }_3& -{\theta }_2\\ -{\theta }_2& -{\theta }_3& {\theta }_0& {\theta }_1\\ -{\theta }_3& {\theta }_2& -{\theta }_1& {\theta }_0\end{array}\right]$$

刚体的质量矩阵

对于第 $i$ 个刚体，其动能可以写成

$$T^i=\frac{1}{2}{\dot{\mathbf{q}}}^{iT}{\mathbf{M}}^i{\dot{\mathbf{q}}}^i$$

其中 ${\mathbf{M}}^i$ 为该刚体的质量矩阵。

如果广义坐标由参考点位置和姿态参数组成，则质量矩阵一般可以写成

$${\mathbf{M}}^i=\left[\begin{array}{cc}{\mathbf{M}}_{rr}^i& {\mathbf{M}}_{r\theta }^i\\ {\mathbf{M}}_{\theta r}^i& {\mathbf{M}}_{\theta \theta }^i\end{array}\right]$$

其中平动部分为

$${\mathbf{M}}_{rr}^i=m_i{\mathbf{I}}_3$$

耦合项

$${\mathbf{M}}_{r\theta }^i=-{\mathbf{A}}^i[{\int }_V^i{\rho }^i[{\mathbf{u}}^i{]}_{\times }d{V}^i]{\mathbf{G}}^i$$

其中${\mathbf{A}}^i$是刚体坐标系到世界系的$3\times 3$旋转变换矩阵，$\mathbf{u}$是刚体系下质点的位矢。

转动部分为

$${\mathbf{M}}_{\theta \theta }^i={\mathbf{G}}^{i\mathrm{\top }}{\int }_V^i{\rho }^i[{\mathbf{u}}^i{]}_{\times }^{\mathrm{\top }}[{\mathbf{u}}^i{]}_{\times }d{V}^i{\mathbf{G}}^i={\mathbf{G}}^{i\mathrm{\top }}{\mathbf{I}}_{\theta \theta }^i{\mathbf{G}}^i$$

其中${\mathbf{I}}_{\theta \theta }^i={\int }_V^i{\rho }^i[{\mathbf{u}}^i{]}_{\times }^{\mathrm{\top }}[{\mathbf{u}}^i{]}_{\times }d{V}^i$为惯性张量，其对角元素称为惯性矩，非对角元素称为惯性积。

如果参考点选在刚体质心，则一阶质量矩为零，因此平动和转动之间的耦合项消失：

$${\mathbf{M}}_{R\theta }^i={\mathbf{M}}_{\theta R}^i=\mathbf{0}$$

于是单个刚体的质量矩阵得到明显简化。

对于由 $n_b$ 个刚体构成的系统，将各刚体的质量矩阵装配为

$$\mathbf{M}=\mathrm{diag}({\mathbf{M}}^1,{\mathbf{M}}^2,\dots ,{\mathbf{M}}^{n_b})$$

KKT方程

对于完整约束

$$\mathbf{C}(\mathbf{q},t)=\mathbf{0}$$

对时间求一次导数：

$${\mathbf{C}}_{\mathbf{q}}\dot{\mathbf{q}}+{\mathbf{C}}_t=\mathbf{0}$$

再次求导得到加速度约束：

$${\mathbf{C}}_{\mathbf{q}}\ddot{\mathbf{q}}={\mathbf{Q}}_c$$

其中 ${\mathbf{Q}}_c$ 是约束方程二次求导后所有不含 $\ddot{\mathbf{q}}$ 的项移到右侧得到的量。

例如对于稳恒约束 $\mathbf{C}(\mathbf{q})=0$，

$${\mathbf{Q}}_c=-{\dot{\mathbf{C}}}_{\mathbf{q}}\dot{\mathbf{q}}$$

系统动力学写成

$$\mathbf{M}\ddot{\mathbf{q}}+{\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}\mathit{\lambda }={\mathbf{Q}}_e+{\mathbf{Q}}_v$$

其中：

• ${\mathbf{Q}}_e$：外部广义力，例如重力、驱动力、弹簧力等；
• ${\mathbf{Q}}_v$：由速度产生的二次速度惯性项，例如科氏项、离心项和陀螺项；
• ${\mathbf{Q}}_c$：加速度级约束方程的右端项，并不是广义力。

Note

对于质心作为参考点、四元数表示姿态时，单个刚体的

$${\mathbf{Q}}_v={\left[\begin{array}{cc}{\mathbf{0}}^{\mathrm{\top }}& -2{\mathit{\omega }}^{i\mathrm{\top }}{\mathbf{I}}_{\theta \theta }^i{\dot{\mathbf{G}}}^i\end{array}\right]}^{\mathrm{\top }}$$

整个系统的$Q_v$通过将各个刚体纵向拼接得到。

对于一般情况，则需要从拉格朗日方程出发计算。例如机器人学中常见的

$$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\mathit{\tau }$$

形式的拉格朗日方程，

$${\mathbf{Q}}_v=-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$$

将动力学方程和加速度约束联立，得到

$$\left[\begin{array}{cc}\mathbf{M}& {\mathbf{C}}_{\mathbf{q}}^{\mathrm{\top }}\\ {\mathbf{C}}_{\mathbf{q}}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\ddot{\mathbf{q}}\\ \mathit{\lambda }\end{array}\right]=\left[\begin{array}{c}{\mathbf{Q}}_e+{\mathbf{Q}}_v\\ {\mathbf{Q}}_c\end{array}\right]$$

求解该方程可以同时得到系统加速度 $\ddot{\mathbf{q}}$ 和约束反力对应的拉格朗日乘子 $\mathit{\lambda }$。

Note

KKT方程的推导——从拉格朗日第二类方程出发

1. 先由系统运动学写出$T(q,\dot{q})$
2. 计算质量矩阵$M={\partial }^{2}T/\partial {\dot{q}}^2$
3. 计算$Qv=-\dot{M}\dot{q}+(\partial T/\partial q{)}^{\mathrm{\top }}$

实际计算一般不采用。有两类做法：冗余坐标（此时质量矩阵直接对角拼装、广义力纵向堆叠，多刚体间的耦合靠约束来实现）、牛顿-欧拉迭代。

Info

KKT方程中的

$${\mathbf{C}}_{\mathbf{q}}\ddot{\mathbf{q}}={\mathbf{Q}}_c$$

只在加速度层满足约束。理论上，如果初始时

$$\mathbf{C}=0,\dot{\mathbf{C}}=0$$

则约束会一直成立；但数值积分会产生误差，使 $\mathbf{C}$ 和 $\dot{\mathbf{C}}$ 逐渐偏离零，这称为 constraint drift。

实际计算中常使用位置/速度投影、Newton-Raphson校正或Baumgarte stabilization等方法抑制约束漂移。 例如在NMPC中，可以将原始约束作为优化约束加进去，以移植漂移。

牛顿-欧拉方程

如果将刚体参考点选在质心，则平动和转动惯性解耦。

$$\left[\begin{array}{cc}{m}_i{\mathbf{I}}_3& \mathbf{0}\\ \mathbf{0}& {\mathbf{I}}^i\end{array}\right]\left[\begin{array}{c}{\ddot{\mathbf{R}}}^i\\ {\mathit{\alpha }}^i\end{array}\right]=\left[\begin{array}{c}{\mathbf{F}}^i\\ {\mathit{\tau }}^i-{\mathit{\omega }}^i\times ({\mathbf{I}}^i{\mathit{\omega }}^i)\end{array}\right]$$

其中上半部分为 Newton 方程：

$$m_i{\ddot{\mathbf{R}}}^i={\mathbf{F}}^i$$

描述质心的平动。

下半部分为 Euler 方程：

$${\mathbf{I}}^i{\mathit{\alpha }}^i+{\mathit{\omega }}^i\times ({\mathbf{I}}^i{\mathit{\omega }}^i)={\mathit{\tau }}^i$$

描述刚体的转动。其中$\mathit{\alpha }$为角加速度。

非完整约束

对于非完整约束，约束关系不能积分为只含 $\mathbf{q}$ 和 $t$ 的形式。

常见的形式(Pfaffian约束)为

$${\mathbf{a}}_0(\mathbf{q},t)+\mathbf{B}(\mathbf{q},t)\dot{\mathbf{q}}=\mathbf{0}$$

其中 $\mathbf{B}$ 为速度约束矩阵。

允许的虚位移满足

$$\mathbf{B}\delta \mathbf{q}=\mathbf{0}$$

因此理想约束产生的广义约束力可以写成

$${\mathbf{Q}}_c^{force}={\mathbf{B}}^T\mathit{\lambda }$$

对速度约束求时间导数，可以得到加速度级约束：

$$\mathbf{B}\ddot{\mathbf{q}}={\mathbf{Q}}_{c,nh}$$

其中

$${\mathbf{Q}}_{c,nh}=-\dot{\mathbf{B}}\dot{\mathbf{q}}-{\dot{\mathbf{a}}}_0$$

因此非完整约束下的动力学同样可以写成类似的 KKT 形式：

$$\left[\begin{array}{cc}\mathbf{M}& {\mathbf{B}}^T\\ \mathbf{B}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\ddot{\mathbf{q}}\\ \mathit{\lambda }\end{array}\right]=\left[\begin{array}{c}{\mathbf{Q}}_e+{\mathbf{Q}}_v\\ {\mathbf{Q}}_{c,nh}\end{array}\right]$$

与完整约束相比，两者的 KKT 结构相同，但完整约束中的约束矩阵来自

$${\mathbf{C}}_{\mathbf{q}}=\frac{\partial \mathbf{C}}{\partial \mathbf{q}}$$

如果进一步考虑一般形式的速度约束

$$\mathbf{\Phi }(\mathbf{q},\dot{\mathbf{q}},t)=\mathbf{0}$$

可以采用 Chetaev 条件规定允许虚位移：

$$\frac{\partial \mathbf{\Phi }}{\partial \dot{\mathbf{q}}}\delta \mathbf{q}=\mathbf{0}$$

记

$$\mathbf{A}=\frac{\partial \mathbf{\Phi }}{\partial \dot{\mathbf{q}}}$$

则理想约束产生的广义约束力为

$${\mathbf{Q}}_c^{force}={\mathbf{A}}^T\mathit{\lambda }$$

对原速度约束求时间导数：

$$\mathbf{A}\ddot{\mathbf{q}}+{\mathbf{\Phi }}_{\mathbf{q}}\dot{\mathbf{q}}+{\mathbf{\Phi }}_t=\mathbf{0}$$

因此可以写成 KKT 形式：

$$\left[\begin{array}{cc}\mathbf{M}& {\mathbf{A}}^T\\ \mathbf{A}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\ddot{\mathbf{q}}\\ \mathit{\lambda }\end{array}\right]=\left[\begin{array}{c}{\mathbf{Q}}_e+{\mathbf{Q}}_v\\ -{\mathbf{\Phi }}_{\mathbf{q}}\dot{\mathbf{q}}-{\mathbf{\Phi }}_t\end{array}\right]$$

Info

对于线性速度约束

$$\mathbf{B}(\mathbf{q},t)\dot{\mathbf{q}}+{\mathbf{a}}_0=0$$

Chetaev条件是经典非完整力学中常用且自然的选择。

但对于一般非线性速度约束

$$\mathbf{\Phi }(\mathbf{q},\dot{\mathbf{q}},t)=0$$

采用

$${\mathbf{\Phi }}_{\dot{\mathbf{q}}}\delta \mathbf{q}=0$$

属于一种建模原则，并不是由约束方程本身唯一推出的结果。其他变分处理，例如vakonomic mechanics，可能产生不同的动力学方程。