基于引导扩展的快速随机搜索树算法

2020-03-08杨馨韵严华

现代计算机 2020年36期

杨馨韵，严华

（四川大学电子信息学院，成都610065）

针对移动机器人在路径规划过程中，快速扩展随机树（RRT）算法随机性强、搜索无偏向性以及在较窄出口环境下搜索效率明显下降等问题，提出一种基于障碍物有效顶点引导扩展的改进算法。该算法通过对碰撞障碍物分析，选取障碍物有效顶点引导随机树扩展，从而提高随机树的搜索效率。最后，在机器人操作系统（ROS）上进行仿真实验，使用ROS可视化工具（RVIZ）显示规划结果，结果显示基于障碍物有效顶点引导扩展的算法性能更优。

移动机器人；路径规划；快速扩展随机树算法；目标偏向；有效障碍物顶点

0 引言

近年来，路径规划广泛应用在产品组装、无人水面艇路径规划、无人机航迹规划等领域，引起了研究者的高度关注和重视[1-3]。路径规划问题描述为根据最短距离、最短规划时间等性能指标搜索出一条从起始点到目标点的最优路径或次优路径[4]。路径规划的方法有很多，大致可以分为两类：传统的路径规划算法，包括可视图法、栅格法、人工势场法等；智能路径规划算法，如遗传算法、神经网络算法等[5]。传统的路径规划算法结构简单，但在障碍物较多的复杂环境中，算法实现困难；遗传算法实现简单，但容易陷入局部最小值，效率不高；神经网络算法学习能力优秀，但是泛化能力差。

快速扩展随机树[6]（Rapidly-exploring Random Tree，RRT）算法是由LaValle等人提出的一种基于采样的路径规划算法。它提出了一种启发式、随机化的动态规划方法，能快速地探索状态空间，并能很好地解决具有高自由度和复杂系统动力学的问题。因不需要对空间建模，能够有效地解决高维空间中机器人路径规划问题，在路径规划领域得到了广泛应用。但RRT算法也存在以下缺陷：①随机树在自由空间中随机采样，搜索无导向性；②收敛速度迟缓、搜索效率低等。

针对上述不足，考虑收敛速度、搜索效率的改进RRT算法被大量提出。其中被广泛提及的要数LaValle等人提出的双向多步RRT（RRT-Connect）[7]，它分别在起始点和目标点构建随机树，使用贪婪的启发式方法来连接两棵随机树，以改善规划时间。时至今日，仍有很多研究者在不断研究提升随机树搜索效率的改进算法，同时将改进算法应用到更多场景[8-11]。

RRT的改进算法很多，但算法本身具有的随机性依然存在，这使得RRT算法在出口较窄的复杂环境中算法效率明显下降。为此，本文基于RRT-Connect算法，提出一种使用障碍物有效顶点引导随机树扩展的改进思想，以提升算法效率。最后，在具有较窄出口的环境中与RRT、RRT-Connect和DRRT-Connect[12]进行对比实验，证明改进算法的有效性。

1 相关工作

1.1 问题定义

为方便叙述，本文借鉴Karama和Frazzoli提出的路径规划问题公式[13]。

定义1（自由/障碍物空间）给定一个环境空间X⊂Rd，用Xfree⊂X表示自由空间，Xobs⊂X表示障碍物空间。

定义2（无碰撞路径）设（xinit,xgoal,Xfree）为路径规划问题，其中：xinit∈Xfree为起始点，xgoal∈Xfree为目标点，Xgoal⊂Xfree为目标点区域。当且仅当σ(0)=xinit，σ()1⊂Xgoal时，自由空间的连续映射σ:[0,1]→Xfree是无碰撞路径。

1.2 基本RRT算法

快速随机搜索树算法的关键思想是利用状态空间中的采样点来引导随机树的扩展，从而使探索偏向于空间中未探索的区域。在每次迭代中，随机树都向随机方向扩展，只要有足够的时间和足够的迭代次数，就没有不被探索的区域，所以该算法总是可以得到一条路径。

RRT算法的扩展过程如图1所示，首先在起始点xinit构建随机树，在每次迭代中，使用函数Random在状态空间中随机采样一个节点xrand，并调用Extend函数来增加叶子节点，当新增节点xnew到达xgoal或者进入xgoal区域，回溯该节点以获得一条由初始点到目标点的路径。Extend函数用于扩展节点，首先在随机树中找到距离xrand最近的节点xnear，然后从xnear向xrand扩展一步得到一个新节点xnew。此时会出现三种情况：如果xnew和xnear的连线与障碍物碰撞，放弃此次扩展返回Trapped；如果到达xrand，将xrand加入随机树返回Reached；否则将xnew添加到随机树返回Advanced。

图1 RRT扩展示意图

1.3 RRT-Connect算法

基于RRT算法搜索空间的盲目性和节点扩展缺乏记忆性，为提高空间搜索效率，Kuffner和LaValle提出RRT-Connect算法。

RRT-Connect算法的扩展过程如图2所示。不同于基本RRT算法，RRT-Connect算法分别在起始点xinit和目标点xgoal构建两棵并行的随机树；在每次迭代过程中，一棵树以随机采样方式扩展一个步长的距离得到xnew，另一棵树使用Connect函数尝试连接xnew。同时，在每次迭代结束后选择节点数较少的树扩展。Connect是一个贪婪的函数，通过在内部迭代调用Extend函数扩展节点，直到碰到障碍物或者到达xnew停止扩展。

图2 RRT-Connect扩展示意图

虽然RRT-Connect采用connect策略可以降低算法的随机性，但是由于在每次迭代中使用了随机方式扩展节点，依然存在效率低下的情况。为此，王坤等人提出DRRT-connect算法，该算法在RRT-Connect的基础上增加第三节点，生成四棵随机树，极大地提高了算法的收敛速度；同时引入目标偏向策略，使随机树能更好地向目标扩展，提高了无障碍条件下的收敛速度。然而，DRRT-connect仍然存在缺点。例如，在一些复杂的环境下，选择第三节点比较困难。还有，当目标连线上存在障碍物且环境出口较小时，随机树往往被困在出口周围。为了找到出口，需要进行大量的迭代，因此会消耗大量的时间。此时，使用有效的方法引导随机树扩展是改进算法的关键。

2 改进算法

根据上节讨论，将随机树扩展分为以下两种情况分析。首先，考虑简单无障碍环境。如图3（a）所示，随机树已扩展到xnow，采用目标偏向策略可以快速扩展随机树。其次，当从xnow到目标节点xgoal连线上存在障碍物时，若采用目标偏向策略引导随机树扩展，会使扩展停在障碍物附近；此时，若沿着障碍物边界扩展，绕开障碍物找到路径是可行的。已知障碍物边界是由一个个障碍物顶点连接而成，若直接在障碍物边界中找到合适、有效的顶点作为临时目标点，并引导随机树扩展，则寻路时间和路径长度会大大缩短（如图3（b）所示）。

因此，本文改进算法策略如下：在目标偏向策略的基础上引入障碍物顶点引导策略。在每次迭代中，判断当前节点到目标点之间是否存在障碍物。在无障碍物情况下，采用目标偏向策略引导随机树快速扩展。反之，找到引发碰撞的障碍物，即最接近当前节点的障碍物，并在障碍物顶点中找到合适的顶点作为临时目标点引导扩展。

图3 障碍物顶点引导扩展

2.1 障碍物有效顶点

为方便描述，首先提出以下概念。

定义3（凸多边形）如果一个多边形的任意一条边向两方无限延长成为一直线时，其他各边都在此直线的同旁，那么这个多边形就叫做凸多边形。

图4 凹凸点（实心圆为凸点，空心圆为凹点）

考虑以下情况，将随机树扩展到xnow，目标点为xgoal（如图4所示），分析三种引导方式：（1）使用目标偏向策略，路径会与障碍物发生碰撞；（2）利用多边形障碍物的凹点A或B引导扩展，随机树将进入障碍物凹处，这与我们逃离障碍物的初衷相违背；（3）利用多边形障碍物的凸点C引导扩展，当随机树到达顶点C的位置时，可以直接与xgoal相连。其次，由凹凸点的定义可以得出，相对于凹点，凸点的内角小于等于180度，可以在更大的范围内扩展。总之，凸点的性质可以使其作为一个合适的临时目标点，引导随机树逃离障碍物而不会进入障碍物死区。

根据定义3、4可知，凸多边形的每一个顶点就是一个凸点，计算多边形的凸点，就是求出多边形的最大凸多边形。计算方法如下：

Step1按顺（逆）时针顺序给定多边形顶点坐标P1( x1,y1),P2( x2,y2),…,Pn(xn,yn)，记录顶点个数为n。

Step2利用定义2判断多边形的每个顶点，记录凸点。

Step3若记录的凸点个数等于多边形顶点个数n，结束算法；否则将所有凸点顺序排列，更新n值，跳转Step2。

2.2 临时目标点选取策略

考虑到机器人实际尺寸，为避免碰撞，需要对障碍物有效顶点进行膨胀处理。首先将障碍物中所有凸点Pl(xl,yl)按照顺时针顺序连接得到一个凸多边形；在凸多边形的外部做平行于的两条平行线，距离为σ，两条线的交点Ql即为膨胀后的Pl，如图5所示。计算公式如下：

图5 膨胀障碍物有效顶点

临时目标点选择步骤如下：首先，找到当前节点和目标节点连线中距离当前节点最近的障碍物；其次，将障碍物有效顶点Pl经过膨胀处理后得到的Ql作为可选对象；最后，按照以下规则选取临时目标点。

（1）该点在自由空间中。

（2）不在已扩展的随机树上。

（3）与当前节点连线无碰撞。

（4）满足以上3点条件且起始点到该点、该点到目标点距离之和最短。

2.3 算法实现

改进算法具体实现步骤如下：

Step1设置两棵树分别从xinit和xgoal出发；

Step2引入目标偏向机制，每次迭代中，以非扩展树最新节点xnew为目标点扩展；

Step3调用NearestNeighbor函数，找到扩展树中距离目标点最近的节点xnear作为当前节点，如果当前节点到目标点连线中存在障碍物执行Step4，反之跳转

Step5；

Step4调用FindTempNode函数，首先找到当前节点和目标点连线中距离当前结点最近的障碍物；用临时目标点选择机制选取合适的临时目标点，执行Step5，若没有找到合适的临时目标点，跳转Step6；

Step5从当前节点以步长step扩展节点，直到到达（临时）目标点，跳转Step7；

Step6交换随机树，如果此时仍然没有找到合适的目标点引导随机树扩展，选取节点较少的随机树，使用随机方式扩展该随机树；

Step7调用Connect函数，判断两棵树是否连接。如果连接，返回路径；反之，执行Step2。

3 实验与分析

根据上述改进设计算法，本文在搭载了机器人操作系统（Robots On System，ROS）的Ubuntu 14.04上进行算法仿真测试，并在ROS可视化工具（3D visualization tool for ROS，RVIZ）上展示完整的路径规划结果。

为证明改进算法的性能，使用基本RRT、RRTConnect和DRRT-Connect算法进行比较。实验在两种出口较窄的环境中进行，地图大小为200×200，起始点坐标设置为（3,3），位于地图左上角；目标点坐标设置为（197,197），位于地图右下角。为方便对比，设置机器人半径为3，步长为3。考虑到机器人半径，设置障碍物有效顶点向外膨胀长度为5。

图6和图7分别是地图1、2的实验结果，从左到右从上到下依次是RRT、RRT-Connect、DRRT-Connect和改进算法，组实线是规划后的路径，细实线是随机树的分支。

图6 地图1情况下路径图

图7 地图2情况下路径图

从结果可以看出，基本RRT算法的规划结果分支较多，探索区域覆盖了整个空闲空间。由于RRT-Connect采用两棵树交替扩展，并且引入具有贪婪特性的连接函数，与RRT算法的规划结果相比，减少了分支，但是仍然存在探索无效区域的情况。DRRT-Connect在RRT-Connect的基础上提出第三节点的概念，增加了随机树的数量，并且引入目标偏向策略使随机树扩展更具方向性，规划结果表明，随机树的分支更少。另外，三种算法在较窄出口处都存在盲目采样的情况。相反，改进算法利用狭窄出口处的障碍物顶点引导随机树扩展，降低了探索的盲目性，使随机树能够更快地找到路径出口，进而规划结果具有更少的分支和更少的无效探索。

为避免实验偶然性，在两个地图上分别对每种算法进行了50次仿真实验，实验结果如表1、2所示。从数据中可以更为直观的看出，相对于RRT、RRT-Connect和DRRT-Connect，改进算法在迭代次数和平均规划时间上都有明显的优势。此外，改进算法还缩短了路径长度法，主要是因为随机树向目标点扩展过程中，在发现障碍物时就以障碍物有效顶点引导扩展，而不是接近障碍物后才选择绕过它。