改进RRT算法的采摘机械臂路径规划

赵 辉,郑缙奕,岳有军,王红君

(1.天津理工大学 电气工程与自动化学院, 天津 300384;2.天津市复杂系统控制理论与应用重点实验室, 天津 300384)

0 引言

机械臂路径规划是果园实现自动化采摘的关键技术,也为采摘的效率、品质提供了重要保证。由于水果果实具有密集程度不一,生长位置各异,树枝遮挡等特点,造成机械臂自动化采摘路径规划的难度较大。目前投入使用的农业采摘机械臂大多是由工业机械臂改制而来,基于果园采摘的实际应用环境需求,6自由度的机械臂得到了广泛应用,因为6自由度在保障空间采摘可行性的同时兼顾了灵活性,为了采摘机械臂能够更好地完成复杂的任务,对于其路径规划的研究有着重要意义[1]。

针对机械臂的运动路径规划问题,国内外学者都做出了巨大的贡献[2-10]。在解决一些二维空间的运动规划问题中,常使用A*[2]、D*[3]、遗传算法[4]等处理,取得了较好的效果,但随着机械臂自由度的增加和环境趋于复杂,这些算法变得效率低下甚至无法解决实际问题。为了解决空间路径规划问题,有学者提出了一种基于采样的快速搜索随机树(RRT)算法,但RRT基础算法有着搜索速度慢,生成的路径拐点多,占用计算内存大等缺点。针对这些问题,张建冬等[5]将障碍物因子、变步长等策略引入了RRT算法,有效提高了机械臂的避障效率;贾浩铎等[6]把人工势场法与Informed-RRT*算法相结合,改进后的算法在路径规划速度上有显著提升,且对于动态环境的适应性也增强;张勤等[7]基于柯西采样函数提出了一种改进的目标引力双向RRT*算法,使得机械臂运动路径更加平滑可靠;张婷婷等[8]把目标导向以及冗余检测策略与RRT算法相结合,解决了原算法存在的路径搜索时间长,路径质量低等问题,最终通过仿真实验证明了算法的有效性;Mohammed等[9]将RRT*随机树节点的生成概率呈正态分布,避免了过度搜索和陷入局部极值;潘振钊等[10]提出了一种改进的P-RRT*算法,将目标概率导向以及剪枝策略与原算法结合,改进的算法对于路径的长度和规划时间均有明显的缩短。

本文中针对传统的RRT算法路径规划速度慢、路径长度过长、采样点随机等问题,融入高斯采样增加算法搜索的目标导向性;加入A*代价函数,提高搜索速度,减少路径冗余;最后再采用贪婪算法优化路径,结合Matlab验证了改进算法的有效性。

1 算法基础

1.1 机械臂运动学建模

选用如图1所示的6自由度机械臂进行研究。首先对6自由度机械臂采用D-H参数法进行运动学建模,该方法首先要在机械臂各连杆的关节轴处建立一个平面直角坐标系,6自由度机械臂的关节坐标系如图2所示。在完成运动学模型的构建并且求解出路径后,就可以经由运动学逆向求解的方法,使机械臂遵循求解路径进行动作。

图1 6自由度机械臂关节示意图

图2 关节坐标系示意图

建立D-H连杆坐标系使用的4个参变量及各自的含义为:关节角用θi表示,连杆偏移量用di表示,连杆长度用ai表示,连杆偏转角用αi表示。由这4个参变量描述的邻接的两连杆间的空间状态关系如图3所示,同时机械臂的D-H参数如表1所示。

表1 D-H参数

图3 D-H连杆空间状态关系示意图

在D-H参数法的基础上,6自由度机械臂相邻连杆坐标系之间的齐次变换矩阵为:

(1)

(2)

再按照机械臂的逆运动学求解途径,可以将机械臂在路径规划中求解出的各点,在平面直角坐标系中的描述方式,转化为在关节构型空间中各连杆要求的变换[11]。基于以上理论,在Matlab中使用Robotics toolbox工具箱仿真出文中所选用的6自由度机械臂,如图4所示。

图4 Matlab机械臂建模示意图

1.2 机械臂碰撞检测

机械臂路径规划过程中最重要的一个步骤是对障碍物进行碰撞检测,基于文章采用的6自由度机械臂自身的属性,在搭建的仿真环境中,果树的树叶因为自身的柔软性,对于采摘工作的扰动可以忽略不记,因此,不考虑机械臂与树叶的碰撞[12],仅需要对机械臂的连杆与树枝、果实之间进行碰撞检测处理。由于实际环境中的障碍物往往大小不一,为了减少多余的时间损耗和内存空间占用,本文中将障碍物抽象简化为长方体和球体,即采用长方体包络检测法[13]以及球体包络检测法[14]进行机械臂的碰撞检测。

首先,将机械臂的各个连杆简化成为圆柱体[15],针对球体障碍物,只需要判断机械臂连杆中心点和球心的间距,即可确定碰撞是否产生;针对长方体包络法处理的障碍物,则需要分为3类:机械臂连杆圆柱体与长方体障碍物各面异面、相交、平行。由于使用长方体包络法处理的障碍物的各个面都平行于坐标平面,因此,仅需要分别判断3个轴方向的交叠部分是否大于设定的阈值,若大于,则产生碰撞。

其次,对于机械臂连杆之间的碰撞,可以通过检测2个圆柱体中轴线线段之间的距离来进行判断,假设线段AB是2条线段之间的最短距离,记作d,通过计算可求得机械臂之间的最小距离为dmin=d-(r1+r2),r1和r2分别是2个圆柱体的底面半径,若最小距离dmin大于0,则连杆之间发生碰撞,若小于0,则无碰撞发生。

1.3 障碍物最小安全阈值

在机械臂工作时,为了避免受自然风影响,关节与树枝果实产生不必要的碰撞,在分别计算得到机械臂的关节与树枝,以及果实之间的距离后,将二者进行比较,取较小者作为机械臂与障碍物的距离。假设q为机械臂位姿,D1(q)则表示树枝和各个关节的最小间距,如式(3)所示。

D1(q)=min{d(Bi,qj)|j=1,2,3,4,5,6}

(3)

式中:B代表树枝;i代表树枝数目;j代表机械臂关节。

机械臂的关节和果实的最小间距D2(q)如式(4)所示。

D2(q)=min{d(Fi,qj)|j=1,2,3,4,5,6}

(4)

式中:F代表果实;i代表果实数目。

由上文可得,机械臂与障碍物间的最小间距可由式(5)表示。

D(q)=min{D1(q),D2(q)}

(5)

同时再设定一个安全阈值Dsafe,只要机械臂与障碍物的距离处于阈值范围内,即可确保机械臂和障碍物无碰撞,如果D(q)>Dsafe,那么选取Dsafe作为间距;如果D(q)

2 算法原理

RRT算法[17]是一种概率完备且基于采样的算法,算法通过对状态空间中的随机采样点进行碰撞检测,避免了空间建模,能够快速有效地搜索高维空间,从而寻找到一条从起始点到目标点的规划路径。

不同于和RRT算法同类型的算法PRM,PRM算法在开始时会先进行随机采样,构建完整的路径拓扑图,之后再进行搜索;RRT算法将二者同时进行,即边采样边搜索。RRT算法比较依赖参数设置的合理程度,若参数设置不恰当,则很难得到合适的解。

图5 RRT算法原理示意图

然而,由于RRT算法的随机采样特性,致使其缺乏明确的目标导向性,最终路径求解的时间也会相应变长[18],同时随着迭代次数逐步增多,传统算法极易陷入局部最优;随机树的扩展存在较多无用节点,造成路径代价的增加;路径生成不平稳,对于需要在复杂环境中完成采摘工作的机械臂而言,则会形成不必要的机械和能量损耗,因此,需要对传统的RRT算法进行局部改良。

3 改进的算法

3.1 高斯分布采样策略(S-RRT)

高斯分布采样是一种在数据聚类中大量使用的采样方法,该方法通过限制空间采样区域,由数学方法对随机搜索树进行区间约束,随着迭代次数的增加,在无障碍区域会有少量的采样点生成,相反,在包含障碍物的复杂区域范围内或者障碍物之间的狭窄过道生成大量的采样点,从而取代RRT算法本身的随机采样策略,进而加速路径规划的进程。高斯密度函数的n维函数表达式如式(6)所示。

(6)

式中σ指高斯函数的标准差,也代表着高斯函数的规模大小。

定义障碍函数obs(c),与障碍物发生碰撞时,值为1,否则为0,这样就可以针对性地进行障碍物区域的采样,构型空间中任意点被采样的概率如式7所示。

(7)

从式(7)中可以看出,采样点周围的障碍物越多,式(7)的值越大,同时为了防止在障碍物内部进行采样,再定义一个函数,如式(8)所示,显然采样点在障碍物中时,式(8)为0,否则g(c;σ)=f(c;σ)。

g(c;σ)=max(0,f(c;σ)-obs(c))

(8)

下面是高斯采样生成采样点的算法流程:

1) 计算出路径规划中起始点与目标点相隔的欧氏距离cmin;

(9)

式中:cbest指搜索得到的可行解中的最小成本解;μ指采样中心点的位置。

3) 最后再从xball中随机选择出一个采样点。

3.2 路径冗余处理(A-RRT)

在进行高斯分布采样策略处理后的RRT算法有效缩短了搜索时间,但仍存在许多冗余随机点。因此,再将改进后的RRT算法与A*搜索算法相结合。利用A*代价函数搜索最佳采样点,减少冗余计算,优化路径。在RRT算法中,从初始点xstart到所选节点的距离称为正向代价,也可以称为启发式函数H(i);xrand和xgoal之间的距离称为后向代价B(i)。则当前节点的代价函数可用F(i)表示,如式(10)所示。

(10)

如图6所示,在得到一系列随机节点6、7、8之后,分别计算与父节点xnearest之间的前向代价,以及与终点的后向代价,二者叠加,选择代价函数最小的节点作为新节点xnew,并将其加入随机树列表。重复计算以上过程,直至扩展到目标节点。

图6 A*代价函数更新节点

3.3 路径简化处理(G-RRT)

加入高斯采样策略以及A*代价函数的RRT算法,能够快速地向目标逼近,然而采样的随机点分布并不规律,导致规划出的路径存在许多转折点,尤其是在果实和树枝交错生长的果园环境中,这样会大幅度降低机械臂的工作效率,并且可能会导致机械臂发生不可逆的机械损耗和不必要的能量消耗,因此,继续采用贪婪算法[19]简化路径。

算法的具体步骤为:首先从路径终点xgoal开始,将xgoal与之前的路径点按顺序进行连接,如果2个节点的连线不会与障碍物相碰撞,那么就把当前路径点剔除,再将xgoal和剔除节点的上一个节点相连,重复上述步骤。若与障碍物发生碰撞就留存碰撞节点的前一个没有发生碰撞的节点,让该节点成为父节点重复进行上述的操作,一直到与起始点xstart相连接算法结束,图7为贪婪算法优化示意图。 改进的算法流程如图8所示。

图7 贪婪算法优化示意图

图8 改进RRT算法流程框图

4 仿真实验

为了验证改进RRT算法的有效性和优越性,在Matlab中针对静态障碍物进行了二维及三维实验仿真,并且对改进的SAG-RRT算法和RRT算法进行了比较。二维环境中,将障碍物设置为方形及一些不规则的形状,三维环境中设置为球体,此外,考虑到RRT算法本身的随机性和对算法性能的客观评价,因此,基于不同的实验场景,针对RRT算法和改进的SAG-RRT算法分别进行100次模拟仿真实验,将RRT算法的步长在二维、三维仿真中均设置为20,以便于记录比较数据,最后计算实验评价指标的均值并做统计。

4.1 二维仿真实验

简单场景的算法测试如图9所示,复杂场景如图10所示,分别对比2组算法仿真图可得:基本RRT算法运行时稳定性较差,特别是在复杂二维环境下,算法会生成很多不必要的节点。然而改进的RRT算法在运行时表现稳定,且在复杂环境下,改进后的算法相比原RRT算法扩展节点数减少,导向性显著增强,节点树简单,路径更加平滑。

图9 简单二维环境算法测试示意图

图10 复杂二维环境算法测试示意图

由表2可知,在二维环境下,RRT算法的平均采样点数为965个,而改进RRT算法的平均采样点数为338个,减少了65%;此外,RRT算法的平均运行时间为7.82 s,而改进后的RRT为4.44 s,缩短了43%,这是因为后者在碰撞检测次数和失败的节点增长次数上比前者有明显缩减。所以改进的SAG-RRT算法性能良好。

表2 二维环境算法性能

4.2 三维仿真实验

设置一个三维环境,起点设置为(0,0,0),终点设置为(700,900,900),实验结果如图11和表3所示,在三维仿真环境中,基本的RRT算法规划的路径由于算法自身局限产生了较多的分支,冗余点多,规划时间长。然而改进的RRT算法则能够在短时间内找到代价相对小的路径,并且对比表2和表3可以看出:算法在三维环境中表现较二维环境更加优越,这也保证了机械臂在实际工作场景中的应用。

表3 三维环境算法性能

图11 三维环境算法测试示意图

在第1节建立的6自由度机械臂运动学模型基础上,经过一系列的正运动学和逆运动学计算,再经由改进的RRT算法可以得到各个关节以及末端的关节序列点,最后结合多项式插值法,可以在Matlab平台上得到机械臂的避障运动轨迹,如图12所示。

5 结束语

利用高斯采样函数提升了RRT算法的导向性,同时加入了A*代价函数去除路径的冗余点,最后融入贪婪算法简化路径。

在Matlab平台通过不同的实验环境证实了改进的SAG-RRT算法避障效果良好,路径较平滑,效率更高,符合采摘机械臂实际工作需求。