基于双向快速探索随机树的狭窄通道路径规划

付久鹏 曾国辉 黄勃 方志军

摘 要：针对移动机器人路径规划过程中基于快速探索随机树（RRT）算法难以对窄道进行采样的问题，提出一种专门用于狭窄通道路径规划的改进桥梁检测算法。首先对环境地图预处理并提取出障碍物边缘节点集合作为桥梁检测算法的采样空间，从而避免了大量无效采样点，并使窄道样本点分布更加合理化;其次改进了桥梁端点的构建过程，提高了桥梁检测算法的运算效率;最后使用一种轻微变异Connect算法快速扩展窄道样本点。对于实验中的窄道环境地图，与原始RRT-Connect算法相比较，所提改进算法的路径探索成功率由68%提高到92%。实验结果表明，该算法能够较好地完成窄道样本点采样并有效地提高路径规划效率。

关键词：路径规划;快速探索随机树;狭窄通道;桥梁检测算法;障碍物边缘检测

中图分类号：TP242.6

文献标志码：A

Abstract： In the process of mobile robot path planning， it is difficult for the Rapidly-exploration Random Tree （RRT） algorithm to sample narrow channels. In order to deal with this problem， an improved bridge detection algorithm was proposed， which is dedicated to narrow channel sampling. Firstly， the environment map was pre-processed and the obstacle edge coordinate set was extracted as the sampling space for the bridge detection algorithm， thus avoiding a large number of invalid sampling points and making the sampling points distribution of the narrow channel more rational. Secondly， the process for bridge endpoint construction was improved， and the operation efficiency of the bridge detection algorithm was increased. Finally， a slight variant Connect algorithm was used to expand the narrow channel sample points rapidly. For the narrow channel environment map in the experiment， the improved algorithm has the success rate increased from 68% to 92% compared with the original RRT-Connect algorithm. Experimental results show that the proposed algorithm can sample the narrow channel well and improve the efficiency of path planning.

Key words：  path planning; Rapidly-exploring Random Tree （RRT）; narrow channel; bridge detection algorithm; obstacle edge detection

0 引言

隨着人工智能等新兴产业的发展，有关机器人智能路径规划理论和技术的研究与应用引起了人们的高度关注和重视[1]。机器人路径规划问题可以描述为机器人在一定约束条件的情况下，依据某个或某些优化准则（如规划时间最短、行走路径最优、占用资源最少），在环境空间内找到一条从起始点到目标点的无障碍路径[2]。近年来，基于快速探索随机树（Rapidly-exploring Random Tree， RRT）的规划方案[3-8]由于其在高维空间中的卓越性能而备受青睐，其中，尤以RRT-Connect算法[5]性能提升最为明显，该算法在双向RRT算法[3]的基础上加入了贪心启发函数，一定程度上提升了算法的收敛速度。

尽管RRT算法在机器人路径规划问题上取得了一定的成功，但面对一些比较特殊的情况，尤其是当环境空间中存在大量狭窄通道时，其性能会出现大幅度下降[9]。主要原因是由于RRT算法普遍采用均匀的全局随机采样策略，相对于整个环境空间而言，窄道所占面积比例很小，这就导致窄道内采样概率很低，随机树难以向窄道扩展;同时窄道的几何约束限制了机器人通过时允许的方向变化，从而使随机树的扩展难以通过窄道到达另一侧，窄道两端无法连通，最终算法因为迭代次数达到上限而路径探索失败[10]。文献[11]中提出了一种将RRT-Connect算法与桥梁检测算法相结合的方法用于解决窄道采样难的问题，利用桥梁两端距离偏差不远的特性改进原始桥梁检测算法从而获取第二个桥端点;尽管该算法一定程度上提高了桥梁构建速度，但仍然没有解决桥梁检测算法样本空间范围大、采样成功率低等一些固有问题。

从上述文献中得到启发，本文在采用RRT-Connect算法进行一般路径规划的基础上，针对狭窄通道采样难的问题，提出了一种用于窄道路径规划的改进桥梁检测算法[12]。这种方法旨在提高桥梁检测算法的采样效率，并使窄道内样本点分布合理化，从而增加窄道路径点的连通性。该算法的核心是通过障碍物边缘检测，用障碍物边缘坐标集合取代原始桥梁检测算法的全局采样空间，从而使桥梁的两个端点都位于障碍物边缘;同时新型的桥梁构建策略减少了不必要的节点采样，提高了桥梁检测算法的检测效率，新的样本点再通过轻微变异的Connect算法快速扩展出更多的新节点。将基于改进桥梁检测算法的窄道路径规划与基于全局随机采样的RRT-Connect算法相结合，可使样本点在环境地图中的分布更加合理，从而提高路径发现的成功率。实验结果表明改进后的桥梁检测算法在狭窄通道采样效率有了明显的提升，保证了RRT-Connect算法的有效性。

1 桥梁检测算法

窄道即存在于环境空间中不同障碍物之间的狭窄通道，机器人沿着窄道移动稍有偏差就有可能与障碍物发生碰撞。窄道问题对于任何基于均匀随机采样的RRT路径规划算法都是一个难点。桥梁检测算法是一种专门针对狭窄通道的特殊抽样策略，通过偏向较短桥梁的连接测试，过滤掉大量开阔空间中的采样点，使得狭窄通道中的样本密度快速增加，提高了窄道的连通性。在桥梁检测算法中主要通过检测三个采样点的配置状态：桥梁短线段的两个端点及其中点。如果两个端点在障碍物中，且中点位于自由空间内，则中点被判定为处于狭窄通道内的有效样本点，由于短线段类似于横跨不同障碍物之间的“桥梁”，因此被称之为桥梁检测。

如图1所示，第一种情况是理想条件下桥梁检测算法成功实现窄道采样，第二种为障碍物内无效采样，第三种为自由空间内无效采样。尽管桥梁检测算法能够有效识别狭窄通道，但当环境空间中障碍物区域分布较为集中的情况下，基于全局均匀采样策略，其采样点可能远离窄道区域，导致样本点利用率低，采样效率下降。鉴于环境空间的连续性与采样策略的随机性，桥梁检测算法不可避免地浪费了大量的时间用于后两种无效的采样中。

从原始桥梁检测算法的伪代码中可以看出：算法通过repeat循环不断采样新节点并添加至窄道样本集G中。在每一次的采样中，首先调用Rand（）函数于环境空间内随机采样一点x，然后根据一个特殊的概率密度函数λq随机采样邻近节点x′，其中概率密度函数λq是个径向对称的高斯分布。接着以x、x′作为“桥梁”的两个端点，计算桥梁xx′的中点q的坐标节点。通过三次调用碰撞检测[13]函数CollisionFree（）确保桥梁端点x、x′为碰撞点，q为自由点，若三个节点状态全部满足，则可以确定q点即为所需的窄道内样本点，将其添加到窄道样本集G中，至此完成一次窄道采样。

2 改进的桥梁检测算法

桥梁检测算法的提出为窄道内采样提供了一种解决方案，通过检测桥梁上三个点的配置状态，从而获取窄道内样本点。但是，由于原始的桥梁检测算法所使用的采样策略依然是全局随机采样，导致该算法浪费了大量时间用于空旷空间内无效节点的采样，从而严重降低了算法的检测效率。

为提高桥梁检测算法的窄道采样效率，本文对原始桥梁检测算法进行改进：使用障碍物边缘坐标节点集合替代原始采样空间，改进桥梁端点构建过程与新节点扩展策略，提高算法窄道采样效率。

2.1 障碍物边缘检测

原始桥梁检测算法首先需要采样桥梁的其中一个端点，端点通过均匀的全局随机采样方法获得并调用碰撞检测函数進行配置状态判断。这种采样方法有一个明显的弊端，即采样的总体范围太大导致样本点利用率低。原因如下：全局随机采样的采样范围为整个环境空间，即使经碰撞检测函数过滤，其样本点也可能为障碍空间内任意一点，假使该点距离窄道较远，不适合作为桥梁的端点，则继续操作并无任何意义。因此，为了缩小待检测桥梁端点采样范围，提高样本点利用率，本文提出了一种障碍物边缘检测方法[14-15]，将障碍物边缘样本点提取出来，作为桥梁检测的样本集来替代原先的采样空间。相对于原始的桥梁检测算法，障碍物边缘样本集的获取有效地降低了采样空间，提高了采样质量，同时还使得桥梁的构建更加合理。

障碍物边缘检测需要对整个环境地图所有节点进行预处理，主要分检测当前节点配置状态和检测当前节点的周边节点配置状态两步。如图2所示，如果当前节点x处于障碍物空间内并且周边节点（序号1～8）至少有一个位于自由空间内，则可判定当前节点为障碍物边缘节点。

在障碍物边缘检测函数伪代码中，碰撞检测函数CollisionFree（）返回True表示节点处于自由空间内，返回False表示节点处于障碍物空间。Near（）用于获取该节点周边8个节点集合y，根据节点x与周边节点集合y的配置状态，从而判断节点x是否为障碍物边缘节点。整个函数通过循环检测整个环境地图所有节点的配置状态，提取出所有满足要求的节点并添加于障碍物边缘节点集合B′中。

2.2 桥梁端点的构建

原桥梁检测算法中桥梁的长度由一个服从高斯分布概率密度函数λq决定，过大或过小的λq都会使窄道采样性能下降，同时还需要对每个自由度都要设计一个不同的高斯方差，且高斯方差依据窄道的宽度得到，这就使得算法的计算难度很大[16]。为避免因λq带来的算法复杂度上升以及采样性能下降等问题，本文提出了一种改进桥梁端点构建策略。其结构示意图如图3所示

首先根据环境地图尺寸大小获得一个相对窄道宽度，并以此为半径r，对障碍物边缘节点集合进行随机采样从而获得桥梁其中一个端点x，以端点坐标x为圆心，绘制一个圆形区域[17]，则该圆形区域内所有障碍物边缘节点集合W即为桥梁第二个端点的采样范围。

圆形区域的半径r可表示为：

其中：Length表示整个环境地图的长度;Width表示整个环境地图的宽度;α、 β分别为Length、Width相对应的比例系数，其大小可根据需要人为设定，一般使半径r的大小略大于窄道最大宽度。

其次，对W中节点根据W中节点到桥梁端点x的欧氏距离由大到小降序排序。

由图3可知，圆形区域内所有障碍物边缘节点集合W可表示为：

其中： p表示节点坐标;ab表示线段ab; p∈ab则表示线段ab上的所有节点; p∈cd表示线段cd上的所有节点。圆形区域内所有障碍物边缘节点集合W由线段ab上的节点与线段cd上的节点两部分组成。经降序排序[18-19]后，由于圆形区域内圆周上的节点距离圆心x最远，因此a、b、c、d四个节点排序靠前，当选择c点或d点任意一点作为桥梁的另一个端点，由图3可知，桥梁中点q必然位于窄道内中心位置，属于自由空间，符合桥梁检测算法要求，将其添加于窄道样本集G中，至此，成功实现一次窄道样本点采样。

改进的桥梁检测算法伪代码如下所示：

对比改进后的算法与原始桥梁检测算法的伪代码，一方面改进后的算法在桥梁端点的采样范围上由整个环境空间缩小为障碍物边缘节点集合B′，从而减少了两次碰撞检测;另一方面，算法改进最大的地方还体现在伪代码的第2）～6）行，其中：Radius（C）函数用于根据环境空间C的尺寸获取半径大小r;NearCircle（x，r，B′）函数用于获取以x为圆心的圆形区域内所有障碍物边缘节点集合W;Sort（W）函数用于对W中节点按其到圆心x的距离进行降序排序;BridgeInspection（x，W）表示利用桥梁检测方法对x与W中节点依次经行检测，查找所需窄道样本点。

2.3 窄道样本点扩展

在通过改进的桥梁检测算法获取一定数量的窄道样本点之后，需要建立起样本点之间的联系，简而言之，就是通过确立每个样本点的父节点与子节点，试图构建一条连接窄道空间的随机树。本文根据窄道环境下特有的线性结构，同时考虑到窄道样本点位置的合理分布，提出了一种轻微变异的Connect算法，原始Connect算法是一种贪婪启发函数，在遇到障碍物或者到达目标区域之前，通过反复调用Extend（）扩展函数不断生成新节点进而扩展随机树。与原始Connect算法不同，本文算法只有当遇到障碍物的情况下才会停止新节点扩展。如图4所示，当采用改进的桥梁检测算法获得窄道内两个样本点q和q′后，需要在两节点之间扩展出更多的新节点，原始Connect算法由节点q向节点q′仅能够扩展出3个新节点，而应用本文所提出的改进Connect算法则可以快速扩展出更多的新节点，当然新节点不能与已存在的窄道样本点相同，节点扩展效率得到较大提升，同时，这种新的扩展方式能够使窄道内样本点突破窄道范围采样限制，扩展出许多处于窄道外的样本点并保持与窄道内样本的连通性。

以下是改进的Connect算法的伪代码，与原始的Connect算法相比，改进部分主要体现在第3）行对终止条件的判断，由原先的两个停止条件变为一个停止条件，即只有遇到障碍物的情况下才停止扩展新节点。

3 仿真与分析

3.1 地图构建与预处理

为验证本文所提出的改进桥梁检测算法的有效性，在CPU为IntelCorei5-4300U的联想thinkpadX240型笔记本上进行了实验仿真，同时还与原始RRT-Connect进行了对比实验，验证了该算法的优越性。

图5为本次仿真实验所使用的环境地图，地图大小为500×800。图5中：黑色区域为障碍物空间;白色区域为自由空间;起始点坐标设置为（10，10），位于地图左上角;目标点坐标设置为（490，790），位于地图右下角。机器人需由起始点经白色自由空间找到一条通向目标点的无障碍路径。为了体现改进桥梁检测算法对于狭窄通道采样的适用性，在环境地图的中心区域特别地设置了一条Z型狭窄通道，且该通道是连接起始点与目标点之间的必经之路。

图6为障碍物边缘检测处理后的环境地图，图中粗线部分即为障碍物边缘坐标所表示的集合，相对于对整个环境空间，处理后的障碍物边缘的样本集合要小很多，同时样本点采样更为有效。

3.2 仿真分析

本文所有实验均基于Matlab仿真平台，设定参数如下：步长为10，桥梁半径为25，桥梁样本点采样为500次，最大迭代次数为5000，实验次数50。实验结果如图7～8所示。图7为原始RRT-Connect在本实验环境地图下仿真结果，从中可看出：由原始RRT-Connect算法分别于起始点与目标点生成的两棵随机树在各自所处的空旷区域浪费了大量时间进行采样，尽管如此，仍然有很大概率无法找到窄道入口;同时，当其中一棵随机树进入窄道，Z型窄道结构又使得其容易陷入窄道内无法扩展。因此，当且仅当两棵随机树同时扩展进入窄道内，RRT-Connect算法才有可能探索成功。图8为结合改进桥梁检测算法后的RRT-Connect算法的仿真结果，从中可看出：在采样点的数量上，改进后的算法具有非常大的优势，样本点减少的同时也意味着算法消耗时间更短。尽管改进后算法在环境地图的预处理上花费了一点时间，但障碍物边缘节点集合的提取有效提高了窄道样本点的采样效率与利用率，又因为窄道样本点均处于窄道中心位置，配合轻微变异的Connect算法能够快速扩展新的窄道样本点，这些样本点又很容易经窄道入口扩展到外部自由区域，同时保持与窄道内样本点的连接关系，窄道的连通性得到增强，从而使得RRT-Connect的两棵随机树不需要刻意地寻找窄道入口，在扩展一定数量的节点之后，两棵随机树能够连接上这些外部窄道样本点，则路径探索成功。

为了验证本文所提出的改进后RRT-Connect算法的规划性能，在所设定的障碍物环境下，除原始RRT-Connect算法外，還加入了文献[11]算法方法进行对比，表1显示了经50次仿真后的实验结果的平均值。

从表1可以更为直观地看出：相对于传统RRT-Connect，改进后的RRT-Connect在寻路时间上减少了63.1%，在迭代次数上减少了77.8%，特别值得注意的是在路径探索的成功率上面，由原来的68%提升到92%，很好地说明了改进后的桥梁检测算法对于窄道路径规划的有效性。与此同时，与文献[11]方法进行比较，本文提出的改进后的RRT-Connect方法在寻路时间、迭代次数与路径探索成功率方面均有不同程度上的优势，这一点主要得益于本文改进算法将桥梁算法端点采样范围重新定义为障碍物边缘区域，缩小了采样范围，从而避免了文献[11]算法采样过程中可能出现的大量空旷区域内桥梁检测算法采样无效的情况，提高了采样效率。实验结果表明改进后的RRT-Connect算法所采取的障碍物边缘采样策略，克服了基本RRT方法随机性强、采样效率低下的缺点，配合轻微变异的Connect节点扩展方法，使得即使在环境地图中存在大量窄道的情况下，算法也能高效地对窄道进行识别采样，从而保证了路径规划的有效性。

[11] 莫栋成， 刘国栋. 改进的RRT-Connect双足机器人路径规划算法[J]. 计算机应用， 2013， 33（8）： 2289-2292. （MO D C， LIU G D. Improved RRT-Connect path planning algorithm for biped robot[J]. Journal of Computer Applications， 2013， 33（8）： 2289-2292.）

[12] SUN Z， HSU D， JIANG T， et al. Narrow passage sampling for probabilistic roadmap planning[J]. IEEE Transactions on Robotics， 2005， 21（6）： 1105-1115.

[13] BASTA R A， MEHROTRA R， VARANASI M R. Collision detection for planning collision-free motion of two robot arms[C]// Proceedings of the 1998 IEEE International Conference on Robotics and Automation. Piscataway： IEEE， 1988： 638-640.

[14] 段瑞玲， 李庆祥， 李玉和. 图像边缘检测方法研究综述[J]. 光学技术， 2005， 31（3）： 415-419. （DUAN R L， LI Q X， LI Y H. Summary of image edge detection[J]. Optical Technique， 2005， 31（3）： 415-419.）

[15] JOSHI S R， KOJU R. Study and comparison of edge detection algorithms[C]// Proceedings of the 3rd Asian Himalayas International Conference on Internet. Piscataway： IEEE， 2012： 1-5.

[16] NADARAJAH S， KOTZ S. Exact distribution of the max/min of two Gaussian random variables[J]. IEEE Transactions on Very Large Scale Integration Systems， 2008， 16（2）： 210-212.

[17] 付妍， 朱曉明， 周秉锋. 基于圆形参数域和重要性采样的三维模型网格重建[J]. 计算机学报， 2007， 30（12）： 2124-2131. （FU Y， ZHU X M， ZHOU B F. 3D model remeshing using circular parameter domain and importance sampling[J]. Chinese Journal of Computers， 2007， 30（12）： 2124-2131.）

[18] 霍红卫， 许进. 快速排序算法研究[J]. 微电子学与计算机， 2002（6）： 6-9. （HUO H W， XU J. A study on quicksort algorithm[J]. Microelectronics and Computer， 2002（6）： 6-9.）

[19] YANG Y， YU P， GAN Y. Experimental study on the five sort algorithms[C]// Proceedings of the 2nd International Conference on Mechanic Automation and Control Engineering. Piscataway： IEEE， 2011： 1314-1317.