水下自主作业机械臂轨迹规划与抓取仿真

张祥

(中国船舶集团有限公司第七一三研究所,河南郑州 450015)

0 引言

水下自主作业[1]技术主要涉及智能装备在水中通信与目标探测识别、水下释放、回收及动平衡作业等关键技术,探索复杂流场对智能装备运动规律的影响,突破微光环境中目标识别、基于智能控制算法的水下对接回收、智能规划作业等关键技术,为安全高效作业提供技术支撑。水下作业机械臂作为水下无人系统的重要组成部分,是执行水下作业任务的关键执行机构,是最终实现抓取任务的执行机构。本研究以水下无人系统中的水下作业机械臂为研究对象,设计机械臂的轨迹规划与抓取控制方法,基于V-REP平台开展机械臂自主作业试验进行验证。

机械臂的水下作业是一个较为复杂的工作过程,其中涉及了大量控制算法的应用,现有的算法开发与验证过程较繁琐,开发周期长且研发成本高。V-REP是一个具有集成开发环境的机器人3D仿真平台,用于机器人模型及其控制系统的快速算法开发、快速原型验证等[2]。V-REP提供了许多功能模块,可以通过API接口和脚本功能集合和组合以满足使用要求,可支持的编程语言包括Python、C/C++、Lua、MATLAB和Octave等。张世超等[3]利用VREP与MATLAB完成机器人加工仿真,在运动仿真中进行碰撞检测和仿真轨迹点的误差分析。任建新[4]等提出了一种基于Visual Studio与V-REP的联合仿真方法,可以验证了机器人运动学模型以及正逆解的准确性。郑怡廷[5]等以UR10工业机械臂为模型,通过Python语言编写的操控软件将UR10机械臂和V-REP平台结合起来,以实现UR10可视化仿真,并进行了仿真交互验证。

机械臂自主作业的任务是在探测到目标物时,通过控制末端执行器手爪运动到指定位置,确认到达抓取位置后控制末端执行器抓取目标物,然后携带目标物进行一定高度的提升。为了完成抓取,首先需要进行机械臂末端的轨迹规划,生成从起始位置到抓取位置的目标运动点。然后,设计机械臂的运动控制结构,便于在V-REP仿真平台中进行控制程序的开发。最终,借助机械臂、目标物和环境模型,开展水下作业机械臂自主抓取实验,验证方法的有效性。

1 机械臂末端轨迹规划方法

机械臂在水下作业时,利用其末端搭载的机械手完成夹持等操作。为了保证机械臂末端执行器可以按照预先设定的连续路径进行作业,需要在笛卡尔空间对机械臂末端进行轨迹规划,分为全局路径规划与局部轨迹插补两个部分。

1.1 全局路径规划

由于机械臂末端执行器和目标物均有一定的形状,在运动过程中为了避免干涉,需要在路径规划时设计机械臂末端的运动轨迹规避目标物的外包络。较为便捷的方法是在机械臂末端的运动起始点和抓取位置终点之间,设置运动过渡点,其主要目的就是将机械臂抓取的运动过程分为如下两个阶段。第一阶段是由起始点运动到过渡点,这两点间的运动轨迹上没有任何障碍物,因为此运动过程中距目标物较远,也不会与目标物产生运动干涉。第二阶段是由过渡点运动到抓取终点,这时机械臂末端执行器距离目标物会越来越近,需要结合末端执行器的形状与目标物的三维特征进行局部规划插补,避免产生抓取时的碰撞。此外,考虑到机械臂在水下运动时水动力特性的干扰,在设计期望轨迹时需要尽可能保证连续与平滑。综上,运动过渡点的设计需要符合下列约束:起始点到过渡点没有任何障碍物;过渡点到终点避免与目标物运动干涉;期望的运动轨迹平滑。

通过在V-REP仿真环境中搭建双目视觉系统,可以实时探测到目标物的中心位置坐标、外部形状和尺寸。根据目标物的测量信息,并且在运动过渡点的设计约束下,通过A*算法搜索全局运动路径。A*算法是一种在已知地图信息的条件下求解运动路径的搜索方法,其通常在二维空间进行路径搜索,而机械臂抓取目标物的操作是在三维空间进行的。因此,在搜索时以V-REP俯视图作为搜索地图。

A*算法搜索时,需要将待搜索的区域简化为二维图形空间,如图1所示,其中灰色区域代表障碍物,黄色区域为搜索路径。设置两个表open和closed,搜索步骤如下:

图1 A＊全局搜索路径Fig.1 Global search path of A＊ algorithm

(1)计算初始节点和其周围节点的代价估计F(n)=g(n)+h(n),其中g(n)为实际代价,其值为当前节点到起始点的距离,h(n)为估计代价,其值为当前节点到终点的距离,并将初始节点周围节点放入到open列表中;

(2)选取open列表中F(n)值最小的点n,计算其周围节点x的F值;

(3)如果节点x已经在表open中,并且重新计算的F值小于原值,则将节点n设置为节点x的父节点,并且更新表open中节点x的F值;如果节点x不在表open中,则将节点n设置为节点x的父节点,并求解节点x的F值添加到表open中;

(4)将节点n添加到表closed中;

(5)判断终点是否已经在表open中,如果没有则返回步骤b继续搜索,如果存在则搜索结束,其路径为从终点开始,每个节点沿着父节点移动至起点。

从过渡点到抓取终点的运动路径需要保证二维特性,即过渡点与抓取终点在Z轴方向上的坐标值需保持一致,由此过渡点在Z轴方向上的位置可以确定。对于过渡点的X轴、Y轴坐标值,可以选取A*搜索路径上距目标物外围最近一点的值。

1.2 局部轨迹插补

全局路径规划确定了机械臂抓取运动过程中的起始点、过渡点和终点,然而在实际运动控制程序开发中,只有这三个点无法进行完整的位置跟踪和速度控制,因为缺失中间过程的位置坐标。因此,需要对整个运动轨迹进行插补,按照时间序列生成一系列运动目标点,输入到运动控制器中进行跟踪。插补内容包括从起始点到过渡点、从过渡点到终点两个阶段的位置坐标插补。常用的轨迹插补有直线插补和圆弧插补,其他轨迹都可以通过直线或者圆弧进行逼近。为了实现机械臂的快速抓取并避免与目标物的运动干涉,本文选取直线插补进行局部轨迹规划。

基于机械臂抓取任务的直线插补可以按照如下流程进行:

(1)设定机械臂末端期望的运动速度v和控制程序单次运行的周期Ts;

(2)计算机械臂抓取运动过程中的起始点到过渡点的距离:

(3)求起始点到过渡点插补的总时间T:

(4)计算插补的总次数N。当T/Ts的余数为零时,N=ent(T/Ts);当余数不为零时,N=ent(T/Ts)+1,其中ent函数表示取整。

(5)计算插补增量:

(6)计算第i点的插补坐标:

对于机械臂抓取运动过程中的过渡点到终点的插补,参照以上流程进行计算。

2 抓取控制结构

机械臂在水下作业时,主要的工作内容为利用末端机械手,对水下识别到的目标物进行抓取,因此要求末端执行器在整个运动过程中保持平稳。对于水下作业的场景来说,在识别出目标物之后,整个系统的工作场景已经相对固定,且光源也比较固定。因此,本研究中将视觉系统和机械臂分离,把成像元件固定在机械臂本体之外,并且确保目标物在视场范围内,V-REP中的结构化环境适宜采用上述配置。

图3 V-REP平台实验环境搭建Fig.3 Experimental environment construction of V-REP platform

图4 机械臂抓取仿真试验过程Fig.4 The experimental process of manipulator grasping simulation

整个系统的控制结构如图2所示。视觉定位系统识别出目标物后,对其进行三维坐标测量。通过机械臂末端轨迹规划,对测量数据经过分析后,求解出机械臂末端执行器运动路径,即机械臂末端轨迹的一系列位置坐标。这些位置坐标作为机械臂最终期望的运动位置。通过上述流程得到的位置坐标作为机械臂运动的输入数据,与实时读取的末端执行器位置数据比较后,将两者的差值输入到偏差控制算法当中。然后通过机械臂的逆雅克比矩阵,将操作空间的数据变换到关节空间当中,从而驱动机械臂各个关节完成相应动作。

图2 系统的控制结构Fig.2 Control Structure of the whole system

3 仿真设置与实验

为了对机械臂的抓取过程进行仿真验证,利用机器人仿真软件V-REP搭建机械臂抓取试验环境,主要内容包括六自由度机械臂UR5、末端抓取机械手、目标物和双目视觉传感器组件,抓取仿真试验的设备配置如表1所示。

表1 机械臂抓取仿真试验配置Tab.1 Experiment Configuration for manipulator grasping simulation

其中,UR5六自由度机械臂作为运动机构,负责抓取作业运动过程的执行。在机械臂的末端搭载有抓取机械手,整体结构仿二指手爪型,通过机构的打开和闭合完成对目标物体的抓取操作。本次试验的目标物选取长方体形状的实体,表面颜色为橘黄。双目视觉传感器组件选取V-REP中的视觉传感器部件,将两只传感器通过一定的间距设置组成双目视觉系统,并建立浮动窗口将双目视觉系统的图像进行实时显示。基于V-REP搭建的实验环境如图3所示。

试验中,将机械臂抓取的运动控制算法通过Lua编程写入到UR5机械臂的线程脚本中。通过脚本函数,将机械臂末端的轨迹进行输入,主要包括起始点、过渡点和抓取终点间的轨迹规划点。整个抓取试验的过程为,首先根据目标物外形尺寸,计算出抓取点和过渡点的位姿数据,然后结合起始点的位姿进行两段轨迹规划,第一段运动为机械臂从起始点运动到过渡点,第二段为从过渡点运动到抓取终点。在到达过渡点时,设定机械臂停留1秒钟,给末端机械手留有充足的时间打开手爪,随后运动到抓取的终点位置,停留1秒钟,闭合手爪,完成对目标物的抓取。整个试验的过程及结果如图4所示。

在整个抓取仿真试验过程中,机械臂末端可以准确运动到指定位置,运动过程中的轨迹平滑,可以正常完成对目标物的抓取,并进行提升。

4 结语

通过在V-REP平台中的机械臂抓取仿真试验,对机械臂末端轨迹规划和抓取方法进行了可行性验证,试验结果表明设计的轨迹规划和抓取算法可行性较高,实用性较强,可以有效地推广到机械臂的运动控制应用当中。