机器人动力学：牛顿欧拉法推导与详解

是刚体在{C}中的惯性张量。惯性张量矩阵求法可以参考相关文献。

那么要得到连杆质心的力和力矩，就必须要求线加速度、角速度、角加速度了。

由于串联机械臂具有强耦合关系，因此相邻关节之间存在一定递推关系。假设你在旋转肩膀的同时转动胳膊，定义肩膀的角速度ω1，胳膊的角速度ω2，此时，你胳膊的实际角速度并非ω2，而是一个与肩膀角速度相关的函数，意味着当前连杆的运动状态依赖于前一个连杆的运动参数。递推法就是计算当前连杆真实运动状态的方法。

正推

首先对连杆 1 进行计算，然后利用递推式，就能得知连杆 2 的运动状态直至关节 n。

我们只关心相邻连杆的运动状态关系：

旋转运动参数

角速度

对于角速度：

$\omega_{i+1} = R_i \omega_i + \hat{Z}_{i+1} \dot{\theta}_{i+1}$

这个式子很好理解，地球自转的同时也绕着太阳公转，因此真实的角速度是公转速度 + 自转速度。在这里，wi 指前一个关节的角速度，即绕太阳的公转速度，第二项是地球绕地轴 Zi+1 的自转速度。

角加速度

对于角加速度：

$\dot{\omega}_{i+1} = R_i \dot{\omega}_i + \omega_i \times (\hat{Z}_{i+1} \dot{\theta}_{i+1}) + \hat{Z}_{i+1} \ddot{\theta}_{i+1}$

角加速度无非是对角速度求时间的微分。 第一项求导很正常。 第二项求导要注意，是乘积求导法则。对于速度

$\dot{\theta}$

求导没问题，重点在于如何对

$\hat{Z}_{i+1}$

求导

$\frac{d\hat{Z}_{i+1}}{dt} = \omega_i \times \hat{Z}_{i+1}$

为了简化计算，我们不妨设

$Z_{i}$

和

$Z_{i+1}$

互相垂直，那么根据下图，此时的

$d\hat{Z}_{i+1}$

表示

$\hat{Z}_{i+1}$

离原来偏移了多少，就可以用

$\hat{Z}_{i+1}$

与

$\hat{Z}_{i}$

叉积表示了！

叉积就是乘以 sin，于是有：

$|dZ_{i+1}|=|Z_{i+1}|\cdot |\omega_i|\cdot dt \cdot \sin(\frac{\pi}{2})$

对于 dt 求导

$\frac{dZ_{i+1}}{dt}=Z_{i+1} \times \omega_i$

这就是第三项的来源！

接着，由于我们是根据 i 系的运动状态计算 i+1 系的运动参数，因此要把所有参数转入{i+1}系中： 同时利用旋转矩阵（{i}系与{i+1}系的旋转矩阵）将所有参数的上下标统一：

$\omega_{i+1} = {}^{i+1}R_i \omega_i + \hat{Z}_{i+1} \dot{\theta}_{i+1}$

平移运动参数

线速度

线速度不仅会由前一个连杆传递（即便前一个连杆不存在平移，也可能继承了前前个平移关节的线速度）； 如果前一个连杆是旋转关节，由于存在连杆长度 R，必然会产生由旋转而产生的线速度

$v_{rotate}=\omega \times R$

也就是 角速度X半径：

$v_{i+1} = v_i + \omega_i \times P^*_{i+1}$

这个

$P^*_{i+1}$

是指{i}系原点指向{i+1}系原点的向量，也就是连杆长度。

线加速度

同理，求加速度就是对速度求时间的微分。 第一项求导很正常。 第二项求导要注意，是乘积求导法则。

$\dot{v}_{i+1} = \dot{v}_i + \dot{\omega}_i \times P^*_{i+1} + \omega_i \times (\omega_i \times P^*_{i+1})$

$\dot{P}^*_{i+1}=w_i \times P^*_{i+1}$

是显然的线速度就是 角速度X半径 嘛

把

$\dot{P}^*_{i+1}$

替换进求导的式子，得到上式。

接着，先将各参数转入{i}参考系，再用{i}系与{i+1}系的旋转矩阵

$^{i+1}R_i$

将运动参数转入{i+1}系中。

$v_{i+1} = {}^{i+1}R_i (v_i + \omega_i \times P^*_{i+1})$

从质心到关节的变换

上面我们刻画了质心的运动参数递推关系，但是在实际控制中，我们肯定控制的是关节，因此还需要将质心处的参数变换到关节上：

$v_{C_{i+1}} = v_{i+1} + \omega_{i+1} \times P^*_{C_{i+1}}$

线速度

规定关节坐标系原点指向质心的向量为

$P^*_{C_{i+1}}$

，那么这显然是一个类似'平移'的操作，因此套用线速度的递推公式：

$v_{C_{i+1}} = v_{i+1} + \omega_{i+1} \times P^*_{C_{i+1}}$

线加速度

同理求导，下式关系显然满足（旋转引起的线速度=角速度 X 半径）。

$\dot{v}_{C_{i+1}} = \dot{v}_{i+1} + \dot{\omega}_{i+1} \times P^*_{C_{i+1}} + \omega_{i+1} \times (\omega_{i+1} \times P^*_{C_{i+1}})$

因此求导可得：

$\dot{v}_{C_{i+1}} = \dot{v}_{i+1} + \dot{\omega}_{i+1} \times P^*_{C_{i+1}} + \omega_{i+1} \times v_{C_{i+1}/i+1}$

同理，将其转入{i+1}系：

$\dot{v}_{C_{i+1}} = {}^{i+1}R_i (\dot{v}_i + \dot{\omega}_i \times P^*_{i+1} + \omega_i \times (\omega_i \times P^*_{i+1})) + ...$

这就是书上这几个不给推导/引用链比你还长的式子的简要理解：

注：

书上的推导是严谨的，但是深邃难懂，我也是二刷才看懂，而且引经据典到 6.15 式，6.15 有引用 6.6 式，6.6 式又引用 5.12 式等，引用链复杂。此外，从三系变换到 i、i+1 系变换过于突兀，极其不易理解。
因此在刷完相关视频后给出这个浅显易懂的推导。

汇总

总而言之，我们已经掌握了线速度、线角速度、角速度、角加速度的递推公式：

同时，牛顿和欧拉两位大佬也给我们提供了力和力矩的递推公式：

至此，我们就完成了正推公式的推导和理解，我们已经能借助前一个连杆的运动参数计算当前连杆的运动参数了。

知道运动参数后，我们就可以通过从第 n 连杆往回推各关节需要的力矩了。

反推

反推就是通过力、力矩和运动参数构成的方程，输入运动参数，获得各关节的力矩。

我们采用改进 DH 参数建模，在改进 DH 建模方式中，最大的区别就是第 i 连杆在第 i 关节的后面。

每一个关节都独立产生控制力矩，我们对第 i 连杆受力分析： 杆 i 会受到杆 (i-1) 的作用力

$f_i$

，也会受到杆 (i-1) 的反作用力

$f_{i-1}$

。

因此列出牛二：

$F_i = m_i \cdot \dot{v}_{c_i} = f_i - f_{i+1}$

$f_i = F_i + f_{i+1}$

其中 F 是该连杆质心处的合力，

$f_i$

、

$f_{i-1}$

是作用在两侧关节的力。

统一到{i}坐标系中：

$f_i = {}^{i}R_{i+1} f_{i+1} + F_i$

接下来，我们对连杆{i}受力分析：

假设连杆{i}存在前后两段连杆{i-1}{i+1}： 我们的目的是分析连杆{i}质心处的受力情况

看这张抽象图： 假设所有关节的 z 旋转轴平行，且垂直纸面向外，这意味逆时针旋转为正向。 首先，**关节{i-1}逆时针旋转，因此会给杆{i}**一个向左上的力

$f_i$

。 **关节{i}也逆时针旋转，因此会给杆{i}**一个向右上的力

$f_{i+1}$

，自身会受到**杆{i+1}**的反作用力

$-f_{i+1}$

。 **杆{i-1}对杆{i}**施加 fi 的力，力臂为 r1，那么根据右手法则，四指由 f 指向 r，得到力矩 N 的方向朝下（纸内）； **杆{i}对杆{i+1}施加 f1+1 的力，力臂为 ri+1，那么杆{i+1}**同样会施加给它一个反作用力-fi+1，于是四指由 f 指向 r，得到力矩 N 的方向朝上（纸外）；

我们的目标是要计算质心处和合力矩。目前已经得到两端相邻连杆对杆{i}的作用力矩了，还要将其转换到质心处：

$N_i = n_i + p_{C_i} \times f_i - n_{i+1} - (p_{i+1} - p_{C_i}) \times f_{i+1}$

假设质心 Ci 向杆{i}坐标系原点的向量是

$-_{}^{i}\textrm{P}_{C_i}$

，那么质心 Ci 向杆{i+1}坐标系原点的向量就是

$_{}^{i}\textrm{P}_{i+1}-_{}^{i}\textrm{P}_{C_i}$

。 对于作用于两端的力矩就可以通过这个公式合成到质心处：得到

$_{}^{i}\textrm{N}_{i}$

就是质心处的合力矩。

$n_i = N_i + p_{C_i} \times f_i$

那么关节 i 应该施加的力可以通过移项得到

$_{}^{i}\textrm{n}_{i}$

，观察公式，显然是和

$_{}^{i+1}\textrm{n}_{i+1}$

相关的，因此，要计算前一个关节的力矩，就必须得知后一个关节的力矩。

这就是为什么计算力矩的过程叫做'逆推'！

而至于为什么要引入质心，就是为了构建上面的等式，构建前后关节的力矩关系，从而递推，他就是个中间变量。

我们继续，

实际上，

$_{}^{i}\textrm{n}_{i}$

并不是最终的关节力矩，而是中间计算过程中的一个变量——惯性力矩。

那它和真正的'关节力矩'是什么关系呢？

惯性力矩考虑了当前杆件和相邻杆件的惯性力矩、外力和重力的影响。 但是，我们的关节只能产生绕 z 轴旋转的力，至于重力或负载给的压力，它根本无法影响，换句话说，重力、压力、等非绕 z 轴的力无法被关节控制。

因此我们还需要提取与绕 Z 轴相关的力矩，这很简单，直接用 Z 轴的方向向量点积即可提取出这个方向上作用的力：

$\tau_i = n_i \cdot \hat{Z}_i$

如图，

$_{}^{i}\textrm{n}_{i}$

是一个考虑了 xyz 三个方向和合力矩，但我们只想要 z 轴方向的力矩，因此乘以了

$[0,0,1]^T$

。

至此为止，你就完全掌握了正逆推的过程！！！

我们快速总结一下：

动力学的目的就是计算在某个运动状态下，各个关节应该施加多大的力，因此核心是构建一个'运动状态 - 力学参数'的方程，牛顿和欧拉两位大佬就提供了。 我们的核心就变成了如何得到各个关节的'真实速度'。 我们已知各个关节的关节空间的速度、角速度、加速度，如何将前一个坐标系的运动参数传递给下一个坐标系计算呢？ 这就用到了正推，构建线速度、线加速度、角速度、角加速度以及质心和端点的速度关系。 这里面最难理解的就是对 Zi 轴的求导，我们把求导拆成先微分再对时间求导，并用红蓝绿坐标系表示了这个过程。 最终得到了运动参数的递推公式。 我们又知道，力的传播是从末端向基座的，因此力的计算就是逆推的过程。 我们先对连杆的相互作用点进行了受力分析，并构建了端点到质心的变换公式，得以计算质心处的合力矩情况。 此时的合力矩指的是 xyz 三方向的合力矩，但是关节电机只能控制绕 z 轴旋转的力矩，因此我们将合力矩中的 Z 方向力矩提取出来，直接用 Z 的方向向量点积即可。 由此，我们就能通过得知各个关节的关节速度、加速度，从而推导出各个关节需要多大的力矩了！

静力学分析

特别地，当机械臂的速度加速度均为零时，也就是静止时，上面的式子可以得到简化： 下式就刻画了静止时各关节力和力矩的计算方式，它可以分析静态下机械臂的负载能力。 （这一节反而在第五章 P108 页）

这就是 Matlab 机器人工具箱的 rne 的实现逻辑。

为什么选择牛顿欧拉法？

时间复杂度最低——仅 O(n)

相对而言，拉格朗日法接近 O(n^3)

当然 RNE 也有缺点：

• 推导过程繁琐，依赖递归顺序。
• 不易直接得到显式动力学模型。

其余参考课件：