CN108818530B - 基于改进rrt算法的机械臂抓取散乱堆放活塞运动规划方法 - Google Patents

Abstract

一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法。先采用D‑H法描述六轴机械臂的位姿，然后计算得到六轴机械臂的D‑H模型；再建立活塞毛坯上料平台的ROS仿真系统：最后主要利用改进的RRT算法完成机械臂的运动规划：改进的RRT算法将规划过程分为两个阶段，在第一阶段解决障碍活塞导致的避障问题和提高算法的规划速度，分成三个步骤，第一步骤对目标活塞周围区域进行3D；第二阶段是引入瞬态测试和细化控制函数，并构造路径最短原则，从而提高改进的RRT算法规划的路径质量；本发明适用于机械臂抓取散乱堆放活塞，节省了规划的时间。

Description

基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法

技术领域

本发明涉及工业机器人运动规划技术领域，具体涉及一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法。

背景技术

中国是第一汽车生产大国，而活塞作为汽车发动机的“心脏”，是最为关键的零部件之一，因此活塞的加工效率对汽车的生产是至关重要的。在活塞生产线上，上料工序依靠人工抓取和放置，劳动强度大，极大降低了整个生产线的自动化程度，所以急需一种代替人工自动上料的方式。为解决该问题，建立了一套基于机器视觉的毛坯活塞上料系统，它主要分为识别和抓取两部分，识别是通过图像处理识别出待抓取的目标及其姿态；抓取是在获取目标及其姿态之后，通过机械臂抓取并按要求的位姿放置活塞。机械臂的运动规划是抓取的关键技术。

在机械臂的运动规划算法的方面，传统的栅格法，如A*，D*和 Phi*等，对于高维复杂问题，环境信息需要很大的存储量，计算效率很低；粒子群优化，蚁群优化和遗传算法等进化算法适用于多目标问题；其他进化算法，如人造蜂群，细菌觅食寻优和生物启发神经网络算法等容易陷入局部最小值，且它们对搜索空间大小和数据的变化非常敏感；而基于随机采样的运动规划算法由于其计算成本低，所以更适用于高维空间问题。最流行的随机采样算法是快速扩展随机树 (RRT)及其各种改进算法，RRT算法解决了运动规划算法速度过慢的问题，但是得到的路径质量并不好，而且对于机械臂抓取散乱堆放活塞的运动规划问题，RRT算法不能解决目标活塞周围的障碍活塞引入的约束问题，并且在接近目标活塞时算法的收敛速度也有待提高。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法，适用于机械臂抓取散乱堆放活塞，节省了规划的时间。

为了实现上述目的，本发明采取的技术方案是：

一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法，包括以下步骤：

步骤1、采用D-H法描述六轴机械臂的位姿，建立六轴机械臂的连杆坐标系，确定六轴机械臂的D-H参数，然后计算得到六轴机械臂的D-H模型；

步骤2、建立活塞毛坯上料平台的ROS仿真系统：

2.1)设计整个ROS仿真系统的结构图；

2.2)通过编写URDF文件，描述仿真系统中的机械臂的各个关节和各个平台的位置，以及各个零件之间的连接状况，再设计仿真系统的节点图，控制仿真系统内的各个节点之间的通信；

步骤3、利用改进的RRT算法完成机械臂的运动规划：改进的RRT 算法将规划过程分为两个阶段，在第一阶段解决障碍活塞导致的避障问题和提高算法的规划速度；第二阶段是引入瞬态测试和细化控制函数，并构造路径最短原则，从而提高改进的RRT算法规划的路径质量，改进的RRT算法包括以下步骤：

3.1)获取目标活塞周围区域的点云Cloud_goal和机械臂的初始姿态 q_init，以及图像识别出的目标活塞的位姿p_goal，通过对目标活塞周围区域的点云进行3D建图得到Octrees模型；

3.2)根据当前目标工件的位姿p_goal和机械臂手爪与Octrees模型碰撞检测的结果，确定出机械臂在抓取目标工件时的姿态q_goal和抓取方向dir_goal；

3.3)在轨迹规划中引入一个机械臂的预姿态q_pr，并记该位置点为预姿态点Pr，设Pr点与目标点之间的距离为l，路径点的数量为 Num，通过空间坐标转换公式的计算出从Pr点到目标点之间的路径点，从而完成第一阶段的规划；

3.4)第二阶段是在原有的RRT算法的基础上，引入瞬态测试和细化控制函数，并构造路径最短原则，规划出从Pr点到作业原点之间的路径。

本发明的有益效果为：

1.通过对目标活塞周围区域的3D建图，不需要识别出障碍活塞的位姿及其简化模型就能获取目标活塞周围障碍物的空间占有信息。

2.改进的RRT算法的在第一阶段运动规划的距离较短，利用这一特性，通过计算出的手爪的抓取方向，进而算出机械臂在Pr点和目标点处的姿态，算法将碰撞检测的时间集中在Pr点和目标点处，节省了规划的时间。

3.通过空间坐标转换公式计算出从Pr点到目标点之间的路径点的效率比RRT及其改进算法更高。

4.改进的RRT算法可以成功运用于活塞毛坯上料平台上，实现利用机械臂完成活塞毛坯的上料。

附图说明

图1为实施例活塞毛坯上料平台的ROS系统结构图。

图2为实施例ROS系统的节点图。

图3为实施例算法流程图。

图4为实施例抓取方向计算流程图。

图5为实施例坐标系。

图6为实施例成功率随规划时间变化折线图。

具体实施方式

下面结合附图和实施例对本发明作详细描述。

一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法，包括以下步骤：

步骤1、采用D-H法描述六轴机械臂的位姿，建立六轴机械臂的连杆坐标系，确定六轴机械臂的D-H参数，然后计算得到六轴机械臂的D-H模型；

基于机器视觉的活塞毛坯上料实验平台由型号为RS20N的川崎六轴机械臂、毛坯工作台、中转台、传输带、三台PC机、kinect相机、工业摄像机等组成；

在活塞上料实验平台上，先识别出在毛坯工作台中的一个活塞的姿态，然后通过改进的RRT算法实现机械臂的运动规划，将活塞从毛坯工作台抓取至中转台，并在中转台上调整好姿态后，最后将活塞抓取放置到传输带上；选取活塞毛坯实验对象的主要尺寸：最大直径 120mm,高度125mm；

对于活塞上料系统中使用的川崎RS20N六轴机械臂，首先建立机械臂各连杆关节的坐标系，然后计算得到川崎机械臂的D-H模型，结果如表1所示，

表1川崎RS20N机器人D-H参数

表1中，a_i表示第i个连杆的长度；α_i第i个连杆的转角；d_i第 i个连杆的偏距；θ_i第i个关节的转角；获取机械臂的D-H参数之后，就可以对机械臂抓取活塞进行运动规划；

步骤2、建立活塞毛坯上料平台的ROS仿真系统：

2.1)设计整个ROS仿真系统的结构图；

在Ubuntu 16.04LTS、ROS kinetic平台下搭建的活塞上料平台的 ROS系统，主要运用Rviz作为3D可视化工具，并使用MoveIt搭建机械臂的运动功能包，它可以完成机械臂的正逆运动学求解、运动路径规划、碰撞检测等，活塞上料系统在ROS中的MoveIt系统结构如图 1所示，在用户接口中，通过move_group_interface实现机械臂的动作控制，它负责和move_group核心进行信息交流，其中包括需要完成的动作、机械臂末端执行器的张合、目标活塞信息等；在ROS参数模块，URDF文件定义了各个部件的物理参量及部件间的连接关系等， SRDF文件中定义了机械臂的运动规划群，Config文件存放路径规划、 PID控制等配置参数；

2.2)通过编写URDF文件，描述仿真系统中的机械臂的各个关节、大臂、小臂和手爪及其环境中载物台的简化模型，以及各个零件之间的连接状况，再设计仿真系统的节点图，控制仿真系统内的各个节点之间的通信；

毛坯台和中转台上的视觉传感器分别发送点云和图片给move_group核心，move_group核心将计算得到的机械臂状态发送给川崎机械臂，最后机械臂完成抓取运动，活塞毛坯上料平台的ROS 系统运行时的节点图如图2所示，图2中椭圆形的代表系统的节点，每条连接线代表节点间通信的话题，/BinPicking为用户编写用于发布控制机械臂指令的节点，/move_group为整个活塞毛坯上料平台的 ROS系统的计算核心，/joint_state负责发布机械臂各个关节的状态，包括位置，速度和加速度，/robot_state负责发布机械臂的位姿；

步骤3、利用改进的RRT算法完成机械臂的运动规划：改进的RRT 算法将规划过程分为两个阶段，在第一阶段解决障碍活塞导致的避障问题和提高算法的规划速度；第二阶段是引入瞬态测试和细化控制函数，并构造路径最短原则，从而提高改进的RRT算法规划的路径质量。改进的RRT算法中，步骤3.1)至3.3)完成的第一阶段构建树T₁，步骤3.4)完成第二阶段构建树T₂，最后合并树T₁和树T₂得到最终规划的路径，改进的RRT算法的流程图如图3所示，包括以下步骤：

3.1)获取目标活塞周围区域的点云Cloud_goal和机械臂的初始姿态 q_init，以及图像识别出的目标活塞的位姿p_goal，通过对目标活塞周围区域的点云进行3D建图得到Octrees模型；

构建初始姿态q_init与预姿态q_pr之间的路径树T₁时，Pr点在机械臂手爪的抓取方向上选取，该点距离目标点的长度为l；传统运动规划算法解决这段路径的思路一般是：通过图像识别出物体的位姿，再计算其简化模型，在运动规划的碰撞检测中增加该简化模型与机械臂的碰撞对；但是在该问题中，目标活塞周围的障碍活塞众多，而且难以保证全部正确的识别出姿态；机械臂接近目标点时，由于进行大量的碰撞检测，会使算法收敛缓慢；

将目标活塞姿态和3D点云的信息直接引入到改进的RRT运动规划算法的第一阶段中，来解决上述问题，其优势在于：1)通过对目标活塞周围区域的3D建图，不需要识别出障碍活塞的位姿及其简化模型就能获取目标活塞周围障碍物的空间占有信息；2)在第一阶段运动规划的距离较短，利用这一特性，通过计算出的手爪的抓取方向，进而算出机械臂在Pr点和目标点处的姿态，算法将碰撞检测的时间集中在Pr点和目标点处，节省了规划的时间；3)通过空间坐标转换公式的计算出从Pr点到目标点之间的路径点的效率比RRT算法更高；因为机械臂在此段路径上运动时，手爪的方向保持不变，所以计算从 Pr点到目标点之间的路径点过程，与求Pr点的方式大致相同，只需要依次改变相对终点的距离值，就可以计算出T₂树中其他的路径点；

3.2)根据当前目标工件的位姿p_goal和机械臂手爪与Octrees模型碰撞检测的结果，确定出机械臂在抓取目标工件时的姿态q_goal和抓取方向dir_goal；机械臂抓取方向的计算流程图如图4所示：

计算的流程是先建立活塞的离线抓取方向集(N₁,N₂,…,N_s)，用四元数表示保存一个在离线库中；

通过基于最小切割的点云分割算法分割点云，再通过模型匹配得到一个活塞毛坯位姿，该活塞毛坯即为机械臂的运动规划中的目标活塞；根据此时目标活塞的姿态，将离线抓取方向集(N₁,N₂,…,N_s)做一次旋转变换得到目标活塞在该位姿下对应的可抓取方向集(N₁,N₂,…,N_s)；记e_n为地面的法向量；遍历旋转后的方向集，按公式(1)计算N_i与e_n的夹角θ_i，如θ_i不小于45°则直接舍弃该方向；公式(1)中，r_i和p_i为抓取方向N_i对应欧拉角的R和P的分量；

θ_i＝arccos(cos(r_i)cos(p_i)) (1)

根据目标活塞的位置，截取固定大小的点云，然后对其进行3D 建图，获得Octrees模型；采用的是OctoMap库进行建图，该框架的建图方法基于八叉树(octrees)数据结构，并使用概率占用估计算法分析点云的空间占有情况；

然后将预先求得机械臂手爪的包围盒置于满足角度要求的方向 N_i上，再与Octrees模型进行碰撞检测，验证手爪在此方向上运动是否会发生碰撞；若不会发生碰撞，则计算该方向的安全系数，当遍历整个离线抓取方向集之后，选取安全系数最大的方向N_i为机械臂手爪的抓取方向，再通过机械臂的逆运动学计算出机械臂在抓取目标活塞时的姿态q_goal；

3.3)在轨迹规划中引入一个机械臂的预姿态q_pr，并记该位置点为预姿态点Pr，设Pr点与目标点之间的距离为l，路径点的数量为 Num。Pr点处机械臂位姿的计算：

如图5所示，记world为世界坐标系，target为目标活塞坐标系，Pre-tar为Pr点处活塞坐标系，Link6为Pr点处机械臂的关节6的坐标系，

目标活塞在世界坐标系下的位姿为T_tar：

[x_tar y_tar z_tar]为目标活塞在世界坐标系下的坐标，R_tar是目标活塞相对世界坐标系的旋转矩阵，从Δ_tar到Δ_{Pre_tar}的坐标变换为T_tar-pre：

记从抓取点到关节6的坐标变换为T_holder：

则机械臂在Pr点时，从Δ_tar到关节6的坐标系变换为T₁₂，求解公式如式(2)所示；

T₁₂＝T_tar-preT_holder (2)

求得此时关节6在世界坐标系中的位姿如公式(3)所示；

T_F＝T_tarT₁₂ (3)

最后通过机械臂的逆运动学运算得出机械臂在Pr点处时各个关节的值；

3.4)在第二阶段中，构建初始姿态q_init与预姿态q_pr之间的路径树 T₂时，其中引入了细化控制(refinementControl)函数和瞬态测试 (transitionTest)函数，从而改善路径，瞬态测试函数中使用的代价函数为路径机械功函数,定义如(4)式：

(1)式中，

是两个相邻正极值之差，ε是一个非常小的量，l表示初始姿态q_init到预姿态q_pr的距离；最后以最短路径为准则，迭代20 次，选取路径最短的为树T₂；

为了验证改进的RRT运动规划算法，测试了机械臂从作业原点位置到抓取目标活塞这一路径的运动规划，在改进的RRT运动规划算法的第二阶段，规划速度不成问题，所以主要测试其在规划的路径质量上的表现；而在第一阶段，主要是观察改进的RRT算法在规划速度上的优越性。

对于机械臂从作业原点位置运动到Pr点这一阶段，在活塞毛坯上料平台的ROS系统中测试了RRT-connect算法与改进的RRT运动规划算法的效果。由于算法都是基于概率估计的，所以每次规划的结果都不太一样，选取两种算法在100次规划实验中的得到典型轨迹，改进的RRT算法规划的轨迹曲线比RRT-connect的曲线光滑，且曲线长度短，表明改进的RRT算法规划路径的质量整体要好于RRT-connect 算法。

对于从Pr点运动到目标点这段短路径，现有的RRT算法需要满足方向约束条件从而避免与非目标活塞发生碰撞，因此计算效率并不高。图6展示了RRT-connect算法的规划时间与规划成功率关系的折线图。定义规划时间是为了判别算法是否成功规划出路径(否则规划失败时，程序会一直运行下去)。从图6可以看出，RRT算法只有当规划时间超过6秒时，规划的成功率才能达到100％，计算效率很差。改进的RRT算法因为计算出抓取方向之后，就可以保证一定可以求出从Pr点运动到目标点这段短路径，所以并不需要规划时间来判别算法是否可以规划出路径。表2对比了改进的RRT算法和 RRT-connect算法从Pr点运动到目标点这段路径上在100次仿真实验中的计算效果。

表2算法运行时间比较

注：1)改进的RRT算法在此处计算9个路径点

从表2可以看出改进的RRT算法在时间上比直接运用 RRT-connect算法要快2个数量级。虽然改进的RRT算法在从Pr点运动到目标点这段路径上，计算时间会随该路径上选取路径点的数量增加而增加，但是由于此路径本身较短，并不需要太多路径点，所以时间也不会增加太多。

Claims (1)

1.一种基于改进RRT算法的机械臂抓取散乱堆放活塞运动规划方法，其特征在于，包括以下步骤：

步骤1、采用D-H法描述六轴机械臂的位姿，建立六轴机械臂的连杆坐标系，确定六轴机械臂的D-H参数，然后计算得到六轴机械臂的D-H模型；

步骤2、建立活塞毛坯上料平台的ROS仿真系统：

2.1)设计整个ROS仿真系统的结构图；

2.2)通过编写URDF文件，描述仿真系统中的机械臂的各个关节和各个平台的位置，以及各个零件之间的连接状况，再设计仿真系统的节点图，控制仿真系统内的各个节点之间的通信；

步骤3、利用改进的RRT算法完成机械臂的运动规划：改进的RRT算法将规划过程分为两个阶段，在第一阶段解决障碍活塞导致的避障问题和提高算法的规划速度；第二阶段是引入瞬态测试和细化控制函数，并构造路径最短原则，从而提高改进的RRT算法规划的路径质量，改进的RRT算法包括以下步骤：

3.1)获取目标活塞周围区域的点云Cloud_goal和机械臂的初始姿态q_init，以及图像识别出的目标活塞的位姿p_goal，通过对目标活塞周围区域的点云进行3D建图得到Octrees模型；

3.2)根据当前目标工件的位姿p_goal和机械臂手爪与Octrees模型碰撞检测的结果，确定出机械臂在抓取目标工件时的姿态q_goal和抓取方向dir_goal；

3.3)在轨迹规划中引入一个机械臂的预姿态q_pr，并记该位置点为预姿态点Pr，设Pr点与目标点之间的距离为l，路径点的数量为Num，通过空间坐标转换公式的计算出从Pr点到目标点之间的路径点，从而完成第一阶段的规划；

3.4)第二阶段是在RRT算法的基础上，引入瞬态测试和细化控制函数，并构造路径最短原则，规划出从Pr点到作业原点之间的路径。

Images