移动机器人实时采样路径重规划
2021-10-28王文格卢成阳
涂 睿,王文格,卢成阳
湖南大学 机械与运载工程学院,长沙 410082
路径规划技术是实现移动机器人导航功能的关键技术[1],影响着移动机器人导航过程中的效率和安全性。移动机器人的路径规划实质上是找到起点到终点的无碰撞路径[2],其路径质量和规划时间是评价规划算法性能的重要指标[3]。
基于采样的路径规划算法是近年来最具影响力的算法类型之一,通过随机分布的采样点作为启发信息来逐步获取地图中的连通信息,直至获取可行路径解。其中最具代表性的是快速搜索随机树(RRT)[4]。RRT因其快速的节点拓展机制和强大的搜索性能而被广泛应用到众多领域[5-7],其改进算法在静态环境中的应用已比较成熟[8-10]。
移动机器人的工作环境往往是动态的(伴随障碍物移动、突然出现和消失),移动机器人需要在工作环境中实时重规划路径以避开动态障碍物,但基于RRT的算法具有强随机性,应用于动态环境会出现严重的路径抖动问题。文献[11]提出了一种具有路径缓存功能的实时重规划算法ERRT,每次重规划会重新生长随机树并将之前的规划路径点作为启发信息进行偏置采样。文献[12]提出了一种反向生长随机树的实时重规划算法DRRT,将规划终点作为随机树树根,在路径受阻时对与障碍物发生碰撞的树枝进行修剪去除,并在此基础上重新生长来获取重规划路径。文献[13]结合文献[11-12]并增加了引力分量,提出了一种动态路径规划方法,减弱了重规划路径的随机性。文献[14]通过人工势场引导加速随机树向目标区域生长并远离障碍物。上述算法通过保留连通信息和增加引导信息来减少路径抖动程度,但执行路径的质量往往不是最优的。文献[15]提出的RRT*算法具有概率完备性和渐近最优性,被用于搜索最优路径。其路径质量会随着采样次数的增加而不断提升,但是其收敛速率会随着采样点增多而不断下降。文献[16]对RRT*算法的随机采样点进行了优化约束,加快了路径收敛速率。为保证算法的实时规划性能,需要对重规划和路径执行过程每步长的优化采样次数进行限制,这会导致实时优化效果变差。文献[17]将动态规划问题转化成了类局部规划问题,在执行路径被阻断后,会重新规划一段路径绕开障碍物并连接到原路径未阻断部分的最近路径点上,这会导致在从当前位置的规划的路径并不是最优的。
针对上述问题,本文提出了一种具备实时优化功能的重规划算法DRT-RRT*。在路径执行过程中首先采用路径剪枝策略减少路径的拐点数量,并通过组合采样和局部终点跳动策略来优化当前待执行路径段,聚焦优化目标来提高采样点的利用率和路径收敛速率;然后在路径重规划时结合目标偏置采样和组合采样策略提高路径搜索速度和稳定程度;最后基于MATLAB进行对比仿真实验,结果表明DRT-RRT*执行路径代价更短,路径质量更加稳定,实时优化效果更好。
1 反向生长RRT*算法
1.1 基本RRT*算法
RRT*算法在RRT算法基础上增加了重选父节点和重新连接两个步骤,使得算法具备了渐近最优性。图1解释了RRT*算法的渐近优化原理,图中节点中的数字代表节点拓展顺序,树枝上的数字代表节点间的路径代价。
图1(a)表示当前新拓展节点xnew为节点7,其父节点xparent为节点4。图1(b)表示在一定半径范围内获取xnew的所有相邻节点集合Xnear,并重新选取Xnear中使xnew到xstart路径代价最低的节点2作为其新父节点xparent,以此来降低xnew的路径代价。图1(c)对xnew及所有邻节点Xnear进行重新连接。如果从xstart经由xnew(节点7)到达Xnear(节点3~6)中的任意节点的总代价比xstart到Xnear中节点的当前代价要低,则将该节点的父节点重新指定为xnew,以此来降低Xnear中节点的代价。重新连接的有效半径范围由公式(1)决定:
图1 RRT*渐近优化原理Fig.1 Asymptotic optimality principle of RRT*
式中,η为拓展步长,γRRT*是一个常数(它取决于规划环境),n代表当前随机树中的节点个数,d代表规划环境的维度。
随着迭代次数不断增加,重新连接半径会逐渐减小并稳定下来。RRT*会在每一个新拓展节点生成后进行父节点重选和重新连接这两个步骤,使路径逐步向最优解收敛,但其收敛速率会因采样点的增加而逐步下降。
1.2 反向生长随机树的剪枝和重新生长
移动机器人在动态环境中执行规划路径时,规划路径会随着环境地图的改变而发生变化。当待执行路径受到阻断时,从机器人位置重新生成一棵随机树并进行优化会占用较多计算资源,影响实时性能。于是将路径终点作为随机树的树根进行反向生长,如图2(a)所示,起点为机器人当前位置,终点为随机树树根。当规划环境发生改变后,仅对少量被障碍物干涉的树枝进行修剪,如图2(b)、(c)所示。并以剩余随机树为基础重新生长随机树,获取重规划路径,如图2(d)所示。这样可以保留更多的随机树信息,减少未受影响部分路径的抖动程度[12]。
图2 随机树的剪枝和重新生长Fig.2 Pruning and regrowth of random tree
2 DRT-RRT*算法
2.1 路径剪枝处理
RRT*算法规划的路径由多段树枝组成且会包含多个冗余树节点,这会使得移动机器人在执行路径的过程中频繁转向。文献[18]提出了一种路径剪枝的处理方式,通过三角不等式逐段比较,减少路径长度和路径中节点数量的同时减少路径抖动程度。由于路径拐点大量减少,因此可以将后续的路径优化过程由对整条路径的采样优化转变为对于拐点位置的优化,这为局部终点跳动策略提供了基础。
如图3所示,在初始路径生成后,该方法会从终点开始向起点进行回溯,将当前节点node、其父节点nodeparent及其祖父节点nodeparent构建为三角形逐步进行修剪。如未与障碍物发生碰撞,则将nodeparent从路径中删除,将nodegrandparent作为当前节点node的新父节点nodeparent,同时将回溯的下一个路径点作为新nodegrandparent继续构建三角形进行新一轮的修剪。如与障碍物发生碰撞后,则将nodeparent作为当前节点node,回溯两个路径点作为其父节点nodeparent和祖父节点nodegrandparent重新构建三角形进行修剪,直到将起点添加进三角形作为nodegrandparent。
图3 基于三角不等式的路径剪枝Fig.3 Path pruning based on triangular inequality
2.2 组合采样
在动态环境中重规划发生的时间点是不确定的,如果沿用RRT*的随机采样策略优化整条执行路径,很大概率会出现多段路径未执行就已经发生变化的情况,这会导致对未执行路径的优化失效,而且在全局范围内进行随机采样会稀释优化效果。而且为满足实时性,动态环境中的路径规划会限制优化采样次数,因此全局随机采样的实时优化效果会变得很不明显。
为提高执行路径的质量,将优化目标从整条待执行路径转变为机器人位置至最近路径拐点的路径段会更加有效。组合采样通过增加当前路径段和其局部终点附近的随机树节点密度来提高附近随机树的收敛程度,从而缩短当前路径的长度和降低路径抖动程度。随机采样点xrand的生成方式如公式(2)所示:
组合采样的第一部分Around(xlocal_goal)会在以局部终点xlocal_goal为中心的圆内进行随机采样。在执行路径优化过程中会对拐点附近的随机树进行优化以提高拐点质量,降低路径代价。在进行重规划时,局部终点附近的密集采样点会增加扩展速度,加快重规划路径的生成;第二部分Bounded称为有界采样,会在机器人和局部终点的连线方向的区域内进行采样,用于补充重规划后执行路径段附近因障碍物碰撞而被修剪掉的大量树枝,同时提高沿途随机树枝的质量,对执行路径进行实时修正,加速路径收敛;第三部分Random为全局随机采样,用于获取随机树稀疏区域的连通信息。由于稀疏区域拓展新节点时邻节点较少,因此拓展速度会相对较快。拓展的树枝会均匀地分布在地图的自由空间中,为下一次重规划提供部分树枝基础。同时随机采样还可以保证算法的概率完备性。
三种采样方式的权重根据规划环境以概率α和β进行分配。后续的仿真实验中α取0.3,β取0.6。
2.3 局部终点跳动策略
RRT*算法的收敛速率会因节点聚集效应而逐渐下降,在接近最优解时收敛速率变得会非常缓慢,较小的质量提升都会花费很大的时间代价。局部终点跳动策略将局部终点模拟为微粒,使其在以自身为中心的一定半径范围内进行随机运动,通过调整局部终点的位置来获取更优的路径解。这个过程无需新的采样点的加入,减少了内存消耗的同时提高了路径收敛的速率。图4表示在规划路径质量较差时,移动机器人在路径执行过程中通过局部终点跳动策略来实时修正路径。
图4 路径实时修正过程Fig.4 Real-time path modification process
2.4 DRT-RRT*实现
DRT-RRT*算法在规划初始执行路径时沿用了RRT*算法的节点拓展策略,同时引入了目标偏置采样,一旦偏置采样开始,只有在碰到障碍物或者到达终点才会停止,这加快了初始路径的搜索。然后在随机树尽量布满环境地图后对执行路径进行剪枝处理,分割为多段直线路径,减少路径拐点,如图5(a)所示。
从图5(b)~(i)可以看出,当移动机器人的待执行路径被突然出现的障碍物阻断时,DRT-RRT*会先对受影响的树枝进行修剪,然后通过组合采样策略快速修复随机树,获取并优化重规划路径。为保证实时性,会对重规划生成后的优化采样次数进行限制,因此此时重规划路径不是最优路径。DRT-RRT*会将优化任务分配到执行过程中,通过组合采样和局部终点跳动策略实时修正当前执行路径段,使其快速向最优路径收敛。
图5(j)、(k)表示环境地图未改变时,执行路径的实时优化过程。可以看出在图5(i)中重规划路径之后,移动机器人会在路径执行过程中不断对未执行路径进行实时优化,不断提升待执行路径段的质量。图5(l)表示最终的执行路径。
图5 DRT-RRT*规划过程Fig.5 Planning process of DRT-RRT*
DRT-RRT*通过基于三角不等式的路径剪枝将多段曲折的执行路径修剪为仅在障碍物处发生转折的直线路径,减少了大量的无效拐点,降低了组合采样第二部分有界采样和局部终点跳动策略的复杂度。组合采样第一部分与局部终点跳动策略共同作用,对拐点位置进行逐步优化,以提高当前执行路径的质量。相比于RRT*算法采用随机采样优化的策略,组合采样结合局部终点跳动策略对于当前执行路径段的优化效果更为明显,在重规划路径较差时通过调整拐点位置来快速修正路径,以此更好的保证执行路径的质量一致性。DRT-RRT*算法流程如图6所示。
图6 DRT-RRT*算法流程图Fig.6 Algorithm flow chart of DRT-RRT*
3 仿真实验分析
为了验证DRT-RRT*算法的性能,分别在存在陷阱的未知静态障碍物环境1、需要进行连续重规划的未知静态障碍物环境2、存在未知动态障碍物的环境3进行了仿真分析,考虑到移动机器人的自身的尺寸问题,对障碍物的体积进行了膨胀处理。并将仿真结果与RRT*与本文优化的增加了三角不等式剪枝策略的RRT*-Pruning进行比较。仿真实验程序在64 bit MATLAB 2016平台中运行,电脑运行环境为Windows 10,配置为Intel i5-8400U@2.80 GHz CPU和4 GB RAM。
3.1 未知静态障碍物环境规划过程
未知静态障碍物环境1中,障碍物为未知位置的静态圆形障碍物。移动机器人需要随着环境地图的改变重新规划执行路径,如图7(b)所示。地图右边界设置了陷阱,当移动机器人即将通过地图右侧边界的通道到达终点时,将通道关闭,如图7(c)所示。该环境用于测试各算法在随机树被大量剪枝后的重规划性能。
图7 环境1规划过程Fig.7 Planning process in environment 1
在未知静态障碍物环境2中,障碍物为连续放置的静态圆形障碍物,用于频繁地触发重规划,测试算法的抗抖动性能,如图8所示。
图8 环境2规划过程Fig.8 Planning process in environment 2
3.2 未知动态障碍物环境规划过程
未知动态障碍物环境3中,障碍物为2个横向移动的未知圆形障碍物,如图9(b)、(c)所示。在终点附近放置了两个静态障碍物用于大范围阻断随机树,用于测试各算法的重规划性能,如图9(d)所示。
图9 环境3规划过程Fig.9 Planning process in environment 3
3.3 性能比较
各算法进行规划的环境均为[500,500],各环境中规划的过程如图7~9所示。在静态环境中初始路径的采样次数为1 000次,在动态环境中初始路径的采样次数为500次,执行路径过程中每步长优化采样次数为20次,各算法在对应环境中均独立运行20次,取平均值作为性能参数。
图10中地图右侧的通道关闭,移动机器人附近的随机树枝都会被修剪掉。RRT*和RRT*-Pruning会因随机树信息的缺失而导致重规划路径的质量较差,且在优化采样次数相同的情况下,实时优化效果不明显,规划的路径会偏离最优路径较远。而DRT-RRT*能在随机数信息较少的情况下快速获取较高质量的重规划路径,并在执行过程中对路径进行实时修正。
图10 环境1规划结果比较Fig.10 Comparison of planning results in environment 1
图11中连续放置的障碍物会频繁触发重规划。RRT*算法会出现严重的路径抖动,路径质量会变得很差。RRT*-Pruning因增加了路径剪枝策略,路径抖动程度会相对减小,但由于比较贴近障碍物,会被修剪掉更多的随机树枝,导致重规划时间变长。DRT-RRT*会不断对当前执行路径段进行优化,以降低路径的抖动程度,使执行路径不断向最优收敛。
图11 环境2规划结果比较Fig.11 Comparison of planning results in environment 2
图12中较大的自由空间使RRT*和RRT*-Pruning的随机采样优化策略对于路径的实时优化效果不明显。而DRT-RRT*能更好地捕捉地图信息,利用好障碍物离开后的自由空间以规划出更优的执行路径。
图12 环境3规划结果比较Fig.12 Comparison of planning results in environment 3
图13为RRT*、RRT*-Pruning和DRT-RRT*在图10~12所示环境中进行20次实验后的路径长度对比结果。结合表1中平均执行路径长度参数可以看出DRT-RRT*在3种环境下的平均执行路径长度均低于其他两种算法。在规划环境1中,相比于RRT*,平均路径长度缩短了17.2%,相比于RRT*-Pruning,平均路径长度缩短了5.1%。在规划环境2中,相比于RRT*,平均路径长度缩短了22.8%,相比于RRT*-Pruning,平均路径长度缩短了3.7%。在规划环境3中,相比于RRT*,平均路径长度缩短了15.4%,相比于RRT*-Pruning,平均路径长度缩短了5.1%。从图13中各算法规划路径长度的波动程度和表1中的路径长度标准差参数可以看出DRT-RRT*在3种环境下规划路径的稳定程度均优于其他两种算法。由此得知DRT-RRT*所规划的执行路径质量更好且更加稳定,路径执行过程的实时优化效果更好。
表1 路径质量和稳定性参数对比Table 1 Comparison of path quality and stability parameters
图13 不同环境下路径长度对比Fig.13 Comparison of path length in different environment
组合采样策略会在发生重规划时将采样点分布在移动机器人周围,从而加快随机树向机器人位置的生长。相比于RRT*和RRT*-Pruning在地图中进行随机采样,组合采样的采样点会在终点附近聚集,得到更多能与终点连通的潜在节点,从而提高重规划效率,同时有利于后续的路径优化过程。
通过表2可知,采用了组合采样策略的DRT-RRT*在不同环境下的平均重规划采样数均低于其他两种算法,提高了节点利用率。在规划环境1中,相比于RRT*,平均重规划采样数降低了39.9%,相比于RRT*-Pruning,平均重规划采样数降低了43.2%。在规划环境2中,相比于RRT*,平均重规划采样数降低了23.3%,相比于RRT*-Pruning,平均重规划采样数降低了40.3%。在规划环境3中,相比于RRT*,平均重规划采样数降低了69.6%,相比于RRT*-Pruning,平均重规划采样数降低了70.8%。由于组合采样需要对路径进行剪枝和分割,会增加部分重规划时间,但采样数量明显减少,使得整体上平均重规划时间低于其他两种算法。由此得知,DRT-RRT*的实时性更好,重规划效率更高。
表2 实时性能参数对比Table 2 Comparison of real-time performance parameters
4 结束语
针对传统的采样规划算法在动态环境中进行重规划时出现的路径质量差,抖动严重,实时优化效果不明显等问题,提出了一种基于反向生长最优快速搜索随机树的实时重规划算法DRT-RRT*。组合采样和局部终点跳动策略增强了重规划性能和执行路径的实时优化性能,提高了路径质量的稳定性。仿真实验数据表明,提出的DRT-RRT*算法规划效率更高,路径质量更优,实时优化效果更好。