0 引　言

水下无人航行器（UUV）因其具有自主航行能力，可完成海洋/海底环境信息获取，固定/移动目标探测、识别、定位与跟踪以及区域警戒，具有重要的民用军用价值，已成为世界各国海洋装备的重要研究方向^[1]，而路径规划决定了UUV的自主航行能力，也是衡量UUV智能化水平最基础的技术之一^[2]。

路径规划的目标，是依据某种优化函数，寻找一条从起始点到目标点的无碰路径^[3]。对于已知全局障碍物信息的情况，图搜索算法^[4]，基于采样的搜索算法^[5]，样条插值规划算法^[6]，数值优化方法^[7]等已经可以较好地解决这一问题，然而对于水下无人航行器来说，获得完整的海图十分困难，这就使得以上方法在这一领域的使用受到了限制，同时许多传统的规划算法计算量较大，很难满足在未知环境中的进行路径规划的实时性要求^[8]。

在路径规划算法中，A*算法因其计算流简单，易于实现的特点，被广泛应用于各种低功耗系统中。传统的A*算法是一种基于图搜索的静态路径规划方法，其规划曲线容易出现折线段，不易于航行器的跟踪，同时每次发现障碍物之后若都进行一次重新规划，存在一定的等待时间，而UUV是没有办法停止当前运动的，容易造成碰撞事故。基于以上问题，采用圆搜索域的A*算法，结合UUV运动学约束选择待扩展节点，并实时利用探测空间的采样结果对各待扩展节点代价函数进行加权赋值，考虑到实时性要求引起的对探测空间采样的不完全，A*算法启发式函数引入“邻域代价”，将障碍物引起的代价从近到远传递，实现实时路径规划与规避障碍的功能。

1 UUV实时路径规划问题描述

带有路径规划的UUV基本总体流程如图1所示。

其中路径规划层位于总体流程的核心部分，特别对于未知环境中实时路径规划来说，需要不断调用障碍探测信息来进行规划决策，同时需要考虑到运动控制约束，这使得路径规划不是一个单一封闭的问题，而如何利用障碍探测信息与控制约束来进行实时的路径规划解算，便是实时路径规划中的难点与核心，也是未来路径规划问题的研究方向。

2 基于探测空间采样的改进A*算法

首先建立算法所用到的基本工具模型、约束，包括搜索域模型、探测空间模型与运动学约束。

2.1 搜索域模型

传统的A*算法基于栅格化的地图，其搜索空间被约束在以当前位置为中心的8个方格中，如图2所示。

为了实现变网格搜索策略，文献[9]提出可以采用圆形域来替代栅格，采用圆形域后的待扩展点如图3所示，可以看到能够轻松对圆域进行不同程度的细分。

实际上对于UUV来说，以上模型还可以继续简化，在一个搜索步长中，UUV所能改变的最大航向角 $\Delta {\psi _{\max }}$ 受最大角速度约束，是一个比较小的值，所以没有必要在360°范围内搜索，只需基于航向角，在最大转向角内进行均匀分割搜索即可，如图4所示。

2.2 探测空间模型

为了简化分析，将航行器视为质点，将探测空间假设为扇形，探测角度为 $\gamma $ ，探测距离为 ${l_d}$ ，如图5所示。

2.3 运动学约束

$ q < {q_{\max }} ，$

(1)

式中： $q$ 为航行器航向角速度， ${q_{\max }}$ 为最大航向角速度，可由控制仿真或总体计算获得。

2.4 基于探测空间采样的改进A*算法

基于探测空间采样的改进A*算法的流程图如图6所示，基本步骤如下：

步骤 1　根据航行器最大航向角速度 ${q_{\max }}$ 计算一个步长下最大航向角改变量 $\Delta {\psi _{\max }}$ 。

步骤2　在 $\Delta {\psi _{\max }}$ 的约束下将搜索域分割为i份，共i+1个待扩展点（见图4），并计算待扩展点坐标 $\left( {{x_t},{y_t}} \right)$ 。其中，不改变航向的待扩展点下标为1，逆时针转向的待扩展点下标由近到远为 $2, \cdots ,p$ ，顺时针转向的待扩展点下标为 $p+1,\cdots ,i+1$ 。

步骤3　将探测空间也均匀分为i份，并对每一条分割曲线取采样长度 $d$ 进行离散采样，得到采样离散点坐标 $\left( {{x_{kj}},{y_{kj}}} \right)$ ，即代表第k条分割线上第j个点。

步骤4　判断第k条分割线上的采样点 $\left( {{x_{kj}},{y_{kj}}} \right)$ 是否存在障碍，存在返回1，不存在返回0，并对第k条分割线的障碍返回值进行求和，得到 $ob{s_k}$ 。

步骤5　将逆时针旋转的分割线与直航分割线的障碍物信息求和，得 $ob{s_{at}}$ ，将顺时针旋转的分割线与直航分割线的障碍物信息求和，得 $ob{s_{lo}}$ ，构造启发式代价函数

$ f(t) = \sqrt {{{({x_t} - {x_d})}^2} + {{({y_t} - {y_d})}^2}} + {c_1}ob{s_t} + g(t) ，$

(2)

$ g(t) = \left\{ {\begin{aligned} & {c_2}ob{s_{lo}} + {c_3}ob{s_{at}}，&t = 1，\;\;\quad \\ &\dfrac{{{c_2}}}{t}ob{s_{lo}} + {c_4}ob{s_{at}}，&p \geqslant t > 1，\;\;\;\\ & {c_4}ob{s_{lo}} + \dfrac{{{c_2}}}{t}ob{s_{at}}，&i + 1 \geqslant t > p。\end{aligned}} \right. $

(3)

式中： $\left( {{x_d},{y_d}} \right)$ 为目标点， ${c_1}\sim{c_4}$ 为常数。

在启发式代价函数中， $\sqrt {{{({x_t} - {x_d})}^2} + {{({y_t} - {y_d})}^2}} $ 代表距离代价， ${c_1}ob{s_t}$ 是直接障碍物信息， $g(t)$ 是间接障碍物信息，也可看做“邻域代价”，即将探测到的某一方向上的障碍物代价由近到远依次传递。由于采用采样的方式进行障碍物信息提取，受采样不完全的影响，如果不引入 $g(t)$ ，航行器容易在障碍物的边缘行驶，甚至是在障碍物内部沿着边缘行驶，这存在一定的风险。

步骤6　对所有待扩展点进行代价计算，寻找到待扩展点中代价最小的点进行扩展，并转入至步骤2进行循环。

探测区域的分割与待扩展点的分割份数这里假设是相同的，这是为了方便进行直接障碍物信息提取，实现了一个一一对应的关系，实际上这里也可以取二者不等，但如何将障碍信息投影到待扩展点的权值就需要进一步讨论。

3 仿真验证

取 $\left( {{x_d},{y_d}} \right) = (50,1\;000)$ ，初始位置 $\left( {{x_0},{y_0}} \right) = (0,0)$ ，初始航向角为180°，规划步长为2 m， ${q_{\max }} = {8^ \circ }/s,\Delta {\psi _{\max }} = $ $ {1.6^ \circ }$ ，探测空间采样步长为1 m，探测距离50 m，探测角度为60°，分割份数为8，即一次扩展计算9个点， $i + 1 = 9$ ，启发式函数中， ${c_1} = 90\;000,{c_2} = {c_3} = 500,{c_4} = $ $ 3\;000$ ，其规划结果如图7所示，航向角速度如图8所示。

从图7可以看到，该方法可以很好地规划一条从起点到终点的无碰路径，整个路径规划不需要预知任何海图信息，采用“感知—决策”的反应式规划策略。同时采用最大角速度作为约束，使得该规划路径较为平稳，避免了出现折线段的问题，同时所有规划点的航向角速度均符合运动学约束 $q < {q_{\max }}$ 。

由于该方法是实时对探测空间进行采样，判断采样点是否存在障碍物来进行路径规划的，单步规划时间必须纳入考虑，若单步规划时间过长，下层运动控制收不到指令信息，容易引发碰撞事故。在Matlab2010b,CPU为Intel core i5-7500的仿真环境下规划所需时间如表1所示。

可以看到，在该仿真环境下，单步规划用时平均只有0.0088 s，耗时很短，符合实时性要求。

4 结　语

针对水下无人航行器路径规划需要提前获得海图以得到全局障碍物信息，难以做到未知环境下实时规划路径的问题，本文首先将航行器的运动学约束引入A*算法，令搜索域为圆域并且仅搜索在运动学约束下可达的区域，使得规划路径较为平滑且符合航行器运动学约束。在此基础上提出了一种对探测空间进行采样，并将采样点的障碍结果加权赋值给A*算法待扩展点的方法，最终实现了一种在未知环境下满足航行器探测范围与运动学约束的反应式动态路径规划策略，通过该方法可有效提升UUV智能规划决策能力。