【现代机器人学】学习笔记七：开链动力学（前向动力学Forward dynamics 与逆动力学Inverse dynamics）

zkk9527

已于 2024-07-30 14:16:52 修改

阅读量8.3k

点赞数 9

分类专栏：【现代机器人学】学习笔记文章标签：现代机器人学机器人动力学前向动力学逆向动力学串联机器人动力学

于 2022-12-25 21:43:49 首次发布

本文链接：https://blog.csdn.net/zkk9527/article/details/128435630

版权

【现代机器人学】学习笔记专栏收录该内容

14 篇文章 107 订阅

订阅专栏

这节的内容主要讲述机器人动力学的内容。相对于本书其他部分运动学内容相比，把动力学一下子合成了一章。看完以后有三个感受：

1.本章难度相对其他章节较大，因此需要反复去看，以求对重要内容的眼熟，不求全部记住，但只求说起某块内容时，心中有数。2.阅读时一定要“心静”，这样才能理解其中的内容。如果急于求成，一定啃不下来。3.阅读的时候高度要高，不能被各种公式绕晕，因此细节可以稍微不太关注，但必须明确的知道各节是在讲什么内容。

另外可以参考开源代码的实现，对于理解文中的公式，很有帮助：

【现代机器人学】学习笔记十三：配套代码解析

本章的一切内容都是以上式为基础，它也叫运动方程。逆向动力学（ID)就是根据上述方程，根据关节位置、速度、加速度计算关节力矩。同样正向动力学（FD）则是根据关节力矩、关节位置、速度计算关节加速度：

动力学和运动学不同，不能把FD和ID理解为根据关节力矩算末端力，或者根据末端力算关节力矩。

动力学主要有两种表示形式，拉格朗日方程与牛顿欧拉方程。

拉格朗日方程

拉格朗日方程主要用于相对简单的低自由度的机器人计算动力学，即dof<=3。

拉格朗日函数定义为动能减去势能：

这样如果包含一个外力，K和P就用整个系统的动能和势能来计算：

而在机器人自身运动这快，所谓外力就是各个关节的关节力矩，控制连杆具有动能和势能。

书中用了一个2R的机械臂，然后还假设质量都处于连杆的末端位置，反正嘎嘎一顿推导，得到：

那么这就符合了动力学的方程的形式。目的就是用这个相对简单的例子告诉读者，关节所需要的力和机器人各个关节所处的位置、速度和加速度都是有关的。

如果对两个关节所处位置（xy）进行二阶微分，再乘以质量得到对应的力的话，可以得到两个公式：

我们可以看到，尤其对第二个关节来说，除了关节加速度，还有关节角速度的平方，以及不同关节之间的角速度乘积。 $\dot{\theta}^2_i$ 项为向心项，而 $\dot{\theta}_i\dot{\theta}_j(i\neq j)$ 的项为科里奥利项。而对于第一个关节而言，自己也是有向心力的作用的。

这一节个人认为应该补充一些知识点以供参考：

1.惯性系可以简单说成是相对地面静止的或者做匀速直线运动的参考系，而非惯性系则是相对地面做加速或者减速运动的参考系。

因为关节坐标并不是相对地面静止或者做匀速直线运动的，比如关节2可能在关节1的加减速的带动下发生移动，因此不是惯性系。关节1也同样不是在做匀速直线运动（哪怕是匀速，也是匀速圆周运动。回忆高中物理：我们把做匀速圆周运动的物体所受的合外力称作向心力。向心力的方向始终指向圆心。

这就解释了为何这里关节角加速度为0的情况下，使得关节质心位置仍然存在一个加速度。这时因为存在向心力和科里奥利力。向心力依旧指向圆心，因为它和运动方向正交，似乎不影响运动，但由于科里奥利力的存在，使得合力的方向不再指向圆心，因此关节质心位置仍然有加速度。

2.惯量：衡量惯性的大小， $J= \int r^2dm=mr^2$ ，m为质量，r为质点与转轴的垂直距离。

3.动量： $P=mv$ ，角动量 $L=rpsin\phi =r \times p = r \times mrw=mr^2w=Jw$ （其中p为动量，r为质点到轴）

4.力矩被定义为角动量的变化率（可以从量纲上得到）。随着质点到轴距离r的下降，惯量J会下降，如果想保持关节速度w不变，则必须让角动量L也下降（L=Jw），而角动量下降意味着向负方向变化，即施加负力矩。

通用方程

其中c为科里奥利矩阵。

这节的目的是，通过拉格朗日法，可以推出一个符合动力学的方程的一般表示形式。

关于拉格朗日法，我找到一个巨好的文章：

哈密顿原理和拉格朗日函数的由来是怎样的？ - 武奘的回答 - 知乎 https://www.zhihu.com/question/49361447/answer/932775249

建议各位抽空一读。

1.至于M怎么算，后面欧拉那节会讲。

这个东西是由推导得到，过程不用深究。但这意味着，需要质量矩阵对关节角进行微分操作。（而后文的牛顿欧拉法可以不用微分）

质量矩阵的解释

说重点：

1.可以算动能，常规的高中物理的 $\frac{1}{2}mv^2$ 也能算动能，区别就是前者意味着，沿着不同加速度方向，有效质量不同。

2.质量矩阵M其实在客观上起到一个从关节加速度到关节力矩互相之间的映射，

粗线在左图是关节角加速度的单位球，经过M映射，得到一个关节力矩上的椭球，这时候就可以发现，当只有沿着主轴方向，即M的特征向量方向时，才服从“加速度是力矩的标量倍数”。如果不沿着主轴方向增大，其实那个倍数是处于改变的状态，即“有效质量”发生了改变。

3.根据前面一阶运动学那章所述的 $V=J(\theta)\dot{\theta}$ ，可以可以带入进去，只关注末端执行器。

即末端执行器的质量矩阵为：

那么对末端执行器也是同样的道理，它自己的加速度，经过这个矩阵的映射，变成一个力椭球，只有力度沿着主轴方向增大的时候，加速度和力才服从同一个标量倍数。

牛顿欧拉动力学

单刚体动力学

这一章名字很关键，看的时候要清楚，这里是“单刚体”，所以暂时先不要把机械臂那些乱七八糟的东西混进来一起看。。

那么这块又分为直线动力学，和旋转动力学。

省去那些繁琐讨厌的推导，移动动力学更关注力，因此得到：

$f_b={\rm{m_i}}(\dot{v_b}+[w_b]v_b)$

其中，这个正体的 $\rm{m_i}$ 指的是点质量，注意它不是质量矩阵中的内容，也不是力矩的那个m。

转动动力学则关注力矩：

$m_b=\mathcal{I}_b\dot{w_b} + [w_b]\mathcal{I}_bw_b$ （这个叫刚体的欧拉方程，这个公式需要反复的看来记个眼熟，因为后面还要用到。辅助记忆，一个角加速度的线性项，一个角速度的二次项）

这里mb指的是力矩，而 $\mathcal{I}_b=-\sum_im_i[r_i]^2 \in \mathbb{R}^{3\times3}$ 称为刚体的转动惯量矩阵。

我们需要在这里区分转动惯量矩阵 $\mathcal{I}_b$ 和质量矩阵 $M(\theta)$ 。

转动惯量矩阵 $\mathcal{I}_b$ 是常数！！！

我们可以有很多种方式计算动能：

$\frac{1}{2}\dot{\theta}^TM(\theta)\dot{\theta}$

或者

$\frac{1}{2}w_b^T\mathcal{I}_bw_b$

我们可以看出，质量矩阵 $M(\theta)$ 实际上是和位形 $\theta$ 有关的，而转动惯量矩阵是常数。

（当然，这节我们研究的是单刚体动力学，还没到机械臂位形这步，提到这个纯粹是为了防止混淆）

ps：

在书中开始的推导中，提到了：

这个指的是质点本身的速度。

我们注意，在旋量体系中，速度指的是“过坐标系原点处的速度”。

但在这里，它指的是某个物体在某坐标系下的速度，即和我们常规所理解的速度的表示一致。

那么在b系下的速度，实际上是b本身移动的线速度，叠加b旋转导致的速度，合起来在i点处的速度。

（对于非旋量体系的运动学表示中，这种表示非常常见。）

接着回到书本：

转动惯量矩阵，可以根据大学高数的方法来计算。

它是一个正定对称矩阵，因此就6个变量就能表达了。（另外我们看到里面有xyz，而xyz依赖于坐标系，因此这里暗示着：虽然转动惯量是常数，但它和坐标系有关。换言之，换个坐标系，它虽然还是常数，但却是不一样的常数。因此这里别把转动惯量矩阵理解为固定的某个数字）

关于转动惯量矩阵的坐标系选择，推荐的做法是，把惯性的主轴（和之前一样，对矩阵特征值分解最长的那个特征向量即为主轴）和body系坐标轴对齐。因为特征向量是正交的，那么这时候如果主轴和body坐标系对齐，那么三个特征向量其实用body系表示就是(x,0,0),(0,y,0),(0,0,z)。结合特征向量的计算方法，计算λI-A=0，特征向量是(x,0,0),(0,y,0),(0,0,z)的话则意味着A这个矩阵是对角阵。

书中给出了刚体力矩的推导结果：

那么这种情况下动力学运动方程则得到了精简。

均匀密度的物体，作者直接给出了积分的求解结果：

再放一个坐标系变换以后，转动惯量矩阵的改变：

这是根据动能守恒轻易推得的。

然后再来一个公式：

意思是，如果q系和b系姿态一样，但是在b的q点处，那么惯量矩阵就是这样变换。但是后文又提到一个Id和Icomm的转换，但感觉和这里也等不下来，套不上啊。因此我无视掉那个公式了。谁看懂可以给我讲讲欢迎讨论。

这里有一个重点内容：对于平面运动（转轴为z轴），一般的开链机械臂都这样，那么空间旋转动力学退化成了平面旋转动力学：

力矩为：

动能为：

运动旋量-力旋量方程

和这本书一贯的特点一样，先讲旋转，再讲连带平移的刚体运动。

这节就是要把上节的角速度转化为旋量理论然后再来走一遍单刚体动力学：

这里新定义一个空间惯量矩阵（相对原先的Ib叫转动惯量矩阵）

公式里的m则表示连杆的质量。

那么利用这个新的惯量矩阵，可以重新计算动能：（注意两边的角速度变为了旋量）

原先的角动量定义为转动惯量矩阵乘以角速度，现在重新定义空间动量：

其实就是空间惯量左乘旋量。因此记忆这些内容一定要对比记忆。

这里给出力旋量和运动旋量互相转换的推导结果：

注意第一项可不是空间动量啊，因为这里的旋量是求导的。

（那么这个公式依然要经常去看，以求眼熟。可以再次配合之前的旋转动力学方程来看：）

ps：之前我们都是各自独立的，在这节是全书首次把力旋量和运动旋量变换关系进行了打通。

上式中ad为旋量李括号计算：

注意：这里的小ad的作用，可不仅仅像刚体运动那节的Ad伴随矩阵，把旋量做个转换。这里小ad符号其实意味着两个旋量在做叉积！！！类似于角速度w的叉积。

那么同理，空间转动惯量也和机器人位形是无关的，但是和坐标系选择有关，因此这里也有一个坐标系之间的变换关系：

这个也可以配合w部分来看：

记忆起来也比较容易，因为旋量之间的运算一向就是用伴随矩阵来变换的。

机械臂动力学

相对之前的单刚体动力学，这一节开始引入机械臂的动力学。也就是题目中所说的牛顿-欧拉逆动力学。

推导部分就不细说了，看书推导即可。

但我这里还是要交代几个内容，因为后面要用到：

1.

即这个Ai，它直观意义是，关节i的初始螺旋轴在连杆坐标系i中的表示。。

2.连杆i的运动旋量，由两部分构成：在i坐标系中表示的i-1连杆的运动旋量，和i关节的速度引起的附加运动旋量构成。

我们回忆两点：1.旋量和螺旋轴的的区别在于： $\mathcal{V}=\mathcal{S}\dot{\theta}$ ，参见【现代机器人学】学习笔记二：刚体运动_zkk9527的博客-CSDN博客

，所以这里其实就是。

不过这里可能有误解了，那既然这样，就第一项好了呀，要第二项干嘛？这里和刚体运动这节内容有区别，因为那里描述的是单个刚体的运动，但是这里除了自己的运动，还有上一个关节带动的运动，也就是说这里不仅仅是刚体在运动了。

3.

关于 $\dot{\mathcal{V}_i}$ 的推导当中，有这一步，让人看的有些迷糊。这里可不能理解为 $\mathcal{V}_i$ 和 $\mathcal{A}_i\dot{\theta}$ 相等，那肯定不相等，因为 $\mathcal{V}_i=\mathcal{A}_i\dot{\theta}+[Ad_{T_{i,i-1}}]\mathcal{V}_{i-1}$ ，而且就算相等，负号怎么没了呢？

揭晓答案：这是因为，ad这个操作，类似于两个向量做叉积，（参见书8.36和8.37式），是两个旋量类似叉积的李括号运算。那么A x B= -B x A，所以内部的 $\mathcal{V}_i$ 和 $\mathcal{A}_i\dot{\theta}$ 进行了交换，并且前面多了负号。

牛顿欧拉逆动力学算法

就是一波前向迭代+后向迭代。

初始化条件： $\mathcal{V}_0$ 是基坐标系0中的运动旋量， $\dot{\mathcal{V}}_0=(\dot{w_0},\dot{v_0}) = (0,-g)$ ， $\mathcal{F}_{n+1}=\mathcal{F}_{tip} = (m_{tip},f_{tip})$ 是末端执行器作用于环境的力旋量。

前向迭代是先算两个关节之间的变换、从小到大把旋量往后算。再根据公式中的样子，算出加速度。（加速度辅助记忆方法： 自身关节加速度 $\mathcal{A}_i\ddot{\theta}$ + i系中连杆i-1加速度引起的分量 $Ad_{T_{i,i-1}}(\dot{\mathcal{V}}_{i-1})$ + 速度叉积分量 $ad_{v_i}(\mathcal{A}_i)\dot{\theta}$ 。

逆向迭代就是反着来，从大到小一路把力旋量反着算回来，并通过得到关节力矩。

1.注意这个力旋量公式，我们可以这样来记忆它：

作用在连杆i上的总的力旋量，等于 “通过i+1施加在连杆上的力旋量” ，以及自身力旋量之和（包括一个旋量加速度的线性项，一个旋量的二次项）。

2. 对于这个公式，辅助记忆：执行器只要在关节旋量轴的方向提供标量力或力矩。因此得到力旋量，配合螺旋轴，就可以对应得到关节力矩。（记得转置）

我们参照之前的单刚体运动公式（反复看到眼熟为止）：

回顾单刚体运动：

1. $m_b=\mathcal{I}_b\dot{w_b} + [w_b]\mathcal{I}_bw_b$ （这个叫刚体的欧拉方程，辅助记忆，一个角加速度的线性项，一个角速度的二次项）

2. $\mathcal{F}_b=\mathcal{G}_b\dot{\mathcal{V}}_b - [ad_{\mathcal{V}_b}]^T\mathcal{G}_b\mathcal{V}_b$ 辅助记忆：一个旋量加速度的线性项，一个旋量的二次项。

3.在这里我们再回顾一下大的Ad和小的ad的区别：

大Ad表示伴随，是基于矩阵的：

提供一个矩阵，可以计算这样的结果，实现旋量的坐标系转换。

小的ad也表示伴随，是这个运算再乘以一个旋量意味着两个旋量在做叉积。

封闭形式的动力学

这节的内容呢，其实就是想表明一个事情：即上面的递归方法，亦可以写成封闭形式的的动力学方程。因为从一开始就吹出了牛，说一定要满足动力学方程的那个形式：

那么这部分主要看一点：

如果末端有额外的力的话，方程会改写。

其他结论呢，我觉得没什么意思。请教了单位那位交大机构学毕业的博士，他表示现实里几乎不会去推导封闭形式的动力学，这个东西可能只有发论文的时候有点用处。。

这种有个需要关注的就是，算质量矩阵M的方法。（在前面用到的一直都是惯量矩阵）

质量矩阵是和位形有关的，因为和雅可比想当然的也肯定存在关系。

其中 $J_{ib}$ 为物体雅可比矩阵（body雅可比），前i列为正常的雅可比列，后n-i列向量均为0。

当然这只是一种计算法方法，另一种计算方法是（逆动力学算法n次调用）：

每次迭代把速度、加速度全置为0，包括重力项也为0，然后仅把第i轴的加速度置为1。然后通过ID算法迭代得到关节力矩，塞到列中就是质量矩阵。

这种算法也暗示了一件事情：质量矩阵的每列，为当前位形下，其他轴的速度，加速度为0，当前关节加速度为1，末端力旋量为0，重力项为0的时候的各关节力矩。

开链机器人的正向动力学：

围绕运动方程来处理：

逆动力学算法（ID）可用于计算（总结一下）：

计算内容	关节位置	关节速度	关节加速度	末端力旋量	重力项	如何计算
质量矩阵	需要	均为0	依次将第i轴加速度置为1，其他轴为0。	0	0	调用逆动力学，计算ID，将ID结果填充质量矩阵的第i列
提供科氏力和向心力的力矩	需要	需要	均为0	0	0	直接根据所述配置计算ID
提供重力项的力矩	需要	均为0	均为0	0	需要，g	直接根据所述配置计算ID
提供提供末端力的力矩	需要	均为0	均为0	需要，Ftip	0	直接根据所述配置计算ID

关于这部分实现，真的强烈推荐看这部分的配套代码解析！！！！！！

【现代机器人学】学习笔记十三：配套代码解析

否则看完这节，迷迷糊糊，不知道怎么实现，也对公式理论理解的不够深入。

而前向动力学则是，根据上面的表格，计算出质量矩阵，提供关节力矩，把需要的各种力矩减掉，然后左乘质量矩阵的逆矩阵，可以得到关节加速度。

迭代算法：

我来总结：这一节的目的是，1.给定初始位置与速度，2.关节力矩和末端力矩随时间变化的函数已知，3.推断这段时间内的关节运动。

做法就是先把时间划分步长（这里划分的越大越好），然后通过FD算出加速度，再用积分步长把它积成下一时刻的速度和位移，（对于关节来说就是角速度与角度），然后重新用下一时刻的速度和位移算再下一时刻的ID...以此类推。初始时刻的速度和位置是要给出的，这样才能继续往后推。

任务空间中的动力学：

可能一次性看到这块的同学，已经有点慌了。表现为看不进去，对公式产生恐慌...

别慌！！！

其实这部分就是一堆推导罢了。。。重要的是目的以及结论。本节内容就是想推导出动力学方程的又一种表示形式罢了，和前面的内容其实关系不是非常密切。

只要把握这个公式，并且对它反过来，嘎嘎一顿求导，带入到最开始的动力学方程中：

然后就一顿推导得到了最后的结论：

我觉得像大部分计算机或者控制等专业的学生，只要不打算在论文中堆公式去吓唬人，其实知道结论就这就够了。

受约束动力学：

在【现代机器人学】学习笔记一：位形空间_zkk9527的博客-CSDN博客_grubler公式在位形空间这章讲过，关于完整性约束和非完整性约束的问题。总之就是如果有一个公式：

$A(\theta)\dot{\theta}=0, A(\theta) \in \mathbb{R}^{k\times n}$

如果这个A不可积，那么它就是非完整约束，即这个约束只能限制速度，而限制不了位形。

所以这节的目的，就是分析在现在这种情况下，动力学方程会怎么样。。

说重点好了（其实我也写累了）：

给定这个A，以及质量矩阵M（根据8.57式，可由惯量矩阵和雅可比算得），就可以得到一个P矩阵。

那么这个P矩阵如何用呢？

它左乘关节处的力或力矩，可以把用于实现加速度的部分的力提取出来，把用于“约束”的力给滤掉。

同理还有一个

那么它的作用则是相反，它左乘一个加速度，可以把加速度中“用于满足约束的加速度分量” 提取出来。（注意，这里不是“滤掉”，而是“提取”出来，即提取出来的是满足约束的加速度分量，而非上面P是把约束部分的力给滤掉了）

这一节其实只是介绍，意味着提醒读者，有这一研究方向。这并非是一两句很简单的就能把这块都学完的。

硬件部分内容：

本章最后一节的内容叫“URDF中的机器人动力学”，但是私以为这节题目起得不合适。但它的内容却蛮合适，给软件工程师在硬件方面提供了一个很好的学习补充。这节的内容可以告诉我们，在我们算出的力矩等内容后，机器人究竟是怎么样执行的。

接下来我就说重点了：

1.因为我们算出来的是力矩，而直流电机可以提供的力矩与电流成正比，所以其实控制电流可以达到控制力矩的作用。

但是电机的特点是，转的很快，但力量太小了。

我们知道机械功率 $P=\tau w$ ，而作为电机，其实功率又是电流和电压一起提供的。功率一定的情况下呢，转速快，就意味着能提供的力矩太小。所以实际情况下，单凭单机转，根本不能带动那么重的机械臂转动。所以这里就需要提供一个齿轮减速器。

这里我放一个非常好的短视频：

谐波减速器原理

让人看完感觉真的很精妙，究竟是什么人能想出这样奇妙的装置！

齿轮减速器降低了输出端的转速，在功率不变的情况下，自然而然的就增加了输出的力矩，这样就可以真正让电机带动机械臂转动了。（虽然在减速的过程中有摩擦等因素的功率损耗）

因此减速比G（又叫传动比），其实就是电机转速/齿轮减速器输出转速，（也可以表示为齿轮减速器的输出力矩/电机输出力矩），这个意味着减速器对电机的输出力量做了多大的提升。。机器人关节传动比一般在100以上。

2.如果没有齿轮减速器，这种叫直驱型机器人。因为没有减速器，所以电机需要做的很大。

3.表观惯量（反映惯量）： $G^2\mathcal{I}_{rotor}$ ，原先我们一直算的力矩，都是认为它是电机提供的。实际在控制的时候，控制关节力矩，那么其实这里我们提供的是“齿轮减速器输出的力矩”。同样动力学方程中，对应的惯性矩阵I，也是有改变的。所以考虑这个表观惯量，我们需要重新来计算牛顿欧拉动力学方程，以求算出电机真正的力矩。（这就是书中8.9.3节所述的考虑电机惯量和齿轮传动的牛顿-欧拉逆动力学算法）