1. 引言

  前面的一系列文章已经把牛顿欧拉法建立机器人动力学模型的原理讲清楚了。这一篇文章的重点在于实践，主要介绍一下在给定机器人模型之后如何推导其动力学方程的表达式。需要注意的是由于本篇文章大量使用了上一篇文章推导的公式，如果你希望了解公式推导的细节，请参考5. 机器人动力学—-串联机构牛顿欧拉方程。

2. 二连杆平面臂

  为了便于说明我们选择二连杆平面臂作为研究对象。选择这个结构其实是出于两方面的考虑：
1.简单：机器人的动力学方程具有十分复杂的形式，如果直接推导六轴机器人的动力学方程恐怕需要用一个文本文件来存储这个表达式的结果；
2.扼要：二连杆平面臂的动力学方程推导过程具有普遍意义。也就是说如果你能够写出二连杆平面臂的动力学推导代码，那么扩展到多连杆是十分容易的事情。

2.1 建立坐标系

  按照上一篇文章提到的，我们利用改进DH法建立二连杆平面臂的连杆坐标系，如下图1所示。

图1. 二连杆平面臂建系

符号约定：
x_0y_0z_0：基坐标系
x_1y_1z_1：连杆1坐标系（link 1 frame）
x_2y_2z_2：连杆2坐标系（link 2 frame）
x_ey_ez_e：末端工具坐标系（end effector frame），末端执行器坐标系是固定在连杆2的末端的
l_1：连杆1的长度
l_2：连杆2的长度

2.2 改进DH参数表

  改进DH坐标系建立完成后，DH参数表就随之确定下来了。如果你还不太了解如何建立DH坐标系以及改进DH坐标系，可以参考前面运动学相关的文章8. 机器人正运动学—-DH坐标系建立技巧，9. 机器人正运动学—-修改DH参数。图1中二连杆平面臂的改进DH参数表如下表1所示。

表1. 二连杆平面臂改进DH参数

3. 前向递推与反向递推

  在5. 机器人动力学—-串联机构牛顿欧拉方程这篇文章中我们已经介绍了牛顿欧拉方程背后的原理以及前向递推和反向递推的公式，公式的具体形式可以参考文中的3.1.5及3.2。这里应用文中的结论进行二连杆平面臂的前向递推和反向递推。

3.1 速度的前向递推

  速度的前向递推是从机器人的基坐标系开始依次计算每一个连杆的质心速度和角速度。

3.1.1 基座到连杆1的速度递推

  对于二连杆平面臂来说基座是固定的，因此速度和角速度为0，也就是说：
\omega_0^0=0，\dot{P}_0^0=0。由于关节1是旋转关节，因此从基座到连杆1角速度的传递关系为：
\omega_1^1=R_0^1\omega_0^0+\dot{q}_1 z_1^1
解释一下各个变量的含义：\omega_1^1表示的是连杆1的角速度在连杆1坐标系下的表达；R_0^1是把向量从基座坐标系转换到连杆1坐标系的旋转矩阵（依据DH参数求解）；\dot{q}_1代表的是关节1旋转的角速度大小（这是一个标量）；z_1^1是连杆1坐标系的z轴在连杆1坐标系下的表达，这其实是一个常量即z_1^1=\begin{pmatrix}0&0&1\end{pmatrix}^T
  根据速度递推公式，可以得到从基座到连杆1速度的传递关系为：
\dot{P}_1^1=R_0^1\left(\dot{P}_0^0+\omega_0^0\times r_{0,1}^0\right)
解释一下各个变量的含义：\dot{P}_1^1表示的是连杆1坐标系原点的线速度在连杆1坐标系下的表达；r_{0,1}^0表示的是基座坐标系原点指向连杆1坐标系原点的矢量在基座坐标系下的表示（在二连杆平面臂中基坐标系和连杆1坐标系原点是重合的，因此这是一个零向量）。
  因为我们要求的是质心的线速度和角速度在质心坐标系下的表达（前面文章已经提过，为了方便，质心坐标系与连杆坐标系朝向相同，只是质心坐标系的原点位于连杆的质心处而连杆坐标系的原点位于连杆的驱动轴上），而刚体上任意一点的角速度是相等的，因此：
\omega_{c_1}^{c_1}=\omega_1^1
刚体上任意两个点之间的速度关系上一篇也总结了，如下：
\dot{P}_{c_1}^{c_1}=\dot{P}_{1}^{1} + \omega_1^1\times r_{1,c_1}^1
解释一下各个变量的含义：\dot{P}_{c_1}^{c_1}表示的是连杆1质心的线速度在质心坐标系下的表达（由于我们规定质心坐标系和连杆坐标系方向相同，因此\dot{P}_{c_1}^{c_1}=\dot{P}_{c_1}^1）；r_{1,c_1}^1描述的是连杆1坐标系原点到连杆1质心的向量在连杆1坐标系下的表达。这个量画个图描述一下就十分明了了，如下图2所示。

图2. 关节指向质心的矢量

连杆1的坐标系是\{x_1y_1z_1\}，我们认为连杆1的质心位于连杆1的几何中心，很显然连杆1的坐标系原点指向质心的向量与连杆1坐标系的x轴是重合的！因此：
r_{1,c_1}^1=\begin{pmatrix}\frac{l_1}{2}&0&0\end{pmatrix}^T

3.1.2 连杆1到连杆2的速度递推

  这个过程其实和上面一个是重复的，不过还是需要把它写出来以便体会这个递推的过程。经过前一步的推导，\omega_1^1；\dot{P}_1^1变成了已知量。在计算连杆2的线速度和角速度时我们就可以利用连杆1的线速度和角速度了。由于连杆2同样是旋转关节，因此从连杆1到连杆2角速度的传递关系为：
\omega_2^2=R_1^2\omega_1^1+\dot{q}_2z_2^2
这里再强调一下所谓z_2^2或者更普遍地z_i^i表达的是坐标系\{O_i\}的z轴在其自身坐标系下的投影，所以这是一个定值z_i^i=\begin{pmatrix}0&0&1\end{pmatrix}^T。R_1^2是把向量从连杆1坐标系转换到连杆2坐标系的旋转矩阵；\dot{q}_2代表的是关节2旋转的角速度大小（这是一个标量）。
  根据速度递推公式，可以得到从连杆1到连杆2速度的传递关系为：
\dot{P}_2^2=R_1^2\left(\dot{P}_1^1+\omega_1^1\times r_{1,2}^1\right)
这里重点提一下r_{1,2}^1的含义，这是一个矢量，起点为连杆1坐标系的原点，终点为连杆2坐标系的原点，同时这个矢量是在连杆1坐标系下描述的。因此这个矢量其实是一个常量r_{1,2}^1=\begin{pmatrix}l_1 & 0 & 0\end{pmatrix}^T
  同样因为我们要求的是质心的线速度和角速度在质心坐标系下的表达，刚体上任意一点的角速度是相等的，因此：
\omega_{c_2}^{c_2}=\omega_2^2
连杆2坐标系原点和连杆2质心的速度关系如下：
\dot{P}_{c_2}^{c_2}=\dot{P}_{2}^{2} + \omega_2^2\times r_{2,c_2}^2
这里同样再强调一下，r_{2,c_2}^2表达的是一个矢量，起点为连杆2坐标系的原点，终点为连杆2的质心，同时这个矢量是在连杆2坐标系下描述的。因此这个矢量其实是一个常量r_{2,c_2}^2=\begin{pmatrix}\frac{l_2}{2} & 0 & 0\end{pmatrix}^T。
  你可以和3.1.1比较一下，这里陈述的内容几乎完全一致，这是递归计算带来的一大好处。这种形式对于编程实现非常友好。另一个好处就是因为我们在连杆质心坐标系下去表达相应的物理量，很多物理量成为了常量！

3.2 加速度的前向递推

3.2.1 基座到连杆1的加速度递推

  机器人的基座角加速度为0，即\dot{\omega}_0^0=0，为了简化重力的表达，我们用加速度场来取代重力，即认为机器人的基座以-g的加速度在运动，那么\ddot{P}_0^0=-g。对于我们这里介绍的二连杆平面臂来说，重力加速度是沿着基坐标系的z轴向下的。因此g=\begin{pmatrix}0 & 0 & -9.81\end{pmatrix}^T。
  根据前一篇文章总结的公式，从基座到连杆1的角加速度传递公式如下：
\dot{\omega}_1^1=R_0^1\dot{\omega}_0^0+\left(R_0^1\omega_0^0\right)\times \dot{q}_1 z_1^1 + \ddot{q}_1 z_1^1
  从基座到连杆1的线加速度传递公式如下：
\ddot{P}_1^1=R_0^1\left(\ddot{P}_0^0 + \dot{\omega}_0^0\times r_{0,1}^0 + \omega_0^0\times \left(\omega_0^0\times r_{0,1}^0\right)\right)
同样我们需要求解的是连杆1质心的角加速度和线加速度在质心坐标系下的表达，因为刚体上任意一点的角速度是相等的，因此角加速度也是相等的：
\dot{\omega}_{c_1}^{c_1}=\dot{\omega}_1^1
连杆1的坐标系原点与质心的加速度关系为：
\ddot{P}_{c_1}^{c_1}=\ddot{P}_1^1+\dot{\omega}_1^1\times r_{1,c_1}^1 + \omega_1^1\times\left(\omega_1^1\times r_{1,c_1}^1\right)

3.2.2 连杆1到连杆2的加速度递推

  这里的递推过程和3.2.1同样是重复的，从连杆1到连杆2的角加速度传递公式如下：
\dot{\omega}_2^2=R_1^2\dot{\omega}_1^1+\left(R_1^2\omega_1^1\right)\times \dot{q}_2 z_2^2 + \ddot{q}_2z_2^2
  从连杆1到连杆2的线加速度传递公式如下：
\ddot{P}_2^2=R_1^2\left(\ddot{P}_1^1 + \dot{\omega}_1^1\times r_{1,2}^1 + \omega_1^1\times\left(\omega_1^1\times r_{1,2}^1\right)\right)
同样我们需要求解连杆2质心的角加速度和线加速度在质心坐标系下的表达，则连杆2质心角加速度为：
\dot{\omega}_{c_2}^{c_2}=\dot{\omega}_2^2
连杆2的坐标系原点与质心的加速度关系为：
\ddot{P}_{c_2}^{c_2}=\ddot{P}_2^2+\dot{\omega}_2^2\times r_{2,c_2}^2 + \omega_2^2\times\left(\omega_2^2\times r_{2,c_2}^2\right)

3.3 反向递推

  通过前面的3.1和3.2我们已经把二连杆平面臂的每一个连杆质心处的线加速度和角加速度都求出来了，接下来就可以进行反向递推来求解每一个关节输出的力矩了。

3.3.1 关节2的扭矩

  我们认为二连杆平面臂没有受到任何的外力，根据上一篇文章的公式，连杆2的牛顿方程具有以下形式：
m_2\ddot{P}_{c_2}^{c_2}=f_2^2
这里着重解释一下f_2^2，它代表的是关节2施加给连杆2的力在连杆2质心坐标系下的表达（请务必时刻牢记连杆坐标系和连杆质心坐标系方向是相同的，因此不管是在连杆坐标系下还是在连杆质心坐标系下力和力矩数值上都是一样的）。根据牛顿方程我们可以解出f_2^2
  连杆2的欧拉方程具备以下形式：
I_{c_2}^{c_2}\dot{\omega}_{c_2}^{c_2} + \omega_{c_2}^{c_2}\times I_{c_2}^{c_2}\omega_{c_2}^{c_2}=\mu_2^2-r_{2,c_2}^2\times f_2^2
根据欧拉方程可以解出\mu_2^2，它代表的是关节2施加给连杆2的力矩在连杆2质心坐标系（或者连杆2坐标系）下的表达。
  由于关节2是旋转关节，因此这里的主动力只有关节2的电机施加给连杆2的扭矩，并且这个扭矩还必须和关节2电机的轴向相同，所以，关节2电机输出的扭矩如下：
\tau_2=\left(\mu_2^{2}\right)^T z_2^2

3.3.2 关节1的扭矩

  关节2施加给连杆2的力和力矩分别为f_2^2和\mu_2^2。根据牛顿第三定律，我们不难想象连杆2通过关节2施加给连杆1的反作用力和反作用力矩分别为-f_2^2和-\mu_2^2。不过目前这两个物理量都是在连杆2坐标系下表达的，因此在计算连杆1的牛顿方程和欧拉方程时需要把这两个物理量通过旋转矩阵先变换到连杆1的质心坐标系下。
  连杆1的牛顿方程具有以下形式：
m_1\ddot{P}_{c_1}^{c_1}=f_1^1-R_2^1f_2^2
其中R_2^1f_2^2代表的就是将关节2施加给连杆2的力在连杆1质心坐标系（或者连杆坐标系）下表示出来也就是说f_2^1=R_2^1f_2^2。根据牛顿方程可以求解出f_1^1。
  连杆1的欧拉方程具有以下形式：
I_{c_1}^{c_1}\dot{\omega}_{c_1}^{c_1}+\omega_{c_1}^{c_1}\times I_{c_1}^{c_1}\omega_{c_1}^{c_1}=\mu_1^1-\mu_{2}^1+\left(-r_{1,c_1}^1\right)\times f_1^1+\left(-r_{2,c_1}^1\right)\times\left(-f_2^1\right)
其中\mu_2^1=R_2^1\mu_2^2代表的是关节2施加给连杆2的力矩在连杆1质心坐标系下的表达。根据欧拉方程可以求解出\mu_1^1即关节1施加给连杆1的力矩在连杆1坐标系下的表示。
  由于二连杆平面臂的关节1也是旋转关节，因此这里的主动力只有关节1的电机施加给连杆1的扭矩，即：
\tau_1=\left(\mu_1^{1}\right)^T z_1^1
  就像我们之前提过的，关节施加给连杆的其他的力和力矩全都是机器人的结构件支撑得到的。

4. 总结

  以上我们详细介绍了一下二连杆平面臂动力学计算时的前向递推和反向递推，你会发现以这种递推的形式进行计算时每一步递推都是重复的，因此二连杆的递推过程已经完全能够说明问题。编程时我们只需要写一个循环就可以完成前向递推，前向递推结束后再来一个循环就可以实现反向递推。因此牛顿欧拉法对于编程是非常友好的。下一篇文章我们将介绍二连杆平面臂动力学推导的代码实现。
  由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

5. 参考文献

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

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