基于改进LGM-BRRT＊的移动机器人路径规划算法

谢志长，严华

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

0 引言

随着移动机器人在核设施、宇宙探索、救援任务等领域应用的快速增长，移动机器人在给定的工作环境中如何选择行走路径、避免碰撞障碍物、快速到达指定目标的路径规划问题成为移动机器人研究热点之一。传统的路径规划算法有：人工势场法、BUG 算法、A*算法、可视图法等。人工势场法是一种成熟的局部避障算法，但是容易陷入死锁现象[1]，存在局部最小问题。为了解决这一问题，Kavraki 在文献[2-3]中提出PRM算法，但是该算法不适合在未知环境下做路径规划[2]。BUG 算法运行时间长，容易陷入局部最优等问题[4]。A*算法在复杂情况下计算量急剧增长，效率低下[5]。RRT（Rapidly-exploring Random Tree）算法是一种单查询方法[6]，能够解决大多数运动规划问题，并且它的改进具有更好的性能。例如：B-RRT[7-8]、RRT-connect[9]、目标偏向RRT[10]。然而，这些算法的局限性在于它们没有考虑路径成本，因此无法保证最佳解决方案。

为了解决这个问题，Karaman 等人引入了RRT 的最佳变体RRT*[11]。RRT*首先找到一条初始路径，然后通过重选父节点，重布线优化路径，这使得RRT*渐进最优，这意味着当样本数无限大时，可以保证收敛到最优解。但是由于RRT*执行的是纯随机探索，它的收敛速度非常地慢。

为了克服RRT*算法收敛速度慢的问题，RRT*及其变体在最近几年得到了广泛的研究。文献[12]提出了B-RRT*算法，通过两棵树交替扩展，提高了算法的收敛速度；文献[13]提出的RP-RRT*算法，通过融入人工势场法优化采样过程，在势力场的作用下执行目标偏差采样，能够在较短时间内生成较优路径；文献[14]提出包围盒顶点引导的BNM 算法，通过障碍物包围盒顶点引导路径扩展，该方法能够产生接近最优的无碰撞路径；文献[15]提出本地引导多个B-RRT*（LGMBRRT*）算法，该算法通过引入桥接测试和基于本地的新颖搜索策略解决了RRT*收敛速度低下的问题。

LGM-BRRT*算法虽然解决了RRT*算法效率低下的问题，但是在选择本地桥接点时可能采用对目标方向无用的桥接点，缺乏导向型，耗费时间。因此，本文引入本地桥接点目标导向策略避免获取无用的桥接点，并在扩展新节点时引入目标偏向策略，进一步提升收敛速度。在此基础上，采用改进的去冗余处理策略和三次B 样条采样拟合路径，以获取更优的路径。最后，在不同环境中与RRT*、B-RRT*和LGM-RRT*进行了对比。仿真实验结果表明改进算法具有更好的收敛性、稳定性与有效性。

1 背景知识

定义1：根据文献[15]给出的路径规划问题定义：X∈Rd表 示 地 图 范 围，Xobs∈X代 表 障 碍 物 区 间，Xfree≔XXobs表示无障碍物区间。Xinit表示起始点，Xgoal表示目标点。路径规划问题就是在Xfree区间找到一条从起始点Xinit到目标点Xgoal的可通行路径。

1.1 RRT＊算法介绍

RRT 算法首先初始化树T，并将起始节点Xinit添加到该树，在无障碍物空间中生成一个随机节点Xrand，并在树T 中找到最近的节点Xnearest。然后，沿Xnearest到Xrand的路线扩展一步得到新节点Xnew，如果T 中最近的节点Xnearest与新节点Xnew之间的路径无障碍物，则将新节点Xnew插入到树T 中，重复此过程n 次，直到找到完整的可行路径为止，如果在n 次迭代后仍未生成可行路径，则意味着规划失败。算法原理如图1 所示。由于RRT 算法以随机方式快速生成一条可行的路径，无法获得渐进最优的路径，RRT*通过在添加新节点时重选父节点和重布线减少路径成本，获得尽可能最优的路径。在添加新节点时，相较于RRT 选择最近的邻居作为父节点，RRT*选择邻域内代价最小的节点作为父节点。在这基础上，如果用新节点替换父节点能够获得更小代价，并且它们之间的路径没有冲突，则重新布线。这些改进帮助RRT*找到接近最优的路径，但代价是执行时间更长。

图1 RRT算法原理

1.2 LGM-BRRT＊算法介绍

通过前述分析，RRT*最大的问题是时间消耗大，所以加快收敛速度是RRT*的主要改进方向。Tahir 等人[12]提出B-RRT*算法提高RRT*的收敛速度。BRRT*使用两棵树Ta和Tb交替扩展，所以在扩展过程中，需要检查两棵树是否已经连接，若已连接，则记录当前路径，若无连接，则进行下一次迭代。如果迭代没有结束，则将不断优化现有路径。然而B-RRT*在面对狭窄通道等复杂地图环境时仍然采用简单的随机扩展，需要更多的迭代次数才能从狭窄通道中搜索出可行路径。

Shu 等人[14]提出的LGM-RRT*算法，引入本地桥接点引导树策略找出地图中的狭窄通道，快速搜索出可行路径，很好地适应了复杂地图环境。LGM-RRT*需要提前对地图进行预处理，找出地图中的狭窄通道点。桥接法[16]是识别狭窄通道的有效方法之一，它在障碍物空间Xobs中随机生成两个端点qf和qs组成的一个线段，若线段中点qm位于自由空间Xfree中，这样的线段称为一个桥，如图2 所示。显然，在狭窄区域建造一座“桥梁”要比在宽阔自由空间建造一座“桥梁”要容易得多[17]。最后，通过聚类分析方法获得每个狭窄通道的唯一识别点[17-18]。Zhong[19]提出正交测试法以避免在拐角附近建造“桥梁”，如图3 所示。

图2 桥接法原理（实线表示成功，虚线表示失败）

图3 拐角附近的“桥梁”

B-RRT*和LGM-BRRT*对地图一的规划结果如图4-5 所示。从图中可以看出LGM-BRRT*算法的路径更短，但存在无效的本地点引导。原因在于LGMBRRT*只采用了一个欧氏距离范围来限制选择本地点引导，距离小于设定阈值时就触发本地点引导，但是此时的引导可能已经完全偏离目标方向。

图4 BRRT*地图一规划结果

图5 LGM-BRRT*地图一规划结果

2 改进的LGM-BRRT＊算法

本节将介绍一种加入目标方向约束的本地点引导策略，消除LGM-BRRT*的冗余引导，以帮助LGMBRRT*快速规划初始路径。同时，采样点加入目标偏向，以削弱采样的随机性，加速找到最佳路径。最后，再做相应的路径优化策略。

2.1 本地点目标约束

LGM-BRRT*本地桥接点引导触发的条件是距离小于设定阈值Ltri。在此基础上，改进算法加入角度阈值θtri约束方向，当新增节点和本地桥接点的距离小于Ltri并且和目标点构成的角度小于θtri时，触发本地引导。加入角度阈值θtri约束方向后，可以避免冗余的本地点引导，能够快速到达目标点，提高收敛速度。

如图6 所示，在二维地图环境中，假设目标为Pgoal(xg,yg) ，本地桥接点为Plocal(xl,yl) ，新扩展节点为Ptemp(xt,yt)，这三点构成的角度为θ。

θ的角度公式为：

图6 约束角度示意图

2.2 目标偏向采样

LGM-RRT*算法扩展新节点时，是在自由空间中生成一个随机点Prand(xr,yr)，然后在扩展树中找到离随机点最近的节点扩展一个步长。这种扩展方式随机性太强，当地图存在较小出口时，从出口向外扩展成功的概率相应减小，使得从出口向外扩展的时间更长。因此，改进算法加入目标方向上的引导能够提升扩展效率。如图7 所示，往随机方向与目标方向的合成向量方向扩展一个步长。其中Pgoal(xg,yg) 为目标点，Pnearest(xn,yn)为扩展树上最近的点，λ1为合成系数，新扩展点的坐标为Ptemp(xt,yt)。

图7 目标偏向采样示意图

扩展新节点函数算法1 所示，generateNewNode 根据公式（2）、公式（3）、公式（4）生成Ptemp点，如果Pnearest到Ptemp点的路径之间无障碍物，将Ptemp点加入树T 中。

算法1：Expand（xnearest，xrand）

2.3 使用本地点约束和目标偏向采样策略

设置本地点目标约束和目标偏向采样策略后，改进算法的扩展策略如算法2 所示，称起始点向目标点扩展的树为Ta，称目标点向Ta扩展的树为Tb。

算法2：改进LGM-BRRT*

2.4 路径优化处理

使用以下三种策略进行路径优化处理。

（1）考虑路径机器人宽度的膨胀检测

碰撞检测时，对机器人路径进行膨胀处理。膨胀后的机器人路径将向外扩张长度为λ2Rrt（Rrt为机器人半径）。此时两个路径节点之间机器人路径可看做是长条矩形，如图8 所示。

图8 碰撞检测示意图

（2）改进去冗余处理策略

从起始点后的第二个节点开始，依次向前遍历，若当前节点与起始点不发生碰撞且该两点与其中间点不在同一直线上，则将中间点删除；否则以中间点为起始点重新向下遍历。如图9 所示，start 到step4 之间有三个路径节点，去除step2 可以减小路径长度。而使用传统策略，去除的是step1 和step3，这样只减少了路径节点，而并未缩短路径。

图9 路径平滑操作

（3）使用路径控制点的曲线拟合处理

在曲线拟合之前对原路经进行插值处理，当路径节点与左右两点的路径长度大于最小障碍物包围盒宽度时，则在该路径节点中间靠近路径节点位置插入控制节点，插值之后在使用三次B 样条曲线拟合路径，以避免直接采用曲线拟合会导致拟合后路径在转弯处偏离原路经而产生碰撞。

3 实验与分析

为了验证改进算法的有效性，在虚拟机Ubuntu 14.04（内存2GB，硬盘25GB，处理器2 核）搭建机器人操作系统（Robots On System，ROS），在该环境下实现改进算法，并在ROS 可视化工具（3D visualization tool for ROS，RVIZ）中显示完整的路径规划结果。用于安装虚拟机的计算机配置为：（处理器：Intel Core i7-4710HQ 2.50 GHz CPU，内存：8GB，系统类型64 位操作系统）。

环境地图设置成200×200，起点坐标（3，3），目标位置（197，197），扩展步长设置为3，当树和目标点之间的距离小于步长时，则认为树已扩展到目标。当两棵树之间的距离小于3 时，则认为两棵树已连接。设定距离阈值Ltri=25，角度阈值θtri=45，设置路径宽度为机器人半径的1.5 倍，即λ2=1.5。不同参数设置对RRT 算法的效率有不同的影响，但是为了便于比较，在模拟测试中设置了相同的值。

在地图一中，存在很多障碍物，包括9 个狭窄出口，这使得扩展难度增加。图10、图11、图12 和图13分别显示了在不同迭代次数下RRT*、B-RRT*、LGMBRRT*与改进LGM-BRRT*的在地图一环境下的仿真实验结果。在图10 中，当迭代次数增加到1000 次时，RRT*才能规划出一条路径，这是四种算法所需的最大迭代次数。而改进LGM-B-RRT*算法仅迭代了150次就规划出了路径，此时RRT*还未找到第二个狭窄出口，B-RRT*两棵树尚未相交。对比LGM-BRRT*，改进的LGM-BRRT*扩展树枝更加收拢，具有目标性，无效扩展少。从图10、图11、图12 和图13 的处理过程可以看出，改进的LGM-BRRT*比RRT*、B-RRT*和LGM-BRRT*更早进入收敛状态。为进一步观察改进LGM-BRRT*算法的稳定性，在地图二中进行了实验，图14 展示了在地图二下部分迭代次数的仿真结果。可以看出，在不同的地图环境下，该算法依然能在较少迭代次数下获得很好的仿真结果，进一步证明了本文所提算法的有效性。

图10 RRT*的地图一仿真结果（从左到右依次是迭代次数150、300、500、1000、1500）

图11 B-RRT*的地图一仿真结果（从左到右依次是迭代次数150、300、500、1000、1500）

图12 LGM-BRRT*的地图一仿真结果（从左到右依次是迭代次数150、300、500、1000、1500）

图13 改进LGM-BRRT*的地图一仿真结果（从左到右依次是迭代次数150、300、500、1000、1500）

图14 改进LGM-BRRT*在地图二下的仿真结果（从左到右依次是迭代次数150、300、500、1500、2500）

为了观察每种算法在不同迭代次数中的规划能力，对每种算法进行迭代次数为150、300、500、1000、1500、2000、2500、3000 的实验，统计十次规划成功的次数。图15 显示了随着迭代次数的增加，四种算法在不同环境中的成功次数的统计。从图15 可以看出，改进LGM-BRRT*最先进入收敛状态。除此之外，路径成本也是度量规划算法的重要依据，为了避免单项测试的偶然性，在每个迭代次数下进行十次实验，并将十次实验的平均路径成本作为最终路径成本值。图16 绘制了在不同迭代次数下的路径成本统计图。从图16 可以看出，RRT*、Bi-RRT*、LGM-BRRT*和改进LGMBRRT*算法的路径成本随着迭代次数的增加而降低。当迭代次数相同时，改进LGM-BRRT*路径成本比其他三种算法要小。

综合以上实验与分析可以证明，改进LGM-BRRT*算法运行结果收敛更快，路径成本更低，适合狭窄通道地图路径规划。

图15 四种算法在不同迭代下成功次数统计

图16 四种算法在不同迭代下路径成本统计

4 结语

为了解决LGM-BRRT*算法存在选择本地桥接点时缺乏导向性导致算法效率低的问题，提出了改进LGM-BRRT*算法，通过引入本地桥接点目标导向策略，并在扩展新节点时引入目标偏向提高收敛速度。在ROS 仿真验证了改进策略的有效性。但是，算法需要对地图做一定的预处理找出本地引导点，而在实际环境中地图是未知的，需要融合实时避障策略进行处理。