1. 引言

  经过前面一系列的铺垫相信各位已经对动力学推导的拉格朗日法及牛顿欧拉法有了初步的认识，从这篇文章开始终于可以正式介绍一下机器人的动力学方程推导了。本篇详细介绍串联机构牛顿欧拉方程推导背后的原理。

2. 几个关键问题

  不知道你是否思考过我们说一个知识很难究竟是难在了哪里，这里我的观点是抽象性的提高和直觉性的丧失。我们说一个问题很难实际上无外乎这两个方面。因此教程或者博客的目的在于降低问题的抽象性，在已有直觉的基础上建立更为高级的直觉。

2.1 建系约定

  连杆坐标系以及质心坐标系的建立是机器人动力学推导的基础。连杆坐标系的建立方式通常有两种，一种是标准DH法，一种是改进DH法。这两种建系方式在机器人运动学的部分已经介绍过了，如果你不太了解可以回看一下这两篇文章：7. 机器人正运动学—-连杆坐标系与DH参数，9. 机器人正运动学—-修改DH参数。
  这两种不同的建系方式将导致动力学的迭代公式不同，所以一定要注意。西西里安诺的《机器人学 建模、规划与控制》动力学的推导用的是标准DH建系，员超的《机器人学导论》动力学的推导用的是改进DH建系。
  我倾向使用改进DH建立坐标系，因为改进DH相对于标准DH有进一步的优势，具体的可以参考前面两篇文章。因此在建立连杆坐标系的时候我们选择改进DH法，上一篇文章提到只有在质心惯性系下描述机器人的动力学才能使用欧拉方程的简单形式。因此除了建立连杆坐标系之外还需要在对应的连杆上建立与质心重合的质心坐标系以便我们在对特定连杆应用牛顿方程和欧拉方程时所涉及到的如速度，角速度，加速度，角加速度等能够在连杆的质心坐标系下表达！为了方便，质心坐标系各个轴的朝向与连杆坐标系保持一致，也就是两者之间只存在平移关系。总结一下就是机器人的每一个连杆都建立两个与之固定的坐标系，一个是连杆坐标系，固连在连杆的驱动轴上；另一个是质心坐标系，固连在连杆的质心上。啰嗦这么多是因为在机器人动力学中坐标系非常多，理解变量在各个坐标系之间的传递对于真正理解动力学至关重要，所以坐标系的建立原则请务必搞明白。还是单拿出一个连杆，在连杆上建立的坐标系以及相关的一些参量如下图1所示。这里需要注意的是图1中所有的变量都是在基坐标系下描述的，另外连杆i的连杆坐标系\{O_i\}和连杆i的质心坐标系\{O_{c_i}\}朝向相同，仅原点位置不同。

图1. 连杆坐标系及相关参量

参量含义：
P_i：连杆i坐标系原点

P_{c_i}：连杆i质心坐标系原点

P_{i+1}：连杆i+1坐标系原点（改进DH参数中连杆i+1的坐标系建立在连杆i的传动轴上，同时也是连杆i+1的驱动轴上）

r_{i,c_i}：连杆i坐标系原点到连杆i质心的向量

r_{i+1,c_i}：连杆i+1坐标系原点到连杆i质心的向量

r_{i,i+1}：连杆i坐标系原点到连杆i+1坐标系原点的向量

f_i：连杆i的前向连杆（即连杆i-1）施加给连杆i的力

\mu_{i}：连杆i的前向连杆施加给连杆i的力矩

f_{c_i}：连杆i质心处所受合外力

\mu_{c_i}：连杆i质心处所受合外力矩

f_{i+1}：连杆i+1的前向连杆（即连杆i）施加给连杆i+1的力（图1中之所以有负号是因为连杆i施加给连杆i+1的力为f_{i+1}，那么根据牛顿第三定律即作用力与反作用力定律，连杆i受到连杆i+1的反作用力为-f_{i+1}）

\mu_{i+1}：连杆i+1的前向连杆施加给连杆i+1的力矩

2.2 关于惯性系

  伽利略相对性原理指出力学定律对所有惯性系都是相同的。后来爱因斯坦扩展了这条定律即所有物理定律在惯性系下都是相同的。言外之意就是所有的惯性系平权（你真的理解这句话的含义了吗？）。什么是惯性系？静止或者匀速直线运动的坐标系（加速度为零），这同时说明如果一个坐标系不是惯性系那么我们就不能在这个坐标系下进行常规物理学现象的推演！
  回到机器人动力学，不知道你是否有这样的疑问，机器人在运动的过程中无论是连杆坐标系还是质心坐标系不可能加速度为零呀，那么为什么我们可以在每一个连杆的质心坐标系下计算这个连杆的动力学方程呢？
  我也曾经在这个问题上困惑了好久，后来接触得多了才慢慢体会到其中的道理。其实关于惯性系的问题有一个很好的思维框架。惯性系的定义告诉我们所有静止的坐标系都是惯性系，那么有多少这样的惯性系呢？答案是无数个！而且这些静止的惯性系可以位于三维空间中的任意位置，拥有任意的姿态，他们充满了我们生活的三维空间！这是思考的第一步。机器人的连杆坐标系和质心坐标系虽然是随着机器人的运动而运动的（显然不是惯性系），但是机器人运动的任意时刻一定存在与连杆坐标系和质心坐标系重合的惯性系（连杆时刻在运动因此与连杆坐标系和质心坐标系重合的惯性系在每个时刻也都不同）。这是第二步。我们所谓的在连杆质心坐标系下进行动力学计算实际上是在与连杆质心坐标系重合的那个惯性系下进行的！这是第三步。

2.3 为什么要递推

关于为什么要用递推式（而不是所有量都在基坐标系下计算）来求解每个连杆的速度、加速度等物理量起码有三个原因：
1.惯性张量在连杆质心惯性系中表达为常量
2.质心惯性系下欧拉方程拥有简单形式（上一篇文章已经介绍过了）
3.速度满足叠加原理，递推求解速度更容易

2.3.1惯性张量的理解

  高中物理中你应该听说过惯性这个词。惯性描述的是物体保持原有运动状态的能力（或者描述的是改变物体运动状态的难易程度）。惯性越大那么它的运动状态就越难以改变。对于质点来说惯性只跟它的质量有关系。想象一下质量越大的物体是不是越难以改变运动状态呢？
  对于一个质量没有集中在一点的刚体来说，惯性描述的应该是改变质心运动的难易程度但是不能描述让刚体发生转动的难易程度，描述改变刚体转动状态难易程度的量就是所谓的惯性张量了。惯性张量描述了刚体在惯性系下的质量分布特性。我们把它的表达式拿来分析一下：

I=\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)

单独看其中的每一项都是对质量微元的积分得到的。其中的x,y,z代表的是质量微元在惯性系下的坐标。因此当我们选择一个与刚体固连的坐标系作为惯性系时，刚体的惯性张量成为一个定值！随便选一个非对角线元素分析一下如第1行第2列\int_V -xy\ dm，如果刚体在惯性系yoz平面内具有对称性（所谓对称性是指如果在p=\left(x,y,z\right)处存在质量微元m_1，那么在p=\left(-x,y,z\right)处也存在同样的质量微元m_1）那么这个积分最终结果将是0（因为\int_V -xy\ dm=\int_V ((-xy)+(-(-x)y))\ dm=\int_V 0\ dm=0）。也就是说如果这个刚体相对于惯性系是完全对称的，那么所有非对角线元素都将变成0！
  再来看主对角线元素，还是任选一个比如\int_V x^2 + y^2\ dm，x^2 + y^2其实描述的是一个距离，这个距离是所有质量微元到惯性系z轴的垂直距离！举个例子，一个可以视为质点的小球位于xoy平面内，绕着z轴作定轴转动，如下图2所示。其中ox_ry_rz_r表示的是与质点固连的一个坐标系，质点位于这个坐标系的x轴上。oxyz代表的是世界坐标系。在初始时刻这两个坐标系是重合的，经过一段时间的旋转后得到了图2的场景。

图2. 质点绕z轴旋转

  那么它的角动量在世界系下可以描述为：

L_1=\vec{r}\times m\vec{v}=\vec{r}\times m \left(\vec{\omega} \times \vec{r}\right)=mr^2\begin{pmatrix}
0\\
0\\
\omega
\end{pmatrix}=J_1\vec{\omega}

其中|r|^2=x^2+y^2，你会发现这和我们前面得出的结论一致。J_1代表的是质点绕z轴旋转的转动惯量，质点所受合外力矩为：

N=J_1\dot{\omega}=J_1\alpha

质点距离z轴越远，转动惯量也就越大，因此产生同样的角加速度所需要的合外力矩越大。这也就意味着质点运动状态改变越困难！这一点也印证了前面提到的惯性张量描述了刚体转动状态改变的难易程度。

2.3.2 速度的依赖关系

  高中物理我们就学到了一个关于速度非常有用的知识：速度是矢量，满足叠加原理，绝对速度=相对速度+牵连速度。这一点不只适用于线速度，角速度同样适用。举个课本上曾经出现的例子，如下图3所示.

图3. 转动的圆盘和沿着圆盘直径运动的小球

一个圆盘绕着z轴以角速度\omega旋转。圆盘上有一个小球，小球当前位置矢量是r。小球沿着圆盘的某一直径向外运动，速度为v。那么小球相对于惯性系的速度是多少呢？这就是相对速度和牵连速度一个非常经典的例子。显然v是小球相对于圆盘的速度，因此这是相对速度。那么牵连速度是什么呢？就是小球与圆盘接触点的速度v_b=\omega\times r。因此小球相对于惯性系的速度为：

v_a=v_b+v=\omega\times r+v

3 前向递推与反向递推

  大家都知道牛顿欧拉法求解机器人动力学方程可以分为两个过程：
1.前向递推用于求解各个连杆的速度，加速度，角速度，角加速度
2.反向递推根据牛顿方程和欧拉方程求解各个连杆受到的外力及外力矩并得到各个关节的驱动力矩

3.1 速度与加速度的前向递推

  所谓前向递推是指从基座开始向末端逐次递推，递推的每一步都是根据当前连杆的速度，加速度等推导下一个连杆的速度和加速度。这样在前向递推结束之后所有连杆的速度和加速度信息就都可以获取到了。
  运动状态（即位置，速度，加速度）之所以需要前向递推获得也是因为前面说的：绝对速度=相对速度+牵连速度。要想求出某个连杆i的速度，需要知道它的前向连杆i-1的速度（牵连速度）以及连杆i相对于连杆i-1的相对速度。所以速度和加速度的递推必须通过前向递推得到。因为这些物理量是“前向依赖”的。

3.1.1 旋转关节速度传递

  下图4用于描述旋转关节从连杆i到连杆i+1速度传递过程（注意图中所有的量都是在基坐标系下描述的）。

图4. 旋转关节的速度传递

前面提到，串联机构速度的传递背后的原理其实是速度叠加原理，对应到图4中也就是说我们可以依据连杆i的速度（牵连速度）以及连杆i+1相对于连杆i的速度（相对速度）求出连杆i+1的速度。
  连杆i原点O_i的线速度为\dot{P}_i，连杆i的角速度为\omega_i（需要注意的是虽然刚体上任意一点的速度不一定相等，但是刚体上任意一点的角速度是相等的，所以说角速度的时候我们只说刚体，不说刚体上的某个点）。
  从图4中可以看出，对于旋转关节，原点O_{i+1}相对于点O_i的相对速度其实就是连杆i以角速度\omega_i旋转引起了点O_{i+1}绕点O_i的“旋转速度”，因此相对速度为：

v=\omega \times r=\omega_i \times r_{i,i+1}

所以，对于旋转关节，从连杆i到连杆i+1的线速度传递关系为：

\dot{P}_{i+1}=\dot{P}_i + \omega_i \times r_{i,i+1}

那么旋转关节的角速度是怎么传递的呢？其实也不复杂，前面提到刚体上任意一点的角速度是相等的，这就意味着连杆i上与O_{i+1}重合的那个点角速度为\omega_i，那么在连杆i+1上与O_{i+1}重合的点的角速度实际上叠加了关节i+1的旋转角速度，这个角速度大小为\dot{\theta}_{i+1}，方向为z_{i+1}。因此对旋转关节来说从连杆i到连杆i+1的角速度传递关系为：

\omega_{i+1}=\omega_i + \dot{\theta}_{i+1}z_{i+1}

3.1.2 平移关节速度传递

  下图5描述的是平动关节从连杆i到连杆i+1速度传递过程（注意图中所有的量都是在基坐标系下描述的）。

图5. 平移关节的速度传递

由于连杆i+1相对于连杆i的传动轴只有平移没有旋转，因此两者的角速度保持一致：

\omega_{i+1}=\omega_i

线速度稍微复杂一些，它其实由三部分构成，为了便于理解，我们需要在连杆i上先确定一个“牵连点”，如下图6所示。为了直观这里略去了不必要的一些变量。

图6. 牵连点的定义

这里我们选择连杆i上与点O_{i+1}重合的点作为牵连点。需要特别注意的是这个牵连点与连杆i是固连的，那么牵连点的线速度为：

v_b=\dot{P}_i+\omega_i\times r_{i,i+1}

连杆i+1相对于牵连点的速度非常简单，就是沿着z_{i+1}，大小为\dot{d}_{i+1}的速度平移。因此连杆i到连杆i+1的速度传递关系为：

\dot{P}_{i+1}=\dot{P}_i + \omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}

总结一下从连杆i到连杆i+1的角速度传递关系为：

\omega_{i+1}=\left\{
\begin{aligned}
\omega_i& + \dot{\theta}_{i+1}z_{i+1}\ \ rotation\\
\omega_i&\ \ prismatic
\end{aligned}
\right.

从连杆i到连杆i+1的线速度传递关系为：

\dot{P}_{i+1}=\left\{
\begin{aligned}
\dot{P}_i& + \omega_i \times r_{i,i+1}\ \ rotation \\
\dot{P}_i& + \omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}\ \ prismatic
\end{aligned}
\right.

3.1.3 速度变换到质心

  前面介绍的角速度和速度还有他们之间的传递实际上描述的都是各个连杆坐标系原点。也就是说\omega_i描述的是连杆i坐标系原点处的角速度，\dot{P}_i描述的是连杆i坐标系原点处的线速度。前面我们提到过为了简化动力学计算，所有的运算都在质心坐标系下完成。所以我们实际上需要的是质心处的线速度和角速度。因为连杆坐标系和连杆质心坐标系实际上都是固连在连杆上的，因此这个速度和角速度的传递实际上是在同一个刚体内部的两个点之间的速度与角速度关系！
  角速度在同一个刚体上处处相等，因此\omega_{c_i}=\omega_i，同一个刚体内线速度传递关系也比较容易，他们之间的相对速度是因为刚体旋转引起的，因此线速度传递关系为：\dot{P}_{c_i}=\dot{P}_i+\omega_i\times r_{i,c_i}
总结一下从连杆i坐标系原点到连杆i质心坐标系之间的速度传递关系为：

\left\{
\begin{aligned}
\omega_{c_i}&=\omega_i\\
\dot{P}_{c_i}&=\dot{P}_i+\omega_i\times r_{i,c_i}
\end{aligned}
\right.

3.1.4 加速度传递

  前面介绍了速度的传递过程，依据的原理很简单就是速度的叠加原理。那么加速度怎么传递呢？这个问题实际上是比较复杂的，加速度的传递很难直接确定递推关系（注意不存在绝对加速度=相对加速度+牵连加速度这样的定理）。
  其实加速度的传递关系之所以难以确定最主要的原因我觉得是科氏力的存在。大家可能有这样一个印象，说明力的时候通常都能找到施力物体，比如说推着自行车前进那么车子的施力物体就是人。科氏力比较奇怪，这个力是找不到施力物体的，因此加速度的关系很不直观。但是加速度和速度之间的关系十分明确，他们是导数关系！因此通过对以上推出的速度传递公式求导就是加速度传递公式了！
  先来看角速度的导数，对于平移关节很简单\dot{\omega}_{i+1}=\dot{\omega}_i，旋转关节按照求导法则：

\dot{\omega}_{i+1}=\dot{\omega}_i+\ddot{\theta}_{i+1}z_{i+1}+\dot{\theta}_{i+1}\dot{z}_{i+1}

z_{i+1}是一个单位矢量，大家都知道矢量是可以在空间内任意平移的，因此可以认为它的起点是基坐标系原点。回看一下图4和图5你会发现z_{i+1}的变化是和前i个连杆的旋转有关系，因此z_{i+1}是以连杆i的角速度\omega_i这样的一个角速度在旋转，因此：

\dot{z}_{i+1}=\omega_i\times z_{i+1}

总结一下，角加速度的传递关系为：

\dot{\omega}_{i+1}=\left \{
\begin{aligned}
\dot{\omega}_i&+\ddot{\theta}_{i+1}z_{i+1}+\dot{\theta}_{i+1}\omega_i\times z_{i+1}\ \ rotation\\
\dot{\omega}_i&\ \ prismatic
\end{aligned}
\right.

  线加速度的传递关系稍微复杂一些，我们分开来说，先说旋转关节的线速度\dot{P}_{i+1}=\dot{P}_i + \omega_i \times r_{i,i+1}。求导的时候先用链式法则可以得到：

\ddot{P}_{i+1}=\ddot{P}_i + \dot{\omega}_i \times r_{i,i+1} + \omega_i \times \dot{r}_{i,i+1}

前面两项在递推的过程中可以直接得到了，主要是最后一项\dot{r}_{i,i+1}要怎样获取。其实回看一下图1就可以了解到r_{i,i+1}=P_{i+1}-P_{i}，因此\dot{r}_{i,i+1}=\dot{P}_{i+1}-\dot{P}_{i}，因此对于旋转关节来说：

\dot{r}_{i,i+1}=\dot{P}_{i+1}-\dot{P}_{i}=\omega_i \times r_{i,i+1}

所以旋转关节的线加速度传递关系为：

\ddot{P}_{i+1}=\ddot{P}_i + \dot{\omega_i} \times r_{i,i+1} + \omega_i \times \left(\omega_i \times r_{i,i+1}\right)

再来说移动关节的线速度\dot{P}_{i+1}=\dot{P}_i + \omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}，求导也是先用链式法则：

\ddot{P}_{i+1}=\ddot{P}_i + \dot{\omega}_i \times r_{i,i+1}+\omega_i \times \dot{r}_{i,i+1}+\ddot{d}_{i+1}z_{i+1}+\dot{d}_{i+1}\dot{z}_{i+1}

这里面除了\dot{r}_{i,i+1}和\dot{z}_{i+1}其他的量都可以在递推的过程中获得。同样的\dot{r}_{i,i+1}=\dot{P}_{i+1}-\dot{P}_{i}，对于平移关节：

\dot{r}_{i,i+1}=\dot{P}_{i+1}-\dot{P}_{i}=\omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}

z_{i+1}导数的求法与前面也是一样的，只不过对于平移关节连杆i和连杆i+1角速度是相等的：

\dot{z}_{i+1}=\omega_i\times z_{i+1}=\omega_{i+1}\times z_{i+1}

因此平移关节的线加速度传递关系为：

\ddot{P}_{i+1}=\ddot{P}_i + \dot{\omega}_i \times r_{i,i+1}+\omega_i \times \left(\omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}\right)+\ddot{d}_{i+1}z_{i+1}+\dot{d}_{i+1}\omega_{i+1}\times z_{i+1}

由于对于平移关节\omega_i=\omega_{i+1}，因此上式可以合并同类项：

\ddot{P}_{i+1}=\ddot{P}_i + \dot{\omega}_i \times r_{i,i+1}+\omega_i \times \left(\omega_i \times r_{i,i+1}\right)+\ddot{d}_{i+1}z_{i+1}+2\dot{d}_{i+1}\omega_{i+1}\times z_{i+1}

总结一下，线加速度的传递关系为：

\ddot{P}_{i+1}=\left\{
\begin{aligned}
\ddot{P}_i& + \dot{\omega}_i \times r_{i,i+1} + \omega_i \times \left(\omega_i \times r_{i,i+1}\right)\ \ rotation\\
\ddot{P}_i& + \dot{\omega}_i \times r_{i,i+1}+\omega_i \times \left(\omega_i \times r_{i,i+1}+\dot{d}_{i+1}z_{i+1}\right)+\ddot{d}_{i+1}z_{i+1}+\dot{d}_{i+1}\omega_{i+1}\times z_{i+1}
\end{aligned}
\right.

最后说一下质心加速度和角加速度，同样是对质心速度和角速度求导：

\left\{
\begin{aligned}
\dot{\omega}_{c_i}&=\dot{\omega}_i\\
\ddot{P}_{c_i}&=\ddot{P}_i+\dot{\omega}_i\times r_{i,c_i}+\omega_i\times \left(\omega_i\times r_{i,c_i}\right)
\end{aligned}
\right.

3.1.5 转化为递归形式

  如果你理解了前面介绍的速度和加速度传递过程那么整理成递归形式是非常容易的。文章开头的时候我们已经提到相对性原理，以上的速度和加速度推导都是在基坐标系下表示的。由于所有惯性系是平权的，因此以上等式在任意惯性系下都是成立的。为了动力学求解方便我们把这些物理量全部映射到连杆i坐标系下。由于这个过程对于所有的物理量是一致的，因此我们拿其中一个进行举例。我们举将旋转关节线加速度映射到连杆i+1坐标系下。
  映射方式其实很简单，利用旋转矩阵作向量变换即可，由于前述物理量都是在基坐标系下表示的，因此使用的变换矩阵是从基坐标系到连杆i+1坐标系即R_0^{i+1}：

R_0^{i+1}\ddot{P}_{i+1}=R_0^{i+1}\left(\ddot{P}_i + \dot{\omega}_i \times r_{i,i+1} + \omega_i \times \left(\omega_i \times r_{i,i+1}\right)\right)

R_0^{i+1}\ddot{P}_{i+1}可以表示为\ddot{P}_{i+1}^{i+1}。

再看等式右侧第一项：R_0^{i+1}\ddot{P}_i=R_i^{i+1}R_0^i\ddot{P}_i=R_i^{i+1}\ddot{P}_i^i

第二项：R_0^{i+1}\left(\dot{\omega}_i \times r_{i,i+1}\right)=\left(R_0^{i+1}\dot{\omega}_i\right) \times \left(R_0^{i+1} r_{i,i+1}\right)=R_i^{i+1}\left(\dot{\omega}_i^i \times r_{i,i+1}^{i}\right)

第三项：R_0^{i+1}\left(\omega_i \times \left(\omega_i \times r_{i,i+1}\right)\right)=R_i^{i+1}\left(\omega_i^i \times \left(\omega_i^i \times r_{i,i+1}^i\right)\right)

因此旋转关节线加速度映射到连杆i+1坐标系下可以表达为：

\ddot{P}_{i+1}^{i+1}=R_i^{i+1}\left(\ddot{P}_i^i + \dot{\omega}_i^i \times r_{i,i+1} + \omega_i \times \left(\omega_i \times r_{i,i+1}\right)\right)

可以看到这个过程其实并不复杂，就是一个坐标系变换问题。
总结一下，角速度的递归传递式为：

\omega_{i+1}^{i+1}=\left\{
\begin{aligned}
R_i^{i+1}\omega_i^i& + \dot{\theta}_{i+1}z_{i+1}^{i+1}\ \ rotation\\
R_i^{i+1}\omega_i^i&\ \ prismatic
\end{aligned}
\right.

线速度的递归传递式为：

\dot{P}_{i+1}^{i+1}=\left\{
\begin{aligned}
R_i^{i+1}&\left(\dot{P}_i^i + \omega_i^i \times r_{i,i+1}^i\right)\ \ rotation \\
R_i^{i+1}&\left(\dot{P}_i^i + \omega_i^i \times r_{i,i+1}^i\right)+\dot{d}_{i+1}z_{i+1}^{i+1}\ \ prismatic
\end{aligned}
\right.

质心速度和角速度在质心坐标系下表达为：

\left\{
\begin{aligned}
\omega_{c_i}^{c_i}&=\omega_i^i\\
\dot{P}_{c_i}^{c_i}&=\dot{P}_i^i+\omega_i^i\times r_{i,c_i}^i
\end{aligned}
\right.

角加速度递归传递式为：

\dot{\omega}_{i+1}^{i+1}=\left\{
\begin{aligned}
R_i^{i+1}\dot{\omega}_i^i& + \left(R_i^{i+1}\omega_i^i\right)\times\dot{\theta}_{i+1}z_{i+1}^{i+1}+\ddot{\theta}_{i+1}z_{i+1}^{i+1}\ \ rotation\\
R_i^{i+1}\dot{\omega}_i^i&\ \ prismatic
\end{aligned}
\right.

线加速度递归传递式为：

\ddot{P}_{i+1}^{i+1}=\left\{
\begin{aligned}
R_i^{i+1}&\left(\ddot{P}_i^i + \dot{\omega}_i^i \times r_{i,i+1}^i + \omega_i^i\times \left(\omega_i^i \times r_{i,i+1}^i\right)\right)\ \ rotation \\
R_i^{i+1}&\left(\ddot{P}_i^i + \dot{\omega}_i^i \times r_{i,i+1}^i + \omega_i^i\times \left(\omega_i^i \times r_{i,i+1}^i\right)\right)+\ddot{d}_{i+1}z_{i+1}^{i+1}+2\dot{d}_{i+1}\omega_{i+1}^{i+1}\times z_{i+1}^{i+1}\ \ prismatic
\end{aligned}
\right.

质心加速度和角加速度在质心坐标系下表达为：

\left\{
\begin{aligned}
\dot{\omega}_{c_i}^{c_i}&=\dot{\omega}_i^i\\
\ddot{P}_{c_i}^{c_i}&=\ddot{P}_i^i+\dot{\omega}_i^i\times r_{i,c_i}^i + \omega_i^i\times \left(\omega_i^i\times r_{i,c_i}^i\right)
\end{aligned}
\right.

3.2 力与力矩的反向递推

  所谓反向递推是指从机器人末端开始向基座方向逐次递推，递推的每一步都是根据当前连杆的速度，加速度以及当前连杆受到的后向连杆的力和力矩来求解当前连杆受到的前向连杆的力和力矩。力的反向递推简单来说就是从后向前依次对每一个连杆计算单刚体动力学！单刚体动力学相关内容我们已在上一篇文章中有所阐述。
  力和力矩之所以需要反向递推是因为每个连杆i都承受了来自于后向连杆i+1的力和力矩，如果这个后向连杆i+1施加给当前连i杆的力和力矩无法确定，那么当前连杆i的动力学就无法计算。因为力和力矩这样的物理量是“反向依赖”的。
  力和力矩的反向递推图如下图7所示（注意图中所有物理量都是在基坐标系下表达的）。

图7. 力的反向递推

f_i,\ \mu_i分别是连杆i-1施加给连杆i的力和力矩，f_{i+1},\ \mu_{i+1}分别是连杆i施加给连杆i+1的力和力矩。根据牛顿第三定律，连杆i+1施加给连杆i的力和力矩应该加负号。以上这些物理量都是在基坐标系下表示的。由于惯性系平权，我们可以在连杆i的质心惯性系下进行这个连杆的动力学计算。可以把上一篇文章的牛顿方程和欧拉方程拿过来。
牛顿方程：

m_i \ddot{P}_{c_i}^{c_i}=f_i^i-R_{i+1}^if_{i+1}^{i+1}

需要注意的是牛顿方程中没有考虑重力，原因是我们基于这样一种考虑：首先我们认为没有重力，然后我们认为机器人的基座以-g的加速度在运动（重力场可以用加速度场来等效替换）。
  根据牛顿方程每一次迭代我们都可以从f_{i+1}求解出f_i（如果是最后一个连杆，外界不施加力的情况下f_{i+1}=0，如果外界施加了力则需要通过一些力传感器计算出施加的外力）。
欧拉方程：

I_{c_i}^{c_i}\dot{\omega}_{c_i}^{c_i}+\omega_{c_i}^{c_i}\times I_{c_i}^{c_i}\omega_{c_i}^{c_i}=\mu_i^i-\mu_{i+1}^i+\left(-r_{i,c_i}^i\right) \times f_i^i+\left(-r_{i+1, c_i}^i\right)\times \left(-f_{i+1}^i\right)

这里需要注意的是重力作用于质心处因此对质心不产生力矩。
根据欧拉方程每一次迭代我们都可以从\mu_{i+1}求解出\mu_i（如果是最后一个连杆，外界不施加力矩的情况下\mu_{i+1}=0，如果外界施加了力矩则需要通过一些力传感器计算出施加的外力矩）。

3.3 关节力矩的计算

  一旦我们通过上面的方法确定了连杆i-1施加给连杆i的力和力矩f_i,\ \mu_i，关节i的输出力/力矩就非常容易确定了，如果关节i是旋转关节，那么关节轴向的力矩是由关节电机提供的，也就是：

\tau_i = \mu_{i}^{iT} z_i^i

如果关节i是移动关节，那么关节轴向的力是由关节电机提供的，也就是：

\tau_i = f_i^{iT} z_i^i

你可能会有疑问连杆i-1施加给连杆i的其他的力和力矩从哪里来呢？其实很简单，其他方向的力和力矩一定是机器人的结构提供的。

4. 总结

这篇文章主要介绍了串联机构的牛顿欧拉方程，内容有点多，其实要传达的并不多：
1.速度传递的依据：绝对速度=相对速度+牵连速度
2.加速度=速度的导数
3.惯性系平权，物理量的转换通过旋转矩阵完成
4.弄清公式中的标量和向量
第三点我认为非常重要，这是一种思想的转变，我们从原始的一维小滑块思想逐渐过度到在坐标系中讨论真正的三维空间问题，这里面最大的挑战就是理解惯性系以及他们之间的平权性。
  由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

5. 参考文献

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

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