同根双向扩展的贪心RRT路径规划算法

杜传胜，高焕兵，侯宇翔，汪子健

1.山东建筑大学 信息与电气工程学院，济南250101

2.山东省智能建筑技术重点实验室，济南250101

随着社会科学技术的不断更新与发展，机器人技术也迎来了其发展的黄金时期，智能机器人逐渐在医疗服务、餐饮服务、病毒消杀、消防救援等行业得到了广泛的应用。路径规划作为机器人技术中的重要研究领域，同时也是智能机器人实现自主导航的关键技术之一，也开始迅速发展[1]。从路径规划角度来看，机器人的路径规划可以假设为已知环境信息，同时假设环境信息为静态信息，在这两个前提下，常用的路径规划算法有A*算法、人工势场法、遗传算法、蚁群算法、RRT 算法等[2-4]。其中，RRT 算法因具有泛用性好、适合移动机器人和多自由度工业机器人等特点而被广泛使用，但由于采样的随机性，基本RRT 算法在树的扩展过程中存在随机性大、冗余节点多、容易在目标点周围发生振荡、规划的路径较长等问题。

为解决上述RRT算法所存在的问题，国内外的研究学者对RTT算法做了许多改进[5-8]。例如，Kuffner等[9]提出了一种双树同时扩展的RRT-connect 算法，该算法利用起点和终点两棵树相向扩展的方式加快了算法的扩展站速度，减少了搜索时间，但采样范围大的问题依然没有解决；张瑞等[10]提出了一种双向动态目标偏置RRT*算法，加快了随机树的生长速度，提高了搜索效率，但仍然存在算法计算量大且复杂的问题；辛鹏等[11]将RRT算法和人工势场法相结合，减少了规划路径的长度和拐点数，但增加了算法的计算成本；张丹萌等[12]通过混沌序列来控制采样范围，降低了RRT-Connect的无效采样节点数，加快了算法的收敛速度，但搜索路径仍存在大量冗余节点；刘奥博等[13]提出改进的GBB-RRT*算法，通过目标偏置策略，每次迭代两棵随机树进行双向拓展，提高了运行效率，但未解决随机树的拓展规模的问题；黄壹凡等[14]提出了RRT-connect算法，对新节点引入重选父节点的环节，同时加入动态步长策略来提高收敛速度，但找到的最终路径依然存在许多的转折点，影响实际机器人稳定性。

上述研究均通过不同的方式对RRT 算法进行了改进和探索，并取得了一定的进展。但采样点随机性大、算法复杂度高、规划路径冗长等问题仍未得到有效解决，因此RRT算法在搜索时间、规划路径长度、路径节点数和算法收敛速度等方面仍然还有进步空间。基于上述问题，本文针对RRT-Connect算法进行改进。通过重选随机树根节点、叠加基于目标点的引力场、引入极度贪心思维和剪枝优化处理等方式优化路径规划过程，从而提高了规划路径的质量，获得了更优的规划结果。

1 RRT-Connect算法

1999 年，LAVALLE 提出了一种基于采样的全局路径规划的RRT算法，但RRT算法的扩展性具有随机性，导致收敛速度慢。针对这个问题，2000年，LAVALLE和KUFFNER 提出了RRT-Connect 算法[15]，该算法分别以起点和终点为根初始化两棵随机树并交替向对方的根节点扩展，同时第二棵树在扩展的过程中引入贪婪策略[16]，加速算法的收敛速度。

RRT-Connect 随机树生长示意图如图1 所示，具体步骤如下：

图1 RRT-Connect随机树生长示意图Fig.1 RRT-Connect random tree growth diagram

（1）初始化状态空间C，标记起始点、终点和障碍物坐标信息，在起点和终点同时初始化两棵随机树Tree1和Tree2，并在状态空间C中生成一个随机点xrand。

（2）遍历Tree1 所有节点找到距离xrand最近的节点，记为xnearest1，从xnearest1指向xrand生长一定步长Step得到节点xnew1。判断xnew1是否满足状态空间中的障碍物约束，如果不满足，则舍弃该点并重新生成随机点；如果满足，则把新节点xnew1加入到随机树Tree1中。

（3）遍历Tree2所有节点找到距离xnew1距离最近的节点，记为xnearest2，从xnearest2指向xnearest1生长一定步长得到xnew2。若通过碰撞检测，则将xnew2加入到Tree2中，同时根据贪婪策略从xnew2继续往xnew1方向生长，直到遇到障碍物或者两棵随机树的最近距离小于设定阈值后停止；如若遇到障碍物，则交换两树的次序并重复随机采样的过程，直至两棵树的距离小于连接阈值，则路径搜索结束并返回路径节点信息。

RRT-Connect 算法的双树扩展相较于RRT 算法的单树扩展，在一定程度上缩短了规划时间提高了搜索效率，但是在实际应用中发现，RRT-Connect算法仍然存在以下缺点：

（1）树扩展的随机性大：随机点xrand的选取是由均匀函数产生的，导致新节点的产生缺少目标性。

（2）扩展较多的无用节点：Tree1在生成新节点xnew1后Tree2会立即向xnew1的方向进行连续扩展，这样容易在扩展的过程中产生许多无用的节点，从而使得规划的路径不合理。

（3）规划的路径有较多的冗余节点：由于步长的限制，使得树在扩展的过程中每次只能扩展固定的步长，这样最终会导致规划的路径中有许多冗余的节点存在。

2 改进RRT-Connect算法

基于上述RRT-Connect 算法存在的问题，本文提出了一种同根双向扩展的启发式贪心RRT 算法，相较于RRT-Connect算法主要改进点为：（1）将原来的两棵随机树改为一棵同根双向扩展的随机树。（2）在扩展新节点时添加基于目标点的引力场，使新节点更偏向于目标点生长。（3）新节点扩展完成后引入极度贪心思想，判断新节点是否可以直达目标点，以减少迭代次数，提高搜索效率。（4）对最终规划路径进行剪枝处理，优化路径长度，减少路径冗余节点。

2.1 根节点的选择

相较于RRT-Connect 算法同时将起点和终点初始化为根节点，本文改进算法选择将根节点初始化在起点与终点连线的中点位置，若起点坐标为(x1,y1)，终点坐标为(x2,y2)，则根节点坐标(xinit,yinit)的计算方式如式（1）与式（2）所示：

若起点和终点的连线的中点位置有障碍物的存在，则需要重新选择树的根节点。为了保证根节点向终点和起点扩展的平衡性，新选取的根节点需要和起点与终点保持大致相等的距离，因此新节点的选取位置位于起点和终点连线的中点沿连线垂直方向向上或向下依次平移一个到多个单位距离。计算方法如式（3）和（4）所示：

其中，n代表沿垂直线移动了n个单位距离，若移动方向是起点和终点连线的正方向则n为正数，反之为负数。根节点选择的示意图如图2所示。

图2 根节点选择示意图Fig.2 Root node selection diagram

如图2 所示，当中点无障碍物时根节点为xinit1，有障碍物存在时根节点为xinit2。

2.2 叠加基于目标点的引力场

关于RRT-Conect 算法在节点扩展过程中随机性大，容易搜索较多无用节点的问题，本文改进算法通过在节点扩展过程中叠加基于目标点的引力场，使新节点偏向目标点的方向生长。叠加引力场的核心思想就是在扩展新节点时除了随机点xrand外，还需将起点或者终点对于节点xnearest的引力加入到评判指标中，即为需要扩展的节点增加一个引力函数G(n)。此时的n节点表示由根节点向外扩展的第n个新节点，扩展方式如式（5）所示：

式中，F(n)表示新节点的扩展函数，R(n)表示新节点的随机扩展函数，G(n)表示目标点对于随机树最近节点xnearest的引力函数。以向起点方向扩展为例，新节点的随机扩展函数的表示方式如式（6）所示：

如式（6）和式（7）所示，扩展新节点时除了考虑随机点xrand外还需考虑xstar对于xnearest1的引力作用，将两个分向量进行矢量合成并乘以固定步长即可得到新节点。新节点的扩展方法如式（8）所示：

叠加引力场后可使得新节点逐渐向目标点的方向进行扩展，加速了算法的收敛速度，将少了无用节点的扩展数量。叠加引力场的新节点扩展示意图如图3所示。

图3 叠加引力场后新节点扩展示意图Fig.3 Schematic diagram of new node expansion after superimposing gravitational field

2.3 极度贪心思想的引入

节点扩展的过程中加入引力场的影响可以引导新节点偏向目标点的方向进行扩展，但由于扩展步长的限制，在新节点xnew与目标点之间没有障碍物时算法仍需迭代多次才能搜索到可行路径，为了避免这种情况，同时为了解决随机树在目标点附近来回震荡的问题，本文改进算法在节点扩展中引入极度贪心思想。在随机树扩展过程中引入极度贪心思想，即当有新节点被加入随机树时，立刻将新节点与目标点进行连线判断，若连线没有跨越障碍物则证明新节点可直达目标点，此时则结束搜索并返回搜索路径；如果连线之间有障碍物存在，则证明新节点不可直接到达目标点，此时则返回主程序继续选取随机点进行采样扩展。引入极度贪心思想的扩展示意图如图4所示。

图4 极度贪心RRT算法示意图Fig.4 Schematic diagram of extreme greedy RRT algorithm

如图4所示新节点xnew2通过极度贪心思想与目标点xgoal进行了直接连接，提前完成了路径搜索。依据两点之间线段最短的原理，如若新节点与目标点进行了直连，则新节点与目标点之间的路径一定为最优路径。新节点直连判断的方式虽然在一定程度上增加了算法的计算量，但其避免了随机树在目标点附近来回震荡，减少了算法的迭代次数，提高了算法的收敛速度，故算法整体计算量还是得到了优化。

2.4 剪枝优化路径处理

RRT-Connect 算法最终规划的路径均由随机点组成，且由于扩展步长的限制，路径存在许多冗余的的节点和转折点，因此有必要对路径进行剪枝优化处理。剪枝处理的方式如下：将初步路径规划得到的节点统一进行处理，首先从起点xstar开始依次与后面的节点相连并判断是否穿越障碍物，如果没有穿越障碍物则继续与下一节点进行连线判断；如果与某一节点xk的连线穿越了障碍物，则将xstar与该节点的前一节点xk－1相连并删除它们之间的所有路径，同时将xk－1作为起点重复上述操作，直至连接到终点。剪枝优化前后对比图如图5所示。

图5 剪枝优化处理前后对比图Fig.5 Comparison before and after pruning optimization

3 仿真及实验验证

为了检验改进算法的有效性，本文将利用Matlab平台在三种不同的实验环境中对基础RRT-Connect、文献[12]改进算法（记为ZRRT-Connect）、DRRT-Connect[17]、GB-RRT[18]和本文改进算法进行仿真对比实验。仿真环境分别为狭窄通道、复杂多障碍物环境和实验室二维仿真环境，地图大小均为800×800 像素，起点与终点坐标分别为（30，770）和（770，30）。仿真地图中黑色代表障碍物，白色为可行区域，紫色圆点代表起点，绿色圆点代表终点，黄色原点为本文改进算法的根节点，实验过程中为五种算法设置相同的扩展步长。为保证实验的公平性，将在同一地图中对五种算法分别进行30次实验，最后将所有实验结果进行汇总并取平均值进行分析。

3.1 狭窄通道环境仿真

在狭窄通道环境中对五种算法进行仿真对比，仿真结果图6所示。

图6 狭窄通道环境仿真结果Fig.6 Simulation results for narrow aisle environments

由图6可知，本文改进算法相较于其余改进算法在狭窄通道环境中有着更强的目的性，大量减少了无效空间的搜索范围，且规划的路径更加合理。进行30 次仿真实验后，将五种算法搜索时间的平均值、路径长度的平均值、路径拐点个数的平均值和算法迭代次数的平均值进行统计，统计结果如表1所示。

表1 30次实验平均值Table 1 Average of 30 experiments

由表1 可知，改进算法的平均搜索时间相较于GBRRT算法下降了57.7%，平均路径长度比DRRT-Connect下降了28.9%，平均路径拐点数比ZRRT-Connect下降了96.9%，迭代次数比GB-RRT 下降了89.9%，证明了改进算法在狭窄通道环境中有着更高的规划效率。

3.2 复杂多障碍物环境仿真

在多障碍物环境中对三种算法进行仿真，仿真结果如图7所示。

图7 复杂多障碍物环境仿真结果Fig.7 Multi-obstacle environment simulation results

如图7所示，在复杂多障碍物环境中本文改进算法所规划的路径明显更优。将五种算法分别进行30次实验，实验数据分析后结果如表2所示。

表2 30次实验平均值Table 2 Average of 30 experiments

根据表2 可知，改进算法的平均搜索时间相较于DRRT-Connect算法下降了44.9%，平均路径长度比GB-RRT 下降了10.1%，平均路径拐点数比GB-RRT 下降了91.0%，迭代次数比GB-RRT 下降了41.9%，验证了改进算法在复杂多障碍物环境中路径规划的有效性。

3.3 实验室二维环境仿真

最后在实验室二维环境中对五种算法进行仿真对比，仿真结果图8所示。

图8 实验室二维环境仿真结果Fig.8 Laboratory environment simulation results

由图8 可知，改进算法在实验室二维仿真环境中从中间向两边进行扩展的方式明显好于从起点和终点向对方进行扩展的方式。对五种算法在该环境内分别进行30 次实验后，将实验数据结果整理分析后如表3所示。

表3 30次实验平均值Table 3 Average of 30 experiments

由表3 可知，改进算法的平均搜索时间相较于DRRT-Connect 算法下降了81.8%，平均路径长度比DRRT-Connect 下降了25.6%，平均路径拐点数比GBRRT 下降了95.7%，迭代次数比ZRRT-Connect 下降了49.8%，验证了改进算法在复杂多障碍物环境中路径规划的有效性。证明了改进算法在真实仿真环境内有更强的实用性。

3.4 实验验证

为验证改进RRT 算法在真实移动机器人中是否可行，将改进算法在基于阿克曼模型的移动机器人（图9所示）中进行实验验证。移动机器人搭配有激光雷达、IMU、编码器、广角摄像头等传感器，由工控机（jetsonnano）上运行ROS（机器人操作系统），进行数据融合和路径规划任务。

图9 路径规划结果Fig.9 Path planning results

实验场地为实验室封闭杂乱的储物间。通过键盘节点控制机器人移动，利用Gmapping 算法构建环境地图，并在Rviz可视化界面中进行显示，自主移动机器人如图9（a）所示，最终导航路径结果如图9（b）所示。

4 结束语

本文针对RRT-Connect 算法搜索时间长、节点扩展缺乏目标性和规划路径冗余节点多等问题，提出了基于同根双向扩展的贪心RRT算法。首先将两棵同时扩展的随机树改为一棵同根双向扩展的随机树，降低了计算量；其次为新节点的扩展叠加了基于目标点的引力场，使新节点的扩展更具有导向性；同时在扩展新节点后加入极度贪心思想，加速了算法的收敛速度；最后对规划的路径进行了剪枝优化处理，降低了路径节点数，缩短了路径长度。通过三种不同环境的仿真对比实验，证明了本文改进算法规划的路径在效率和质量上均具有更优性。未来的研究中将会把动态障碍物的影响加入到算法改进的考虑中，以提高算法的真实性和实用性。