改进Bi-RRT算法的AGV全局路径规划

宋永杰，孟祥印，2，翟守才，冯一凡

（1.西南交通大学机械工程学院，四川 成都 610031；2.轨道交通运维技术与装备四川省重点实验室，四川 成都 610031）

1 引言

随着智能制造业的崛起，自动导引运输车（AGV）以其控制便捷、定位精准、持续工作时间长的特点，在物料运输环节中发挥着愈发重要的作用［1］。传统AGV通过在地面铺设磁条、色带等方式来人为规划出固定的路径，施工复杂且难以升级扩展［2］，无法满足其在无人环境下自主灵活运行的要求。而路径规划算法［3］旨在空间中自动搜寻出连通起始点与目标点的无碰撞路径，是AGV运动智能化的基础，吸引了国内众多学者对其进行研究。

常见的路径规划算法包括人工势场法、A*算法、神经网络法、遗传算法、蚁群算法［4-8］等。这些算法虽然能在简单地图上规划出较优路径，但存在着计算复杂度高或易陷入局部最小值的缺点，而且未能很好地将车辆实际运行状态纳入考虑，规划出的路径不一定适用于现实环境下AGV的寻迹。

快速随机搜索树（Rapidly-Exploring Random Tree，RRT）算法［9］是一种基于随机采样的算法。该算法不需要对原始地图进行预处理建模，相较于其他算法搜索速度快，且具有概率完备性［10］，在众多移动机器人路径规划算法中优势突出。而传统RRT算法的缺点也很明显，采样过于均匀导致了算法收敛速度慢，所得路径偏离最优解且重复性差［11］。

针对传统RRT 算法的不足，文献［12］提出双向生成随机树（Bidirectional RRT，Bi-RRT）算法，将原有算法的单向搜索改为双向搜索，提高了路径收敛速度，但生成路径仍过于随机。文献［13］则提出RRT*算法，以降低运行效率为代价，获取趋近于概率最优解的路径。文献［14］中添加了动态步长特性，使得算法兼备路径确定性和避障能力。文献［15］提出混沌搜索策和局部引导新节点生成策略对局部最小问题进行优化。除了对算法本身进行改进，RRT 算法与其他算法的融合也得到了研究。如在文献［16］中，人工势场法的合力引导机制被应用到RRT算法的树节点扩展过程中，大大降低了路径生成随机性，却增加了陷入局部最小值的风险。文献［17］则在RRT算法基础上，加入了车辆转向约束和目标偏向约束对其改进，并利用三次B样条曲线对路径平滑处理。

为弥补RRT算法中随机采样过于均匀的缺陷，提出双重目标偏向策略进行改进，即在生成随机样点过程中采用基于概率p的目标偏向策略，同时引入A*算法中代价评估的思想，启发式地选择最近点。在此策略引导下，分别以起点和终点为根节点的两棵随机树均向着对方生长。还在两棵树的扩展过程中加入直接连通判断，推进两棵树相连的过程。在提高路径生成效率的同时，结合AGV实际运行状态，对路径的扩展引入安全距离判断，提高了生成路径的质量。最后，从终点开始遍历各点，在保证安全距离的前提下，裁剪路径上的冗余点，简化出一条实际可执行的全局路径。

2 背景知识

2.1 Bi-RRT算法基本原理

Bi-RRT算法继承了传统RRT算法的所有优点，适用于非完整约束下AGV 的全局路径规划［18］。二维地图由状态空间Z表示，包含互补的无障碍空间Zfree和障碍空间Zobs。起点Pstart与终点Pterm均属于无障碍空间Zfree。T1与T2分别代表从起点Pstart和终点Pterm开始构建的随机树。两棵随机树按给定步长增量式地向未知空间延伸，直至相连。

Bi-RRT算法随机树的扩展过程，如图1所示，步骤如下：

图1 Bi-RRT算法扩展过程Fig.1 Extension Process of Bi-RRT Algorithm

（1）初始化：在二进制地图上，给定处于无障碍空间Zfree的起点Pstart与终点Pterm，分别作为随机树T1与T2的根节点。并设置扩展步长ε。

（2）随机树生长：在状态空间Z中生成随机样点Psample，选取T1上到随机点Psample欧式距离最短的点为最近点P1near。然后以P1near为基点，朝着Psample方向按照给定步长ε扩展出新节点P1new。若P1near与P1new之间的连线进行通过障碍检测，即不经过障碍空间Zobs，则P1new成为随机树T1上的新节点。否则将舍弃P1new，重新生成Psample进行扩展。

（3）相连判断：检测新节点P1new与随机树T2上的各点的最短欧式距离是否小于步长ε，且连线是否通过障碍检测。若都成立，则两棵随机树连通，随机树停止生长。否则，按（2）中同样方式扩展随机树T2。循环直至两棵树相连，若循环次数超出设定上限N，则判定为路径搜索失败。

（4）路径生成：当随机树T1与T2相连，采取由新节点向最近点不断回溯的迭代方法，找出路径点，生成连通Pstart与Pterm的路径Path。

Bi-RRT算法伪代码，如算法1所示。

算法1 Bi-RRT算法伪代码

Bi-RRT Algorithm

1.Z=Zfree∪Zobs；｛Pstart，Pterm｝∈Zfree；

2.｛T1，T2｝←InitializeRRTree（Pstart，Pterm，ε）；

3.whilei＜Ndo

4.Psample←RandPoint（Z）

5.P1near←ChoosePnearest（T1，Psample）；

6.P1new←GeneratePnew（Psample，P1near）；

7.if PathFree（P1new，P1near，Z） then

8.T1←T1∪｛P1new｝；

9.if CheckDistance（P1new，T2，ε） then

10.return Path=GetPath（T1，T2）；break；

11.else

12.P2near←ChoosePnearest（T2，Psample）；

13.P2new←GeneratePnew（Psample，P2near）；

14.if PathFree（P2new，P2near，Z） then

15.T2←T2∪｛P2new｝；

16.if CheckDistance（P2new，T1，ε） then

17.return Path=GetPath（T1，T2）；break；

18.end；end；end；end

19.i=i+1；

20.end

2.2 A＊算法估价思想

A*算法是一种基于二维栅格地图的直接搜索算法，能够规划出最短路径，但计算繁琐且耗时长［19］。传统A*算法的核心思想是，通过一个估价函数，从起点开始计算栅格点到临近各个不同栅格点的代价值，选择代价值最小的点向外扩散，直至到达终点。因为路径上每个点的代价值最小，因此生成路径的总代价值是最小的，路径为最优。A*算法的代价估计函数f(n)可表示为式（1）。

式中：f(n)—地图上任意节点n的估价函数；g(n)—从起始点到节点n的实际代价；h(n)—节点n到终点的估计代价。在传统A*算法中，两点间的移动代价为其欧式距离。

3 Bi-RRT算法的改进

由Bi-RRT算法的基本原理可知，随机树的优劣主要由三个要素所决定，即随机样点的生成、最近点的定义和随机树的扩展。对此，首先提出双重目标偏向策略对算法进行改进，启发式地生成随机样点和最近点。同时，在随机树的扩展过程中加入安全距离判断以及直接连通判断来改善算法。最后，对生成路径后处理，去除冗余节点。

3.1 双重目标偏向

3.1.1 随机样点的选取

随机树上的节点均根据随机样点Psample扩展而来，因此Psample的选取在路径生成过程中起着关键性的作用。在传统Bi-RRT算法中，Psample在地图上完全随机产生，导致生成的路径往往偏离最优解，而且重复度低，实用性不强。

引入基于概率p的目标偏向策略，在选取点Psample时，按随机概率p与设定参数pth的比较结果，来决定Psample为完全随机点Prand或目标点Pterm。基于概率p的目标偏向算法伪代码，如算法2所示，对算法1中第4行作出改进。

算法2 目标偏向算法伪代码

Improved_RandPoint（pth，Pterm，Z） Algorithm

1.p←Rand（1）；

2.if 0＜p＜pththen

3.Psample←RandPoint（Z）；

4.else ifpth＜=p＜1 then

5.Psample←Pterm；

6.end

7.returnPsample

经过多次试验来观察路径的收敛情况，发现当pth过低时，生成路径与传统Bi-RRT算法相差不大，随机性高；而当pth太大时，算法易陷入局部最小值，难以在达到给定迭代上限次数前到达目标点。因此文中选定pth=0.5，使得路径在生成过程中既偏向目标，又不会陷入局部最小值。

3.1.2 启发式选取最近点Pnear

最近点Pnear作为新节点Pnew的父节点，对随机树的生长方向也有着巨大影响。在算法1中，选取随机树T上各点Pn(xn，yn)与样点Psample(xsa，ysa)之间欧式距离Dns最短的点作为最近点Pnear，导致随机树T的生长对随机样点Psample的依赖严重。欧式距离Dns的算法，如式（2）所示。

以基于概率p的目标偏向算法取得随机样点Psample后，引入A*算法中代价评估的思想，启发式地选取随机树上代价值最小的点作为最近点Pnear，有利于路径进一步朝着目标扩展。

仍采用式（1）中函数f(n)作为随机树T上各点的估价函数，而改进后的代价估计函数，如式（3）所示。

式中：Dns—随机树T上各点Pn(xn，yn)到Psample(xsa，ysa)的实际代价，即Dns=g(n)；Dnt—点Pn(xn，yn)到终点Pterm(xt，yt)的估计代价。

相较于传统Bi-RRT算法中遍历随机树选取最近点的方式，此算法在没有增加陷入局部最小值风险的同时提高了路径规划的目标偏向性。基于算法1第5行，改进后算法的伪代码，如算法3所示。

算法3 启发式选取最近点算法伪代码

Improved_ChoosePnearest（Psample，Pterm，T） Algorithm

1.Pn∈T，n∈［1，length（T）］；

2.g（n）←GetDistance（Pn，Psample）；

3.h（n）←GetDistance（Pn，Pterm）；

4.f（n）=g（n）+h（n）；

5.Pnear←min（f（n））；

6.returnPnear；

3.2 随机树的扩展

3.2.1 安全距离判断

在传统Bi-RRT算法中，扩展路径常出现贴合障碍物边缘的现象，使得实际应用中存在着AGV与障碍物碰撞的安全隐患，易造成财产损失。白色区域为无障碍空间Zfree，代表可行驶区域；黑色区域为障碍空间Zobs，代表障碍物所在范围，如图2所示。

图2 传统Bi-RRT算法路径图Fig.2 Path Fraph of Traditional Bi-RRT Algorithm

蓝色与绿色两段曲线分别代表从起点和终点出发的随机树T1、T2。红色线条表示两棵随机树连通后所得的路径Path。而从图中黄色椭圆区域可看出，路径Path与障碍空间Zobs有着贴合或相近部位，十分不利于现实情形下AGV的寻迹。

针对上述问题，在随机树的扩展中加入安全距离判断机制，使规划出的路径符合AGV行车安全距离要求。

线段Ln1、Ln2关于Pnear与Pnew之间的连线Ln对称，距离均为Width，如图3所示。同时对三条线段进行障碍碰撞检测，若都通过检测，则Pnew才算选取成功。若其中任意一条线段与障碍物相接触，则生成的Pnew失效。考虑AGV的车身宽度为WAGV，在AGV安全行驶过程中，还需保证侧边与障碍物之间的安全距离为Wsafe，则Ln1、Ln2与Ln的距离Width如式（4）所示。

图3 安全距离判断示意图Fig.3 Safety Distance Judgment Diagram

安全距离判断伪代码，如算法4所示：

算法4 安全距离判断算法伪代码

CheckWidth（P1，P2，Z） Algorithm

1.［P1a，P1b］=P1+（null（P2-P1））*Width；

2.［P2a，P2b］=P2+（null（P1-P2））*Width；

3.if PathFree（P1a，P2a，Z）&&PathFree（P1b，P2b，Z） then

4.return True；

5.else

6.return False；

7.end

3.2.2 直接连通判断

图2中，以人类视角可判断出随机树T1上的点P1与T2上的点P2能在无障碍空间Zfree内直接相连。但在图像上，P1与P2之间的存在着许多无意义的曲折线段。而且由于算法中Psample有一定几率在全地图随机产生，这种多余的线段实际上比两点间显示的更多，对计算资源和规划时间造成了不必要的浪费。为了减少这类浪费，加入直接连通判断机制对算法进行改进。若随机树T1或T2上的新节点Pnew可以与另一棵树上任意节点无障碍连通，且通过安全距离判断，便提前终止随机树的扩展。具体过程的伪代码，如算法5所示，在算法1的第9和16行之前改进：

算法5 直接连通判断算法伪代码

CheckConnection（P1new，T2，Z） Algorithm

1.Path=［］；

2.for n=1：size（T2）

3.if PathFree（P1new，Pn，Z）&&CheckWidth（P1new，Pn，Z）then

4.return Path=GetPath（P1new，T2）；break；

5.end；

6.end

3.3 路径后处理

图2 中红色线段代表传统Bi-RRT 算法所生成的路径，由多段短小的线段拼接而成，细碎曲折，十分不利于AGV 的平稳运行。采用遍历的方法对生成的原始路径进行后处理，使最终路径简洁干净，可降低AGV 在寻迹过程中的能量消耗和物理磨损。

从终点开始，以倒序的方式遍历路径上各个节点，寻找可与起点直接连通的点Pj，用线段连接起点与该点。然后，重新从终点开始遍历可以与点Pj直接连通的节点，直至连通终点。最终路径将只保留起点、终点和必要转折点，并且各相邻点之间的连线通过安全距离判断，大大降低了路径的曲折程度。生成最终简洁路径的伪代码，如算法6所示。

算法6 路径后处理算法伪代码

Post-processing（Path，Z） Algorithm

1.Pstart=Path（1）；Pterm=Path（end）；

2.TidyPath=［Pstart］；

3.forj=size（Path）：-1：1

4.if CheckWidth（Pstart，Pj，Z） then

5.TidyPath=［TidyPath；Pj］；

6.Pstart=Pj；

7.ifPstart!=Ptermthen

8.j=size（Path）；end

9.end；end

10.return TidyPath；

4 仿真实验

为了验证改进Bi-RRT 算法的有效性和优越性，在以2.6 GHz CPU为处理器、内存8G的PC机上，基于Matlab 2016b编程，分别对RRT算法、Bi-RRT算法以及改进Bi-RRT算法进行仿真实验，通过对比来验证改进算法的优势。

根据AGV现实工作环境，绘制两张复杂度不同的二维地图，并在算法中直接将地图导入为二进制图像，手动设置起点和终点坐标。地图中黑色区域表示障碍物所在位置，白色区域表示AGV可到达空间。蓝色星状点表示起点，绿色三角点表示终点。根据AGV尺寸与环境对比数据，设定步长和安全距离值。

传统RRT算法、Bi-RRT算法、改进Bi-RRT的直接连通算法以及路径后处理算法在简单和复杂地图中的仿真结果，如图4、图5所示。从图4（a）和5（a）中可以看出，传统RRT算法由于缺乏目标导向性，需要在全地图中进行随机采集样点，即使在障碍物较少的简单地图中，随机树也会在目标点附近徘徊，生成路径曲折且效率极低。传统Bi-RRT 算法在RRT 算法的基础上，添加了从终点生长的随机树，增强了路径生成的效率，但仍产生了大量的随机节点，且同样存在路径与障碍物贴近的现象。如在图4（b）与图5（b）中，传统Bi-RRT 算法中两棵随机树相连的周围空白区域存在许多无意义的树节点，最终生成的红色路径与多处障碍物贴合。

图4 简单地图仿真结果Fig.4 Simulation Results in a Simple Map

图5 复杂地图仿真结果Fig.5 Simulation Results in a Complex Map

如图4（c）与图5（c）所示，改进Bi-RRT算法加入直接连通判断机制后，大大减少了随机树节点扩展过程中路径连通前的计算资源与时间损耗。尤其在简单地图中，从起点出发的随机树在双重目标偏向策略的引导下，绕过障碍物，便能直接连通到终点，节省了计算时间。在复杂地图中也明显简化了两棵随机树的相遇过程。而且改进算法有效规避了路径与障碍物贴合的现象发生。图4（d）与图5（d）则展示了改进Bi-RRT算法对所得路径进行后处理的效果。效果显示，处理后路径更平直，转弯节点更少，满足安全距离判断。

不同算法在两种复杂程度地图中仿真50次后的实验数据，如表1、表2所示。根据表1数据，改进Bi-RRT算法在简单地图中的平均路径生成时间低于传统RRT算法的十分之一，约为传统Bi-RRT算法的三分之一。表2则显示出，对于复杂地图，改进Bi-RRT算法的平均路径生成时间约是传统RRT的三分之一，传统Bi-RRT算法的一半。改进Bi-RRT算法生成的路径节点均为RRT算法与Bi-RRT 算法的一半。而耗时极短的路径后处理取得的效果十分显著，大大删减了路径节点，获得平直的最终路径，更利于实际应用中AGV的运行。

表1 简单地图中仿真数据对比Tab.1 Comparison of Simulation Data in a Simple Map

5 结论

基于传统Bi-RRT算法提出的AGV全局路径规划改进算法，将双重目标偏向策略应用于随机样点和最近点的选取，在随机树的扩展过程中加入直接连通判断与安全距离判断机制，最后对生成路径进行后处理，给出了算法伪代码来描述实现过程，并对算法性能进行仿真验证。实验表明，改进Bi-RRT算法相比于传统Bi-RRT算法，能在二维地图上更高效快速地规划出一条概率更优且有利于AGV 实际执行的简洁路径，构建了智能生产线中AGV 自主运行的基础。后续将对该算法进行深入改进，实现AGV在行驶过程中的实时避障功能。