基于RRT＊算法的水下机器人全局路径规划方法

丁 帅，陈苗苗，王 猛，谷志珉，任 峰

(1. 上海交通大学 水下工程研究所，上海 200240；2. 国家海洋局 北海海洋技术保障中心，青岛 266033)

0 引 言

随着海洋资源的开发与利用，自主水下机器人作用愈发突出，而精细化作业的需求对潜器智能控制及自主导航的要求也越来越高。路径规划作为关键技术之一，直接影响到潜器的性能。潜器的路径规划具体可描述为：在具有障碍物的水下环境中，依据预设的评价标准，规划出一条从起点到终点最优或准优的无碰撞路径。

Dijkstra 算法是最早被发明出来的规划搜索算法，它主要利用贪心思想在图里寻找最短路径；1968 年，Hart 在Dijkstra 算法的基础上加入了启发式函数而创造出了A*算法，加快了搜索速度[1]；Wang Z 在2017 年成功的将A*算法应用在水下机器人全局路径规划上[2]。Fast Marching（FM）算法用到了非线性程函方程的一阶数值近似，它可被认为是Dijkstra 算法的连续版本；Yu H 在2015 年实现了FM 算法在水下机器人路径规划上的应用[3]。人工势场法是一种虚拟力法，它是假设目标点对机器人存在吸引力，障碍物对机器人存在斥力，由此建立一个虚拟的势场，并让机器人沿着势场下降最快的方向前进直到目标点；然而该算法存在局部最小值问题[4]，杨建在2017 年成功的将人工势场法应用到了水下机器人避障运动上[5]。继而有学者提出进化算法，例如粒子群算法，蚁群算法和遗传算法，这些算法在智能潜器中也得到了相关的应用[6-10]。PRM 算法和RRT 算法则是在许可范围内，通过牺牲最优、随机采样实现快速可行的路径规划算法。PRM 算法的基本思想是在空间里随机撒点并连线形成一个图，然后在该图上运行A*等搜索算法来寻找路径。RRT 算法则是以起始点作为一棵树的根节点，通过不断的随机扩展这棵树上的节点，直到扩展的节点接近目标点，则认为在这棵树上找到了一条唯一的路径到达目标点，该算法的优点是计算速度较快，且可以在规划过程中加入动力学约束。但也有明显的缺点，找到的路径不一定最优。Yu L 在2017 年成功地将RRT 算法应用到水下机器人的路径规划里[11]。2010 年，S.Karaman 通过引入代价函数来优化RRT 算法，经过逐次迭代来改善之前的路径，优化后的算法被定义为RRT*算法[12]。RRT*算法不仅继承了RRT 算法的优点，还克服了RRT 算法的缺点，最终会得到一条最优或准优的路径，这额外的最优性使得RRT*算法在高维和实时工况下能快速有效的实现规划。RRT*算法在水下机器人路径规划中的应用鲜有实现。

实际应用中海洋环境比较复杂，有时还会存在洋流因素，洋流的速度和方向都会对水下机器人的能量消耗产生影响。因此在路径规划时，除了要考虑路径长度、障碍物信息外，还应考虑能耗问题。本文根据RRT*算法的优点，考虑洋流对水下机器人能耗的影响，设计实现基于RRT*的路径最短和能耗最低的路径规划算法，并通过相应的仿真模拟和实验室平台的真机实现验证了该算法的可行性。

1 RRT 算法

1.1 问题定义

让 C ⊂Rn表示结构空间，其中 n表示结构空间的维数。 结构空间可进一步被划分为障碍物空间Cobs⊂C 和非障碍 物空间Cfree=CCobs。让T =(V,E)表示随机生长树，其中 V表示节点， E表示连接点的边。让qinit和 qgoal表示初始状态和目标状态。让p(qi,qj)表示连接任意2 个状态qi∈Cfree和qj∈Cfree的路径。

问题1（可行路径）：在非障碍物区域内找到一条从qinit到 qgoal的路径。

问题2（最优路径）：在非障碍物区域内找到一条从qinit到 qgoal的最优路径，使路径代价花费最小。

1.2 RRT 算法

RRT 算法是一种基于概率采样的搜索方法，图1为RRT 算法程序流程图，具体实现如算法1 所示。图2为随机树的关键扩展过程（Extend 函数）程序流程图，具体实现如算法2 所示。

图1 RRT 算法程序流程图Fig. 1 Program flow diagram of RRT algorithm

图2 扩展过程（Extend 函数）程序流程图Fig. 2 Program flow diagram of Extend function

RRT(qinit)

算法1：

算法2：Extend(T,qrand)

其中，各函数含义及作用如下：

S ample(i)：这个程序返回非障碍物区域内一个独立的均匀分配的随机点 qrand∈Cfree。

Nearest(T,qrand)：这个程序返回一个在树中离qrand欧式距离最近的点qnearest

S teer(qnearest,qrand)： 这程序返回一个新的点qnew使 d(qnew,qrand)最 小，并满足条件 d(qnew,qnearest)＜η，η是我们设定的一个值。

ObstacleFree(p(qnearest,qnew))这程序检查路径p(qnearest,qnew) 是否属于非障碍物区域Cfree。如果路径p(qnearest,qnew) 属于Cfree，则会返回真值，否则返回假值。

从RRT 算法过程中可以看出随机树的扩展偏向于未被访问过的区域。这使得随机树在刚开始时扩展得很快，并能完全覆盖结构空间。随机树上的节点在结构空间里是均匀的。如果可行路径存在，那在节点数量足够的前提下，RRT 算法就一定能找到一条从起始点到目标点的可行路径。通过上述分析可知，RRT 算法所找到的可行路径不一定最优。

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

针对RRT 算法存在的问题，S.Karaman 提出通过引入代价函数来优化，经过逐次迭代来改善之前的路径，从而得到一条最优或准优的路径。

2.1 RRT＊算法

在R R T 算法内容基础上扩展定义如下：1）d(qi,qj) 表示任意2 个状态 qi∈C 和 qj∈C之间的正欧式距离；2） φq,r:={qi∈C:d(q,qi)≤r} 表 示 中 心 点 在 q，半径为 r ∈R,r ＞0 的闭环球形区域，其中 q ∈C可以是任 意 状 态；3） c(qi,qj) 表 示 从 状 态 qi∈Cfree到 状 态qj ∈Cfree的路径代价。

RRT*算法的初始化过程同算法1 中一致，而不同的是RRT*算法在它的扩展过程中引入了代价函数，并通过代价函数来判断是否会更新之前的路径，以此来改善路径。图3 则为RRT*算法中扩展过程（Extend 函数）程序流程图，具体实现如算法3 所示，接下来的则是一些在扩展过程中被调用的函数。

图3 扩展过程（Extend 函数）程序流程图Fig. 3 Program flow diagram of Extend function

算法3：RRT* Extend(T,qrand)

算法4：Getsortedlist(qnew,Cnear)

算法6：InsertVertex(qnew,qmin,T)

其中，各函数含义及作用如下：Cnear ⊆V：更精确一点，Cnear={q ∈V:d(q,qnew)≤γ(logi/i)1/n}，其中i是节点数量， n是结构空间的维数， γ是个常数。

Getsortedlist(qnew,Cnear)：这个程序建立了一个列表 Ls，其中每一个元素都由(q′,C′,p(q′,qnew))组成，并使列表Ls按照代价{c(qinit,q′)+c(q′,qnew)}的升序顺序排序。

ChooseBestParent(Ls)：这程序被用来搜索列表Ls的一个状态qmin∈Ls，它能提供一条从qinit到qnew经过 qmin的代价最低并无碰撞的路径。

InsertVertex(qnew,qmin,T)：这个程序将qnew点插入到了树上形成一个新的节点。

RewireVertices(qnew,Ls,E)：这个程序被用来更新q′∈Cnear的父节点如果一些明确的条件被满足的话。

如同算法1 的开始一样，在初始化后RRT*算法开始它的迭代过程。先通过从非障碍物空间里随机采样qrand状态，然后进入扩展过程。在扩展过程里，qnearest最先通过 Nearest(T,qrand) 程序被获得，然后 qnew通过S teer(qnearest,qrand)程序被产生。之后，通过NearVertices(T,qnew,r)程序发现qnew点在树上的邻近点集合Cnear。如果Cnear集合为空，那么Cnear将会被赋值qnearest。集合 Cnear被用在G etsortedlist(qnew,Cnear)程序里来生成一个元素为(q′,C′,p(q′,qnew))并按代价{c(qinit,q′)+c(q′,qnew)} 升序排序的列表 Ls。然后这列表 Ls被用在ChooseBestParent(Ls) 程序里来返回一个状态qmin∈Ls使{c(qinit,q′)+c(q′,qnew)}最小并提供一个从 q′到qnew的无碰撞路径。如果qmin不为空，qmin将会成为qnew的父节点，而且 qnew点 则通过 InsertVertex(qnew,qmin,T)程序被插入到这棵树中。然后执行RewireVertices(qnew,Ls,E)改写程序，这程序将会检查每一个节点 q′∈Cnear。如果 现存的从 qinit到q′的路径的代价多于从 qinit到 qnew再到q′的路径的代价且路径p(qnew,q′)属于非障碍物区域，那么 qnew将 会成为 q′的父节点。如果这些条件并不被满足，那么这棵树不会发生变化，程序则会继续去检查下一个在Cnear中的节点。

2.2 路径距离代价

本文只考虑欧式空间，两点间的路径距离代价则为两点间的正欧式距离，可表示为：

2.3 考虑洋流因素的水下机器人路径能耗代价

基于Fossen 模型[13]，对于左右对称的低速水下机器人来说，非线性阻尼矩阵可以被忽略，而忽略垂荡、横摇和纵摇后的线性化的阻尼矩阵可被写成：

式中，X{.}，Y{.}和N{.}为经典的水动力系数；u和v分别为水下机器人的纵向速度和横向速度；r为水下机器人的角速度。

在真实的海洋环境中，有时还会存在洋流因素，而洋流的速度和方向都会对水下机器人路径上的能量消耗产生影响。

图4 水下机器人体坐标系下2D 无旋洋流速度分量示意Fig. 4 The velocity component of 2D irrotational ocean current in ROV body coordinate system

如图4 所示，当存在一个缓变的速度为 vc，流向为 β的2D 无旋洋流 (rc=0) 时， uc和 vc分别为洋流投影到水下机器人纵向和横向的速度，可表示为：

其中 φ是水下机器人的首向角。

设vr=v-vc是 相对速度，此时在2D 无旋洋流环境下水下机器人所受到的阻尼力和阻尼力矩[13]则为：

假设在水下机器人路径跟踪里会使水下机器人的艏向永远与规划出的路径方向保持一致，并保持匀速行驶，则水下机器人横向上没有位移，可认为水下机器人横向上的阻力不做功，则水下机器人在路径上的能量消耗只需要考虑水下机器人在纵向上所受到的阻力做功和原地旋转时受到的阻力矩做功。则从 qi点到与它直接相连的 qj点的路径能耗代价可表示为：

式中：φ2为水下机器人在路径p(qi,qj)上的首向角；φ1为水下机器人在路径p(qparent,qi)上的首向角；qparent为qi的父节点。

若将式（1）与式（5）结合到一起，则可得到总的路径代价函数表示为：

当 α=1时，则式（6）变成水下机器人在2D 无旋洋流下的能耗最低路径规划算法中的代价函数。当α=0时，则式（6）变成水下机器人路径最短路径规划算法中的代价函数。

3 仿真

3.1 RRT＊算法和RRT 算法的仿真对比

基于前文理论分析及算法设计，利用Matlab 编程实现RRT 算法和RRT*算法（路径最短），并进行仿真对比，仿真结果如图5 和图6 所示。

图中矩形区域（黑色）代表障碍物，Starting Point 代表起始点，圆形区域则代表目标区域，粗实线则为所寻找到的路径，将两图中的路径长度通过计算得到表1。

从仿真结果的对比可以看出，RRT 算法能找到一条可行路径，但不一定是最优或准优路径；RRT*算法因代价函数的引入，找到的路径不仅是可行路径，相对RRT 算法所得路径更优，可视为准优路径。

图5 RRT 算法仿真结果Fig. 5 Simulation results of RRT algorithm

图6 RRT*算法仿真结果Fig. 6 Simulation results of RRT* algorithm

表1 两种算法路径长度对比Tab. 1 Comparison of path length between two algorithms

3.2 2D 无旋洋流环境中路径最短与能耗最低路径规划的仿真模拟

假设流场环境为2D 无旋洋流，流速为 vc，流向β；仿真计算时设定水下机器人艏向会保持与规划出的路径方向一致，并保持匀速行驶；仿真水动力参数则根据实验室现有的水下机器人（ROV）以往的经验值设定,设置仿真条件如表2 所示，仿真结果如图7 所示。

从图7 中可以看出，当无洋流因素影响时，ROV 能耗最低路径规划得到的路径（b）与ROV 路径最短路径规划得到的路径（a）一致，为左边路径。而当存在洋流时，ROV 能耗最低路径规划得到的路径（c）和（d）则始终为右边路径。将这左、右2 条路径在上述3 种情况下的能耗和长度通过计算并整理得到表3。

表2 仿真参数表Tab. 2 Simulation parameter table

图7 ROV 两种路径规划的仿真结果Fig. 7 Simulation results of two path planning of ROV

表3 两条路径的能耗和长度对比表Tab. 3 Comparison of energy consumption and length of two paths

可以看出，左边路径的长度要比右边路径短。然而当洋流速度越大，左边路径所需要的能耗就越大，而右边路径所需要的能耗则越小。当不存在洋流影响时，左边路径的能耗要低于右边路径，故ROV 能耗最低路径规划得到的路径是左边路径；当洋流速度为1 kn 或2 kn 时，左边路径的能耗要高于右边路径，故ROV 能耗最低路径规划得到的路径是右边路径。该仿真结果验证了ROV 路径最短和能耗最低路径规划算法可行有效。

4 RRT*算法的实验室实现

4.1 实验条件

本次实验是在上海交通大学拖曳水池进行的，实验水池水深7.5 m，长300 m，宽16 m。实验对象则是采用实验室现有的水下机器人（ER3K，见图8），该水下机器人的基本性能参数见表4。地形环境则通过搭载在水下机器人本体上的声呐（Super SeaKing DST，见图9）获取，声呐性能参数见表5。

图8 水下机器人Fig. 8 Remote operated vehicle

4.2 实验方案

在本次实验中，采用2 个油桶来充当障碍物，然后利用搭载在水下机器人上的实验声呐去扫描检测获取环境里的障碍物信息，最后再利用基于RRT*算法的路径最短和能耗最低路径规划算法得到准优路径。图10 为本次实验示意图，图11 为本次实验现场图。

表4 水下机器人基本性能Tab. 4 Basic performance of remote operated vehicle

图9 实验声呐Fig. 9 Experimental sonar

表5 Super SeaKing DST 声呐基本属性Tab. 5 Basic performance of sonar

4.3 实验结果

利用水下机器人上所搭载的实验声呐进行扫描检测，获得的声呐图像如图12 所示。

图12 中，可看出图中深色区域为障碍物或杂波。在实际情况中，水下机器人并无法分辨出深色区域是障碍物还是杂波，所以此处将深色区域全部视为障碍物以此来规划路径。将声呐图像进行图像处理后得到如图13 所示。

图10 实验示意图Fig. 10 Experimental schematic diagram

图11 实验现场Fig. 11 Experimental scene

图12 声呐图像Fig. 12 Sonar image

图13 图像处理后的地图Fig. 13 Map after image processing

图中黑色区域被认为是障碍物，白色区域则被认为是可通行区域。进一步为了安全考虑，此处通过将图13 中的障碍物扩大来保证水下机器人通行的安全距离，得到的最终地图如图14 所示。受实验条件所限，实验环境为无流水池，则使水下机器人在图14 上分别实施路径最短和能耗最低（无洋流情况下）路径规划算法，规划出的路径结果如图15 所示。

从图15 可看出，水下机器人在该环境下实施路径最短路径规划和能耗最低路径规划得出的路径完全一致。该水池实验路径规划结果表明了所设计的基于RRT*的路径最短和能耗最低的路径规划算法可行有效。

图14 扩大障碍物后的地图Fig. 14 Map after enlarging the obstacle

图15 水下机器人路径规划结果Fig. 15 Path planning results of underwater vehicle

5 结 语

本文针对复杂海洋环境中水下机器人路径规划问题展开研究，综合考虑规划路径长度与全程能耗，设计出基于RRT*的路径最短和能耗最低的路径规划算法，继而进行仿真模拟和实验室现有水下机器人平台的真机实现，仿真及真机验证结果显示所设计的路径规划算法可行有效，为后期海上应用奠定基础。