韩晓微,石泽亮,王 晓
(沈阳大学 a.科技创新学院,b.信息工程学院,辽宁 沈阳 110044)
随着社会的快速发展,智能机器人在生活中的应用越来越广泛[1],应用环境也越来越复杂。机器人规划路径的好坏直接决定了机器人工作的稳定性和智能性。智能机器人的无碰撞运动规划受到国内外学者的广泛关注。同时,机器人无碰撞运动规划的快速发展也影响和改变着各行各业的生产生活方式[2]。
常见的运动规划方式分为搜索、智能、采样等3类[3]。其中基于搜索的路径规划算法包括Dijkstra和A*等;基于智能的路径规划算法包括遗传算法、蚁群算法等;基于采样的路径规划算法包括RRT(rapidly-exploring random trees)、RRT*等[4]。RRT类算法不仅能够广泛应用于复杂高维空间,而且无需对构型空间障碍物进行描述,适合应用于多自由度机器人在复杂环境下和动态环境中的路径规划问题[5]。RRT是由LaValle等[6]在1998年提出的一种基于概率采样的运动规划算法,它通过状态空间的随机采样导向空白区域,从而找到一条可行的规划路径,能快速有效地搜索高维空间,但是其生成的路径非最优[7],且环境复杂度会影响收敛速度。鉴于RRT规划的路径非最优,Karaman等[8]在2011年提出RRT*算法,通过对指定范围内为当前节点重新寻找父节点和范围内节点的重新连接2步来产生一个最优路径,但是由于其搜索范围大且需要修剪冗余的枝叶导致速度变慢。Gammell等[9]在2014年提出Informed-RRT*算法,主要针对RRT*在最后2步修剪枝叶的时候采样范围过大导致速度变慢的问题,在初始路径的基础上建立一个渐变椭圆来规定采样空间,效率更高[10]。国内外针对上述变种的RRT类算法的不足提出了不同的方法来改进和实现。张建东等[11]针对RRT搜索速度慢的问题,将子目标搜索方式和目标偏向策略引入采样过程,虽然算法搜索速度较快,但路径还是非最优解。刘顿等[12]针对路径规划耗时长与非最优等问题提出在机械臂构型空间中预设采摘点,提出4棵树同时采样的方法,加入自适应步长函数可以保证采样质量和降低路径规划时间。欧阳子路等[13]在保留RRT*算法特性的同时,通过改进碰撞评价函数、引入动态目标区域采样等方式来保证规划速度的提高。Kim等[14]提出的基于包裹的知情RRT*将称为“包裹过程”的尺寸减小过程与知情RRT*相结合,效率虽然提高了,但是容易陷入局部最优解。Ghosh等[15]提出了一种运动学约束Bi-RRT(KB-RRT)算法,生成平滑轨迹,扩大重选父节点和剪枝的范围,路径更好地收敛达到渐进最优,但导致了搜索效率变低。
本文研究分析了目前已有的RRT类路径规划算法,针对路径规划过程收敛速度慢、效率低等问题,从规划方式、采样范围、生长转角和生长步长等4个方面提出一种双向Informed-RRT*的路径规划算法。
1.1 RRT算法
图1 RRT算法搜索路径Fig.1 Search path of RRT algorithm
Informed-RRT*算法是基于RRT算法发展到RRT*算法后的变种。RRT主要优势为能快速地找到一条可实现路径,主要取决于其概率采样的随机性[16],正因如此才能快速找到一条可实现路径,但这种规划方式导致了其解非最优或渐近最优,RRT*算法通过修枝来解决这个问题[17],但其烦琐的遍历过程导致效率低下,而Informed-RRT*算法就是为了减少其采样空间范围而实现搜索效率提升。原始的RRT算法利用初始点作为根节点,扩展新的子节点以完成随机树的构建,见图1。
RRT算法的主体伪代码如下:
1)V←{qinit};E←Φ;i←0;#对顶点集V和边集E进行初始化,其中,i代表迭代次数;
2) whilei 3)G←(V,E);#设置一个新的图集; 4)qrand←sample(i);i←i+1;#利用sample(i)函数采样一个新的采样点qrand; 5) (V,E)←extend(G,qrand);#更新V和E,每次更新完的节点与终点求欧氏距离。 表1为实验仿真中部分欧氏距离d,若d小于规定阈值,即步长2.00,且通过碰撞检测,满足算法结束条件,直接连接最新节点和终点,如第40号节点欧氏距离为1.70,满足结束条件。 表1 更新节点与终点的欧氏距离Table 1 Euclidean distance of updated node from the end point RRT算法的核心为extend函数[18],伪代码如下: 1)V′←V;E′←E;#暂存现有顶点集和边集; 2)qnearst←nearst(G,q);#利用nearst函数求在图集中离采样点欧氏距离最小的节点qnearst; 3)qnew←steer(qnearst,q);#利用steer函数求得qnearst到q的方向,以规定步长r扩展新的节点qnew; 4) if ObstacleFree(qnearst,qnew) then #碰撞检测,以圆形障碍物为例,碰撞检测具体过程即求所有障碍物圆心到最新扩展的边的距离,比较此距离与当前障碍物半径的大小,若小于半径即碰撞检测失败,反之通过检测; 5)V′←V′∪{qnew};#若通过碰撞检测则更新顶点集; 6)E′←E′∪{(qnearst,qnew) };#若通过碰撞检测则更新边集; 7) returnG′=(V′,E′);#更新整体图集。 图2为在碰撞检测后顶点集与边集的更新过程,所有折线拐点的集合即顶点集。 图2 碰撞检测与顶点集、边集更新过程Fig.2 Clash detection and vertex set and edge set update process 基于采样的运动规划算法中,随着采样点趋近于无穷可以证明其收敛到最优解的概率为0;RRT*算法即为基于此提出的一种渐进最优算法。相较于RRT算法最主要的区别是修剪枝的2个步骤保证了渐近最优解,第1步是形成1个节点后在规定的范围内为当前节点重新选择父节点;第2步是在当前规定范围内为节点重新连接。 RRT*算法整体的路径规划步骤与RRT算法相似,extend函数是它与RRT算法的主要差距。伪代码如下: 1)V′←V;E′←E;#暂存现有顶点集和边集; 2)qnearst←nearst(G,q);#利用nearst函数求在图集中离采样点欧氏距离最小的节点qnearst; 3)qnew←steer(qnearst,q);#利用steer函数求得qnearst到q的方向,以规定步长r扩展新的节点qnew; 4) if ObstacleFree(qnearst,qnew) then #碰撞检测,以圆形障碍物为例,碰撞检测具体过程即求所有障碍物圆心到最新扩展的边的距离,比较此距离与当前障碍物半径的大小,若小于半径即碰撞检测失败,反之通过检测; 5)V′←V′∪{qnew};#若通过碰撞检测则更新顶点集; 6)qmin←qnearst; #把离当前采样点最近的节点qnearst暂存于代价最小的变量qmin; 7)Cnear←near(G,qnew,|V|); #根据当前最新节点规定一个范围,即RTT*算法修剪枝叶的范围; 8) for allqnear∈Cneardo #遍历规定范围内的所有节点,为新节点重新寻找父节点; 9) if ObstacleFree(qnear,qnew) then #把新节点和遍历的每一个节点做碰撞检测; 10)c′←cost(qnear)+pc(line(qnear,qnew)); 11) ifc′ 12)qmin←qnear; 13)E′←E′{(qmin,qnew)}; 14) for allqnew∈Cnear{qmin} do 15) if ObstacleFree(qnew,qnear) and cost (qnew)>cost(qnew)+c(line(qnew,qnear)) then 16)qparent←parent(qnear); 17)E′←E′{(qparent,qnear)}; #把原来的边集去掉当前遍历点和它原来父节点的路径一次更新边集; 18)E′∪{(qnew,qnear)}; #增加当前遍历点和新节点的连接路径到边集中,至此RTT*算法2步修剪完成; 19) returnG′=(V′,E′) #更新整个图集。 其中前5步与RRT算法相同;6)～13)表示为当前节点重新寻找父节点,其中10)～12)表示通过碰撞检测的点qnear的代价加上新节点qnew和qnear连线的代价暂存于c′中,如果新构造父节点时qnew的路径代价c′比原来qnew的代价小,则把当前遍历点qnear存入当前新节点qnew的最小代价父节点qmin中,这样遍历完后就可以找到当前新节点在规定范围内的最小代价父节点;14)～16)是为了计算规定范围Cnear内(除了上面最优父节点外的其他节点)把新节点qnew重新作为自己的父节点,比较之前的代价,遍历的这些点和新节点若是通过碰撞检测,且这些遍历点原来的路径代价比新连接的代价大,则更新当前遍历点的父节点。 图3是关于RRT*算法重选父节点示意图,点4是根据采样点qrand新生成的节点qnew。现在在灰色范围内遍历节点为点4寻找最优父节点。把点1和点3作为点4的新父节点与原来对比,若为点4重新寻找点1为父节点,则点4的路径代价更小。于是更新点1与点4之间的连线加入边集,如图3中虚线所示。 图4是RRT*算法重新连接图,点4为新生长的节点qnew,重新连接即遍历点2、3且它们以点4为父节点来进行路径代价比较,点2保持原来路径较好,点3以点4为父节点重新连接,路径代价比原来更小,于是去除点2与点3之间的边,重新连接点4与点3之间的连线更新到边集中,以此完成范围内重新连接的枝修剪。 图3 RRT*算法重选父节点Fig.3 Parent node reselecting of RRT* algorithm 图4 RRT*算法重新连接Fig.4 Reconnecting of RRT* algorithm 图5 Informed-RRT*采样空间Fig.5 Sampling space of Informed-RRT* Informed-RRT*和传统的RRT*基本原理相似,只是在生成基本路径后的采样中改变了采样空间,以此加快导向渐近最优化路径解的速度[19]。 图6 双向搜索Fig.6 Two-way search Informed-RRT*算法在进行变椭圆采样时需先生成一条初始路径,为了加快整体算法的搜索效率,考虑在生成初始路径的过程中加快搜索速度,本文提出在生成初始路径时引入一种双向搜索的方式,如图6所示。 引入的双向搜索算法的具体步骤伪代码如下: 1)V1←{qinit};E1←Φ;G1←(V1,E1);#初始化树1; 2)V2←{qgoal};E2←Φ;G2←(V2,E2);i←0;#初始化树2; 3) whilei 4)qrand←sample(i);i←i+1;#利用sample(i)函数采样一个新的采样点qrand; 5)qnearst←nearst(G1,qrand);#利用nearst函数求在图集中离采样点欧氏距离最小的节点qnearst; 6)qnew←steer(qnearst,qrand);#利用steer函数qnearst到q的方向,以规定步长r扩展新的节点qnew; 7) if ObstacleFree(qnearst,qnew) then #碰撞检测,以圆形障碍物为例,碰撞检测具体过程即求所有障碍物圆心到最新扩展的边的距离,比较此距离与当前障碍物半径的大小,若小于半径即碰撞检测失败,反之通过检测; 8)V1←V1∪{qnew};#若通过碰撞检测则更新顶点集; 9)E1←E1∪{(qnearst,qnew)};#若通过碰撞检测则更新边集; 10)q′nearst←nearst(G2,qnew);#寻找树1新产生的节点qnew到树2上所有节点的欧氏距离最近节点的q′nearst,即把树1的新节点qnew作为树2的采样点以此生长,这也是同树1不同的地方; 11)q′new←steer(q′nearst,qnew);#从树2上的最近节点q′nearst沿着树1的新节点qnew的方向以规定步长r生长到树2的新节点q′new; 12) if ObstacleFree(q′nearst,qnew);#进行q′nearst和qnew的碰撞检测; 13)V2←V2∪{q′new};#若通过碰撞检测则把树2新产生的节点加入树2的顶点集V2; 14)E2←E2∪{(q′nearst,q′new)};#若通过碰撞检测则把树2新产生的q′nearst和q′new之间的连线加入树2的边集E2中; 15) do 16)q″new←stree(q′new,qnew); 17) if ObstacleFree(q″new,q′new); 18)V2←V2∪{q″new}; 19)E2←E2∪{q″new,q′new}; 20)q′new←q″new; 21) else break; 22) while not ‖q′new-qnew‖ 23) if ‖q′new-qnew‖ 24) if |V2|<|V1| then return(V1,E1);#考虑2棵树节点的平衡性,通过比较顶点集的大小交换次序选择小数的扩展。 其中步骤15)～22)表示基于树2的扩展方向继续扩展,直到树2新扩展的节点q′new连接上树1新扩展的节点qnew才结束树2的扩展。 作为一种基于采样的运动规划算法,随机采样决定了新节点的生长方向,这种随机性必然会导致缺乏目标导向性[20]。本文为了改善目标规划效率,从采样的角度提出一种P概率扇形约束采样的方法。即以1～P的概率采用传统的随机采样,这样能增加算法的容错率,消除局部最优。以概率P执行扇形约束采样增加目标导向性。以下将从初始路径规划和椭圆规划2方面讨论。 如图7所示,在生成初始路径的时候由于没有椭圆约束,连接上一次的更新节点qnew(i-1)和目标节点qgoal作为扇形的中线,以2θ作为扇形夹角,射线L(qnew(i-1),A)与射线L(qnew(i-1),B)即为采样的扇形区域,qrand即为本次扇形随机采样点,r为生长步长,qnew(i)为新生长的节点,再进行碰撞检测,若通过则加入图集。图8是椭圆区域P概率筛选采样示意图,取椭圆和扇形的交集区域,采样范围为qnew(i-1)、A、C、B所围成的区域。 图7 P概率扇形采样Fig.7 P probability sector sampling 图8 椭圆区域P概率扇形采样Fig.8 P probability sector sampling in elliptic region 图9 生长转角偏置Fig.9 Growth corner offset 由1.3节可知传统Informed-RRT*算法的采样椭圆严重依赖初始路径的质量[21],这直接决定了后续椭圆规划的效率,因此初始路径的优化也是值得考虑的方面;而且在椭圆规划过程中如果能加速导向目标点也会大大加速算法的收敛速度。传统算法在采样结束后新节点的生长方向往往是沿着采样点qrand的方向,这会使得靠近目标点的速度变慢,于是提出引入生长转角偏置策略。 如图9所示,夹角α为直线最近节点qnear1与目标节点qgoal所得向量与qnear1到采用节点qrand所得向量之间的夹角;若是没有生长方向偏置则原本生长的节点为q′new1,但是通过添加可调偏置系数μ(0<μ<1),便可改变偏置方向,更加有利于导向目标生长。 在确定搜索方式、采样空间和生长方向后,生长步长也是可以考虑的方面。传统的RRT类算法都是以固定步长r生长[22]。若是环境复杂,障碍物密集度较高,固定步长将很容易使得树的生长收敛速度减慢。为了在障碍物密集的复杂环境中路径规划有更好的适用性,根据环境复杂度引入动态步长生长策略。 如图10所示(见封2),以qnearst为半圆的圆心,绿色半圆直径为2M(M>r),方向与qnearst到qrand的方向垂直,r为初始化固定步长,绿色半圆构成的区域为qnearst环境复杂度所考虑的范围,在范围内的节点1、2、3分别做半径为N的蓝色圆,黑色代表障碍物,设蓝色圆相交的黑色障碍物的个数为S,相交个数设置2个阈值等级U和W(0 图10 变步长生长Fig.10 Variable step growth (1) 通过每次采样可求得S的大小,根据环境复杂度定义动态步长rd如下: (2) 通过以上4个方面完成对于本文算法的改进,图11是改进算法的整体流程。 图11 改进算法流程Fig.11 Flowchart of the improved algorithm 本文设计了2组测试实验,第1组为不同环境复杂度下的针对规划路径效果测试的对比实验;第2组为针对算法性能指标的测试实验,可证明本文算法的正确性。系统环境为Windows 10操作系统和PyCharm 2020.3.3 (Community Edition)。 第1组仿真实验主要针对相同步长和采样次数下,不同环境复杂度的RRT、RRT*、Informed-RRT*算法和本文改进的双向Informed-RRT*算法进行规划路径的质量对比,此处以2维规划为例,具体仿真结果如图12、图13所示。 图12 简单环境下算法仿真结果Fig.12 Simulation results of the algorithm in a simple environment 图12是在简单环境下对比算法的规划结果,图12(a)显然非渐近最优路径,图12(b)～图12(d)的路径逐渐变优。在简单环境下传统Informed-RRT*算法由于在椭圆内部采样是随机的,虽然路径比之前的2种传统算法好,但是可以看出在采样次数受限制下,改进的Informed-RRT*的路径导向性更好,虽然路径长短相差不大,但是路径转弯幅度更小。 图13是在复杂环境下对比算法的规划结果,在相同采样迭代次数相同的情况下,图13(d)改进算法的规划路径比其他路径长度和转弯幅度更小,且相比较于其他规划的路径,在采样次数有限且相同的情况下,对于复杂度较高的环境其适应性更强。 第2组实验对算法耗时进行定量测试。首先在相同环境和基础步长2下对比传统Informed-RRT*算法和本文算法进行20次实验,测试初始路径平均规划时间和总规划时间,可以看出无论在初始路径的规划过程中还是在总规划过程中总体性能都有一定的提升,且总规划性能提升相较于初始路径更加明显,具体实验结果如表2所示。 表2 引入生长转角偏置性能比较Table 2 Introduced growth corner bias performance comparison table 在相同环境和基础步长2下对比传统Informed-RRT*算法和本文改进的算法进行20次实验,可以看出随着采样迭代次数的增加,2种算法耗时差距开始拉大,本文路径规划算法效率更高,算法平均实时性变化曲线如图14所示。 图13 复杂环境下算法仿真结果Fig.13 Algorithm simulation results in complex environments 图14 算法运行时间折线图Fig.14 Algorithm running time line chart (3) 式中:n为树上已规划路径的总节点数;第i号节点qi的坐标为(xi,yi);IAT为平均转弯指数值。不同环境相同迭代次数下,分别进行20组实验得到的性能比较结果如表3所示。 无论是在简单环境下还是在复杂环境下,平均规划路径长度、平均规划时间、初始路径迭代次数、规划成功率和ATI等参数性能都有所提升;初始路径平均迭代次数在复杂环境和简单环境性能提升差不多;除了初始路径平均迭代次数和规划成功率,其他参数的参数性能提升复杂环境都更优于简单环境,这是因为改进算法在生成初始路径后完成了对生成初始路径后规划过程的优化。对于ATI参数,因为改进算法应用了变步长思想,本文改进的算法对于复杂环境的性能提升更加明显,更适用于复杂环境的路径规划。整体改进算法对于规划效率、规划成功率、规划路径的质量和环境适用性都有较大的提升。 表3 算法性能比较Table 3 Algorithm performance comparison 针对Informed-RRT*算法应用于运动规划过程中存在规划效率低、规划路径冗余且转弯较多等问题,提出一种双向Informed-RRT*算法。首先提出初始路径生成采用双向搜索的方式,有效提高了初始路径生成效率;其次提出一种P概率扇形约束采样的方法,增加了路径规划导向性和容错率;接着提出在节点扩展时引用生长转角偏置,有效加快算法收敛速度;最后提出变步长生长的方式有效提高了环境自适应性。 全文设计了大量的对比仿真实验。与传统的Informed-RRT*路径规划算法相比,改进算法的平均规划路径长度、平均规划时间、初始化路径平均迭代次数、平均转弯指数分别减少了3.63%、19.55%、18.99%、32.55%,规划成功率提高了9.45%,实验验证了该路径规划算法的正确性和可行性。进一步的研究工作希望能在本算法的基础上解决动态障碍物的局部路径规划问题和路径平滑问题。
1.2 RRT*算法
1.3 Informed-RRT*算法
2.1 初始路径引入双向搜索方式
2.2 引入P概率扇形约束采样
2.3 引入生长转角偏置
2.4 引入变步长生长
3.1 实验设计
3.2 结果与分析
