中正平和的机器人学笔记——5. 机械臂动力学

0. 基础知识

0.1 线加速度

上一篇中我们讲到了在坐标系{A}和坐标系{B}原点重合时，${}^{B}Q$的速度矢量的表示方式：

${}^A{V}_Q$ = ${}_B^A{R}^B{V}_Q$ + ${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$ (5.1)

我们可以将左侧改写为如下形式：

$\frac{\mathrm{d}}{\mathrm{d}t}$(${}_B^A{R}^{B}Q$) = ${}_B^A{R}^B{V}_Q$ + ${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$ (5.2)

记住这种形式的方程，会在求解加速度方程时很有用。
接下来推导线加速度的公式，大家一起动手推一下吧！
对式5.1求导，可得当坐标系{A}和坐标系{B}原点重合时，${}^{B}Q$的加速度在坐标系{A}中的表达式：

${}^A{\dot{V}}_Q$ = $\frac{\mathrm{d}}{\mathrm{d}t}$(${}_B^A{R}^B{V}_Q$) + ${}^A{\dot{\Omega }}_B$ $\times$ ${}_B^A{R}^{B}Q$ + ${}^A{\Omega }_B$ $\times$ $\frac{\mathrm{d}}{\mathrm{d}t}$(${}_B^A{R}^{B}Q$) (5.3)

对式5.3右侧的第一项和最后一项代入式5.2，整理后可得，

${}^A{\dot{V}}_Q$ = ${}_B^A{R}^B{\dot{V}}_Q$ + 2${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^B{V}_Q$ + ${}^A\dot{\Omega }$ $\times$ ${}_B^A{R}^{B}Q$ + ${}^A{\Omega }_B$ $\times$ (${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$) (5.4)

将上式扩展到原点不重合的情况，就得到了一般的表达式：

${}^A{\dot{V}}_Q$ = ${}^A{\dot{V}}_{BORG}$ + ${}_B^A{R}^B{\dot{V}}_Q$ + 2${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^B{V}_Q$ + ${}^A\dot{\Omega }$ $\times$ ${}_B^A{R}^{B}Q$ + ${}^A{\Omega }_B$ $\times$ (${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$) (5.5)

另外要指出，当计算旋转关节时，${}^{B}Q$为常量，那么，

${}^B{V}_Q$ = ${}^B{\dot{V}}_Q$ = 0
因此相应地，式5.5就可以简化为：

${}^A{\dot{V}}_Q$ = ${}^A{\dot{V}}_{BORG}$ + ${}^A\dot{\Omega }$ $\times$ ${}_B^A{R}^{B}Q$ + ${}^A{\Omega }_B$ $\times$ (${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$) (5.6)

0.2 角加速度

接下来我们推导角加速度公式。假设坐标系{B}以角速度${}^A{\Omega }_B$相对于坐标系{A}转动，同时坐标系{C}以角速度${}^B{\Omega }_C$相对于坐标系{B}转动。

${}^A{\Omega }_C$ = ${}^A{\Omega }_B$ + ${}_B^A{R}^B{\Omega }_C$ (5.7)

对式5.6求导可得，

${}^A{\dot{\Omega }}_C$ = ${}^A{\dot{\Omega }}_B$ + $\frac{\mathrm{d}}{\mathrm{d}t}{(}_B^A{R}^B{\Omega }_C)$ (5.8)

同样我们将式5.2带入上式最后一项，可以得到，

${}^A{\dot{\Omega }}_C$ = ${}^A{\dot{\Omega }}_B$ + ${}_B^A{R}^B{\dot{\Omega }}_C$ + ${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^B{\Omega }_C$ (5.9)

上式用于计算机械臂连杆的角加速度。

0.3 质量分布

这里引入惯性张量（inertia tensor） 的概念，他可以表征刚体质量分布的方式，是对物体惯性矩的广义度量。给出如下3 $\times$ 3矩阵，

${}^{A}I$ = $\left(\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{xy}& I_{yy}& -I_{yz}\\ -I_{xz}& -I_{yz}& I_{zz}\end{array}\right)$

其中各个元素分别为：

$I_{xx}$ = ${\iiint }_V^{}(y^2+z^2)\rho dv$
$I_{yy}$ = ${\iiint }_V^{}(x^2+z^2)\rho dv$
$I_{zz}$ = ${\iiint }_V^{}(x^2+y^2)\rho dv$
$I_{xy}$ = ${\iiint }_V^{}xy\rho dv$
$I_{yz}$ = ${\iiint }_V^{}xz\rho dv$
$I_{zx}$ = ${\iiint }_V^{}yz\rho dv$

$I_{xx}$、$I_{yy}$、$I_{zz}$称为惯性矩（mass moments of inertia），其余三个称为惯量积(mass products of inertia)。我们在选择坐标轴时，最好使得刚体的惯量积为零，此时坐标系的轴也被称为主轴（principal axes），相应的惯量矩被称为主惯性矩（principal moments of inertia）。

1. 牛顿—欧拉递推动力学方程（Iterative Newton-Euler Dynamic Formulation）

这个方法就是假设已知关节的位置、速度、加速度（$\Theta$, $\dot{\Theta }$, $\ddot{\Theta }$)。结合前面我们铺垫过的运动学和质量分布知识，就可以计算出驱动关节运动所需要的力矩。这个算法主要包括三部分：
其一是外推法，运用运动学知识，通过对每个连杆运用牛顿-欧拉方程依次从连杆1到连杆n向外递推计算连杆的速度和加速度；
其二是运用牛顿欧拉方程，通过每个连杆质心的速度和加速度，计算作用在连杆质心上的惯性力和力矩；
其三是内推法，通过连杆间的相互作用力和力矩，从连杆n到连杆1向内迭代计算关节驱动力矩。
通常在一些教材将这个算法分为两部分：内推法和外推法。
中间运用牛顿欧拉方程有的写在内推法里，有的写在外推法里，有点混乱。比如《机器人学导论》这本书里，文字上解释的时候把牛顿欧拉方程这个算到内推法里，但是在算法中又算到外推法里，真实捉鸡。。。所以这里从我的观点来看，干脆就当作三部分得了。不过仔细想想，从算法角度的话，算到外推法里很合理，同一个循环算了速度和加速度，接着算力和力矩；但是从理解角度，把这部分算到内推法里也很合理，外推法运动学算速度和加速度，内推法算力和力矩。老铁没毛病！所以怎么理解就看大家了。。。

1.1 外推法（outwards iterations）——计算速度和加速度

上一篇雅可比矩阵部分我们已经讲过了速度的“传递”问题：

${}^{i+1}{\omega }_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\omega }_i$ + ${\dot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.10)

那么再根据式5.2，

$\frac{\mathrm{d}}{\mathrm{d}t}$(${}_B^A{R}^{B}Q$) = ${}_B^A{R}^B{V}_Q$ + ${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$ (5.2)

就可以推导出角加速度的变换公式：

${}^{i+1}{\dot{\omega }}_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\dot{\omega }}_i$ + ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ + ${}_i^{i+1}{R}^i{\omega }_i$ $\times$ ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.11_1)

如果第i+1个关节式移动关节，那么上式就可以化简为：

${}^{i+1}{\dot{\omega }}_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\dot{\omega }}_i$ (5.11_2)

接下来是推导线加速度公式，首先以免大家遗忘，再给出线速度公式，

${}^{i+1}{v}_{i+1}$ = ${}_i^{i+1}R$ $\left({}^i{v}_i{+}^i{\omega }_i{\times }^i{P}_{i+1}\right)$ (5.12)

应用式5.6如下，

${}^A{\dot{V}}_Q$ = ${}^A\dot{\Omega }$ $\times$ ${}_B^A{R}^{B}Q$ + ${}^A{\Omega }_B$ $\times$ (${}^A{\Omega }_B$ $\times$ ${}_B^A{R}^{B}Q$) (5.6)

就可以得到每个连杆坐标系原点的线加速度，

${}^{i+1}{\dot{v}}_{i+1}$ = ${}_i^{i+1}R$ $\left[{}^i{\dot{v}}_i{+}^i{\dot{\omega }}_i{\times }^i{P}_{i+1}{+}^i{\omega }_i\times \left({}^i{\omega }_i{\times }^i{P}_{i+1}\right)\right]$ (5.13_1)

如果第i+1个关节是移动关节，那么上式就应当为：

${}^{i+1}{\dot{v}}_{i+1}$ = ${}_i^{i+1}R$ $$\begin{gathered} \left[{}^i{\dot{v}}_i{+}^i{\dot{\omega }}_i{\times }^i{P}_{i+1}{+}^i{\omega }_i\times \left({}^i{\omega }_i{\times }^i{P}_{i+1}\right)\right]+2^{i+1}{\omega }_{i+1}\times {\dot{d}}_{i+1}{ \\ }^{i+1}{\hat{Z}}_{i+1}+{\ddot{d}}_{i+1}{ \\ }^{i+1}{\hat{Z}}_{i+1} \end{gathered}$$ (5.13_1)

同理我们可以得到每个连杆质心的线加速度：

${}^i{\dot{v}}_{C_i}$ = $$\begin{gathered} {}^i{\dot{\omega }}_i\times { \\ }^i{P}_{C_i}+{ \\ }^i{\omega }_i\times \left({}^i{\omega }_i\times { \\ }^i{P}_{C_I}\right){+}^i{\dot{v}}_i \end{gathered}$$ (5.14)

整理一下外推法，就是以下公式：

${}^{i+1}{\omega }_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\omega }_i$ + ${\dot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.10)

${}^{i+1}{\dot{\omega }}_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\dot{\omega }}_i$ + ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ + ${}_i^{i+1}{R}^i{\omega }_i$ $\times$ ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.11_1)

${}^{i+1}{v}_{i+1}$ = ${}_i^{i+1}R$ $\left({}^i{v}_i{+}^i{\omega }_i{\times }^i{P}_{i+1}\right)$ (5.12)

${}^{i+1}{\dot{v}}_{i+1}$ = ${}_i^{i+1}R$ $\left[{}^i{\dot{v}}_i{+}^i{\dot{\omega }}_i{\times }^i{P}_{i+1}{+}^i{\omega }_i\times \left({}^i{\omega }_i{\times }^i{P}_{i+1}\right)\right]$ (5.13_1)

${}^{i+1}{\dot{v}}_{C_{i+1}}$ = $$\begin{gathered} {}^{i+1}{\dot{\omega }}_{i+1}\times { \\ }^{i+1}{P}_{C_{i+1}}+{ \\ }^{i+1}{\omega }_{i+1}\times \left({}^{i+1}{\omega }_{i+1}\times { \\ }^{i+1}{P}_{C_{i+1}}\right){+}^{i+1}{\dot{v}}_{i+1} \end{gathered}$$ (5.14)

1.2 牛顿-欧拉方程

前面通过外推法计算出每个连杆质心的线加速度和角加速度后，运用牛顿-欧拉公式便可计算出作用在连杆质心上的惯性力和力矩：

$F_i$ = $m{\dot{v}}_{C_i}$ (5.15)

$N_i$ = $$\begin{gathered} {}^{C_i}I{\dot{\omega }}_i+{\omega }_i\times { \\ }^{C_i}I{\omega }_i \end{gathered}$$ (5.16)

式中坐标系{$C_i$}的原点位于连杆质心，各坐标系方向与原连杆坐标系{$i$}方向相同。

1.3 内推法（inward iterations）——计算力和力矩

上一节计算出每个连杆上的力和力矩后，我们就要计算出产生这些力和力矩的需要施加在关节上的关节力矩。
先规定两个符号：
$f_i$ = 连杆$i-1$作用在连杆$i$上的力；
$n_i$ = 连杆$i-1$作用在连杆$i$上的力矩。

首先我们将作用在连杆$i$上所有的力相加，得到力平衡方程：

${}^i{F}_i$ = ${}^i{f}_i{-}_{i+1}^{i}R$ ${}^{i+1}{f}_{i+1}$ （5.17）

将所有作用在质心上的力矩相加，得到力矩平衡方程：

${}^i{N}_i$ = $$\begin{gathered} {}^i{n}_i-{ \\ }^i{n}_{i+1}+\left({-}^i{P}_{C_i}\right)\times { \\ }^i{f}_i-\left({}^i{P}_{i+1}-{ \\ }^i{P}_{C_i}\right)\times { \\ }^i{f}_{i+1} \end{gathered}$$ (5.18)

利用力平衡方程和附加旋转矩阵的办法，可以将上式化简为

${}^i{N}_i$ = $$\begin{gathered} {}^i{n}_i-{ \\ }_{i+1}^{i}R{ \\ }^i{n}_{i+1}-{ \\ }^i{P}_{C_i}\times { \\ }^i{F}_i{-}^i{P}_{i+1}\times { \\ }_{i+1}^{i}R \end{gathered}$$ ${}^i{f}_{i+1}$ (5.19)

最后我们排列一下力和力矩方程，让相邻连杆按照序号大的向序号小的顺序（也就是内推）来迭代：

${}^i{f}_i$ = $$\begin{gathered} {}^i{F}_i+{ \\ }_{i+1}^{i}R \end{gathered}$$ ${}^{i+1}{f}_{i+1}$ （5.20）

${}^i{n}_i$ = $$\begin{gathered} {}^i{N}_i+{ \\ }_{i+1}^{i}R{ \\ }^i{n}_{i+1}+{ \\ }^i{P}_{C_i}\times { \\ }^i{F}_i+{ \\ }^i{P}_{i+1}\times { \\ }_{i+1}^{i}R \end{gathered}$$ ${}^i{f}_{i+1}$ (5.21)

那么怎么求得关节力矩呢？
在静力学中，可以通过计算一个连杆施加于相邻连杆的力矩在$\hat{Z}$方向上分量求得：

${\tau }_i$ = $$\begin{gathered} {}^i{n}_i{ \\ }^{Ti}{\hat{Z}}_i \end{gathered}$$ (5.22)

如果是移动关节，则有
${\tau }_i$ = $$\begin{gathered} {}^i{f}_i{ \\ }^{Ti}{\hat{Z}}_i \end{gathered}$$ (5.23)

式5.23和式5.24中的$\tau$表示线性驱动力。

1.4 牛顿-欧拉递推动力学算法

我们把前面三节推导的结果整理以下，就是这个算法啦！
外推：$i:0\to 5$ (把牛顿欧拉方程放到外推法里)

${}^{i+1}{\omega }_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\omega }_i$ + ${\dot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.10)

${}^{i+1}{\dot{\omega }}_{i+1}$ = ${}_i^{i+1}R$ ${}^i{\dot{\omega }}_i$ + ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ + ${}_i^{i+1}{R}^i{\omega }_i$ $\times$ ${\ddot{\theta }}_{i+1}$ ${}^{i+1}{\hat{Z}}_{i+1}$ (5.11_1)

${}^{i+1}{v}_{i+1}$ = ${}_i^{i+1}R$ $\left({}^i{v}_i{+}^i{\omega }_i{\times }^i{P}_{i+1}\right)$ (5.12)

${}^{i+1}{\dot{v}}_{i+1}$ = ${}_i^{i+1}R$ $\left[{}^i{\dot{v}}_i{+}^i{\dot{\omega }}_i{\times }^i{P}_{i+1}{+}^i{\omega }_i\times \left({}^i{\omega }_i{\times }^i{P}_{i+1}\right)\right]$ (5.13_1)

${}^{i+1}{\dot{v}}_{C_{i+1}}$ = $$\begin{gathered} {}^{i+1}{\dot{\omega }}_{i+1}\times { \\ }^{i+1}{P}_{C_{i+1}}+{ \\ }^{i+1}{\omega }_{i+1}\times \left({}^{i+1}{\omega }_{i+1}\times { \\ }^{i+1}{P}_{C_{i+1}}\right){+}^{i+1}{\dot{v}}_{i+1} \end{gathered}$$ (5.14)

${}^{i+1}{F}_{i+1}$ = $$\begin{gathered} m_{i+1}{ \\ }^{i+1}{\dot{v}}_{C_{i+1}} \end{gathered}$$ (5.15)

${}^{i+1}{N}_{i+1}$ = $$\begin{gathered} {}^{C_{i+1}}{I}_{i+1}{ \\ }^{i+1}{\dot{\omega }}_{i+1}+{ \\ }^{i+1}{\omega }_{i+1}\times { \\ }^{C_{i+1}}I{ \\ }^{i+1}{\omega }_{i+1} \end{gathered}$$ (5.16)

内推：$i:6\to 1$（注意序号）

${}^i{f}_i$ = $$\begin{gathered} {}^i{F}_i+{ \\ }_{i+1}^{i}R \end{gathered}$$ ${}^{i+1}{f}_{i+1}$ （5.20）

${}^i{n}_i$ = $$\begin{gathered} {}^i{N}_i+{ \\ }_{i+1}^{i}R{ \\ }^i{n}_{i+1}+{ \\ }^i{P}_{C_i}\times { \\ }^i{F}_i+{ \\ }^i{P}_{i+1}\times { \\ }_{i+1}^{i}R \end{gathered}$$ ${}^i{f}_{i+1}$ (5.21)

${\tau }_i$ = $$\begin{gathered} {}^i{n}_i{ \\ }^{Ti}{\hat{Z}}_i \end{gathered}$$ (5.22)

1.5 总结与补充

1.5.1 考虑重力的动力学算法

如果要考虑重力的话，只需要令${}^0{\dot{v}}_0$ = $G$即可。

1.5.2 动力学方程的结构

用牛顿-欧拉方程对机械臂进行分析时，动力学方程可以表示为如下形式

$\tau$ = $M\left(\Theta \right)\ddot{\Theta }+V\left(\Theta ,\dot{\Theta }\right)+G\left(\Theta \right)$ (5.23)

式中$M\left(\Theta \right)$为机械臂的$n\times n$ 质量矩阵，$V\left(\Theta ,\dot{\Theta }\right)$是$n\times 1$的离心力和哥氏力矢量，$G\left(\Theta \right)$ 是$n\times 1$的重力矢量。质量矩阵可分为四块，主对角线的两块为有效惯量，副对角线上的两块为耦合惯量；哥氏力部分在高速时影响较为显著。
其实上面这个式子对于学过自控原理的同学应该比较熟悉，就是状态空间表达式。

1.5.3总结：

牛顿欧拉法逻辑还是很严密的，如果跟着一起推导的话，理解起来并不是太难，就怕一上来看到算法的一大坨公式就第一印象觉得很难，这样的心态在学习新知识的时候是最要不得的。说起来，刚学完运动学部分觉得很简单，然后甫一接触动力学部分，看到成片公式也举得有点烦躁。但是如我所说，一条条自己推导，结合实际去理解也就过去了。学习本就不是一件容易的事，在这个过程里体会数学和力学的魅力岂不美哉（Or2）。。。

2. 拉格朗日动力学方程（Lagrangian Dynamic Formulation）

牛顿-欧拉方法是建立在动力学基本公式以及作用在连杆之间的约束力和力矩之上的。而另一种方法，也就是这一节要讲的拉格朗日动力学方程法，则是从能量角度分析的，当然，最终两种分析方法得到的结果应该是相同的。

2.1 动能方程

首先分析第$i$根连杆的动能$k_i$，可以表示为

$k_i$ = $\frac{1}{2}{m}_i{v}_{C_i}^T{v}_{C_i}+{\frac{1}{2}}^i{\omega }_i^T{}^{C_i}{I}_i{}^i{\omega }_i$ (5.24)

式中第一项是基于连杆质心线速度的动能，第二项是连杆的角速度的动能。机械臂的动能是所有连杆动能之和，亦即

$k$ = ${\sum }_{i=1}^n{k}_i$ (5.25)

式5.24中的$v_{C_i}$和${}^i{\omega }_i$是$\Theta$和$\dot{\Theta }$的函数，由此我们可知机械臂的动能可以表示为关节位置和关节速度的标量函数。事实上也确实如此，机械臂的动能可以写成

$k\left(\Theta ,\dot{\Theta }\right)$ = $\frac{1}{2}{\dot{\Theta }}^{T}M\dot{\Theta }$ (5.26)

2.2 势能方程

接下来我们考虑连杆的势能。第$i$根连杆的势能$u_i$可以表示为

$u_i$ = $-m_i{}^0{g}^T{}^0{P}_{C_i}+u_{ref}$ (5.26)

注意这里的${}^{0}g$是$3\times 1$的重力矢量，${}^0{P}_{C_i}$位于第$i$个连杆质心的矢量。

机械臂的总势能为各个连杆势能之和，即

$u$ = ${\sum }_{i=1}^n{u}_i$ (5.27)

2.3 拉格朗日函数

拉格朗日动力学公式为我们提供了一种利用标量函数来推导动力学方程的方法，这个标量函数就是拉格朗日函数，亦即一个机械系统的动能和势能之差，它可以表示为

$\mathcal{L}\left(\Theta ,\dot{\Theta }\right)$ = $k\left(\Theta ,\dot{\Theta }\right)-u\left(\Theta ,\dot{\Theta }\right)$ (5.28)

则机械臂的运动方程为

$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{\Theta }}-\frac{\partial \mathcal{L}}{\partial \Theta }=\tau$ (5.29)

进一步得到

$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial k}{\partial \dot{\Theta }}-\frac{\partial k}{\partial \Theta }+\frac{\partial u}{\partial \Theta }=\tau$ (5.30)

2.4 总结

以上就是机械臂动力学的拉格朗日方程。当时自己学完这一部分突然联想到，高中做力学物理题的时候（就是那些小物块，斜面，圆坑一类的）往往按受力分析去做会很麻烦，但是如果用能量守恒的动能势能方程做就很简单。这一点在本章分析机械臂的动力学方程也是这样，算是一点挺有趣的事情。

3. 总结

动力学部分终于也总结完了。相比运动学部分来说确实更有难度一点，但是还是那句话，自己多手算多推导，多联想。这一部分耗费心力太多，到现在已经懒于举一个实例或者MATLAB代码了。另外一个收获，就是这几天上手了一点LaTex输入公式的技巧，算是意外收获吧。