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 \subset {R^n}$ 表示结构空间，其中 $n$ 表示结构空间的维数。结构空间可进一步被划分为障碍物空间 ${C_{obs}} \subset C$ 和非障碍物空间 ${C_{free}} = C\backslash {C_{obs}}$ 。让 $T = (V,E)$ 表示随机生长树，其中 $V$ 表示节点， $E$ 表示连接点的边。让 ${q_{init}}$ 和 ${q_{goal}}$ 表示初始状态和目标状态。让 $p({q_i},{q_j})$ 表示连接任意2个状态 ${q_i} \in {C_{free}}$ 和 ${q_{\rm{j}}} \in {C_{free}}$ 的路径。

问题1（可行路径）：在非障碍物区域内找到一条从 ${q_{init}}$ 到 ${q_{goal}}$ 的路径。

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

1.2 RRT算法

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

算法1： $RRT({q_{init}})$

1 $V \leftarrow \{ {q_{init}}\} $ ； $E \leftarrow \varnothing $ ； $i \leftarrow 0$ ；

2 while $i < N$ do

3 　 $T \leftarrow (V,E);$

4 　 ${q_{rand}} \leftarrow Sample(i);i \leftarrow i + 1;$

5 　 $(V,E) \leftarrow Extend(T,{q_{rand}});$

6 return $T = (V,E)$

算法2： $Extend(T,{q_{rand}})$

1 ${q_{nearest}} \leftarrow Nearest(T,{q_{rand}});$

2 ${q_{new}} \leftarrow Steer({q_{nearest}},{q_{rand}});$

3 if $ObstacleFree(p({q_{nearest}},{q_{new}}))$ then

4 　 $V \leftarrow V \cup \{ {q_{new}}\} ;$

5 　 $E \leftarrow E \cup \{ p({q_{nearest}},{q_{new}})\} ;$

6 return $T = (V,E)$

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

$Sample(i)$ ：这个程序返回非障碍物区域内一个独立的均匀分配的随机点 ${q_{rand}} \in {C_{free}}$ 。

$Nearest(T,{q_{rand}})$ ：这个程序返回一个在树中离 ${q_{rand}}$ 欧式距离最近的点 ${q_{nearest}}$

$Steer({q_{nearest}},{q_{rand}})$ ：这程序返回一个新的点 ${q_{new}}$ 使 $d({q_{new}},{q_{rand}})$ 最小，并满足条件 $d({q_{new}},{q_{nearest}}) < \eta $ ， $\eta $ 是我们设定的一个值。

$ObstacleFree(p({q_{nearest}},{q_{new}}))$ 这程序检查路径 $p({q_{nearest}},{q_{new}})$ 是否属于非障碍物区域 ${C_{free}}$ 。如果路径 $p({q_{nearest}},{q_{new}})$ 属于 ${C_{free}}$ ，则会返回真值，否则返回假值。

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

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

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

2.1 RRT*算法

在RRT算法内容基础上扩展定义如下：1） $d({q_{\rm{i}}},{q_j})$ 表示任意2个状态 ${q_{\rm{i}}} \in C$ 和 ${q_{\rm{j}}} \in C$ 之间的正欧式距离；2） ${\phi _{q,r}}: = \{ {q_{\rm{i}}} \in C:d(q,{q_i}) \leqslant r\} $ 表示中心点在 $q$ ，半径为 $r \in \mathbb{R},r > 0$ 的闭环球形区域，其中 $q \in C$ 可以是任意状态；3） $c({q_{\rm{i}}},{q_j})$ 表示从状态 ${q_{\rm{i}}} \in {C_{free}}$ 到状态 ${q_j} \in {C_{free}}$ 的路径代价。

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

算法3：RRT* $Extend(T,{q_{rand}})$

1 ${q_{nearest}} \leftarrow Nearest(T,{q_{rand}});$

2 ${q_{new}} \leftarrow Steer({q_{nearest}},{q_{rand}});$

3 ${C_{near}} \leftarrow NearVertices(T,{q_{new}},r)$

4 if ${C_{near}} =\varnothing$ then

5 　 ${C_{near}} \leftarrow {q_{nearest}}$

6 　　 ${L_s} \leftarrow Getsortedlist({q_{new}},{C_{near}});$

7 　　 ${q_{\min }} \leftarrow ChooseBestParent({L_s});$

8 if ${q_{\min }} \ne \varnothing $ then

9 　 $T \leftarrow InsertVertex({q_{new}},{q_{\min }},T)$

10 　 $T \leftarrow \operatorname{Re} wireVertices({q_{new}},{L_s},E)$

11 return $T = (V,E)$

算法4： $Getsortedlist({q_{new}},{C_{near}})$

1 ${L_s} \leftarrow \varnothing $ ;

2 for ${q'} \in {C_{near}}$ do

3 　 ${C'} \leftarrow c({q_{init}},{q'}) + c({q'},{q_{new}})$

4 　 ${L_s} \leftarrow ({q'},{C'},p({q'},{q_{new}}));$

5 ${L_s} \leftarrow SortList({L_s});$

6 return ${L_s}$

算法5： $ChooseBestParent({L_s})$

1 for $({q'},{C'},p({q'},{q_{new}})) \in {L_s}$ do

2 　 if $ObstacleFree(p({q'},{q_{new}}))$ then

3 　　 return ${q'}$

4 return $\varnothing$

算法6： $InsertVertex({q_{new}},{q_{\min }},T)$

1 $V \leftarrow V \cup \{ {q_{new}}\} $

2 $E \leftarrow E \cup \{ p({q_{\min }},{q_{new}})\} $

3 $c({q_{init}},{q_{new}}) = c({q_{init}},{q_{\min }}) + c({q_{\min }},{q_{new}})$

4 return $T = (V,E)$

算法7： $RewireVertices({q_{new}},{L_s},E)$

1 for $({q'},{C'},p({q'},{q_{new}})) \in {L_s}$ do

2 　 if $(c({q_{init}},{q_{new}}) + c({q_{new}},{q'})) < c({q_{init}},{q'})$ then

3 　　 if $ObstacleFree(p({q_{new}},{q'}))$ then

4 　　　 ${q_{parent}} \leftarrow Parent(E,{q'})$

5 　　　 $E \leftarrow (E\backslash \{ ({q_{parent}},{q'})\} ) \cup (\{ {q_{new}},{q'}\} )$

6 　　　 $c({q_{init}},{q'}) = c({q_{init}},{q_{new}}) + c({q_{new}},{q'})$

7 return $E$

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

$NearVertices(T,{q_{new}},r):=\{ q \in V:q \in {\phi _{{q_{new}},r}}\}\to$ $ {C_{near}} \subseteq V $ ：更精确一点， $ {C_{near}} = \{ q \in V:d(q,{q_{new}})$ $ \leqslant \gamma {(\log i/i)^{1/n}}\} $ ，其中 $i$ 是节点数量， $n$ 是结构空间的维数， $\gamma $ 是个常数。

$Getsortedlist({q_{new}},{C_{near}})$ ：这个程序建立了一个列表 ${L_s}$ ，其中每一个元素都由 $({q'},{C'},p({q'},{q_{new}}))$ 组成，并使列表 ${L_s}$ 按照代价 $\{ c({q_{init}},{q'}) + c({q'},{q_{new}})\} $ 的升序顺序排序。

$ChooseBestParent({L_s})$ ：这程序被用来搜索列表 ${L_s}$ 的一个状态 ${q_{\min }} \in {L_s}$ ，它能提供一条从 ${q_{init}}$ 到 ${q_{new}}$ 经过 ${q_{\min }}$ 的代价最低并无碰撞的路径。

$InsertVertex({q_{new}},{q_{\min }},T)$ ：这个程序将 ${q_{new}}$ 点插入到了树上形成一个新的节点。

$RewireVertices({q_{new}},{L_s},E)$ ：这个程序被用来更新 ${q'} \in {C_{near}}$ 的父节点如果一些明确的条件被满足的话。

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

2.2 路径距离代价

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

${\rm{d}}({q_{\rm{i}}},{q_j}){\rm{ = }}{\left\| {{q_i} - {q_j}} \right\|_2} \text{。}$

(1)

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

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

$ {{D}} = - \left[ {\begin{array}{*{20}{c}} {{X_u}}&0&0 \\ 0&{{Y_v}}&{{Y_r}} \\ 0&{{N_v}}&{{N_r}} \end{array}} \right] \text{。} $

(2)

式中， ${X_{\left\{ . \right\}}}$ ， ${Y_{\left\{ . \right\}}}$ 和 ${N_{\left\{ . \right\}}}$ 为经典的水动力系数； $u$ 和 $v$ 分别为水下机器人的纵向速度和横向速度； $r$ 为水下机器人的角速度。

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

如图4所示，当存在一个缓变的速度为 ${{{v}}_c}$ ，流向为 $\beta $ 的2D无旋洋流 $({r_{\rm{c}}}{\rm{ = }}0)$ 时， ${u_c}$ 和 ${v_c}$ 分别为洋流投影到水下机器人纵向和横向的速度，可表示为：

$\begin{align} {u_c} = {{{v}}_c} \cdot \cos (\beta - \varphi ) {\text{，}}\\ {v_c} = {{{v}}_c} \cdot \sin (\beta - \varphi ) {\text{，}} \\ \end{align} $

(3)

其中 $\varphi $ 是水下机器人的首向角。

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

$ \begin{align} {{\bf{\tau }}_{{D}}} =& - {{D}} \cdot {{{v}}_r}= \\ &\left[ {\begin{array}{*{20}{c}} {{X_u} \cdot \left( {u - {{{v}}_c} \cdot \cos (\beta - \varphi )} \right)} \\ {{Y_v} \cdot \left( {v - {{{v}}_c} \cdot \sin (\beta - \varphi )} \right) + {Y_r} \cdot r} \\ {{N_v} \cdot \left( {v - {{{v}}_c} \cdot \sin (\beta - \varphi )} \right) + {N_r} \cdot r} \end{array}} \right]{\text{。}} \end{align} $

(4)

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

$\begin{split} e({q_i},{q_j}) = &\left( {{X_u} \cdot \left( {u - {{\bf{v}}_c} \cdot \cos (\beta - \varphi )} \right)} \right) \times {\left\| {{q_i} - {q_j}} \right\|_2}+ \\ & \left( {{N_v} \cdot \left( {v - {{\bf{v}}_c} \cdot \sin (\beta - \varphi )} \right) + {N_r} \cdot r} \right) \times \left( {{\varphi _2} - {\varphi _1}} \right){\text{。}}\\[-10pt] \end{split} $

(5)

式中： ${\varphi _2}$ 为水下机器人在路径 $p({q_i},{q_j})$ 上的首向角； ${\varphi _1}$ 为水下机器人在路径 $p({q_{parent}},{q_i})$ 上的首向角； ${q_{parent}}$ 为 ${q_i}$ 的父节点。

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

$ c({q_i},{q_j}) = \alpha \times e({q_i},{q_j}){\rm{ + }}\tilde \alpha \times {\rm{d}}({q_{\rm{i}}},{q_j}){\text{。}} $

(6)

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

3 仿真 3.1 RRT*算法和RRT算法的仿真对比

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

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

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

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

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

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

可以看出，左边路径的长度要比右边路径短。然而当洋流速度越大，左边路径所需要的能耗就越大，而右边路径所需要的能耗则越小。当不存在洋流影响时，左边路径的能耗要低于右边路径，故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。

4.2 实验方案

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

4.3 实验结果

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

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

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

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

5 结　语

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