基于改进RRT*算法的无人艇全局避障规划

杨左华，王玉龙,，戚爱春

(1.江苏科技大学 电子信息学院，江苏 镇江 212003；2.上海大学 机电工程与自动化学院，上海 200444)

0 引 言

水面无人艇（Unmanned Surface Vehicle，USV）是一种可以自主感知环境、探测目标、智能避障，完成特定海洋任务如侦察、巡逻、监视等的小型船舶[1]。为提高无人艇执行任务的自主权，减少人工监督以降低运营成本，水面无人艇的智能避障一直是研究热点。USV 的智能避障分为已知环境的全局避障和动态未知环境的局部避障。全局避障是指在已知水面环境地图上为无人艇规划出一条从起始点到终点的无障碍路径，局部避障规划是指根据无人艇的周围实时动态环境进行有效避障[2-3]。

基于随机采样的快速搜索树算法（Rapid-Exploring Random Tree，RRT）无需对环境空间进行预处理，已被证明可以用于各种复杂环境的路径探索，完成无人艇的全局避障规划[4-5]。RRT 算法首先由Lavalle[6]提出用于路径规划。然而基本的RRT 算法迭代次数过多，搜索节点遍布整个环境地图，浪费了大量内存资源，同时收敛速度慢，实时效果差。针对基本RRT 算法存在的不足，众多学者提出了改进算法，如RRT-Connect 算法、RRT*算法、B-RRT*算法、IB-RRT*算法。RRT*算法通过改变新节点连接方式以获取最低路径成本，加快了收敛速度，生成的路径渐近最优[7-8]。RRT-Connect 算法提出起始点终点并行生成搜索树的方法，提高了路径搜索的速度[9-10]。B-RRT*算法则是RRT-Connect 算法和RRT*算法的结合，生成一条最优路径[11]。文献[12]提出了一种基于动态步长的路径规划自适应RRT 算法，并利用三次多项式曲线拟合来平滑规划路径。文献[13]采用自适应步长RRT 方法进行双机器人路径规划，实现了双机器人在各自构型空间中的协同路径规划。文献[14]提出一种基于路标引导和增长采样区域的混合策略来引导RRT 算法向目标搜索。文献[15]改进了RRT-Connect 算法，以便设计管线最优路径，采用贪心算法去除多余节点，并采用Catmull-Rom 曲线拟合关键节点处曲线。

对于无人艇的全局避障规划，这些算法仍存在一些问题：

1）在避障规划时未考虑与障碍物的安全距离。

2）随机采样带来搜索路径的不稳定性；

3）路径长度仍需优化；

4）规划的路径折点过多，仍需后续光滑处理。

为解决这些问题，本文提出一种改进的无人艇全局避障规划方法，即（Line Segment Theorem-RRT*）LT-RRT*算法。考虑到USV 实际航行时应与障碍物保持一定安全距离，首先运用栅格法对环境地图进行预处理。然后再根据终点采样概率选取采样点，改善全局采样引起的路径不稳定性，节约内存资源，提高收敛速度。根据线段定理重新选取新节点和附近节点的父节点，跳过中间节点直接连接树节点，减少路径折点及路径成本，使算法最终生成相对平滑的规划路径。最后在相同水面环境下，将本文算法与现有算法进行比较，结果表明LT-RRT*算法可以为USV 规划出更短的避障路径，其转折点数量大大减少，同时距离障碍物有一定安全距离，更有利于USV 的安全行驶。

1 现有算法研究

1.1 问题定义

1.2 基本RRT 算法

基本RRT 算法首先在无障碍空间 Dfree中进行随机采样，生成随机采样点 xsample，寻找树上距离随机采样点最近的节点 xnearest，从该点向采样点扩展一个固定步长生成新的节点 xnew， 连接最近点 xnearest与新节点xnew，成为树的新分支。重复上述过程进行多次迭代采样，直到新节点 xnew距离终点 xgoal一定距离，最终找到起始点到终点的最优路径。基本RRT 算法用于无人艇的全局避障规划，在添加新节点时进行避障检测，如果 xnearest与新节点 xnew的连线与障碍物产生碰撞则放弃当次采样，重新进行迭代采样。RRT 算法示意图及具体算法流程如图1 所示。

图 1 RRT 算法示意图Fig.1 The diagram for RRT algorithm

1.3 RRT*算法

由于基本RRT 算法收敛过慢，路径过长，RRT*算法改变了新节点的连接方式，以新节点 xnew为中心，找出一定距离内的附近节点，以最小路径成本在附近节点和最近节点 xnearest中选择新节点的父节点，从而修正了路径。

2 基于LT-RRT*的路径规划算法

2.1 环境建模

由于障碍物边缘区域为不利于无人艇安全航行的危险区域，规划的避障路径应避开障碍物一定距离。考虑无人艇航行的安全性，在算法执行过程中对路径段进行避障距离检测，虽然可以使最终生成的路径避开障碍物一定距离，但会大大增加算法计算时间，影响算法收敛速度。因此在进行无人艇全局避障规划之前，应先对环境地图进行预处理，根据障碍物位置标示出其周围不可航行区域。

运用栅格法[16-17]，将环境地图按合适比例划分成M*N 格。每个障碍物单元的8 个方向上的栅格部分处理为其他颜色，定义为无人艇行驶的危险区域，避障规划时同时检测与危险区域的碰撞，可以使无人艇与障碍物保持安全距离。

2.2 改进采样点选取

快速随机搜索树算法RRT 在整个环境空间生成随机采样点，扩展了树节点，很好地探索了未知空间。但生成大量偏离终点的无用节点，导致迭代次数过多，占用内存过多，生成路径过长，搜索路径过慢，不能很好地满足无人艇避障规划的实时性要求。

针对此问题，引入终点采样概率p（0 ≤ p≤1），一次迭代以p 概率直接选择终点为采样点，以1-p 的概率随机选择环境空间中的点。随机采样点不再过于随机，p 的值越大，避障路径的搜索更加偏向终点，大大减少无用空间的探索，有利于树节点更加快速逼近终点，提高收敛速度。

2.3 改进父节点选取

2.4 基于线段定理的路径重连

RRT*算法在添加新节点时同时考虑了新节点的周围节点，通过周围节点到达新节点的成本若低于通过最近节点到达新节点的成本，以周围节点作为新节点的父节点，从而修正航迹，提高收敛率，减少路径成本。但上述方法仍不可避免节点过多，路径过于曲折。为此，基于线段定理（两点之间直线最短），改进父节点的选择，拉直路径。每一次迭代中，选取的父节点为改进后的的父节点，路径的成本就会低于经过再连接到。基于两点之间线段最短，跳过中间节点直接连接，可以大大降低路径成本。同时，LT-RRT*算法删除 xnew的周围节点的上一次连接线段，重新和 xnearnest的父节点连接，降低了到达附近节点的成本。图2 和图3 分别显示了RRT*算法与LT-RRT*算法的显著差异。RRT*算法中附近的节点未得到改进，而在LT-RRT*算法中不仅拉直了到达xnew的路径，也拉直了到达 xnew的周围节点的路径。

图 2 RRT*算法父节点选择和路径重连Fig.2 RRT* algorithm′s parent node selection and path reconnection

图 3 LT-RRT*算法父节点选择和路径重连Fig.3 LT-RRT* algorithm′s parent node selection and path reconnection

2.5 基于线段定理的路径重连

本文提出的LT-RRT*算法规划无人艇避障路径的步骤如下：

1）运用栅格法对环境地图进行处理，以其他颜色标示出障碍物周围的危险区域，使无人艇距离障碍物一定距离，保证其行驶的安全性。

2）以p 概率（0 ≤ p ≤ 1）直接选择终点为采样点xsample，1-p 的概率随机选择环境空间中的点作为采样点 xsample。如果新节点 xnew距离终点固定步长，则找到路径，转到步骤6，否则进行步骤3。

3）在整个树T 上寻找距离采样点 xsample最近的节点，从节点向采样点扩展固定步长得到新节点。

4）以新节点 xnew为中心寻找树T 上距离其一定距离的周围几个节点 xnear1、 xnear2……。

5）删除周围节点的上一次连接，连接 xnearnest的父节 点和 周 围 节 点，连 接和 新 节 点。转入下一次迭代，进入步骤2。

6）找到路径，计算路径长度和搜索时间。

节点之间的每一次连接，都要进行避障检测，避开障碍物以及障碍物周围危险区域。

3 算法性能分析

在简单障碍物环境和复杂障碍物环境下，对比现有算法和LT-RRT*算法的避障规划效果。

3.1 简单环境

避开单个障碍物的水面环境是无人艇避障规划中比较简单的一种情况。图4 和图5 为某湖泊水面地图，地图大小为1 384 m*1 216 m，以地图左上角为原点，向右为x 轴正方向，向下为y 轴正方向，起始点为（600 m，600 m），终点为（720 m，260 m），无人艇起始点与终点的直线连接只有单个障碍物。为保证几种算法性能对比的有效性，本文考虑在相同环境地图上进行避障规划，且保证规划参数一致。

图 4 RRT 规划路径和RRT*规划路径Fig.4 RRT algorithm′s planning path result and RRT*algorithm′s planning path result

图 5 RRT-Connect 规划路径和LT-RRT*规划路径Fig.5 RRT-Connect algorithm′s planning path result and LT-RRT* algorithm′s planning path result

结合表1 列出的几种算法规划的避障路径结果，如图4（a），可以看出经典RRT 算法在整个环境空间进行采样，因而树节点遍布整个环境空间，规划时间较长，不能很好满足无人艇行驶的实时性要求，探索到的避障路径由多个树节点组成，路径比较曲折，路径长度较长。RRT*算法由于改进了新节点的连接方式，考虑了路径成本，缩短了规划时间和路径长度，但路径节点数没有大的改善。由图5（a）可知，双向搜索树算法RRT-Connect 由于从起始点和终点同时向对方探索避障路径，方向性较强，减少了对环境空间中无用区域的探索，大大节约了避障规划时间，但最终搜索到的避障路径仍然较长，节点数过多，路径较曲折，不利于无人艇的行驶。

表 1 简单环境Tab.1 Simple environment

图5（b）中LT-RRT*算法的规划时间短，且其规划的路径被拉直，大大缩短了路径长度。经过重新选取父节点的规划路径长度缩短为RRT 算法规划路径长度的64.3%，RRT*算法规划路径长度的71.4%，RRTConnect 算法规划路径长度的80.3%；路径节点数包括起始点终点只有3 个，分别比RRT 算法规划路径少28 个，比RRT*算法规划路径少25 个，比RRT-Connect 算法规划路径少22 个；同时与障碍物之间留有安全距离。因此可以说在简单环境下LT-RRT*算法表现优于其他算法。

3.2 复杂环境

如图6 和图7 所示，湖泊水面地图不变，坐标轴不变，起始点为（470 m，1 000 m），终点为（1 200 m，250 m）。无人艇起始点与终点的直线连接存在多个障碍物，在这样的复杂环境中，算法在探索新节点时增加了对避障检测程序CollisionFree 的调用次数，计算时间也增长。表2 列出了几种算法在该复杂环境下的避障规划结果。对比实验结果，LT-RRT*算法在时间上比RRT 算法和RRT*算法短，略长于RRT-Connect 算法，对全局避障规划的影响较小，但路径长度和节点数大大减小，同时与障碍物之间留有安全距离。LTRRT*算法规划的避障路径长度为1 175.6 m，分别缩短为RRT 算法规划路径长度的67.8%，RRT*算法规划路径长度的73.2%，RRT-Connect 规划路径长度的77.7%，路径节点数只有10 个，分别比RRT 算法规划路径减少77 个，比RRT*算法规划路径减少71 个，比RRT-Connect 规划路径减少67 个。上述结果表明在复杂环境下，LT-RRT*算法规划的避障路径效果优于其他算法。

图 6 RRT 规划路径和RRT*规划路径Fig.6 RRT algorithm′s planning path result and RRT*algorithm′s planning path result

图 7 RRT-Connect 规划路径和LT-RRT*规划路径Fig.7 RRT-Connect algorithm′s planning path result and LT-RRT* algorithm′s planning path result

表 2 复杂环境Tab.2 Complex environment

4 结 语

本文在RRT*算法的基础上提出一种改进的无人艇全局避障规划方法LT-RRT*（Line Segment Theorem-RRT*）。考虑无人艇航行的安全性，首先对环境地图预先进行处理，使算法最终生成的路径避开障碍物一定距离。其次根据终点采样概率选取随机采样点，改善了全局采样引起的路径不稳定性，提高了算法收敛速度。依据线段定理重新选取新节点和附近节点的父节点，降低了规划路径节点，减少了规划路径成本，使算法最终生成相对平滑的避障路径。后续考虑加入无人艇的动力学约束，并探索不同算法规划路径对后续轨迹跟踪的影响。