基于多安全度模型的改进RRT路径规划方法*

朱旻华 任明武

（南京理工大学计算机科学与工程学院 南京 210094）

1 引言

近年来，随着智能化信息处理技术的进步，自动驾驶技术得到了显著的发展，该技术能最大限度地减轻驾驶员负担、减少交通事故从而提升道路交通的安全性。路径规划的主要任务是根据全局地图数据信息规划出一条从起点至终点、无碰撞的路径，是无人驾驶系统中的关键技术，具有重要的研究价值［1］。

传统的路径规划算法可分为以下几类：基于图形学的算法提供了一些环境建模的方法，如可视图法、栅格图法、Voronoi图法、切线图法［2～5］等；Dijkstra、A*、D*、人工势场法［6～9］等前向图搜索算法在建模基础上提供了路径搜索方法；通过对仿生学的研究提出的算法主要有蚁群算法、遗传算法、模拟退火算法［10～12］等。前向图搜索算法和智能仿生算法虽然都能收敛到全局最优或较优路径，但随着机器人自由度和规划空间维度的增加，会导致算法复杂度激增，造成维度灾难。随机采样类的规划算法很好的解决了这个问题，主要有概率路标算法［13］、快速扩展随机树［14］等方法。

快速扩展随机树（Rapidly-exploring Random Tree，RRT）由 Steven M.LaValle于 1998年首次提出，是一种单查询随机搜索方法。树中的子节点根据当前环境信息随机扩展生成，避免了对环境空间的建模，可以有效地扩大问题规模。由于RRT算法在扩展时的随机性，不可避免地导致了规划路径的随机性和不稳定性，无法高效稳定地完成规划任务。针对基本RRT的这些缺陷，文献［15］提出了目标偏向的Goal-biasing算法，在扩展子节点时以一定的概率以目标为导向向着终点方向扩展；文献［16］提出了双向搜索树Bi-direction RRT算法，同时从起点和终点进行双向搜索，加快了收敛速度；文献［17］提出了渐进最优的RRT*算法，采用代价函数来选取扩展节点邻域中代价最小的节点；文献［18］对于三种不同运动模型进行分析，提出了非完整约束下的RRT方法。

由于RRT算法中扩展子节点时的随机性强，算法在扩展节点时缺少引导、扩展效率低。路径的随机性使得规划路径不稳定，且缺乏对自主车安全度的考虑。

2 基本RRT算法

RRT算法将状态空间中的初始点作为根节点，通过随机采样子节点的方式增量构造随机扩展树，当随机树中的子节点包含目标区域的点或目标点时停止扩展，就可以在随机树中找到一条从根节点到目标点的路径。其扩展方式如图1所示。

图1 基本RRT扩展示意图

以初始状态点qinit为根节点，在状态空间内随机生成一个采样点qrand，qnear为离该随机点最近的子节点，在qnear和qrand的连线上以一定的步长step扩展一个新的子节点qnew，若qnew与障碍物未发生碰撞，则将qnew加入随机扩展树中，反之则舍弃该点重新选择qnew。重复上述步骤直至qnew到达qgoal或附近的目标区域，则成功找到一条从qinit到qgoal的规划路径。RRT算法伪代码如下。

其中，函数Random_State（）根据均匀分布的概率获取状态空间中的随机采样点qrand；Nearest_Neighbor（qrand，Tree）寻找随机树中离随机采样点 qrand最近的 qnear点；New_State（qrand，qnear）对新节点进行碰撞检测，若无碰撞则将新节点加入随机树，直到新节点与目标点足够接近，表示搜索成功，返回随机树。

3 基于势场引导的RRT算法

3.1 人工势场法

Khatib最早于1986年提出了一种虚拟力法，即人工势场法［19］。该方法的主要思想是将机器人所处的周围环境模拟成一个势场，将机器人在空间环境中的运动视作机器人在该虚拟力场中的运动：目标点的引力场（attraction）对机器人有吸引性，引导物体向目标点方向运动；障碍物的斥力场（repulsion）对机器人有排斥性，避免物体与之发生碰撞。自主车在虚拟力场中的受力为目标点的引力和障碍物的斥力作用在自主车上的合力，合力牵引着自主车在状态空间中沿着势场下降的方向做无碰运动。

假设自主车在状态空间中的位置为q，该点的引力势能和斥力势能分别表示为和，则自主车在该点的势能为

其中，引力势能函数和斥力势能函数为

在实际规划问题中，每个障碍物可能由于其危险程度的高低［20］、辐射范围大小的区别，对自主车规划路线也会造成不同的影响。如若增加对自主车安全度的考虑，路径规划问题的维度增大，可能会导致算法效率的大幅下降。

3.2 基于人工势场引导的概率分布模型

对于RRT算法和人工势场法的研究发现，RRT算法节点扩展时的随机性导致的盲目性使得规划出的路径节点状态突变较大，路径点可能距离障碍物很近，不易得到安全可靠的最优解且算法不稳定，但正是由于其随机性强，规划路径不会出现陷入局部极小值无法逃脱的问题；人工势场法相对稳定，规划出的路径相对平滑且安全，但由于局部极小值的存在可能使得目标不可达，导致规划失败。两种路径规划算法在一定程度上可以优势互补。所以，本文在基本RRT算法的基础上引入了人工势场引导的概率分布模型，提出了一种安全高效的改进算法。

原RRT算法的随机性主要由Random_State（）函数体现，而Random_State（）函数采用的是整个状态空间内的均匀分布，即状态空间内的点被选中作为下一个扩展点的概率是一样的。根据人工势场的思想，自主车离障碍物越近，受到的斥力越大，自主车向着障碍物方向运动的概率越小，离障碍物越近的点被扩展为下一个随机点的概率越小；同理，由于目标点的引力作用，自主车有向着目标点运动的趋势，距离目标点越近的点被扩展的概率越大。即将Random_State（）函数中均匀分布的概率模型改为人工势场引导的概率分布模型，使随机树在远离障碍物的安全区域有较大概率生成下一个随机点，而在靠近障碍物的危险区域有较小或没有概率生成下一个随机点，从而改善原RRT方法缺乏安全度的不足。

该改进方法适用于较大地图任务空间中的全局规划。将任务空间离散化为栅格地图，若每个栅格表示20m*20m的空间，对于空间中的每个栅格，其8方向邻域的栅格在无障碍的前提下对于自主车都是可达的。将RRT算法中的子节点扩展步长限制在栅格的8邻域中，通过计算8邻域栅格在人工势场下的引力权重和斥力权重，原RRT中随机扩展点的生成则转化为当前栅格点8邻域间的轮盘赌问题。

在实际情况中，每个障碍物都有不同的危险程度，例如战场环境里的爆炸物比道边危险度高很多，与障碍物距离越远的栅格点的危险程度越低，安全度越高。假设一个障碍物点的危险度为3，则该障碍物辐射范围内的栅格点的危险度如图2所示。对于在多个障碍物辐射范围内的栅格点，取最高的危险度作为该点的危险度。

图2 危险度为3的障碍物周围栅格的危险度示意图

同理，由于目标点对自主车引力的影响，每个地图中栅格都存在一个“趋向度”，距离目标点越近的节点趋向度越高。对于自主车所在栅格位置的8邻域中，根据与目标点间的距离由小到大排序，距离最小的邻域栅格的趋向度为8，距离最大的栅格趋向度为1。

若每个邻域栅格的初始权重为weight0，则栅格权重的计算公式为

其中，katt和krep分别为引力系数和斥力系数。则每个邻域栅格被扩展为随机树下一个子节点的概率为

该改进算法的流程图如图3所示。

图3 本文改进算法的流程图

3.3 路径优化的后处理方法

由于每次扩展的新节点是从起点栅格依次根据启发信息扩展邻域栅格得到的，所以路径搜索过程中可能会多次到达同一个栅格点，形成自相交的路径。规划路径是一个包含了若干有序路径点的集合，若从起点出发后，多次到达了同一个坐标点，则认为该规划路径中存在着冗余环，首次到达该点和最后一次到达该点间经过的栅格点都是多余的，需要从路径中删除。

去除了路径中的冗余之后，路径中不再有重复环路。但路径中的折点较多，且很多折点是不必要的。在该优化路径基础上继续对路径进行剪枝优化，去除路径中不必要的折点，使路径更光滑。

通过两次对路径的优化，可以得到一个较优路径。

4 仿真实验结果分析

基础RRT算法和本文的改进RRT方法的仿真实验结果图分别如图4（a）、（b）所示。地图大小为1024*768，将该地图映射到栅格地图中，栅格地图的大小为60*50。起点用实心圆的点表示，终点用空心圆的点表示，灰色的线体现了路径的搜索过程，黑色的线条即为算法最终搜索到的一条路径。在基于势场引导的改进RRT结果图中，虚线是去除重复环之后的优化路径，黑色的线是剪枝优化后的最终路径。

图4 RRT算法和本文改进算法的实验结果图

在该地图上对两种算法分别进行了100次仿真实验，实验结果如表1所示。

表1 RRT和本文改进算法的仿真结果

通过表1中数据可以看出，改进方法的效率更高，且通过对规划路径的优化，使得规划路径较短。由于改进方法中有人工势场的启发信息，降低了RRT算法中随机性带来的盲目性，算法运行结果相对稳定。

改进算法中的多安全度模型主要体现在障碍物危险度上，障碍物的危险度越高，算法在附近尝试扩展节点的概率越低。图4中的（a）为障碍物危险度为2的局部地图，（b）为障碍物危险度为7的局部地图，可以看出算法在图（b）中障碍物周围尝试扩展的次数远小于图（a）。

图5 在不同危险度的障碍物周围的路径搜索结果

5 结语

RRT算法不需要对状态空间进行预处理而能快速搜索空间状态点，有效避免了Dijkstra、A*等算法中复杂度高的问题。但RRT扩展节点时的盲目性使得算法效率较低，且在搜索路径时没有考虑到设备的安全性，规划路径可能处于障碍物周围的危险区域。本文引入人工势场，提出了一种基于多安全度模型的改进RRT算法，该算法能够模拟人工势场法中引力斥力对自主车的影响，使路径尽可能远离危险障碍，保证了规划路径的安全性，且启发信息的加入使得算法能够更快地收敛到一个解决方案。在此基础上对路径进行优化，优化后的路径更接近于最优解，更符合实际情况。