1. 引言

  前面的文章主要介绍了质点动力学，这篇文章作一步扩展，考虑单刚体动力学的情况。相对于质点而言，刚体的复杂性在于它本身质量的分布特性。换句话说刚体本身是有形状的，而且它的质量并不是集中在一点上。因此对于一个刚体我们除了要考虑它的平移运动还要考虑它本身的旋转。

2. 牛顿方程

  对于刚体而言，牛顿方程（牛顿第二定律）依然能够用于描述刚体质心的运动。在大物中我们已经了解到作用在刚体上的任意复杂力系都可以等价为作用于质心的合外力以及合外力矩。如下图1描述的是合外力F作用于刚体质心处。那么刚体线加速度\dot{v}_c和合外力之间的关系如下：

F = m\dot{v}_c

图1. 合外力作用于质心

3. 欧拉方程

  前面已经提到作用在刚体上的任意复杂力系最终都能等价为作用于质心的合外力以及合外力矩。合外力用于产生刚体质心加速度，那么合外力矩用于产生什么呢？自然是角加速度啦。如下图2描述的是合外力矩N作用于刚体质心处。根据欧拉方程给出的定义，刚体所受合外力矩与角加速度之间的关系如下（注意这个公式要求惯性系的原点必须与质心重合，原因将在欧拉方程的推导中介绍）：

N=I_c\dot{\omega}+\hat{\omega}I_c\omega

图2. 合外力矩作用于质心

老实说最开始接触这个公式的时候我被吓退了无数次，从此认为机器人动力学是个不可理解的东西。直到很长时间之后才尝试着去理解它。当然理解它的方式是通过一些公式推导。公式推导的过程并不复杂但是比较繁琐，因此为了保证连续性，欧拉方程的推导放在了文章的末尾第5节，请移步观看。

4. 刚体动力学

  接下来我们对刚体进行受力分析进而得到它的动力学方程。如下图3是连杆i的受力分析。

图3. 连杆受力分析

先解释一下各个物理量的含义。首先这里认为所有物理量都是在连杆的质心坐标系下描述的。

f_i: 关节i施加给连杆i的力

n_i: 关节i施加给连杆i的力矩

f_{i+1}: 关节i+1施加给连杆i的力

n_{i+1}: 关节i+1施加给连杆i的力矩

P_i: 关节i的中点在连杆i质心坐标系\{O\}下的的位置矢量

P_{i+1}: 关节i+1的中点在连杆i质心坐标系\{O\}下的位置矢量

F: 刚体所受合外力

N: 刚体所受合外力矩

根据连杆受到的力，列出连杆质心所受合外力为：

F=f_i+f_{i+1}+m_ig

根据连杆受到的力和力矩，列出连杆质心所受合外力矩为：

N=n_i+n_{i+1}+P_i \times f_i+P_{i+1}\times f_{i+1}

设连杆i的质心速度和角速度在其质心坐标系\{O\}下的表示为v_c和\omega_c，惯性张量为I_c，那么根据牛顿方程（牛顿第二定律）可得：

F=f_i+f_{i+1}+m_ig=m\dot{v}_c

根据欧拉方程可得：

N=n_i+n_{i+1}+P_i \times f_i+P_{i+1}\times f_{i+1}=I_c\dot{\omega}_c+\omega_c\times I_c\omega_c

以上便是单刚体动力学方程。对于多连杆的串联机器人来说本质上也是逐个分析其中的每一个连杆。但是对机器人来说如何求解各个连杆的速度，加速度，角速度，角加速度在其质心坐标系下的表达才是基于牛顿欧拉法的机器人动力学真正难的地方。

5. 刚体动力学欧拉方程推导

5.1 动量与角动量

  大家都知道质点动量是质量和速度的乘积p=mv，动量的时间导数等于ma，也就是说动量对时间的导数是合外力。在物理学当中还有另外一个概念叫做角动量，质点的角动量是质点的位置矢量与动量的外积L=r\times mv。那么角动量对时间的导数又是什么呢？来推导一下。

\dot{L}=\dot{r}\times mv+r\times m \dot{v}

由于r是位置矢量，因此它的导数是速度v。我们知道如果两个矢量方向相同，那么他们的外积为0向量。因此\dot{r}\times mv=v\times mv = 0。而速度v对时间的导数对应于加速度，因此\dot{v}=a。而ma代表质点所受合外力F。所以上面的等式可以简化为：

\dot{L}=r\times F

这个公式相信大家都不陌生。位置矢量和力的外积（叉乘）代表的是力矩。如果你觉得不太好理解可以想一下杠杆原理是什么：力乘以力臂等于力矩。当然这是标量的说法。如下图4所示。根据杠杆原理力F对原点O的力矩N=Fl。如果只考虑大小那么\left | r \times F\right |=\left | r\right | \left | F\right |sin(\theta)=|r|sin(\theta)\left | F\right |=l\left | F\right |。

图4. 杠杆原理

总结一下就是角动量对时间的导数等于合外力矩。接下来就用这个结论来推导刚体角动量对时间的导数看看结果是什么。

5.2 刚体的角动量

  对于质点而言，它的角动量为L=r\times mv。但是对于刚体来说它的每一部分速度都不相同。因此刚体的角动量需要用到微元思想以及积分。如下图5所示描述了刚体的一个体积微元。

图5. 刚体动量

我们知道刚体的任意运动都可以分解为刚体质心的平移以及绕质心的转动。这里刚体质心平移的速度为\dot{p}_c。刚体角速度为\omega。此处默认所有的物理量都是在惯性系\{O\}中描述的。体积微元可以看成一个质点，因此它的角动量为：

dL=p \times \dot{p}\ dm

其中p代表微元的位置矢量，\dot{p}代表微元的速度，dm代表微元的质量。对以上微元进行体积分就可以得到整个刚体的动量了：

L=\int_V p\times \dot{p}\ dm = \int_V p\times \dot{p}\ \rho dV

简单的理解刚体的角动量就是所有体积微元角动量的叠加。角动量对时间求导结果如下：

\dot{L}=\int_V \dot{p}\times\dot{p} + p \times \ddot{p} \ dm

这里应用的链式法则，因为叉乘本质上是乘法运算，因此叉乘的导数等于对叉乘的两项分别求导再相加。根据向量叉乘的定义可以知道，一个向量叉乘它自己等于零向量。因此上面的公式又可以简化为：

M = \dot{L}=\int_V p \times \ddot{p} \ dm

接下来我们来研究一下被积函数p\times \ddot{p}。前面已经提到刚体的任意运动都可以抽象为质心的平移和绕质心的旋转。因此刚体上体积微元的速度可以看成质心的速度加上体积微元绕质心旋转的速度，即：

\dot{p}=\dot{p_c} + \omega\times \left(p-p_c\right)

其中\omega代表刚体在惯性系\{O\}中旋转的角速度，\left(p-p_c\right)代表的是质心指向体积微元的矢量，这几个量在图5中都有画出来。接下来可以对上述速度矢量求导：

\ddot{p}=\ddot{p}_c + \dot{\omega}\times\left(p-p_c\right) + \omega \times \left(\dot{p} - \dot{p}_c\right)

其中\dot{p}-\dot{p}_c=\omega\times\left(p-p_c\right)。因此上式等价于：

\ddot{p}=\ddot{p_c} + \dot{\omega}\times\left(p-p_c\right) + \omega \times \left(\omega\times(p-p_c)\right)

在这些推导中一定注意保留原有的括号，因为叉乘本身是不满足结合律的，即\left(a\times b\right) \times c \not=a\times \left(b \times c\right)。稍微展开一下：

\ddot{p}=\ddot{p}_c + \dot{\omega}\times p-\dot{\omega}\times p_c + \omega \times \left(\omega\times p-\omega\times p_c\right)

把这个加速度公式代入被积函数p\times \ddot{p}中可以得到：

p\times\ddot{p}=p\times\ddot{p_c} + p\times(\dot{\omega} \times p)-p\times(\dot{\omega} \times p_c)+p\times\left[\omega \times \left(\omega\times p-\omega\times p_c\right)\right]

这个公式虽然很长，但是其实并不难理解，唯一复杂的地方在于叉乘要怎么处理。

  我们先来回忆一下关于叉乘以及反对陈矩阵的知识。在12. 机器人正运动学—-姿态描述之轴角（旋转向量）这篇文章中我们曾经介绍过叉乘具备如下的等价关系：

\omega \times p=\begin{bmatrix} 0& -\omega_{z} & \omega_{y} \\ \omega_{z}& 0& -\omega_{x} \\ -\omega_{y}& \omega_{x}& 0 \end{bmatrix}p=\hat{\omega}p

其中\hat{\omega}代表矢量\omega对应的反对陈矩阵。matlab中有一个指令专门把三维矢量转换为其对应的反对陈矩阵即: skew(a)。这个变换最大的好处就是把叉乘运算转换为矩阵乘法（矩阵乘法是满足结合律的因此可以去掉括号）。观察上面的反对陈矩阵\hat{\omega}不难发现：-\hat{\omega}=\hat{\omega}^T。根据叉乘的性质：a\times b=-b\times a。

  接下来利用叉乘和反对陈矩阵来处理一下前面得到的方程。首先看p\times(\dot{\omega} \times p)可以怎么处理呢？

p\times(\dot{\omega} \times p)=p\times(-p \times \dot{\omega})=\hat{p}\hat{p}^T\dot{\omega}

另外一项p\times(\dot{\omega} \times p_c)形式与前一项相同，但是出于对后面积分的考虑这里可以处理为：

p\times(\dot{\omega} \times p_c)=p\times(-p_c \times \dot{\omega})=p\times\left(\hat{p}_c^T\dot{\omega}\right)

再来看另外一项p\times\left[\omega\times \left(\omega \times p\right)\right]，可以尝试用几何法证明：

p\times\left[\omega\times \left(\omega \times p\right)\right] \equiv \omega\times\left[p\times \left(\omega \times p\right)\right]

我们在图上分别画出p\times\left[\omega\times \left(\omega \times p\right)\right]和\omega\times\left[p\times \left(\omega \times p\right)\right]的运算过程（这是有可能的，因为叉乘本身实际上就是一种向量的变换）。如下图6画出了这两个叉乘的过程。为了清晰，绿色的向量和辅助线都在平面s内。黄色的向量都垂直于平面s。我们可以依据叉乘的顺序依次画出每一步叉乘生成的向量。你会发现这两个叉乘的过程得到的最终向量方向是一致的。向量相等要求两个条件，一是大小相等，二是方向相同。现在我们已经分析出方向相同。

图6. 叉乘的过程

现在来看一下大小的问题。根据叉乘性质：|a\times b|=|a||b|sin\theta。可知：

\left |p\times\left[\omega\times \left(\omega \times p\right)\right]\right |=|p||\omega\times \left(\omega \times p\right)|sin\left< p, \omega\times \left(\omega \times p\right)\right >=|p||\omega\times \left(\omega \times p\right)|sin\theta_1

因为\omega\times p \perp \omega因此|\omega\times \left(\omega \times p\right)|=|\omega||\omega \times p|所以：

\left |p\times\left[\omega\times \left(\omega \times p\right)\right]\right |=|p||\omega||\omega \times p|sin\theta_1

同样的道理：

\omega\times\left[p\times \left(\omega \times p\right)\right]=|\omega||p||\omega\times p|sin\theta_2

根据三角函数定义sin\theta_1=sin\theta_3。因此只要能证明\theta_2和\theta_3是相等的我们的目的就达到了。根据叉乘的定义p\perp p\times(\omega\times p)，\omega \perp \omega \times (\omega \times p)。因此

\theta_3+\theta_4 = 90^{\circ}\ \ \ \theta_2+\theta_4=90^{\circ}

因此\theta_2和\theta_3确实是相等的。

以上我们证明了p\times\left[\omega\times \left(\omega \times p\right)\right] \equiv \omega\times\left[p\times \left(\omega \times p\right)\right]。那么被积函数中的p\times\left[\omega\times \left(\omega \times p\right)\right]可以简化为：

\omega\times\left[p\times \left(\omega \times p\right)\right]=\omega\times\left[p\times \left(-p \times \omega \right)\right]=\omega \times(\hat{p}\hat{p}^T\omega)

另外一项p\times\left[\omega\times \left(\omega \times p_c\right)\right]可以简化为：

p\times\left[\omega\times \left(\omega \times p_c\right)\right]=p\times\left[\omega\times \left(-p_c \times \omega \right)\right]=p \times(\omega\times(\hat{p}_c^T\omega))

至此我们总结一下前面的推导：

M =\int_V p\times\ddot{p}_c + \hat{p}\hat{p}^T\dot{\omega} -p\times(\hat{p}_c^T\dot{\omega})+\omega \times(\hat{p}\hat{p}^T\omega) -p \times(\omega\times(\hat{p}_c^T\omega)) \ dm

把积分常量提取出来可以得到：

M=\int_V p\ dm\times\ddot{p}_c + \\
\int_V \hat{p}\hat{p}^T dm\ \dot{\omega} - \\
\int_V p\ dm\times(\hat{p}_c^T\dot{\omega})+ \\
\omega \times \left ( \int_V \hat{p}\hat{p}^T dm \ \omega \right )- \\
\int_V p\ dm\times \left(\omega \times (\hat{p}_c^T\omega)\right)

分别看一下这个公式中的两种积分。我们知道对于刚体而言，它的质心定义为p_c=\int_V p\ dm。这一点是比较好理解的。那么\int_V \hat{p}\hat{p}^T dm是个什么含义呢？可以把这个矩阵乘开看一下：

\int_V \hat{p}\hat{p}^T dm=\left(\begin{array}{lll}
\int_V y^2 + z^2\ dm & \int_V -xy\ dm & \int_V -xz\ dm \\
\int_V -xy\ dm & \int_V x^2+z^2\ dm & \int_V -yz\ dm \\
\int_V -xz\ dm & \int_V -yz\ dm & \int_V x^2+y^2\ dm \\
\end{array}\right)

这就是书上提到的惯性张量了。因此设I=\int_V \hat{p}\hat{p}^T dm，因此，刚体合外力矩进一步化简得：

\color{red}{M=mp_c\times\ddot{p}_c + I \dot{\omega} -
m\hat{p}_c\hat{p}_c^T\dot{\omega}+
\omega \times I\omega-
\omega \times (m\hat{p}_c\hat{p}_c^T\omega)
}\\=mp_c\times\ddot{p}_c + \left(I-m\hat{p}_c\hat{p}_c^T\right)\dot{\omega}+\omega\times\left(I-m\hat{p}_c\hat{p}_c^T\right)\omega

以上就是针对任意惯性系都适用的刚体转动的欧拉方程。如果我们的惯性系选择在刚体质心上这个公式将得到大大地简化。惯性系选择在质心上意味着p_c=0。所有含质心p_c的项全都为0。因此合外力矩可以进一步简化为：

\color{red}{M=I_c \dot{\omega}+
\omega \times I_c\omega
}

这也就是为什么我们强调这个公式要求惯性系必须与质心重合的原因。因为如果不重合，许多与质心相关的项就无法消掉。

6. 总结

  这篇文章主要介绍了单刚体动力学以及动力学欧拉方程的推导过程。下一篇文章将介绍二连杆平面臂的动力学方程推导。二连杆平面臂是多连杆机器人最简洁的形式，我们将介绍速度在连杆之间的传递以及加速度的计算方式。
由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

7. 参考文献

[1]. 机器人学导论 员超

[2]. 机器人学 建模、规划与控制 西西里安诺

[3]. 仿人机器人 梶田秀司