基于新型RRT算法与视觉定位的机械臂工件抓取*

李 哲，于海生，吴贺荣，孟祥祥

(青岛大学a.自动化学院；b.山东省工业控制技术重点实验室，青岛 266071)

0 引言

随着机器人技术的发展，工业机器人已广泛应用于各类工业现场，而随着工业任务的复杂多样化，机器人在运动规划、抓取精度等方面有着更加严格的要求[1]。机械臂在执行任务过程中，通常会被工作空间的一些物体所阻碍，如果以传统示教的方式进行控制，在避免碰撞等方面会有很高的难度。智能避障路径规划算法[2]可以有效地解决这种问题，使得机械臂可以适应更加复杂的环境。同时传统示教模式下执行任务大都是进行机械式地重复，如果工件位置出现偏差，机器人就可能与工件发生碰撞而导致作业失败。而随着机器视觉技术的迅速发展，将机器视觉应用到工业机器人可以赋予其对目标的识别定位能力[3]，机器人智能性得到极大的增强。

针对机械臂避障问题，国内外学者提出了一些路径规划的方法，主要有人工势场法[4]，A*算法[5]，遗传算法[6]，快速扩展随机树 (RRT)[7-8]等。其中RRT算法因其不需要预处理状态空间，以及搜索速度快,便于处理约束等特点,更适用于本系统。但基本的RRT搜索算法具有盲目性的缺陷[9-10]，近年来各个学者提出了许多基于RRT的改进算法。CAO等[11]在节点扩展时考虑了目标偏向思想，引导节点向目标点扩展并采用遗传算法优化所生成路径。代伟等[12]使用Sobol序列代替快速扩展随机树中的伪随机序列，提高了采样点质量和采样效率。针对目标位置不确定问题，国内外学者提出了一些利用机器视觉技术[13-14]进行识别定位的方法。刘正琼等[15]采用提取工件上特征点进行形状匹配的方法来实现工件的识别，并且设计了由粗到精抓取网络的方案。JIANG等[16]提出了基于多重腐蚀和膨胀算子的图像处理方法，并基于掩模图像的多次减法来唯一确定抓取角度。

基于以上分析，本文提出一种基于目标偏置的拓展点选择策略，使得算法在采样过程中以一定概率向目标点快速拓展，达到算法快速收敛的目的，并通过路径优化和三次样条插值法，除去规划路径中的冗余点，使得规划的路径精炼且平滑连续。在视觉定位方面，本文分别采用霍夫圆检测算法与轮廓提取算法对工件与托盘凹槽进行识别，通过对目标工件和托盘的双重检测，使目标识别更加精准，增加了目标定位的准确性和稳定性。

1 机械臂抓取系统设计

本文建立的机械臂智能抓取系统，共分为机械臂避障路径规划与目标工件识别两个模块，由ABB公司生产的IRB120型6轴机械臂、维视图像的MV-E EM系列工业相机、PC机构成。其中PC端与相机系统总体构成如图1所示。

图1 系统总体结构图

在避障路径规划模块中，通过新型快速拓展随机树算法对起始点到原目标点上侧之间进行路径规划，采用包络法将障碍物以及机械臂关节进行包络以简化碰撞检测，并采用三次样条插值法针对所规划路径点进行连接平滑化。

当机械臂到达原工件位置上侧，托盘和工件即使发生位置偏差也会出现在相机视野中。在目标视觉定位模块中，利用维视图像SDK采集图像并进行预处理和边缘检测。在处理完成后，本文通过霍夫圆检测算法进行圆型工件识别，同时通过轮廓提取和多边形逼近法识别出托盘凹槽。最终，将凹槽内工件的坐标经解算后发送给机器人控制系统。

2 避障路径规划

2.1 碰撞检测

节点拓展算法中每随机出一个新节点，都需要通过碰撞检测来判别节点的有效性，所以机械臂路径规划的效率和碰撞检测的效率直接相关。因机械臂在工业任务中，所需考虑的碰撞对象为机械臂连杆和环境中的刚体，所以本文通过包络法将碰撞检测进行简化。

为了简便运算，通过包络法将环境中的障碍物用球体进行包络，将机械臂连杆用圆柱体进行包络，同时将圆柱体的半径叠加到球体的半径上r=r1+r2，由此将碰撞检测问题转换成线段和球的位置关系，计算量得到大大简化。

设球心O到直线AB的垂直距离为dvert，到点A和点B的相对较短距离为dmin=min(dOA,dOB)，则根据包络体之间的位置关系分为以下情况讨论：

(1)球心在线段上的垂足p位于连杆AB上，且dvert>r，则连杆与障碍物不发生碰撞；

(2)球心在线段上的垂足p位于连杆AB上，且dvert

(3)球心在线段上的垂足p位于连杆AB的延长线上，且dmin>r，此时连杆与包络球体没有交点，即与障碍物不会发生碰撞；

(4)球心在线段上的垂足p位于连杆AB的延长线上，且dmin

图2 包络法障碍物建模

2.2 RRT算法原理

图3 随机树拓展

本文利用RRT算法在机械臂关节空间中进行搜索，得到一条从初始角度qinit到目标角度qgoal的无碰撞路径，避免了在笛卡尔空间中进行规划所需的大量逆运动学求解，算法示意图如图3所示。

算法基本原理是：

步骤1：将拓展树T进行初始化，并添加初始点qinit；

步骤2：通过节点生成算法产生随机点qrand；

步骤3：在拓展树中搜索与随机点qrand距离最短的邻近点qnear；

步骤4：通过拓展算法沿向量qnear、qrand的方向生成qnew；

步骤5：将新产生的节点qnew和邻近点qnear之间进行碰撞检测，在不发生的碰撞的情况下保留qnew，否则转到步骤2重新寻找节点；

步骤6：检查节点qnew与目标点qgoal之间的距离，如果新产生的节点已到达目标点或者在一定约束范围以内，则成功结束算法；

步骤7：如果算法没有成功结束，则重复进行步骤2～步骤6。

虽然传统的RRT算法可以在机械臂工作空间中进行应用，但是在规划过程中存在很多缺陷，比如算法规划时间长、导向性差、路径点冗余以及路径不够光滑等。针对以上问题，本文提出相应的解决方案进行改进。

2.3 RRT算法优化

2.3.1 拓展点选择策略

针对传统的RRT算法在路径规划中的高随机性、收敛时间长等特点，本文提出一种基于目标偏置的拓展点选择策略。基于概率Pthr的目标偏置策略的RRT算法伪代码描述如表1所示。

表1 拓展点选择策略

策略核心思想主要是在迭代过程中引入目标偏置概率Pthr，并在0～1范围内取随机概率Pran，若Pran大于阈值Pthr，则拓展树向目标点qgoal方向开拓新节点，否则，拓展树向随机生成函数产生的新节点方向进行生长。

另考虑拓展过程中所收集到的空间和障碍物信息，对目标偏置概率Pthr进行调整。当新节点拓展过程中没有与障碍物发生碰撞时，增大Pthr，当拓展过程中与障碍物发生碰撞时，减小Pthr，以使随机树更快地覆盖较少受阻的空间区域。

2.3.2 路径优化

在使用上述拓展点选择策略的情况下，因算法本身的随机性所产生的节点仍有可能冗余，使得规划的路径有所偏离，所以有必要对路径中的节点进行筛选优化。路径优化的伪代码描述如表2所示。

表2 路径优化策略

算法首先确定RRT成功搜索后的路径点数组Pplan长度，并将Pplan中的初始路径点添加到优化路径点数组Pnew当中。之后对初始路径点Pplan[1]与相差一个路径点的Pplan[3]进行碰撞检测，如果没有发生碰撞，则说明中间路径点Pplan[2]不是必要路径点，删去该点并检查Pplan[1]与Pplan[4]的碰撞情况，以此类推，直到检测出发生碰撞为止；如果发生碰撞，则该中间点为必要路径点，将其添加到优化路径点数组Pnew当中，接下来从该中间路径点开始进行以上步骤，最终得到优化后的路径点数组Pnew。

2.3.3 平滑处理

算法规划的路径在通过优化之后仍会存在曲线折段，若节点之间差距过大则会导致机械臂在运行过程中产生较大的抖动和冲击，机械臂的平稳性不能得到有效保证，甚至可能受到损坏。

本文采用三次样条插值法对经过优化过后的路径点进行平滑连接，使规划后的末端轨迹更加平滑，减少机械臂在运动中的卡顿和抖动现象。

θi,j=a0+a1t+a2t2+a3t3

(1)

式中，i∈(1,n)为规划后的路径点个数；j∈(1,6)分别为机械臂的6个关节轴。

3 目标工件的视觉定位

3.1 预处理及边缘检测

相机直接采集的图像，通常会受到各种噪声的干扰，因此本文采用图像滤波、图像分割等方法对图像作预处理以改善目标的特征信息。

中值滤波采用非线性的方法，可以保护图像尖锐的边缘，在平滑脉冲噪声方面非常有效，所以本文在对图像进行灰度化后，采用3×3的中值滤波模板进行图像滤波，针对图像中的每一个像素值，都采用该点与其邻域中内8个像素点的中值代替。

(2)

图像分割方法一般分为全局二值化和局部二值化，其中全局二值化可能会过滤掉图像中必要的特征信息，故本文采用局部二值化进行图像分割，将图像分成25×25的邻域块，对不同的邻域块取相应的局部平均值作为二值化阀值。中值滤波和局部二值化的效果如图4b和图4c所示。

(a) 初始图 (b) 中值滤波 (c) 局部二值化 (d) 边缘检测

因实验平台中的轮廓相对复杂，所以本文采用能够尽可能多地标识图像边缘的Canny算子，检测效果如图4d所示。Canny算法使用Sobel算子来计算像素点的梯度，相应的卷积模板如式(3)所示。

(3)

(4)

在得到梯度幅值与方向后，沿着梯度方向找出其邻域内两个相邻点Gs1Gs2，并判断该点是否为局部最大值G(i,j)≥max(Gs1,Gs2)，将不满足条件的非边缘点剔除。同时采用双阈值算法，去除梯度幅值过低的点，并对双阈值范围之间的点进行二次筛选，最后连接所有边缘以得到图像轮廓。

3.2 目标识别

3.2.1 圆型工件检测

针对待识别的圆型目标工件，本文采用霍夫圆变换算法来检测图像中的圆形，基本思想是将图像的空间域(u,v)变换到圆形参数空间(x,y,r)，首先进行圆心估计，对边缘检测图再执行一次Sobel算子，计算出所有像素点的邻域梯度。算法之后遍历图中的非0像素点，沿着所计算梯度方向画线，并将线段所经过点(a,b)对应的累加器N(a,b)+=1，最后统计排序N(a,b)，得出可能的圆心。

图5 霍夫圆变换

半径估计首先计算所有非0像素点和圆心的距离，将像素点按照距离进行排序，并根据阈值选取可能半径，之后再次遍历像素点，对于每个点所估计的半径r，使其相应累加器N(r)+=1，最后统计排序N(r)，得到可能的半径值。在算法进行检测后，过滤掉半径微小的圆，最终效果如图5所示。

3.2.2 托盘凹槽检测

在运用Canny算子对图像进行边缘检测后，通过findContours( )函数进行外层轮廓提取，得出一系列点集。针对所提取的闭合轮廓，本文通过多边形拟合法进行近似逼近，以轮廓中相距最远的两个点为基础，不断将其他点添加到拟合轮廓当中，直到新的拟合多边形轮廓与原轮廓的弧长差值满足精度要求，最终多边形拟合效果如图6a所示。针对拟合多边形，以矩形特征为标准进行筛选，并过滤掉过小的多边形，筛选效果如图6b所示。

(a) 多边形拟合 (b) 原图显示

在筛选之后提取出对应的矩形轮廓信息，得到x×n个轮廓点的图像坐标pi(ui,vi)，其中x为筛选所得矩形数量。通过类比质心法，可计算得到所拟合矩形的形心，具体公式为：

(5)

图7 目标工件识别

3.3 位置坐标计算

相机标定可以得到内参矩阵MI和相应外参矩阵MO，并利用外参矩阵MO和对应机械臂末端姿态Thand2base进行手眼标定，得出机械臂末端与相机的坐标变换矩阵Tcam2hand。通过内参矩阵MI和手眼矩阵Tcam2hand即可将目标工件在图像坐标系中的坐标(u,v)转化到机械臂坐标系上。

首先已知基坐标系转换到图像坐标系公式：

(6)

式中，内参矩阵MI、手眼矩阵Tcam2hand均可由上述标定环节计算得到。Zc为相机坐标系下目标工件的高度，可由目标实际高度Zw与式(6)推导得出。

对上述公式进行逆变换，进一步可得：

(7)

式中，(Xw,Yw,Zw)为目标工件在机械臂坐标系下的对应坐标，并可根据与基坐标系之间的坐标转换矩阵Thand2base得到基坐标下的工件空间坐标(Xb,Yb,Zb)。

4 仿真及实验验证

4.1 路径规划仿真

本文通过MATLAB验证所设计的新型RRT算法，利用robotics toolbox针对IRB120机器人的DH参数进行建模，并分别在坐标(0.2,-0.35,0.1)、(0.25,-0.25,0.35)处进行直径0.1 m的障碍物模拟，设置机器人起始角度为(-90°,15°,20°,0°,50°,0°)，目标角度为(0°,0°,0°,0°,90°,0°)，任务为从机械臂初始位姿到目标位姿之间规划出一条无碰撞的路径，其中初始位姿如图8a所示，目标位姿如图8b所示。

(a) 初始位姿 (b) 目标位姿

为了说明本文所提新型RRT算法的有效性,将新型RRT与传统RRT算法进行对比。两种算法相对应的节点拓展情况和最终规划路径如图9和图10所示。

(a) 末端路径搜索 (b) 机械臂模拟运行

(a) 末端路径搜索 (b) 机械臂模拟运行

图中浅灰色线条是RRT算法最终寻找到的路径，灰色线条是算法在搜索过程中进行的尝试。经过对图9、图10进行比较，可见本文所提出的新型RRT算法的空间节点数明显少于传统RRT方法，搜索过程更具有导向性，且显著提升了算法效率，路径比较平滑。结合表3数据可得，新型RRT算法相较于传统RRT算法的平均采样节点数由145.3缩减到38.2，减少了约73.71%，平均路径长度由1.292 m缩短到0.848 m，减少了约34.37%，平均规划时间层面上由26.8 s降低到10.3 s，减少了约61.57%。

表3 实验结果平均值

本文采用三次样条插值法对RRT算法拓展得到的一系列路径点进行连接插值，最终对机械臂各关节所规划的角度曲线如图11所示。

图11 三次样条插值法

从图中可以看出，机械臂的各关节角度都平稳变化，避免了因角度发生突变而造成的机械臂抖振情况，满足了机械臂控制的平稳性要求。

4.2 实验平台验证

图12 机器人实验平台

为验证本文所述机器人智能抓取系统的可行性和有效性，本文搭建实验平台如图12所示。系统硬件部分由IRB120机器人、PC端、维视图像MV-E EM系列工业相机构成，软件任务由QT完成，负责路径规划、图像的采集与处理、以及与机器人的通讯。

实验共分为机械臂路径规划与机器视觉识别定位两个部分：首先通过新型RRT算法进行路径规划，控制机器人沿规划路径点到达原工件的上侧位置，当机械臂到达当指定位置后，通过TCP/IP通讯向PC端发送成功信号；PC端接收到信号之后，启动视觉模块以识别出等待抓取的圆型工件，将其中心坐标经过处理转化到机器人坐标系，并发送到机械臂，机械臂则根据由视觉信息进行精准抓取。其中机械臂控制过程如图13所示。

(a) 初始位姿 (b) 路径规划 (c) 目标位姿 (d) 识别抓取

实验结果表明，机械臂经过路径规划成功地由初始位姿到达了原工件位置的上方，系统切换为视觉定位控制方案。在工件识别实验中，将托盘位置进行适量的偏移，随机地将圆型工件进行摆放且确保有一个工件位于托盘凹槽内，该工件即为待抓取的目标工件，通过本文所提出的机器视觉算法对目标进行识别定位，累积进行50次实验，识别结果以及位置误差如图14和图15所示。

(a) 目标位于右侧凹槽 (b) 目标位于左侧凹槽 (c) 目标位于左侧凹槽 (d) 目标位于中间凹槽

图15 位置误差图

结果表明，本文所提出的视觉定位方案成功对目标工件进行识别，计算的工件坐标与实际位置在X方向的平均误差为1.382 mm，在Y方向的平均误差为1.132 mm，由于机械臂末端执行装置为气动吸嘴，所以误差范围内工件都可以成功吸取。

5 结论

本文针对传统RRT算法的规划时间长、收敛性较差以及路径尖点较多等缺点，提出一种结合基于目标偏置的拓展点选择策略以及路径平滑优化策略的改进方案，结果表明改进后的新型算法在搜索效率、路径长度以及用时等方面都优于传统RRT算法，在经过路径优化以及三次样条插值后，机械臂可以平滑地通过更加简捷的路径点。在到达目标路径点后，本文提出的采用霍夫圆变换法和多边形逼近法进行双重检测的视觉定位方案，可以有效地识别出圆型工件和托盘凹槽，成功地对等待抓取的工件进行定位。结果证明了所提算法在机械臂上应用的可行性，具有良好的发展空间。但本文设计的机器视觉算法以固定高度的工件为处理对象，所以未来的研究重点将主要考虑获取目标的深度信息以对工件高度进行精准定位等。