详细推导机器人学中常用的刚体旋转动力学（角动量定理、惯量张量、欧拉动力学方程）

刚体定点转动&欧拉动力学方程的推导

1. 刚体定点转动的角动量

首先，我们定义静止惯性坐标系S，对象刚体绕S系原点作定点转动；然后定义一个固连于刚体之上的动坐标系B，其原点与S系原点重合。

记刚体相对于S系的角速度为$\mathbf{\omega }$，并将刚体看作由无数个微小质点$m_i$组成，质点$m_i$相对原点的位置矢量为${\mathbf{r}}_i$，则该刚体相对原点的角动量为：

$$\begin{array}{r}\mathbf{L}=\sum _i{\mathbf{L}}_i=\sum _i{\mathbf{r}}_i\times m_i{\dot{\mathbf{r}}}_i=\sum _i{\mathbf{r}}_i\times m_i(\mathbf{\omega }\times {\mathbf{r}}_i)\end{array}$$

将(1)式中的矢量用在S系下的坐标表示：

$$\begin{array}{r}\begin{array}{rl}{\mathbf{L}}^S& =\sum _i{\mathbf{r}}_i^S\times m_i({\mathbf{\omega }}^S\times {\mathbf{r}}_i^S)\\ & =-\sum _i{\mathbf{r}}_i^S\times m_i({\mathbf{r}}_i^S\times {\mathbf{\omega }}^S)\\ & =-\sum _i{m}_i[{\mathbf{r}}_i^S{]}_{\times }^2{\mathbf{\omega }}^S\\ & ={\mathbf{J}}_S{\mathbf{\omega }}^S\end{array}\end{array}$$

其中

$$\begin{array}{r}{\mathbf{J}}_S=-\sum _i{m}_i[{\mathbf{r}}_i^S{]}_{\times }^2=\sum _i{m}_i\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)\end{array}$$

为刚体B相对于S系的惯性矩阵，也称为惯量张量。可见随着刚体的转动，${\mathbf{J}}_S$是一个变化的量。

类似的，我们也可以求刚体对固连坐标系S的惯性矩阵：

$$\begin{array}{r}{\mathbf{J}}_B=-\sum _i{m}_i[{\mathbf{r}}_i^B{]}_{\times }^2=\sum _i{m}_i\left({\mathbf{r}}_i^{B\top }{\mathbf{r}}_i^B{\mathbf{I}}_3-{\mathbf{r}}_i^B{\mathbf{r}}_i^{B\top }\right)\end{array}$$

根据刚体的定义，${\mathbf{r}}_i^B$并不会随刚体运动而变化，则${\mathbf{J}}_B$是一个常量，因此我们常常对${\mathbf{J}}_B$更感兴趣，因为它更能反映刚体惯性的固有属性。

将B系在S系中的姿态用旋转矩阵表示为${\mathbf{R}}_B^S$，则我们可以推导出${\mathbf{J}}_B$和${\mathbf{J}}_S$之间的关系：

$$\begin{array}{r}\begin{array}{rl}{\mathbf{J}}_S& =-\sum _i{m}_i[{\mathbf{r}}_i^S{]}_{\times }^2=-\sum _i{m}_i[{\mathbf{R}}_B^S{\mathbf{r}}_i^B{]}_{\times }^2\\ & =-\sum _i{m}_i({\mathbf{R}}_B^S[{\mathbf{r}}_i^S{]}_{\times }{\mathbf{R}}_B^{S\top }{)}^2\\ & =-{\mathbf{R}}_B^S\sum _i{m}_i[{\mathbf{r}}_i^S{]}_{\times }^2{\mathbf{R}}_B^{S\top }\\ & ={\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }\end{array}\end{array}$$

2. 刚体定点转动的角动量定理

记作用在每个微小质点$m_i$上的力为${\mathbf{F}}_i$，则根据牛顿第二定律，作用在刚体上的合力矩为：

$$\begin{array}{r}\begin{array}{rl}\mathbf{\tau }& =\sum _i{\mathbf{r}}_i\times {\mathbf{F}}_i\\ & =\sum _i{\mathbf{r}}_i\times m_i{\ddot{\mathbf{r}}}_i\\ & =\sum _i\underset{0}{\underbrace{{\dot{\mathbf{r}}}_i\times m_i{\dot{\mathbf{r}}}_i}}+{\mathbf{r}}_i\times m_i{\ddot{\mathbf{r}}}_i\\ & =\frac{d}{dt}\sum _i{\mathbf{r}}_i\times m_i{\dot{\mathbf{r}}}_i\\ & =\dot{\mathbf{L}}\end{array}\end{array}$$

这就是刚体定点转动角动量定理的表达式，即刚体所受的合力矩等于角动量的变化率。

每个质元$m_i$所受的力可以分解为外力和内力：

$$\begin{array}{r}{\mathbf{F}}_i={\mathbf{F}}_{ext,i}+{\mathbf{F}}_{int,i}\end{array}$$

内力${\mathbf{F}}_{int,i}$又可以分解为其余质元对$m_i$的作用力之和：

$$\begin{array}{r}{\mathbf{F}}_{int,i}=\sum _{j\ne i}{\mathbf{F}}_{int,i,j}\end{array}$$

显然${\mathbf{F}}_{int,i,j}$和${\mathbf{F}}_{int,j,i}$为一对相互作用力。根据牛顿第三定律一对相互作用力是等大、共线、反向的，则有：

$$\begin{array}{rl}& {\mathbf{F}}_{int,i,j}+{\mathbf{F}}_{int,j,i}=0\\ & {\mathbf{r}}_i\times {\mathbf{F}}_{int,i,j}+{\mathbf{r}}_j\times {\mathbf{F}}_{int,j,i}=0\end{array}$$

即每对内力的力矩也是相互抵消的，而每个内力总是成对出现，所以

$$\begin{array}{r}\sum _i{\mathbf{r}}_i\times {\mathbf{F}}_{int,i}=0\end{array}$$

于是

$$\begin{array}{r}\mathbf{\tau }=\sum _i{\mathbf{r}}_i\times ({\mathbf{F}}_{ext,i}+{\mathbf{F}}_{int,i})=\sum _i{\mathbf{r}}_i\times {\mathbf{F}}_{ext,i}={\mathbf{\tau }}_{ext}=\dot{\mathbf{L}}\end{array}$$

即刚体所受总力矩等于外力的力矩，内力的力矩为0，刚体所受外力力矩等于刚体角动量的变化率。

3. 欧拉动力学方程的推导

我们记作用在刚体之上的外部力矩为$\mathbf{\tau }$，则根据定点转动刚体的角动量定理，作用在刚体上的外部力矩等于刚体角动量的变化率，即：

$$\begin{array}{r}{\mathbf{\tau }}^S={\dot{\mathbf{L}}}^S=\frac{d}{dt}\left({\mathbf{J}}_S{\mathbf{\omega }}^S\right)={\dot{\mathbf{J}}}_S{\mathbf{\omega }}^S+{\mathbf{J}}_S{\dot{\mathbf{\omega }}}^S\end{array}$$

根据之前的推导，刚体相对于静系S系的惯性矩阵${\mathbf{J}}_S$是随时间变化的量，因此其对时间的倒数并不恒为0，下面给出两种方法对其时间导数进行推导。

• 方法1：根据(5)式有

$$\begin{array}{r}\begin{array}{rl}{\dot{\mathbf{J}}}_S& =\frac{d}{dt}\left({\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }\right)\\ & ={\dot{\mathbf{R}}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }+\underset{0}{\underbrace{{\mathbf{R}}_B^S{\dot{\mathbf{J}}}_B{\mathbf{R}}_B^{S\top }}}+{\mathbf{R}}_B^S{\mathbf{J}}_B{\dot{\mathbf{R}}}_B^{S\top }\\ & =[{\mathbf{\omega }}^S{]}_{\times }{\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }+{\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }[{\mathbf{\omega }}^S{]}_{\times }^{\top }\\ & =[{\mathbf{\omega }}^S{]}_{\times }{\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }-{\mathbf{R}}_B^S{\mathbf{J}}_B{\mathbf{R}}_B^{S\top }[{\mathbf{\omega }}^S{]}_{\times }\\ & =[{\mathbf{\omega }}^S{]}_{\times }{\mathbf{J}}_S-{\mathbf{J}}_S[{\mathbf{\omega }}^S{]}_{\times }\end{array}\end{array}$$

• 方法2：根据(3)式和(4)式硬推：

$$\begin{array}{r}\begin{array}{rl}{\dot{\mathbf{J}}}_S& =\frac{d}{dt}\sum _i{m}_i\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)\\ & =\sum _i{m}_i\left(\underset{0}{\underbrace{({\mathbf{\omega }}^S\times {\mathbf{r}}_i^S{)}^{\top }{\mathbf{r}}_i^S{\mathbf{I}}_3}}+\underset{0}{\underbrace{{\mathbf{r}}_i^{S\top }({\mathbf{\omega }}^S\times {\mathbf{r}}_i^S){\mathbf{I}}_3}}-({\mathbf{\omega }}^S\times {\mathbf{r}}_i^S){\mathbf{r}}_i^{S\top }-{\mathbf{r}}_i^S({\mathbf{\omega }}^S\times {\mathbf{r}}_i^S{)}^{\top }\right)\\ & =\sum _i{m}_i\left(-[{\mathbf{\omega }}^{\mathbf{S}}{]}_{\times }{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }+{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }[{\mathbf{\omega }}^{\mathbf{S}}{]}_{\times }\right)\\ & =\sum _i{m}_i\left(\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S[{\mathbf{\omega }}^S{]}_{\times }-[{\mathbf{\omega }}^S{]}_{\times }{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)-\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S[{\mathbf{\omega }}^S{]}_{\times }-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }[{\mathbf{\omega }}^S{]}_{\times }\right)\right)\\ & =\sum _i{m}_i\left([{\mathbf{\omega }}^S{]}_{\times }\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)-\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)[{\mathbf{\omega }}^S{]}_{\times }\right)\\ & =[{\mathbf{\omega }}^S{]}_{\times }\sum _i{m}_i\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)-\sum _i{m}_i\left({\mathbf{r}}_i^{S\top }{\mathbf{r}}_i^S{\mathbf{I}}_3-{\mathbf{r}}_i^S{\mathbf{r}}_i^{S\top }\right)[{\mathbf{\omega }}^S{]}_{\times }\\ & =[{\mathbf{\omega }}^S{]}_{\times }{\mathbf{J}}_S-{\mathbf{J}}_S[{\mathbf{\omega }}^S{]}_{\times }\end{array}\end{array}$$

将(14)式代入(13)中就得到了刚体的Euler动力学方程：

$$\begin{array}{r}\begin{array}{rl}{\mathbf{\tau }}^S& ={\mathbf{\omega }}^S\times {\mathbf{J}}_S{\mathbf{\omega }}^S-\underset{0}{\underbrace{{\mathbf{J}}_S{\mathbf{\omega }}^S\times {\mathbf{\omega }}^S}}+{\mathbf{J}}_S{\dot{\mathbf{\omega }}}^S\\ & ={\mathbf{\omega }}^S\times {\mathbf{J}}_S{\mathbf{\omega }}^S+{\mathbf{J}}_S{\dot{\mathbf{\omega }}}^S\end{array}\end{array}$$

在无人机等机器人应用中，我们一般是从CAD模型中获取机器人相对于自身固连坐标系B的惯性矩阵，以及常用的IMU传感器测量的机器人角速度也是在B系中表示的，我们将Euler动力学方程(9)式两边同时乘以${\mathbf{R}}_S^B$得到

$$\begin{array}{r}\begin{array}{rl}{\mathbf{\tau }}^B={\mathbf{R}}_S^B{\mathbf{\tau }}^S& ={\mathbf{R}}_S^B({\mathbf{\omega }}^S\times {\mathbf{J}}_S{\mathbf{\omega }}^S)+{\mathbf{R}}_S^B{\mathbf{J}}_S{\dot{\mathbf{\omega }}}^S\\ & ={\mathbf{R}}_S^B{\mathbf{\omega }}^S\times {\mathbf{R}}_S^B{\mathbf{J}}_S{\mathbf{\omega }}^S+{\mathbf{R}}_S^B{\mathbf{J}}_S{\dot{\mathbf{\omega }}}^S\\ & ={\mathbf{\omega }}^B\times {\mathbf{R}}_S^B{\mathbf{J}}_S{\mathbf{R}}_S^{B\top }{\mathbf{R}}_S^B{\mathbf{\omega }}^S+{\mathbf{R}}_S^B{\mathbf{J}}_S{\mathbf{R}}_S^{B\top }{\mathbf{R}}_S^B{\dot{\mathbf{\omega }}}^S\\ & ={\mathbf{\omega }}^B\times {\mathbf{J}}_B{\mathbf{\omega }}^B+{\mathbf{J}}_B{\dot{\mathbf{\omega }}}^B\end{array}\end{array}$$

可见Euler动力学方程在B系和S系中的形式是相同的。

💡 注意，${\dot{\mathbf{\omega }}}^B$是B系相对S系的角速度变化率在B系中的坐标，即${\dot{\mathbf{\omega }}}^B={\left(\frac{d}{dt}\mathbf{\omega }\right)}^B={\mathbf{R}}_S^B{\dot{\mathbf{\omega }}}^S$；而不是角速度在B系中的坐标的变化率，即${\dot{\mathbf{\omega }}}^B\ne \frac{d}{dt}({\mathbf{\omega }}^B)$

4. 质心系中的欧拉动力学方程

之前的讨论都是基于刚体绕定点转动这个假设，S系为惯性系。

然而在一般情况下我们的机器人会在空间中作复杂的运动，如果我们将之前的S系换作以原点固连在刚体上随刚体进行平动的质心系C，则刚体的运动可以分解为质心的平动和绕质心的定点转动。

我们对此时刚体绕质心的定点转动进行分析。刚体固连坐标系B的原点依然与质心系C的原点（即质心）重合，而与之前不同的是平动参考系S不再是惯性系，在此参考系中刚体会受到等效惯性力的作用，此时的欧拉动力学方程写为：

$$\begin{array}{r}{\mathbf{\tau }}^C+{\mathbf{\tau }}_{惯}^C={\mathbf{\omega }}^C\times {\mathbf{J}}_C{\mathbf{\omega }}^C+{\mathbf{J}}_C{\dot{\mathbf{\omega }}}^C\end{array}$$

其中${\mathbf{\tau }}_{惯}$是等效惯性力对质心的力矩。设该时刻刚体质心的线加速度为$\mathbf{a}$，那么：

$$\begin{array}{r}\begin{array}{rl}{\mathbf{\tau }}_{惯}& =\sum _i{\mathbf{r}}_i\times m_i\mathbf{a}=\left(\sum _i{m}_i{\mathbf{r}}_i\right)\times \mathbf{a}\\ & =\left(\sum _i{m}_i\right){\mathbf{r}}_C\times \mathbf{a}\end{array}\end{array}$$

其中${\mathbf{r}}_C$是C系下的质心位置矢量，咱们的C就是质心系，原点就是质心，那当然有${\mathbf{r}}_C=0$，所以

$$\begin{array}{r}{\mathbf{\tau }}_{惯}=0\end{array}$$

即平动质心参考系中惯性力力矩为0。

💡 因为(19)式中没有任何刚体约束，所以这个结论实际上对非刚体也成立。

因此在质心参考系中的刚体欧拉动力学方程与惯性系中相同：

$$\begin{array}{r}{\mathbf{\tau }}^C={\mathbf{\omega }}^C\times {\mathbf{J}}_C{\mathbf{\omega }}^C+{\mathbf{J}}_C{\dot{\mathbf{\omega }}}^C\end{array}$$