基于RRT算法的无人艇航线重规划方法

钟富川，杨雪锋，2*，袁志瑶

(1.重庆交通大学 航运与船舶工程学院，重庆 400074;2.交通安全应急信息技术国家工程实验室，北京 100011)

航线规划是无人艇的核心技术，是实现无人艇自主航行的关键[1-2]，目前，航线规划方法大致可分为3类：①以历史航迹数据为基础，通过建立推荐航线库，结合季风、洋流、台风等气象因素推荐计划航线[3-5]；②通过建立船舶航行区域内障碍物的数学模型，利用传统的智能算法和图论知识进行航线规划[6-8]；③以利用RRT(rapidly-exploring random tree)算法为代表的随机采样算法进行航线规划[9-10]。RRT算法目前应用较为广泛[11]，不需要对复杂的任务空间进行数学建模，只需对采样点进行碰撞检测；RRT算法工作原理简单，且在搜索过程中可以考虑运动学约束条件，从而能够有效地解决复杂环境下的运动规划问题[12]。然而，由于采用随机采样的方式进行规划，RRT算法又存在实时性低、规划的路径不是最优解等问题。许多学者提出了一系列改进的策略[13-18]。D. Ferguson等提出了一种基于反向随机树生长策略的实时重规划算法DRRT(dynamic rapidly-exploring random trees)，将规划终点作为随机树生长的起始点，分割去除在路径受阻时与障碍物发生碰撞的随机树部分，并在此基础上重新生长以获取重规划路径[19]。利用该思想，重规划只分删除部分受障碍物影响的随机树节点，保留大部分未受影响的节点，在一定程度上保留并利用初始航线的规划成果，可缩短规划时间。

可见，利用分割随机树的思想进行重规划的方法是缩短耗时的有效手段，然而，新增障碍物的位置对重规划效率方面的影响研究尚未见报道，而且大部分研究未将受障碍物影响失效的随机树节点与边分开考虑。为此，考虑针对无人艇航行区域的特征，提出一种针对失效节点和失效边的随机树分割方法，解决无人艇的航线重规划问题，并利用贪心算法对航线规划结果进行优化。根据广州港附近水域、大连港附近水域和天津港附近水域构建任务空间，通过仿真实验探讨新增障碍物位置对重规划耗时的影响，对比利用RRT算法的传统航线重规划方法与利用随机树分割的航线重规划方法，在规划耗时和航程方面的性能。

1 航线重规划方法

1.1 整体流程

在无人艇执行原计划航线任务时，当任务空间更新后，发现航行任务空间中有新增障碍物，此时需要判断初始计划航线是否受阻，如能安全通行则继续执行航行任务，否者就需重新规划航线。具体流程见图1。

图1 航线重规划算法流程

1.2 具有偏置概率的RRT(BP-RRT)

BP-RRT算法进行路径规划主要步骤见表1。

表1 BP_RRT算法伪代码

1)设定起始位置Uinit为随机树T的根节点。

2)产生一个随机数x(0～1之间)，如果x小于偏置概率Pg，则将目标点Ugoal作为随机采样点Urand；否则于偏置概率Pg，将在状态空间中随机选择一点作为Urand进行生长。

3)寻找随机树T中与Urand最近的节点Unear，并根据步长λ计算得到子节点Unew。

4)判断Unear与Unew之间是否存在障碍物，若不存在障碍物，则将Unew作为T的子节点，添加边[Unear,Unew]；否则，返回步骤上一步。

5)当随机树中存在子节点与Ugoal之间的距离小于步长，且无障碍物，便可以在随机树中找到一条由节点组成的从出发点到目标点的路径。

1.3 随机树分割方法

借鉴DRRT算法的分割逻辑，结合无人艇的任务空间特性，当任务空间中产生新的障碍物时，初始随机树中部分节点(下换失效节点)或者边将处在无人艇不可航行的障碍区域内。根据随机树的生成原理可知，以失效节点为父节点的所有节点，或者以失效边连接节点为父节点的所有节点，将不可到达，这些节点均应被当作失效节点。随机树分割的目的就是根据新增的障碍物，删除初始随机树中的失效节点和失效边，得到一棵残余随机树，为无人艇航线重规划提供基础条件，其过程及伪代码见图2和表2。

图2 随机树分割过程示意

表2 随机树分割算法伪代码

图2中第一棵随机树表示初始随机树，黑色矩形表示新增障碍物。

对于一个包含有M个节点的随机树(T)，根据障碍物和随机树，利用FIND_DEAE_NODE函数确定失效节点，并将这些节点的Parent属性设置为0。循环遍历随机树中的所有节点，查找父节点Parent属性为0的节点，并将这些节点的Parent属性也设置为0，直到随机树中所有节点的Parent属性不再改变为止。

1.4 航线平滑步骤

对于一个包含N个节点的航线(route)，假设每个点表示为Ui(i=1～N)。

1)令Ustart=U1，Ugoal=UN，连接Ustart和Ugoal。

2)通过直线连接Ustart和Ugoal，如果2个点之间有障碍物，则将Ugoal替换为前一个点，即令Ugoal=UN-1。

3)重复上一步骤，直到Ustart和Ugoal之间的直线上无障碍物为止。

4)一旦Ustart和Ugoal无障碍物，删除Ustart与Ugoal之间所有节点，令Ustart=Ugoal，Ugoal=UN。

5)重复步骤2)～4)，直到Ustart=UN。

航线平滑方法伪代码见表3。

表3 航线平滑处理伪代码

2 无人艇航线重规划仿真实验

仿真实验软件平台：Matlab R2020a。

硬件条件：Windows 7 旗舰版Intel(R)core(TM)i3CPU M370 @2.40 GHz 2.39 GHz。

2.1 可行性实验

分别选择大连港、广州港和天津港附近水域的实际环境，建立黑白二值地图(尺寸：500×500像素)作为规划任务空间，图中黑色区域表示非可航区域(陆地、近岸岛礁、浅水区等)，白色代表可航区域,见图3。

图3 任务空间黑白二值地图

以广州港附近水域建立的航线规划空间为例，在检测到初始航线受阻见图4a)，随后获取初始随机树见图4b)，利用障碍物区域对初始随机树进行分割得到残余随机树结果见图4c)，利用残余随机树生长见图4d)，生成重规划航线，并做平滑处理见图4e)。具体参数设置如下：航线任务的起点坐标为[10,10]，终点坐标为[400, 250]，偏置概率Pg为0.1，其中圆点分别表示起点和终点位置，在原有计划航线的中点处构建5×5(像素)大小的矩形区域模拟新增的障碍物。

建立的在广州港、大连港及天津港附近水域任务空间中开展仿真实验。针对不同任务空间，将算法步长分别设置为5、10、15、20 pixels，共计12个实验项目，每个实验项目分别进行100次重复实验，统计重规划耗时的平均值与标准误差，结果见图5。

图5 不同步长下的重规划耗时平均值

从图5可以看出，利用随机树分割的航线重规划方法在所构建的实际的任务空间中确实可行，航线重规划耗时均小于2 s。此外,在(5～20)pixels步长范围内，很明显，规划耗时随步长的增加而明显降低，且在复杂程度不同的任务空间中，规划耗时差异明显。

2.2 对比实验

将利用随机树分割的航线重规划方法与利用传统RRT、BP-RRT的重规划方法，在重规划耗时方面进行对比。各算法的初始参数设置如下：步长λ=10 pixels；偏置概率Pg=0.1；在初始计划航线1/2处模拟新增障碍物(5×5 pixels)阻断计划航线。

2.2.1 算法耗时

统计航线重规划耗时、航程长度的平均值和标准误差，具体见图6、7。

由图6可见，利用随机树分割的航线重规划方法能够显著降低航线重规划耗时，且明显优于传统RRT。相比于偏置RRT的航线重规划方法，在广州港任务空间中规划耗时降低约30%，在大连港任务空间中规划耗时降低约80%，在天津港任务空间中规划耗时降低约71%。

整体来说，进行重规划时，分割RRT算法的耗时少于偏置RRT算法，但在个别航线重规划案例中，会出现传统RRT算法优于分割RRT算法的情况。如广州港附近水域航线重规划时，第73组与87组实验，传统RRT和偏置RRT算法均优于分割RRT算法，见表4。

表4 广州港航线重规划耗时统计特例 s

2.2.2 重规划航线长度

从图7，分割随机树方法规划的航程最短，相较于传统RRT与偏置RRT方法，在广州港任务空间，分别缩短了约22.49%、16.14%；在天津港任务空间，分别缩短了约21.36%、19.94%；在大连港任务空间，分别缩短了约18.74%、13.82%，效果明显。

图7 航线长度对比

3 新增障碍物位置的影响

3.1 仿真实验

在初始随机树中，如果新增障碍物出现的位置不同，那么最终得到的失效节点数量也将不同，残余随机树的节点个数也会有较大的差异，会对航线重规划产生影响。调整新增障碍的位置进行仿真实验，探讨新增障碍物位置对航线重规划的影响。以广州港、天津港及大连港附近水域为例，设每个地图的起始点与终点和上述实验条件一致，偏置概率为Pg=0.1，步长λ=10 px，最小阈值mix=9 px。令初始计划航线的长度为L，新航行障碍区域的位置分别位于距离起始点(1/6)L、(1/3)L、(1/2)L、(2/3)L、(5/6)L处。根据新增障碍物位置不同，可得到5个组实验，共计15各项目，每组实验项目分别进行100次重复实验统计算法耗时，得到耗时情况见图8。

图8 障碍物不同位置耗时平均值对比

由图8，在天津港的任务空间中，重规划耗时随着障碍物出现的位置离出发点越远，则重规划的耗时就越短，即残余随机树节点个数与初始随机树节点个数之比，该比值越大，航线规划耗时越少，符合一般设想。

然而在广州港(1/2)L处和大连港(2/3)L处却出现耗时突增的现象。在广州港任务空间中由(1/3)L处到(1/2)L处，平均耗时由0.38 s突增到1.54 s增加了近75.3%；在大连港任务空间中由(1/2)L处到(2/3)L处，平均耗时由1.69 s突增到2.57 s增加了近34.2%。

3.2 算法耗时突增现象分析

一般而言，新增障碍物出现的位置离终点距离越近，则航线重规划所需的时间越少。然而，从图8各种航线重规划算法耗时的统计结果来看，这一结论并不完全正确。以广州港和大连港附近水域的航线规划任务空间为例进行分析说明。

对广州港附近水域而言，无人艇从起点到终点有2条通道可以选择，分别是通道1与通道2，见图9。当原计划航线处于通道2时，新增障碍物可能会完全堵塞通道2。此时，如果再以残余随机树为基础进行航线规划，大量的新增节点Unew出现在被堵塞通道内，极大可能无法通过碰撞检测，这将导致随机树生长失败的概率升高，重规划算法耗时增加。

图9 任务空间通道口示意

对大连港附近水域而言，无人艇从起点到终点的通道如图9b)所示，在初始计划航线(2/3)L处增加障碍物时，障碍物恰好位港池的入口处，堵塞通道，其效果与广州港附近水域类似，将导致随机采样失败率增加，提高算法耗时。

综合上述分析，一般情况下新增障碍物离终点的距离越近，利用残余随机树进行航线重规划耗时就越少。但是，当新增障碍引起任务空间结构变化较大，即减少了任务空间通路或使通路变狭窄等情况时，航线重规划耗时将显著增加。

4 结论

1)利用分割随机树的方法进行无人艇航线重规划可行，通过随机树分割得到残余随机树，同时利用残余随机树进行随机树生长可缩短算法重规划耗时。

2)利用航线平滑处理方式能明显缩短所规划的航线长度，使航线趋于最优。

3)一般而言，新增障碍物在初始计划航线上出现的位置，即残余随机树节点个数与初始随机树节点个数之比，该比值越大，航线规划耗时越少，然而当新增障碍物严重影响了任务空间的约束，则会导致重规划效率急剧降低。