基于VS-IRRT算法的采摘机械臂路径规划

荀 一 李道政 王 勇 黄旭婷 王志恒 杨庆华

(1.浙江工业大学特种装备制造与先进加工技术教育部/浙江省重点实验室，杭州 310023;2.浙江艾佳食品有限公司，衢州 324000)

0 引言

采摘机器人大多依赖于多自由度机械臂与视觉结合的方式实现定位和抓取过程[1-6]。其关键技术之一就是机械臂的路径规划，即机械臂从起始位置运动到目标位置，且中间不与非目标障碍物发生碰撞的一项技术。目前，国内外学者针对路径规划技术进行了大量的研究，并取得了相应成果[7-10]。但将面向低维度空间使用的路径规划算法运用在多自由度机械臂上时，随着维度的增加，这些算法的复杂度就会呈“指数”增长，影响了规划的实时性[11]。为解决这个问题，基于采样的路径规划方法和一些降维的规划算法被提出。邹宇星等[12]提出一种改进的概率地图(Probabilistic roadmap, PRM)路径规划算法应用在采摘机械臂上，算法将机械臂发生碰撞的位姿遍历，映射到构型空间中，再使用PRM算法搜索路径。姚立健等[13]针对茄子采摘机器人，将三维空间的路径规划降低为二维空间后再应用A*算法规划，降低了计算复杂度。张勤等[14]等研究出基于空间分割的实时运动规划算法最优采摘空间(Optimal picking space, OPS)，引入评价函数选取最优采摘空间，最后在采摘任务的基础上加入实时避障子任务，引导机械臂躲避障碍完成任务。面对非结构环境的复杂情况，马冀桐等[15]利用在构形空间中增加先验知识引导点的方式，在路轨规划中避开封闭多边形障碍物，再使用RRT-connect规划引导点前后两段路径并将它们进行合并。而阳涵疆等[7]则是将混联采摘机械臂中串联和并联的部分分开，对串联机械臂部分使用RRT算法规划，降低了规划的维度，加快了规划速度，且混联方式可以更好地应对复杂非结构环境。张勤等[16]为解决RRT算法采样效率低下的问题，提出了柯西分布的采样方式增加采样的目的性，并引入目标引力和动态调整步长的方式，在增加目的性同时减小碰撞概率，其中用到的节点拒绝策略减少长路径节点的增加。但是其生成的路径为断续的折线，直接运用在机械臂上会致其发生碰撞，且在较远处开始规划时，没有考虑相机精度，可能导致采摘点产生误差。GAMMELL等[17]根据第1次RRT*得出的路径长度来设置一个对应长轴长度的超椭球采样空间，将后续采样都限制在该采样空间中，每次有更优路径产生时，就更新长轴长度，逐步缩小采样空间，直至找到最优路径。这种方式加快了规划速度，具有更高的效率。除了基于采样的算法外，近几年还出现了利用深度学习和强化学习规划的算法[18-19]。

但上述算法均依靠视觉提供的目标点空间信息，属于“看后动”的开环控制形式，若视觉信息或者机械臂位置信息有误差，将导致机械臂末端执行器的最终位姿和目标位姿有偏差。针对这种情况，有学者提出将视觉信息用于闭环控制的方式，即视觉伺服控制[20]。最常用于农业采摘的视觉伺服控制方式就是用于“眼在手上”(eye-in-hand)的基于图像的视觉伺服方式(Image based visual servoing, IBVS)。金玉成等[21]建立手眼协调的坐标关系，据此提出了由远及近手眼协调策略，提高了采摘效率和精度。MEHTA等[22]根据球形果实的形心和距离等少量特征，设计了旋转控制器和平移控制器用于IBVS控制，并用李雅普诺夫证明了其稳定性。基于视觉伺服的运动规划精度高，稳定性好，利于应用于采摘工作。但是其无法考虑环境碰撞因素而导致应用范围受限，只能用于无障碍的空旷场景。

为解决传统采样方法目的性弱、采样效率低,采样终点定位存在误差,普通视觉伺服控制无法避障的问题，本文提出结合视觉伺服的改进随机快速搜索树算法(VS-IRRT)。针对传统RRT算法所存在的采样无目的性和路径杂乱的情况，通过使用超椭球引力偏置和密度减小措施改变采样方式，同时用贪婪思想和B样条曲线优化所得路径。在接近目标点时使用基于平移控制器的图像视觉伺服方式消除位置误差，最后到达目标点完成采摘过程。

1 空间环境建模

目标果实周围会存在如枝干和非目标果实等障碍，启动采摘过程之前，采摘机器人需要先对其周围环境进行感知和建模。本文使用的是eye-in-hand的相机安装模式，如图1所示，RGB-D相机安装在末端执行器上，跟随末端执行器运动。采摘机器人通过使用由远及近手眼协调策略[21]，在初始位姿处获取全局图像信息，根据针孔相机模型，将像素平面上的点结合相机内参和对应的深度信息，计算出目标果实周围的点云信息。对点云信息滤波后再转换为八叉树图信息(Octomap)来提高避障效率。除了障碍物信息，还需要对图像中的目标果实识别提取，通过将果园拍摄的500幅果实图像使用Labelme软件打上标签训练，用YOLO v5模型以高准确率和快速度得到目标果实在像素平面上的位置，最后再结合RGB-D相机的深度信息，获取果实中心在空间中的坐标，整个空间环境信息处理如图2所示。图2a所示的是原图像经过YOLO v5识别后的效果，数字部分为预测准确率；图2b为对应的点云图像；图2c为点云转换的八叉树图，用像素块来近似表示障碍物，减小了避障的计算复杂度。

图1 空间环境示意图

图2 空间环境信息处理

2 VS-IRRT算法

2.1 VS-IRRT算法组成

本文提出的VS-IRRT算法主要由改进RRT算法和基于平移控制器的视觉伺服方法两部分组成，如图3所示。图中VS-IRRT分为两部分，其中改进RRT算法中包含了采样方式的改进和路径优化；而采样方式的改进体现在超椭球引力偏置采样方法和密度减小策略上。图中箭头表示同一层次部分算法的运行顺序。

图3 VS-IRRT算法组成示意图

2.2 基于超椭球的引力偏置采样方法

在传统的RRT算法中，采样点搜寻使用的是随机均匀采样的方法，这种方式的路径搜索没有目的性，收敛速度慢，整个算法效率不高。针对这一点，引入了基于超椭球的引力偏置采样方法来代替随机均匀采样，如图4所示。引力偏置是指采样点被超椭球吸引，出现在超椭球内部的概率变大。搜索树通过随机采样节点从采样开始节点搜索出一条无障碍路径到达采样终点。

图4 超椭球引力偏置节点扩展过程

采样开始时分别设置采样概率阈值Pout和Pgoal，其中Pgoal为偏向目标的采样概率，一般取值为0.9～0.95，本文取0.9。采样概率P在(0,1)中随机均匀采样。当P>Pgoal时，采样点Xrand=Xend,即直接选择目标点作为采样点进行生长；当Pout

‖Xrand-Xstart‖+‖Xrand-Xend‖≤2a

(1)

其中

a2=(1+γ2)c2

(2)

式中Xrand——随机采样点

Xstart——采样起始点

Xend——采样终点

a——超椭球长轴长度

c——超椭球焦点间距，也是Xstart和Xend间的距离

γ——比例系数，取0～1

γ决定超椭球的短轴长度，γ越小，超椭球越扁平，采样目的偏向性更强，当障碍物较少时，该值可增大，本文取γ=0.5。

使用上述采样方式虽然在特定的环境下Pout有不错的效果，但如果每次更换采样环境时，都需要寻找最合适的采样阈值，这使得算法的环境适应性下降。针对这种问题，本文提出了自适应阈值的方式，即在采样过程中采样阈值动态变化适应环境。采样阈值自适应的思想是：当采样到的碰撞点增多时，说明采样目标偏向的方向有大量障碍物，此时需要提高采样的随机性，即超椭球外的采样概率。据此，可以得到自适应阈值的公式为

(3)

(4)

式中Pmin——自适应阈值的限定最小值

Pmax——自适应阈值的限定最大值

β——影响因子

α——此次采样之前的采样点碰撞次数

λ——环境复杂系数，环境越复杂，λ取值越大，使得Pout变大，采样偏向于随机均匀采样，λ一般取值大于1，本文取1.25

θ——机械臂自由度，取6

这种采样方式增加了采样的目标偏向性，加快了收敛速度，同时还保留了一定的随机性，避免因为障碍物过多导致采样困在局部区域的现象。

2.3 密度减小策略

在传统RRT算法中，确定完采样点后，将选择树中距离采样点最近的节点Xnear向采样点方向扩展一个固定步长得到新节点Xnew。但固定步长使扩展的新节点可能出现在树的内部，如图5a所示，使得树内存在其它节点与新节点的距离更近，最后导致树的节点密度激增，算法效率降低。

为解决固定步长导致密度激增的问题，本文提出了一种密度减小措施，如图5b所示，通过找到距离新节点Xnew最近的树内节点Xnearest，若该节点就是新节点的父节点，即Xnearest=Xparent，则保存新节点到树中，否则抛弃新节点。密度减小措施可以减小整个生成树的密度，通过筛选减少采样点的个数来提高路径搜索算法的速度。

2.4 路径优化方法

在算法执行完成后，生成的路径不一定是整个已连接节点组成路径中最短的，且路径为折线。这样的路径直接应用在机械臂路径规划中可能会引起机械臂的振荡，影响采摘效果。针对这些问题，本文使用贪心思想和B样条曲线来优化生成的路径。贪心思想的整个过程如图6a所示，在已生成的路径中，从Xstart开始依次向后面的节点连接，若两点连接路线上没有障碍物，则继续向后面的节点发起连接，直至连接(Xstart,Xn)经过了障碍物。此时，将连接(Xstart,Xn-1)代替节点Xn-1前的所有连接，然后将Xn作为新的起点重复上述步骤，在连接到Xend时结束。

图6 路径优化示意图

贪心思想可以缩短原始路径，但是并不能解决路径曲折的问题。针对这个问题，考虑到B样条曲线的局部性和连续性[23]，本文使用B样条曲线对优化后的路径平滑拟合，公式为

(5)

式中Pi——曲线的控制点，本文选择相连的路径点作为控制点来形成局部的三次B样条曲线

Fi,k(t)——k阶B样条基函数

平滑后的图像如图6b所示。

2.5 基于平移控制器的图像视觉伺服方法

仅依靠改进RRT算法仍然不能解决因视觉定位信息和机械臂位置产生误差导致采摘失败的情况，还需要引入连续视觉图像作为反馈信息，从而形成闭环的视觉伺服控制方法。为将两种路径规划方法结合起来，本文提出了分段控制的思想：在开始规划路径之前，先选择采样终点Xend作为两段控制的分界点，即在机械臂到达Xend前采用改进RRT的规划方法，到达Xend之后采用视觉伺服的规划方法。本文以目标果实为中心处建立半径为R的随机空间作为Xend的选择空间，当R选择适当值时，机械臂在视觉伺服过程中和果实的距离较小，可看作在无障碍环境下规划路径。

如图7所示，Xend包括在随机空间中，其中随机空间为两虚线代表的球中间夹着的空间，表示为

图7 采样终点选取

(6)

Rmin≤(X-XO)2+(Y-YO)2+(Z-ZO)2≤Rmax

(7)

(8)

Rmin=dmin+r

(9)

式中Trand——采样空间中随机位置

Rrand——采样空间中随机位姿

X、Y、Z——Xend坐标

XO、YO、ZO——果实中心坐标

σx、σy、σz——(0,1)间互相独立的随机数，随机空间是半径为Rmin和Rmax的两个球之间的空间

dmin——深度相机能够测量深度的最小距离

r——果实半径

考虑机械臂不与附近实体发生干涉，根据实地情况测量估计Rmax=1.5Rmin比较合适。

确定完Xend的平移量Trand后，还需要对其旋转量Rrand进行筛选。式(8)中的旋转量使用单位四元数形式表示。Rrand的值会反映到机械臂末端的姿态上，进而影响到相机对目标果实的信息获取情况，如图8所示，本文使用的视觉伺服方式是基于图像的视觉伺服，故在整个伺服过程中目标果实要保持出现在相机的视野范围内(Field of view, FOV)，相机在图中Xend处需完整观测到果实部分。

图8 采样终点处相机视野示意图

为判断目标果实是否位于相机视野内，就需要获取目标果实与相机的相对位姿关系。根据各坐标系间的转换关系，可以得到

(10)

(11)

式中O为目标果实的坐标，其左上标B、e、c分别表示参考坐标系为机械臂基坐标系、机械臂末端参考坐标系、相机坐标系，T为坐标系转换矩阵，转换对象分别标注在左上方和左下方，下标init表示在机械臂初始位姿的条件下。坐标系e、c的相对关系可以通过手眼标定[24]的方法确定下来，坐标系B、e也可以通过机械臂D-H建模后[25]，使用正运动学得出。

在随机生成Xend后，由式(10)、(11)可以推导出目标果实和相机的位置关系，cO=(Xc,Yc,d),其中d为RGB-D相机测出的深度信息，与相机坐标系下目标果实在Zc方向的值相等。如图8所示，相机的视场角为δ×γ，相机视野范围为一矩形平面，矩形的尺寸与深度d有关。综合上述信息可以得出目标果实在相机视野范围内的判定条件

(12)

(13)

(14)

若满足式(12)的条件，则判定Xend为有效采样终点，否则重新随机生成新的采样终点。在得到采样终点后，从该点开始对目标果实实行图像视觉伺服规划。视觉伺服中最重要的就是图像雅可比矩阵Jv，它的逆可以将图像当前特征点和目标特征点的误差转换映射为相机的笛卡尔坐标下的速度。Jv是维度2×6的矩阵，对其求逆时可能会在奇异点附近而使得到的速度产生突变，这不利于机械臂的控制，故特征点的数量一般要求大于3个。在使用YOLO v5识别果实时，会发现除了果实中心坐标外，没有稳定的特征点供选择，传统的视觉伺服无法直接应用在采摘过程中。参考文献[22]，将特征设计成果实在图像像素中的坐标(u,v)以及果实深度距离d，应用其平移PD控制器，得到相机的速度为

(15)

(16)

(17)

(18)

式中kpv1、kdv1、kpv2、kdv2——平面误差控制和深度误差控制PD控制器系数

ev1——果实在图像像素平面中与原点的偏差

ev2——果实深度距离和采摘距离的偏差

J′v——图像雅可比矩阵Jv的前2列

J″v——图像雅可比矩阵Jv的第3列

vz——相机在z方向的速度

vp——相机x-y平面上的速度

I2——2×2单位矩阵

本文只参考文献[22]中平移控制器，没有使用旋转控制器，原因在于旋转控制器的计算过于复杂，且作用是将果实保持在相机视野范围内，在本文上述内容中已经解决这个问题。但只使用平移控制器来完成视觉伺服会产生新的问题：当相机距离目标果实很近时，相机和果实间即使发生很小的错位，也会导致果实在像素平面内产生巨大误差，从而使得x-y平面速度突变。可表示为d→0,ev1→∞,vp→∞。为解决该问题，本文修改相机速度vp的表达式为

(19)

式中e为当前特征点位置和目标特征点位置的距离。K为比例因子，取0.01，用于减小速度vp，使得ev1能够在ev2接近0之前收敛为0，防止速度突变发生。

基于VS-IRRT算法的采摘机械臂路径规划流程图如图9所示。

图9 基于VS-IRRT算法的采摘机械臂路径规划流程图

3 算法仿真

本文使用Matlab 2020a分别对VS-IRRT中的改进RRT算法和仅基于平移控制器的视觉伺服算法进行仿真，验证算法的有效性和可行性。

3.1 改进的RRT算法仿真

本文将改进的RRT算法与RRT、RRT*-connect、Informed-RRT*算法分别在二维空间与三维空间中进行比较，各算法的路径规划仿真过程如图10所示。图10中，2个红色圆点分别为起始点位置和终点位置，蓝色路径为树的拓展路径，红色路径为找到的目标路径，其它几何体为被视为障碍物的非目标果实。对比后发现改进的RRT算法目的性更强，无效的采样点数量较少，执行效率就高，同时其路径振荡程度小，路径平滑，利于在机械臂上直接应用。

图10 改进的RRT算法路径规划

考虑到RRT是基于采样的算法，其随机性较大，每次计算的过程都有差异。为了更客观比较各算法的性能，本文使各算法在相同的仿真环境下各运行200次，性能数据取200次运行得出的平均值，数据如表1所示。

由表1可以发现，RRT*算法因为使用重选父节点和重布线的方式，使得其路径成本最小，但其带来的时间代价是巨大的。RRT*-connect算法通过结合RRT-connect算法的速度优势和RRT*算法的渐近最优的优势，能以更快的方式获取最优路径，图10c、10g中蓝色和红色路径分布代表2颗树的路径。而Informed-RRT*算法在RRT*算法的基础上添加了超椭球范围限制，在第1次使用RRT*算法得到最优路径后，根据路径代价更新Informed-RRT*算法的超椭球长轴长度来缩小采样范围，以获取更快的规划速度。本文提出的改进RRT算法通过超椭球引力偏置采样的方式提高了采样的目的性，用密度减小策略减少采样次数，提高效率，最后用贪婪思想和B样条曲线优化生成路径。不同的是改进RRT算法的超椭球不会随着上一轮迭代找到的更优的路径而缩小超椭球的空间，而且改进RRT算法的超椭球依然保持在超椭球外部的采样概率，这使得路径规划具有完备性，虽然改进RRT算法不会渐近最优，但根据实际应用场景，在采摘过程中，其拥有更高的采摘效率。从结果上来看，改进RRT算法采样点数较RRT*-connect算法减少92.9%，规划时间较RRT*-connect算法减少86.1%，路径成本虽然没有RRT*-connect算法小，但较RRT算法也减少17.2%。

表1 算法性能比较

3.2 基于平移控制器的视觉伺服方法仿真

本文用数值模拟的方式在Matlab上对平移控制器的视觉伺服方法仿真。设置相机的初始坐标tinit、初始旋转矩阵Rinit以及目标点坐标O为

tinit=(50,100,1 500)mm

O=(250,350,2 000)mm

使用相机提供的SDK对其自身标定，得到内参矩阵为

根据文献[21]中的PD控制器的参数选择方式，选择PD控制器的比例系数和微分系数为kdv1=0.1,kpv1=10,kdv2=0.4,kpv2=1。

图11 平移控制器的笛卡尔空间和像素空间路径

图12 平移控制器速度与目标误差曲线

4 模拟环境采摘试验与分析

以胡柚采摘为例，为验证VS-IRRT应用在采摘中的效果，保证其在实际环境中的可行性，将VS-IRRT算法移植入机器人操作系统(Robot operating system, ROS)，结合采摘机械臂在搭建的模拟试验环境中试验验证算法，如图13所示。

图13 采摘试验平台

图13中采摘机械臂使用UR3六自由度机械臂，工作空间是以基座为球心的半径500 mm的近似球形空间，最大负载3 kg；用于控制的上位机搭载Intel Core i5处理器、16 GB内存、GTX960m显卡，操作系统为Windows 10以及虚拟机上的Ubuntu kinetic；使用Intel RealSense D435i型相机，调整使其分辨率为640像素×480像素，深度测量范围为0.16～2.00 m，FOV为69°×42°，安装在UR3机械臂的末端，便于实现视觉伺服。

在采摘过程中，本文使用ROS中带有的MoveIt插件导入相关路径规划算法进行仿真试验，并在Rviz中显示采摘相应过程与RGB-D相机感知周围环境所产生的点云信息，便于更清晰观察机械臂在运动过程中的表现，如图14a所示。机械臂从初始状态开始，经过改进RRT规划到达采样终点，之后改用基于平移控制器的视觉伺服，最终到达采摘点完成采摘，实际流程如图14b所示。

图14 机械臂VS-IRRT采摘过程

通过订阅运动过程中各个关节的角度变化信息，绘制出VS-IRRT算法运动规划时的关节角度变化图，如图15所示。观察图15发现试验过程中关节变化分为2部分，在1.5～5 s内，VS-IRRT中的改进RRT算法发挥作用，各关节角度变化较大。在5～6 s内，机械臂切换为视觉伺服算法，获取图像和深度误差信息，这个过程中机械臂没有运动。在6 s之后，机械臂使用基于平移控制器的视觉伺服算法，这个过程主要是缩小误差，各个关节角变化较小。

图15 机械臂各关节角变化曲线

通过对3.1节中每个算法各试验100次，试验结果取平均值，见表2。其中的路径成本是机械臂在运动过程中各关节角变化的弧度总和。由表2可知，VS-IRRT算法的采摘速度比RRT*-connect算法只提升48.36%，与改进的RRT算法在仿真中的表现不同，这是因为VS-IRRT算法在算法转换以及视觉伺服的过程中，程序运行速度较慢，导致总的采摘时间变长。在路径成本方面，VS-IRRT算法路径成本为6.53 rad，相较RRT算法减少了17.14%。VS-IRRT算法采摘成功率最高，与RRT*-connect算法相比，提升了2.1个百分点。

表2 采摘试验结果

5 结论

(1)针对自动化采摘过程中出现的定位误差以及视觉伺服无法考虑障碍物的问题，提出了结合视觉伺服的改进随机快速搜索树算法。该算法通过使用超椭球引力偏置和密度减小措施改变采样方式，同时用贪婪思想和B样条曲线优化所得路径，引导机械臂以快速平滑的避障方式到达采样终点。在接近目标点时使用基于平移控制器的图像视觉伺服方式缩小机械臂末端执行器与目标果实间的误差，完成采摘过程。

(2)采摘试验结果表明，VS-IRRT采摘1次的时间为17.79 s，与RRT*-connect算法相比降低48.36%，在路径成本上较RRT算法降低17.14%，采摘成功率较RRT*-connect算法提升2.1个百分点，是上述算法中最高的。证明了VS-IRRT算法极大提高了机械臂采摘规划的综合性能。