【V-REP自学笔记（六）】基于V-REP逆运动学模块的机械臂轨迹规划
导语
在这一系列的V-REP自学笔记中，我们定了一个小目标，完成一个Demo。使用官方提供的KUKA公司的YouBot机器人模型来实验机器人的感知和控制过程，控制机器人从A点抓取物品，然后移动到B点将物品放置在B点的工作台上，这其中涉及到V-REP环境中的机器人感知和控制过程。没有看过前期学习笔记的读者，可以在文末找到往期文章地址。
在上一期的自学笔记中，我们讲述了YouBot机器人底盘的运动学模型，并使用V-REP中的Dummy和Path来进行路径规划，控制YouBot机器人按照规划的路径进行运动。在本期笔记中，我们将学习如何在V-REP软件中使用Dummy和Path以及V-REP软件自带的逆运动学模块来控制YouBot的机械臂按照设定好的路径运动。话不多说，先放一个最终效果的演示视频：
学过机器人学的读者都知道，机械臂的运动学建模常用的方法是D-H法，近年来也有一些新兴的方法用来构建机器人的运动学和动力学模型，例如旋量法（旋量理论实际上就是李群李代数在机器人运动学上的应用，此方法在书籍《Modern Robotics: Mechanics, Planning, and Control》中有详细介绍，感兴趣的读者可以自行阅读）。本文介绍的是使用V-REP软件自带的逆运动学模块（IK模块）来解决机器人的逆运动学问题。
1. Dummy和Path简介
在介绍使用V-REP的IK模块进行逆运动学规划的内容前，我们先来了解V-REP提供的两个基本模型：Dummy和Path。在V-REP中提供一种带有姿态信息的“虚拟点”，叫Dummy，其本身可以被视为没有重量和体积的质点，跟数学上“点”的概念差不多，但是这个质点本身是携带姿态信息的，所以是一个特殊的点。
Dummy可以用来做什么呢？主要是两种应用场景：
作为“领航点”。Dummy常常与Path相结合来作为目标运动位置的“领航者”，例如作为机械臂末端执行机构的目标运动点，包含位置和姿态信息。Dummy与Path相结合以后，可以随着Path运动，而Path就是我们自己定义好的目标运动轨迹，属于轨迹规划的内容，有许多方法可以做。
作为“传感点”。因为其本身可以携带姿态信息，因此我们可以将它与机器人的某些部件“绑定”，从而在部件运动的时候可以从它这里获取到部件在V-REP仿真环境中的运动信息：位置、速度、姿态等，起到类似位置和姿态传感器的作用。
Dummy与其他模型一样，可以设置大小和颜色，也通过拖拽和V-REP提供的界面改变它的位置和姿态等信息。
那么Path是什么呢？Path就是其字面意思：路径。Path用来定义一条三维的运动轨迹，不管是机械臂末端的空间运动轨迹，还是小车在地面上的行进轨迹，都可以使用Path来做。在V-REP中，我们使用鼠标右键add来添加Path，主要有2种类型的Path：
Circle Type：顾名思义，就是封闭的圆圈型的Path，Path的首位相连组成一个连续封闭的路径；
Segment Type：是一种非封闭的路径，基本形状是一个由起始点和终点2个点连接组成的“线段”。
这两种类型的Path是创建复杂Path的基本组件，我们可以使用这两种组件组合来生成多种多样的Path，当然，这两种Path也可以通过设置其属性来相互转换。这里我们以Circle型的Path为例来讲一下Path如何构建。使用鼠标右键新建一个Path以后，我们可以鼠标操作改变它的位置和姿态。
在创建好一个基本的Path以后，我们选中该Path，可以通过点击下图所示的按钮来编辑这条Path：
点击进去以后，我们就可以开始编辑这条Path了。由于Path是有一些基本的控制点来生成的，我们不用指定Path上的每一个数据点，只需要设置好这些控制点即可，然后V-REP会使用这些控制点结合贝塞尔曲线方法来生成Path。Path编辑的方式包括平移整条Path，旋转整条Path，新增和删除Path的控制点（下图中的Path point）等。
这些控制点可以被改变位置和姿态，通过鼠标拖拽或者点击下图所示的状态栏按钮以后使用右上角的设置对话框来直接设定对应的参数，读者可以慢慢摸索：
除了平面的Path，通过设置这些控制点的空间位置，我们也可以生成三维空间的Path，例如本文中用到的Path就是通过这种方法来生成机械臂末端运动的空间Path的：
当然，除了手动设置，我们也可以用程序代码来生成Path，这里本文不做深入介绍，欢迎关注后续文章。Dummy和Path的基本内容讲完了，那么他们如何配合起来使用呢？首先，我们要讲Dummy绑定到Path下面，绑定的方法很简单，鼠标拖拽Dummy到Path的下方就可以了。
然后给Dummy创建一个Lua脚本，记得一定要创建为thread类型的，新开一个线程：
在创建好的Dummy脚本里面填充下面的代码，设置Dummy跟随Path运动，设置运动速度为0.1：
function sysCall_threadmain()
dummy_handle = sim.getObjectHandle('Dummy')
path_handle = sim.getObjectHandle('Path0')
print('start follow path')
sim.followPath(dummy_handle, path_handle, 1, 0, 0.1, 1)
end启动软件的仿真，就可以看到Dummy跟随Path运动的效果啦。
有了Dummy和Path，我们只需要将YouBot的机械臂末端与Dummy绑定，并结合V-REP提供的逆运动学（IK）模块即可实现机械臂的运动控制。
2. YouBot的机械臂简介
YouBot机械臂一共有5个自由度，由5个旋转关节（A1-A5）和相应的连杆机构串联组成，是典型的串联型机械臂，每个关节的运动角度范围、关节运动方向和连杆长度如图所示。这种串联型的机械臂的运动学模型在传统上一般采用D-H法来构建，但本文打算使用V-REP软件自带的逆运动学模块，不需要用户自行构建机械臂的运动学模型，只需要遵循V-REP逆运动学模型的使用方法对机械臂的各个关节进行设置即可。
3. 使用V-REP自带的逆运动学模块
V-REP自带的逆运动学（IK）模块不仅可以支持官方提供的模型，也支持用户自己设计的机器人模型，所以用户在设计好自己的机器人模型以后可以导入到V-REP中，定义好机器人模型连杆和关节的约束关系，构建好机器人模型的“树结构”，然后就可以使用V-REP自带的逆运动学模块了。在V-REP官方提供的模型当中，许多需要使用IK模块的机器人模型的关节已经被设定为IK模式了，例如YouBot机器人，因此我们可以直接使用。但是对于一个新设计的机器人模型来说，V-REP逆运动学模块的使用过程大致可以分为以下6个步骤：
将所有参与逆运动学计算的关节设定为IK模式；
添加target dummy、tip dummy；
添加Path，并让target dummy跟随Path运动；
Link target dummy 和 tip dummy；
添加IK group和IK element；
添加script，开始仿真。
3.1 将所有参与逆运动学计算的关节设定为IK模式
如下图所示，依次选择YouBot机器人机械臂的joint0 - joint3，点击左边的“放大镜”，将Joint设置为Inverse Kinematics Mode。
此外，关节可选的控制模式还有Passive Mode、Torque/force Mode等，这里YouBot机械臂的加持机构就采用的Torque/force Mode，直接给关节驱动力进行扭矩控制，而非关节的位置控制。
3.2 添加target dummy、tip dummy和Path
第2和第3个步骤是添加target dummy、tip dummy、Path和控制dummy随Path运动，添加完成以后可以改一个自己喜欢的名字。target dummy用于跟随Path运动，作为机械臂末端机构的“领航者”，而tip dummy作为机械臂末端执行机构的“传感点”，用于反馈机械臂末端的位置和姿态，同时如果将target dummy与tip dummy绑定并且构建逆运动学模型，则tip dummy可以带动机械臂跟随 target dummy运动。添加dummy和Path的过程和设定dummy跟随Path运动的过程在前面已经介绍过了，这里不再赘述。
这里我们使用Circle型的Path，添加Path以后按照前面介绍的过程调整Path为一个三维的封闭路径，如下图所示。Target dummy与Path绑定之前已经讲过了，而tip dummy需要绑定到YouBot机械臂的某一个连杆上，同样的，直接使用鼠标拉拽到对应的树形结构下即可，可以是youBotArmJoint3，也可以是youBotArmJoint4。然后按照前面介绍的设置Dummy位置的办法将taget dummy和 tip dummy的初始位置设定机械臂的夹持机构中间位置，作为路径的起点。当然，我们也可以设定到任意位置，但是一旦开始运动，机械臂会以最快的速度到达那个初始位置，一般不要这么做。如果是自己设计的机器人模型，这个初始位置就根据你所要完成的任务来设定了。
3.3 将Target dummy 和 tip dummy链接起来
这里介绍一下如何将target dummy 和 tip dummy绑定到对应的机构以及如何Link彼此，也非常的简单，点击左边的放大镜图标，在邮编选择linked dummy到youBot_positionTip，选择完成以后，在target dummy和tip dummy之间会有一个红色的箭头链接彼此，出现了这个箭头就说明设定成功了，如下图所示：
到了这一步，我们已经可以让target dummy随着Path运动了，运动的效果如下图所示：
我们只看到了target dummy随着Path运动，而机械臂却不动作，因为这还不够，我们还有2个步骤没完成。
3.4 添加IK group和IK element
前面我们将机械臂的各个关节都设定为了IK模式，这里我们需要将他们组合起来共同运动，这里需要用到V-REP的Calculation Modules，也就是左边的f(x)按钮，如题下图所示：
点击f(x)，在弹出的窗口中选择Kinematics，在下面添加新的IK Group，可以自定义名字。然后点击新建的IK_Group设定Calc.method，这里可选的有两个，一个是Pseudo inverse，一个是DLS，这里任意选一个都可以。点击Edit IK Elements，添加IK Element with tip，一个IK Group可以添加多个IK Element，将下面的Base设定为youBot表示以youBot为参考基准，此外，下面的Constraints里面的X、Y、Z表示空间位置的三维坐标信息，Alpha-beta 和 Gamma表示三维姿态信息，由于youBot的机械臂自由度不够，这里我们不选姿态信息，仅仅设置跟随位置信息即可，否则就会出现下面这种情况：
3.5 给YouBot添加Script
前面的教程里面我们已经详细的介绍了如何使用Lua代码来控制机器人，这里不再赘述，对于本文的目标来说，我们只需要做一个工作：调用sim.setJointTargetVelocity()函数控制机器人的四个轮子不让其随意运动即可，即将其速度设置为零，详细的代码见Gihub仓库。最后的效果如下图所示：
到此为止，使用V-REP自带的IK模块控制YouBot机器人的机械臂末端跟随一个自定义的路径运动就已经介绍完了，其中定义路径的Path可以由用户根据需要进行自定义，本项目涉及到的代码都已发布在Github上，欢迎大家Star和下载使用。
项目地址：https://github.com/chauby/V-REP-YouBot-Demo.git
往期回顾
【V-REP自学笔记（三）】用代码控制机器人
【V-REP自学笔记（四）】键盘控制YouBot机器人
【V-REP自学笔记（五）】YouBot底盘运动学与路径规划
参考
YouTube上有一个国外小哥的教学视频，英文解说的，口音有点重，听起来像印度小哥，如果感兴趣的可以去看一看，地址：
https://www.youtube.com/watch?v=X73NzCc2E2c
作者知识有限，有不正确的地方还望读者能够指出来，大家共同学习和进步，不胜感激！想要获取更多文章，欢迎扫描或长按下方二维码关注公众号：博士的沙漏。
