0 引言

路径规划是移动机器人领域的一项热门研究，目前已在自动驾驶^[1]、自主探测^[2]、计算生物学^[3]、图形动画学^[4]等领域获得广泛应用。路径规划的最终目的是寻找到一条可以从起始点到目标点的无碰撞路径^[5]，该路径可以保证移动机器人在移动过程中不会与障碍物发生碰撞，同时这条路径需要满足距离最近、用时最短或耗能最低等指标中的一种或几种。目前在路径规划方面已经出现了众多可行的算法^[6~12]。在低维空间中，这些算法尚有一定的优势，但在高维空间中，由于计算的复杂性，其生成路径的效率较低。

为解决高维空间中的路径规划问题，基于采样的路径规划算法被提出并得到发展，其代表算法是LaValle等提出的RRT^[13]算法，该算法不需要对环境进行建模，只需要通过随机采样生成随机树的方式来探索整个空间，随机点的选取保证了搜索路径的全局性，从而使算法具有概率完备^[14]的特点。由于该算法可以大幅降低高维空间路径规划的计算量，在提出后受到了众多学者的关注。但该算法存在不足：1)由于扩展的随机性，使得算法收敛速度缓慢，路径生成效率低；2)生成的路径只是可行路径，而非最优或次优^[15]；3)路径生成中没有采用平滑处理，导致生成的路径较为粗糙，不适合机器人的实际移动。

针对RRT算法存在的不足，国内外学者纷纷提出了一系列的改进算法，其中由Kuffner等提出的RRT-Connect算法^[16]在规划效率上取得明显进步。该算法通过在起始点和目标点同时生成两棵随机树的方式，用以加速算法的收敛。本文基于RRT- Connect算法，通过在起始点和目标点中间选取第三节点以生成四棵探索树的方式来提高算法的收敛速度，同时在算法扩展过程中，加入启发函数，使随机树在扩展中可以自适应调节步长，并通过引入目标偏置策略，使改进后的算法在无障碍空间中能够以最快速度探索未知区域，增加扩展效率。

1 RRT与RRT-Connect算法

RRT算法作为单一查询算法，不需要预先对整个构型空间进行采样与存储，而是通过函数迭代的方式探索整个状态空间^[17]。

图 1中，n_init、n_goal分别为路径的起始点和目标点，n_rand为状态空间中的随机采样点，n_nearest为随机树中离随机采样点n_rand最近的节点，n_new为n_rand方向上随机树下一步扩展点，ε为随机树每一步的扩展步长。

当随机采样函数通过对空间随机采样获得n_rand后，RRT算法便以该点为下一步扩展方向向外扩展，扩展过程中有以下几种情况：1)当n_rand与n_nearest之间的距离大于ε时，则在n_rand与n_nearest之间的连线上取一点n_new插入到树中，记为Reached；2)当n_rand与n_nearest之间的距离小于ε时，则直接将n_rand插入到树中，记为Advanced；3)在扩展中遇障，则舍弃原先的扩展方式，并重新生成随机点n_rand，记为Trapped。

RRT算法由RRT建树(BU_RRT(n_init))和RRT扩展(EXT(T，n_rand))两部分组成。RRT建树过程如下：

RRT扩展如下：

其中，T表示随机树，T.V表示随机树中的节点，T.E表示随机树顶点之间的路径。BU_RRT函数在状态空间中采样随机点，确定扩展方向。通过循环调用EXT函数来扩展未知空间，最终返回生成的搜索树。EXT函数主要用于扩展随机树。根据设定步长判断n_new节点的位置，并将新节点添加到树中。

由于RRT算法在扩展中的随机性，导致其存在收敛速度缓慢，为了提高RRT算法的收敛速度，Kuffner和LaValle提出了RRT-Connect算法^[16]，该算法分别在起始点和目标点构建两棵并行的随机树，并在每次迭代过程中，使两棵树分别朝着彼此的方向交替扩展，且在扩展中引入了具有贪婪特性的CON函数，实现随机树的快速扩展。

RRT-Connect建树过程如下：

其中：T₁、T₂表示分别从起始点n_init和目标点n_goal生成的随机树：CON函数如下：

该算法首先调用EXT函数，从起始点n_init出发，通过EXT扩展生成节点n_new。此后，T₂开始以n_new为方向进行扩展，在T₂扩展中，如果没有发生碰撞或者n_new与当前扩展节点之间的距离不小于步长ε时，T₂通过调用CON函数来实现快速向n_new扩展，当扩展过程中出现障碍物时，T₂停止扩展，T₁以当前T₂的终止点为目标点开始扩展，通过SWAP函数使两棵树交替扩展，最终相遇并连接。

2 DRRT-Connect算法

RRT -Connect算法虽对探索树的扩展速度有很大幅度的提高，但在如防反弹袭击、无人机避障等一些需要高灵敏度的领域仍存在规划效率低的问题。基于上述原因，本文提出的DRRT-Connect，对RRT-Connect算法做出以下两点改进：首先，由于RRT-Connect只能同时生成两棵树，所以DRRT -Connect算法在RRT -Connect算法的基础上引入第三节点，使算法可以同时生成四棵树，以达到加倍扩展RRT-Connect的效果；其次，DRRT-Connect在RRT-Connect算法的基础上加入可自适应调节步长的启发函数，使随机树在探索无障碍空间时的扩展效率更高，同时在算法执行时，引入目标偏置策略，使算法的扩展更具方向性，进一步加速随机树的扩展，减少迭代次数，以下对算法改进原则做详细介绍。

2.1 第三节点选择方式

为使DRRT-Connect算法可以实现四棵树同时扩展，需要选择一个第三节点。为提高算法效率，本文以起始点与目标点连线的中心点作为第三节点，假设起始点在构型空间中的坐标为(x₁，y₁)，目标点的坐标为(x₂，y₂)，则求取第三节点n_center (x₃，y₃)坐标的公式为:

(1)

(2)

2.2 自适应步长调节原则

RRT-Connect算法中步长是给定值，这种给定步长在探索无障碍空间时会限制随机树的扩展速度^[18]。本文在DRRT-Connect算法中加入了自适应调节步长的函数，具体为：随机树以初始步长ε探索未知空间，当扩展至新节点时，如在该节点处没有发生碰撞且未与另一棵树相连，则在下一次的扩展中，随机树将在原始步长上增加一个步长ε的宽度继续扩展，以此类推；当扩展中发生碰撞时，则将步长调节为原始步长ε，然后按步长ε进行扩展。

2.3 避障原则

由于环境的复杂性，在DRRT-Connect执行过程中会遇到障碍物的情况，所遇障碍物主要包括第三节点生成时遇障与随机树扩展时遇障两种情况。

对于第一种情况，在DRRT-Connect采样第三节点之前，首先进行障碍检测，当第三节点位于障碍物中时，为了提高路径生成效率，则直接选择舍弃第三节点，采用RRT-Connect进行扩展。对于第二种遇障情况，即DRRT-Connect在扩展中遇到障碍物，当此时只有一棵树处于障碍物时，则采用交换扩展顺序的方式开始扩展另一棵树，若在扩展中两棵树同时遇障，此时则调用随机点生成函数，使其在构型空间中随机采样，然后选择较短树以该点为方向向前扩展一步，如脱离障碍物，然后以目标点为方向继续扩展，若该点与目标点仍存在障碍物，则继续调用该函数进行扩展随机树，直至摆脱障碍物。

2.4 算法实现

基于以上三点原则，DRRT-Connect算法构建过程如图 2所示，首先，算法分别以n_init、n_goal和n_center为扩展树的起点，然后n_init、n_goal皆以中间点n_center为目标点，同时并行向目标点扩展，而此时中间点n_center则分别以n_init、n_goal为目标点开始扩展，四棵树同时并行生长。在扩展中，当第一步扩展没有遇到障碍物，则在下一步将扩展步长增加ε，当在扩展中遇到障碍物时，则将步长恢复至初始状态。

M_EXT函数为改进的随机树扩展函数，该函数主要目的是通过改进原始EXT扩展函数将原先步长固定的扩展方式改为步长随扩展状态的不同进行自适应调节，即当此时扩展状态为Advanced时，在下一次扩展中，算法将在原始步长基础上增加一个ε步长，以此类推。如果下次扩展中返回状态仍为Advanced，则在上次步长基础上继续增加ε，若下次扩展中返回状态为Trapped，说明在扩展中随机树遇到障碍物，则首先将步长还原为原始步长，扩展至再次检测到障碍物。M_EXT函数如下：

BU_DRRT-Con函数为DRRT-Connect算法建树过程，算法首先调用M_EXT函数，分别将从起始点n_init开始的随机树T₁和从目标点n_goal开始的随机树T₂向外扩展一步，生成n_new1和n_new2。此后，通过引入目标偏置策略，使算法以n_center为起点，分别以n_new1和n_new2为目标点，向外扩展随机树T₃和T₄，并调用CON函数，将两树快速扩展，直至在扩展过程中遇到障碍后停止，通过SWAP函数交换扩展顺序，使得T₁、T₂以T₃、T₄停止点为目标点进行扩展。如此循环，直至四棵树连接，生成一条完整路径。

3 仿真实验与分析 3.1 仿真实验

为了对DRRT-Connect算法进行性能比较，本实验对图 3所示的地图进行仿真，地图中空白区域代表无障碍物区，红色区域代表障碍物区。

仿真所使用的硬件平台为Intel Core(TM)i5- 4200M 2.5GHz CPU，RAM 6GB，算法通过Python编程实现。

实验中，分别以RRT、RRT* ^[19]和RRT-Connect作为改进算法的比较对象，通过比较证明DRRT- Connect性能的优越性。图 4为DRRT-Connect算法随机树探索过程，从图 4中可以看出，改进后的算法分别以起始点、目标点以及中间第三节点为起点同时生成四棵随机树，从而加快对未知区域的探索，以加速最终路径生成。

图 5中(a)、(b)、(c)、(d)为DRRT-Connect算法与RRT、RRT*、RRT-Connect三种算法的仿真结果图。从图 5中可以看出，相比于其他三种算法，DRRT -Connect算法所生成的路径具有平滑性好且具有一定的方向性。该结果充分展示出通过四棵树生成的路径在应用到实际场景中时，具有较好的表现。

在实验中，分别对每种算法进行50次仿真验证，图 6所示为4种算法寻路时间对比图。从图 6中可以看出，DRRT-Connect算法在50次的仿真实验中耗时远远低于RRT与RRT*算法，相较于RRT- Connect算法而言，由于坐标精度原因，无法明显观测出速度的提升，DRRT-Connect算法平均路径生成时间将在下文分析。

图 7所示为4种算法在生成路径中所需迭代次数示意图，由于自适应步长的加入，DRRT-Connect算法在迭代次数上相较于RRT-Connect算法也有较为明显优势。

表 1所示为在50次实验中4种算法的迭代次数、路径长度与生成路径所花费时间的平均值，从数据中可以更为直观的看出，相比于RRT与RRT*算法，DRRT-Connect算法在迭代次数与路径生成时间上都有非常明显的优势；相比于RRT-Connect算法，DRRT-Connect算法在路径规划时间上只需要RRT -Connect算法的50%，在迭代次数上相比RRT-Connect算法降低了32.3%，大大提高了路径的生成效率。

3.2 性能分析

在上述实验结果中，改进后的DRRT-Connect算法在迭代次数与路径生成时间上的性能均明显优于改进算法，而在路径长度上相较于RRT*算法不具有明显优势。

迭代次数降低的原因归功于算法中引入的目标偏置策略，该策略使原始RRT算法中使用的随机采样扩展变为具有目标性的扩展方式，将原来每一步扩展都需要首先进行随机采样的方式变为以随机树停止位置作为目标点，直接使算法朝向目标点扩展一步，只有当向目标点扩展时遇到障碍物，才调用随机采样函数，从而使随机树摆脱障碍物，保证算法的概率完备性。

路径生成速度大幅提高的原因主要是DRRT- Connect算法将RRT-Connect算法的两棵随机树扩展方式变为四棵树通过扩展，大幅加速算法生成路径的效率；在无障碍空间中，算法调用自适应步长调节函数，该函数与目标偏置策略相结合，可以使随机树更高效的向目标点进行探索，达到提高路径生成效率的期望。

DRRT-Connect算法在生成路径长度上并未获得提升，主要原因在于DRRT-Connect算法以RRT -Connect算法为基础，并未考虑路径的最优性，而是以提升路径生成效率为目标。通过自适应步长调节函数，可以在一定程度上提高路径的平滑性，但并不能很大幅度降低路径长度。

4 结语

本文通过对RRT-Connect算法进行改进，提出了一种可以实现同时生成四棵树并且扩展中步长可自适应调节的DRRT-Connect算法，对改进后算法的原理及实现过程进行了详细分析，通过仿真实验结果，验证了改进后的算法相对于其他算法在路径生成效率及路径平滑性上都具有明显提升。

本文所提算法目前主要用于障碍物较少的简单环境下实现快速路径规划，下一步将尝试针对复杂环境下以及第三节点难以选取的情况进行改进，从而提高算法的泛化性，以便更好地满足工程实践的需要。