基于双树Quick-RRT*算法的移动机器人路径规划
2021-08-09魏武韩进李艳杰高天啸
魏武 韩进 李艳杰 高天啸
(华南理工大学 自动化科学与工程学院，广东 广州 510640)
近几十年来，路径规划问题的研究在机器人领域兴起并成为研究的热点[1- 2]。路径规划问题就是在环境完全已知或部分已知的前提下寻找一条与障碍物无碰撞的路径。环境完全已知的路径规划问题为全局路径规划，环境部分已知的路径规划问题为局部路径规划。路径规划算法主要包括基于采样的方法[3- 4]、人工势场法[5]、可视图法[6]、生物智能算法[7- 9]和基于深度学习的方法[10]，其中基于采样的方法以概率路图法(PRM)[3]和快速扩展随机树(RRT)[4]为代表，生物智能算法的典型代表为遗传算法[7]和蚁群算法[8- 9]。
基于采样的路径规划方法具有强大的搜索能力，将规划算法从几何学模型中分离[2]，在探索高维空间方面具有突出的表现。因此，众多学者展开了相关研究，其中RRT算法[4]引起了高度关注并得到了较为广泛的应用[10- 12]。RRT算法虽然具有概率完备性[13]和计算量小的优点，但RRT算法本身不考虑所获得的路径代价，故无法保证路径的质量[14]，即不具备渐进最优性的特性，而这一特性对于实际应用尤为重要。
为了加快RRT算法寻找路径的速度，文献[13]提出了双向RRT算法，与基本的RRT算法最大的不同在于，双向RRT算法同时存储从起点、终点处生成的两棵随机树而不是仅有一棵树。在计算资源充足的前提下，双向RRT算法的多树思想为加快收敛速度提供了一个新的思路。针对RRT算法不具备渐进最优性的缺点，大量学者基于RRT算法提出了相应的改进算法。文献[15]提出了具备渐进最优性的RRT*算法，其核心在于增添了选择最优父节点和剪枝两个优化过程，但此算法存在收敛速度慢的缺点。文献[16]从限定随机点采样范围的角度提出了Informed-RRT*算法，虽然其在一定程度上加快了RRT*算法的收敛速度，但其采样过程在很大程度上依赖于当前最优解，且当超椭球超出规划问题本身时，算法无法进行下去。文献[17]受多棵扩展树可加快收敛速度的启发而提出的双向RRT*算法，在保证渐进最优性的前提下很大程度上提高了收敛速度。文献[18]通过扩大RRT*算法两个优化过程的追溯范围，提出了Quick-RRT*算法，该算法为RRT*算法提供了一种新的树扩展框架。
基于多树结构的双向RRT*和Quick-RRT*算法的优势，本文提出了双树Quick-RRT*算法，并通过仿真实验分析了该算法的性能。
1 问题描述及背景知识
1.1 问题描述
路径规划空间常采用位形空间进行描述。依据是否与障碍物发生碰撞，整个位形空间C划分为自由位形Cfree和碰撞位形Cobs，其中Cobs=CCfree。
路径规划问题是给定起点vstart和终点vgoal(vstart,vgoal∈Cfree)的前提下寻找一条从vstart到vgoal的无碰撞路径。
1.2 RRT*算法
RRT*是一种单查询树状结构的搜索算法，从起点vstart初始化变量Tree后进入迭代过程，具体迭代过程如下：
(1)使用随机采样函数Sample()产生随机采样点vrand。
(2)产生新节点vnew。先使用Nearest(vrand,Tree)函数遍历Tree得到最近节点(vnearest)，然后Steer(vnearest,vrand,δstep)函数以vnearest为起点，朝着vrand前进δstep，从而产生vnew。
(3)在vnearest和vnew无碰撞的情况下，优化vstart到vnew的路径。Near(Tree,vnew,rnear)函数以vnew为圆心、rnear为半径找出邻域Vnear，ChooseParent(Vnear,vnew)函数判断vnew父节点替换成Vnear中的点后是否会缩短vstart到vnew的距离，如果缩短则进行替换操作，否则不操作。
(4)优化vstart到Vnear中点的路径。Rewire(Vnear,vnew)函数判断Vnear中每个节点是否可以将其父节点更新为vnew，如果路径距离减小则进行父节点更新操作，否则不操作。优化过程使得RRT*算法具有渐进最优性[15]。RRT*算法的具体实现过程如下：
{Tree.init(vstart);
for i =1 to K do
vrand←Sample(i);
vnearest←Nearest(vrand,Tree);
vnew←Steer(vnearest,vrand,δstep);
if CollisionFree(vnearest,vnew,map)
Vnear←Near(Tree,vnew,rnear);
vparent←ChooseParent(Vnear,vnew);
Tree←Link(vparent,vnew);
Tree←Rewire(Vnear,vnew);
end if
end for
returnTree;}
1.3 Quick-RRT*算法
Quick RRT*算法利用三角不等式定理对随机树结构进行优化。与RRT*算法相比，Quick-RRT*算法扩大了ChooseParent(Vnear,vnew)和Rewire(Vnear,vnew)函数中Vnear的范围，在保持时间复杂度和空间复杂度相同的情况下，获得了更优的初始路径和更快的收敛速度[18]。Quick-RRT*算法的具体实现过程如下：
{Tree.init(vstart);
for i=1 to K do
vrand←Sample(i);
vnearest←Nearest(vrand,Tree);
vnew←Steer(vnearest,vrand,δstep);
if CollisionFree(vnearest,vnew,map)
Vnear←Near(Tree,vnew,rnear);
VprtNear←Ancestry(Tree,Vnear,dnear);
vparent←ChooseParent(Vnear∪VprtNear,vnew);
Tree←Link(vparent,vnew);
VprtNew←Ancestry(Tree,vnew,dnew);
Tree←Rewire(Vnear,vnew∪VprtNew);
end if
end for
return Tree;}
Quick RRT*算法相较于RRT*算法,增加了Ancestry(Tree,V,d)函数，该函数的作用是追溯邻域V的父节点集合。d为父节点层数，d值越大，得到的随机树结构越优，但计算资源的需求会越强[18]。
2 双树Quick-RRT*算法
基于多树结构的双向RRT*算法在提高收敛速度上和Quick-RRT*可与多种启发方法结合的优势，本文提出了双树Quick-RRT算法，其具体实现过程如下：
{Tree1.init(vstart);
Tree2.init(vgoal);
for i = 1 to K do
vrand←Sample(i);
vnearest←Nearest(vrand,Tree1);
vnew←Steer(vnearest,vrand,δstep);
if CollisionFree(vnearest,vnew,map)
Vnear←Near(Tree1,vnew,rnear);
VprtNear←Ancestry(Tree1,Vnear,dnear);
vparent←ChooseParent(Vnear∪VprtNear,vnew);
Tree1←Link(vparent,vnew);
VprtNew←Ancestry(Tree1,vnew,dnew);
Tree1←Rewire(Vnear,vnew∪VprtNew);
Tree2←Connect(Tree2,vnew);
if isConnected(Tree1,Tree2)
Path=FillPath(Tree1,Tree2);
end if
Swap(Tree1,Tree2);
end if
end for
return path;}
起点树Tree1和终点树Tree2进行初始化后,进入迭代过程，具体迭代过程如下：
(1)产生新节点，使用Sample()、Nearest(vrand,Tree1)函数产生vnearest，使用Steer(vnearest,vrand,δstep)函数产生vnew。
(2)在vnearest和vnew无碰撞的情况下，优化vstart到vnew的路径。Near(Tree,vnew,rnear)和Ancestry(Tree,Vnear,dnear)两个函数找出vnew的潜在父节点，ChooseParent(Vnear∪VprtNear,vnew)函数在潜在父节点中选择父节点，使得从vstart到vnew的路径距离最小，然后使用Link(vparent,vnew)函数连接vnew和vparent这两个节点。
(3)优化vstart到Vnear中点的路径。使用Ancestry(Tree,vnew,dnew)函数找出vnew的父节点vprtNear，将vnew和vprtNear作为VprtNear中点的潜在父节点，使用Rewire(Vnear,vnew∪VprtNew)函数在潜在父节点中找出父节点，使得路径距离最小。
(5)使用FillPath(Tree1,Tree2)函数拼接路径。在Tree2与vnew相连接的情况下，Tree1从vnew追溯父节点直到vstart，这是Tree1部分的路径，Tree2采用相同的方法，从与vnew相连接的节点开始追溯父节点直到vgoal，这是Tree2部分路径，将这两段路径进行拼接，得到可行路径。
(6)使用Swap(Tree1,Tree2)函数交换两棵树中的内容，因为上述迭代过程Tree1只增加了一个节点，Tree2中增加了多个节点，所以需要交换两棵树内容，使得两棵树经过数次迭代后节点数量保持平衡。
Quick-RRT*和双树的融合关键在于Connect(Tree2,vnew)和Swap(Tree1,Tree2)函数。当Tree1完成vnew的拓展后，Connect(Tree2,vnew)将Tree2与vnew进行直线连接，连接过程会向Tree2中增加多个点，且直线连接不需要优化，拓展只是增加一个点且需要优化路径，所以Connect()函数提高了算法的效率。Swap(Tree1,Tree2)将两棵树中的内容交换，该函数可以保证两棵树中点的数量大致平衡。
3 算法性能分析
3.1 概率完备性
对于双向RRT算法，随机树中的点v在Cfree中服从均匀分布，且该算法具有概率完备性[13]。双树Quick-RRT*算法较于双向RRT算法，产生点的方法相同，区别在于优化了点之间的连接方式，故本算法中的点v在Cfree中同样服从均匀分布。同样地，双树Quick-RRT*算法具有概率完备性。
3.2 渐进最优性
(1)
文献[19]中指出，若算法满足概率完备性，且满足该文献中的假设11(代价函数的叠加性)、12(代价函数的连续性)和13(以Cfree中的点x为圆心，ε(ε∈R+)为半径的圆(或超球体)Bε,x⊂Cfree)，则算法满足渐进最优性。
代价函数的叠加性和连续性确保了两个相似路径的代价也相似，假设13确保了最优轨迹附近有自由空间以允许收敛。概率完备性确保了当时间充足时，地图中存在n(n→∞)个点的连通图。当算法迭代时，连通图中的路径会不断缩短，代价函数的连续性确保了在路径缩短的同时，代价函数也会缩小，最终趋近于最优路径，也就是渐进最优性。
双树Quick-RRT*算法满足概率完备性，路径长度lpath满足假设11、12，假设13是技术性假设，对地图的一种要求，与算法本身无关，故双树Quick-RRT*算法满足渐进最优性。
4 仿真实验
为了分析本文提出的算法的性能，设计了3种环境进行仿真分析，考虑到移动机器人的自身半径，对障碍物进行了膨化处理，并将机器人看做一个质点。仿真实验平台及配置为：Matlab R2018b，64位Windows 10，处理器Intel(R)Core(TM)i7- 7500U，CPU主频2.7 GHz，内存12 GB。
仿真地图环境如图1所示，所有环境的地图规模均为[1 184,872]，以地图左上角为坐标原点，最优路径长度为lopt，次优路径长度lsubOpt满足lopt 图1 仿真实验环境 采用本文提出的双树Quick-RRT*算法与RRT*、Quick-RRT*和双向RRT*算法进行对比仿真实验，其中RRT*和Quick RRT*是单树算法，双向RRT*和双树Quick-RRT*是双树算法，算法中设置的变量及参数如下：步长δstep=30像素，产生Vnear的半径rnear=80像素，Quick-RRT*和双树Quick-RRT*算法中选择父节点的深度dnear=dnew=1。 使用以下5个参数对上述4种算法进行比较：初始路径的长度(linit)、tfind、t5%，算法发现路径的成功率(Rsuc)和路径长度(lpath)。前3个参数取各算法运行100次后的均值；Rsuc和时间呈正相关，4种算法都具备概率完备性，当时间充足时，Rsuc都会达到100%。本文采用以下方法计算Rsuc：算法运行100次，记录每次的tfind，计算tfind从0 s到指定时刻的分布数量，就是该时刻的成功率。lpath用以显示算法的路径收敛效果。由于算法具有随机性，在相同条件下多次运行同一算法，tfind都会不同，这就导致算法在刚开始运行时没有发现可行路径，或者只有少数次数产生了lpath，这些少数次数无法代表100次运行的结果，所以需要Rsuc作为判断依据。本文按以下方法统计lpath：算法运行100次，每次运行都会定时记录lpath的值，故当100次运行完毕，每一个时刻都会有100次有效或者无效的数据，无效数据指该时刻没有产生lpath(此时lpath=0)，当该时刻有60次以上的有效数据才计算该算法的lpath均值。采用此种方法计算时，因算法之间的时间差异较大，故不同算法间的时间跨度会较大。 上文说明的数据是分为两批实验记录的，因为同时记录所有数据难度较大。lpath是算法运行100次定时记录的，linit、tfind、t5%和Rsuc是另外运行100次记录的。linit和lpath的起始值有差异，主要受两个因素的影响，一是数据不在同一批实验中获得，二是linit不受Rsuc的影响，而lpath受Rsuc的影响。 U形障碍物环境中的vstart设置为[592,436]，vgoal设置为[1 000,436]。4种算法的仿真结果如图2所示。 图2 4种算法在U形障碍物环境下的仿真结果 从图2可知，在U形障碍物环境下，双树Quick-RRT*算法的性能最佳，路径完全贴合U形障碍物边缘。4种算法的lfirst、tfind和t5%如表1所示，相同时间的成功率和路径长度如图3所示。 从表1可知，双树Quick-RRT*算法的性能表现最优，相较于Quick-RRT*算法，tfind缩短了89.8%，linit却更短，t5%缩短了68.0%；相较于RRT*和双向RRT*算法，tfind分别缩短了79.1%和57.0%，t5%分别缩短了83.0%和77.7%。可以得出，双树Quick-RRT*算法大大缩短了tfind和t5%，产生的路径更优。 表1 4种算法在U形障碍物环境中的性能比较 图3 U形障碍物环境下4种算法的路径长度和发现路径的成功率 图3(a)更加具体地展示了tfind的时间区间分布，双树Quick-RRT*算法全部成功所需时间最少，曲线上升最快，双向RRT*算法次之，Quick-RRT*所需时间最长。可以发现，双树结构算法比单树结构算法到达Rsuc=100所需的时间更短。 从图3(b)可知：双树Quick-RRT*算法出现数据的时间最早，说明在100次实验中，有60次以上的lpath所需时间最短，该曲线最为平缓，说明算法不需要花费过多的时间优化路径；双向RRT*和RRT*算法虽然产生数据的时间也比较早，但曲线前后差值大，说明算法需要更多的时间优化路径；Quick-RRT*算法有60次以上的lpath所需时间最多，曲线前后差值也比双树Quick-RRT*算法大。 综合上述分析可知：Quick-RRT*算法发现路径需要的时间最长；相较于其他4种算法，双树Quick-RRT*算法的tfind最短，相同时间发现路径的成功率最高，路径收敛速度最快。 狭长通道环境中，vstart设置为[100,100]，vgoal设置为[1 100,700]。4种算法在该环境下的仿真结果如图4所示。 图4 4种算法在狭长通道环境下的仿真结果 从图4可知，Quick-RRT*和双树Quick-RRT*算法的性能明显优于RRT*和双向RRT*算法。4种算法的linit、tfind和t5%参数如表2所示，相同时间的成功率和路径长度如图5所示。 表2 4种算法在狭长通道环境中的性能比较 因为狭长通道空间有限，限制了路径优化的空间，使得linit与lsubOpt的差距减小，路径优化时间减少，例如图5(b)中曲线比图3(b)中曲线更加平缓，表2中tfind和t5%的差值减小，Quick-RRT*需要增加一位小数以显示值的不同。双树Quick-RRT*算法相较于Quick-RRT*，linit虽略长，但tfind缩短了64.0%，t5%缩短了63.9%，而相较于RRT*和双向RRT*算法，tfind分别缩短了67.4%和54.4%，t5%分别缩短了81.5%和54.9%。从表2可知：双树Quick-RRT*算法在保证路径质量的前提下，大幅度提高了算法的搜索路径效率。 图5 狭长通道环境下4种算法的的路径长度和发现路径的成功率 狭长通道空间的限制，使得算法的有效采样点范围大大缩小，加快了算法找到有效路径的效率，也减小了算法之间成功率的差异。图5(a)中曲线之间的差异比图3(a)中小，RRT*和双向RRT*算法的曲线在后期几乎重合，说明狭长通道环境减小了单树结构算法和双树结构算法之间的效率差异。双树Quick-RRT*和Quick-RRT*算法的曲线依旧有明显的差异，说明本文提出的算法在效率上仍有明显的优势。 图5(b)中双树Quick-RRT*算法在2 s时的路径长度因为有新产生的lpath加入而有微小的上升，而不是路径没有收敛。双树Quick-RRT*与Quick-RRT*算法的曲线几乎重合，但前者的时间缩短了40%。双树Quick-RRT*与双向RRT*算法的曲线相比，开始时间相同，但后者的路径长度更长。相较于RRT*算法，双树Quick-RRT*算法的搜索路径和路径收敛效率高，产生的路径短。综合上述分析可知：即使在狭长空间这种有严重空间限制的环境中，双树Quick-RRT*算法使用相同的时间产生的路径长度最短。 简易迷宫环境中，vstart设置为[100,700]，vgoal设置为[1 000,100]，4种算法在简易迷宫环境下的仿真结果如图6所示，linit、tfind和t5%如表3所示，相同时间的成功率和路径长度如图7所示。 图6 4种算法在简易迷宫环境下的仿真结果 从图6中可知，在简易迷宫环境下，Quick-RRT*和双树Quick-RRT*算法的性能优于RRT*和双向RRT*算法。 表3 4种算法在简易迷宫环境下的性能比较 从表3可知，双树Quick-RRT*算法的参数值仍然是4种算法中最小的，相较于RRT*、Quick-RRT*和双向RRT*算法，tfind分别缩短了85.0%、89.7%和34.7%，t5%分别缩短了81.6%、55.9%和68.6%，说明本文提出的算法明显缩短了tfind和t5%。 从图7(a)可知，双树Quick-RRT*算法的Rsuc依旧是最快到达100%，双向RRT*算法次之，Quick-RRT*算法最慢，说明双树Quick-RRT*算法在较为复杂的环境下仍然大幅度缩短了tfind。从图7(b)可知：双树Quick-RRT*和Quick-RRT*算法的曲线几乎重合，但前者出现数据的时间是后者的1/10，大幅度缩短了时间；与另外两种算法相比，双树Quick-RRT*算法的搜索路径效率和路径长度都具有优势。 图7 简易迷宫环境下4种算法的路径长度和发现路径的成功率 综合上述分析可知：在简易迷宫环境中，相较于其他3种算法，双树Quick-RRT*算法的tfind仍旧是最小，路径收敛效果最好。 为进一步提高Quick-RRT*算法的收敛速度，本文提出了一种双树Quick-RRT*算法，该算法使用Quick-RRT*算法建立起点树和终点树，并分析了该算法的概率完备性和渐进最优性。在3种不同环境下的仿真实验结果结明：相较于其他3种算法，本文算法提高了寻路效率和质量，tfind缩短了约69%，t5%缩短了约70%，linit缩短了约5%。 本文的研究对象是二维环境中移动机器人，未来可以考虑将双树Quick-RRT*算法应用于多维度关节空间的移动机械臂的路径规划问题。
4.1 U形障碍物环境
4.2 狭长通道环境
4.3 简易迷宫环境
5 结语
