0 引　言

随着科学技术的发展，AUV在海洋资源开发、环境质量监测、水下作战等领域发挥着越来越重要的作用。面对复杂多变的水域环境，三维路径规划是保证AUV安全完成任务的关键技术之一。全局路径规划和局部路径规划是复杂多变环境下的AUV根据障碍物类型进行的分类，由于全局路径规划主要针对环境中静止不变且信息已知的障碍物，能够更好地提供路径规划的解决方案，也因为如此近年来，AUV三维空间下的全局路径规划成为大家研究的热点之一。目前，人工势场法、A*算法、神经网络算法、遗传算法、人工免疫算法、模糊推理算法、粒子群算法等是最常用的路径规划算法。

本研究在蚁群算法的基础上对该算法进行优化，融合了粒子群算法和人工势场法，并在多目标环境路径规划中增加了路径代价的函数，使在融合算法下得到最优路径长度基础上充分考虑外界环境及探测干扰和自身能源的消耗问题。

1 路径规划相关模型建立 1.1 环境模型建立

环境模型是AUV路径规划的首要环节，将三维现实环境特征进行提取和解析，并转化为计算机模拟路径规划过程中的地图模型数据。环境建模的方法很多，如可视图法、自由空间法、栅格法等。由于栅格法的地图创建和维护比较容易，主要内容是以栅格为基础单位，离散化三维环境模型，所以本研究采用栅格法。设置三维空间为：水平面与XY平面重合，AUV的运动主方向定位X轴且机身头为X轴的正方向（起点与终点分别对应对应X轴的最大值和最小值），垂直于水平面方向定位Z轴。这样三维空间就可以分为若干基本单位栅格，以AUV开始运动的点作为空间的零点位置，其他障碍物及空位区域也就进行了相应的方位坐标确定。栅格^[1]在计算时可选择使用坐标法或是序号法，本研究为了直观地判断点与点的位置关系将采用坐标法。

在路径规划过程中对环境模型建立的要求：

1）将AUV的中心位置用质点表示，同时对障碍物的尺寸按AUV的半径作适当扩展，以保证AUV能够无碰撞地运动；

2）AUV每次运动只能在所有栅格的中心位置（也就是栅格体的中心），不可在边界或是端点处；

3）AUV的下一个位置只能是相邻的26个栅格体的自由栅格体中；

4）为避免没有必要的局部最优，某一个栅格体的上下左右前后有5个是障碍栅格，则此栅格体就默认为障碍栅格体；

5）不考虑环境中海流、潮流等对路径规划的干扰。

1.2 干扰模型建立 1.2.1 障碍模型

AUV在障碍环境下进行路径规划，可使用人工势场法计算AUV与障碍间的障碍代价，对AUV的运动路径进行调整。AUV在运动过程中，始终远离障碍代价函数值大的点运动，以实现避障。在此过程中还需要不断朝目标点运动，因此建立目标代价函数，越趋近于目标点则目标代价函数值越小，AUV会始终寻找趋近目标代价函数值小的点运动，以实现趋向目标点的运动。

1）障碍代价函数

障碍代价函数是采用人工势场法的思想建立，将AUV所识别的障碍物看作斥力势场，其势能函数为：

$U(q) = \left\{ \begin{aligned}& 0,{\rho _0} < \rho \left( {q,{q_{ob}}} \right)\text{；}\\& k \frac{1}{{{r_0}^2}} \left( {\frac{1}{{{r_0}}} - \frac{1}{{{\rho _0}}}} \right) \left( {1 - \frac{{\rho \left( {q,{q_{ob}}} \right) - {r_0}}}{{{\rho _0} - {r_0}}}} \right)\text{，}\\ & \quad {r_0} < \rho \left( {q,{q_{ob}}} \right) \leqslant {\rho _0}\text{；}\\& k \! \left( {\frac{1}{{\rho \left( {q,{q_{ob}}} \right)}} \!- \!\frac{1}{{{\rho _0}}}} \right) \! \frac{1}{{{\rho ^2}\left( {q,{q_{ob}}} \right)}},\,\rho \left( {q,{q_{ob}}} \right)\! < {r_0}\text{。}\end{aligned} \right.$

其中： $q$ 为AUV的位置矢量； $\rho \left( {q,{q_{ob}}} \right)$ 为AUV到障碍物中心的距离，m； ${\rho _0}$ 为距离障碍物中心的最小安全距离，m； $k$ 为势能系数； ${r_0}$ 为障碍物的外围半径，m。

由上式可知当AUV越靠近障碍物时其势能越高，其障碍代价函数值越大，令其为 ${f_1} = \max (U(q))$ 。

2）目标代价函数

AUV在空间中运行，在其一段路径中目标点只有一个，暂定离目标点距离越远其目标代价函数值越大，相反当离目标点距离越近其目标代价函数值越小，从而建立目标代价函数公式为：

${f_2} = \sigma E\left( {\rho {{\left( {{q_i},{q_{goa}}} \right)}^{ 2}}} \right)\text{，}$

其中， $\rho \left( {q,{q_{goa}}} \right)$ 为AUV当前点到目标点中心的距离。

3）障碍总代价函数

$f = {k_1}{\sum {{f_1} + k} _2}{f_2}\text{，}$

式中， ${k_i}$ 为权重系数（ ${k_i} > 0$ ）， ${k_i}$ 权重系数选取与AUV路径规划过程中障碍代价与目标代价那个更为重要，其中AUV路径规划总是趋于向总代价函数最小的点运动。

1.2.2 探测模型

AUV在水下运动过程中，除了躲避可见的障碍之外，还需要躲避来自人为因素的影响，其中最主要的是雷达的探测，其主要来自水面及水上探测雷达和水下潜艇及其他AUV的探测雷达。根据雷达回波强度与位置的关系，得到雷达探测威胁如下：

${J_{radar}} = \sum\limits_{i = 1}^n {\int_0^{{l_i}} {\frac{1}{{{d_i}^{ \wedge 4}(l)}}} }{\rm{d}}l \text{。}$

其中， ${J_{radar}}$ 为雷达探测威胁； $n$ 为威胁的数量或编号，个； ${l_i}$ 为路径范围内的第 $i$ 个长度值，m； ${d_i}(l)$ 为路径上当前计算点与第 $i$ 个雷达探测威胁的距离，m。

AUV在水下某点收到第 $i$ 个雷达探测位置的影响指数为：

$H(x) = \left\{ \begin{aligned}& \frac{{{\beta _i}{K_i}}}{{d_i^{ 4}}},{\text{被威胁}}i{\text{覆盖}}\text{，}\\& 0,{\text{未被威胁}}i{\text{覆盖}}\text{。}\end{aligned} \right.$

式中： $H(x)$ 为雷达探测影响指数可限制AUV在威胁较小的区域航行； ${K_i}$ 为第 $i$ 个威胁的强度； ${\beta _i}$ 为乘数因子，当第 $i$ 个威胁没有收到预警信息时， ${\beta _i} = 1$ ，否则 ${\beta _i} > 1$ 。

由障碍模型中的目标代价函数可知，当AUV在路径规划过程中，虽然目标点确定，但是还是存在多个路线去选择的情况，如图1所示。

AUV在路径规划过程中始终需要满足其物理性能要求和任务约束，保证其稳定安全的完成任务，多条路径必定有长度和安全的差异，因此建立路径代价函数为：

$g = \sum\limits_{i = 1}^n {({\omega _1}} l_i^2 + {\omega _2} h_i^2 + {\omega _3} {\left| l \right|^2} + {\omega _4} H)\text{。}$

式中： $g$ 为路径代价函数； ${h_i}$ 为第 $i$ 段路径上的高度变化，km； $\left| l \right|$ 为AUV在水下环境下左右摇摆机体的位移； ${\omega _1}$ ， ${\omega _2}$ ， ${\omega _3}$ ， ${\omega _4}$ 为权重系数，通过调整 $\displaystyle\frac{{{\omega _2}}}{{{\omega _1}}}$ 与 $\displaystyle\frac{{{\omega _3}}}{{{\omega _1}}}$ 的相对大小，可以确定AUV是在障碍间穿行或绕过障碍航行，调整 $\displaystyle\frac{{{\omega _4}}}{{{\omega _1}}}$ 的相对大小可以确定其航行路径是在雷达探测威胁区间穿行或绕过威胁区间航行。

1.2.3 路径规划过程中总代价函数

在AUV的路径规划过程中必定优先在环境模型的基础上进行，考虑到外界环境的干扰，AUV在航行过程中总代价函数为障碍模型和探测模型的总和，也就是其干扰模型下AUV为获得最优路径所作出的决策和付出的代价，公式为：

$D = \sum\limits_{i = 1}^n {f + \sum\limits_{i = 1}^n g } \text{。}$

2 改进型蚁群算法的AUV三维路径规划 2.1 蚁群算法的各参数优化

在路径规划过程中，将AUV看作一只蚂蚁，设有 $n$ 只蚂蚁，蚂蚁 $M$ 在运动过程中收到来自多个路径上的信息素浓度的“吸引”，在 $t$ 时刻，蚂蚁 $M$ 位置为 $i$ ，那么向 $j$ 运动的转移概率为 ${P_{ij}}$ ，常规公式为：

${P_{ij}} = \left\{ \begin{aligned}& \frac{{{{\left( {{\tau _{ij}}(t)} \right)}^\alpha }\cdot {{\left( {{\eta _{ij}}(t)} \right)}^\beta }}}{{\displaystyle\sum\limits_{s \in {\rm{allow}}M} {{{\left( {{\tau _{ij}}(t)} \right)}^\alpha }\cdot {{\left( {{\eta _{ij}}(t)} \right)}^\beta }} }},j \in {\rm{allow}}M \text{；}\\& 0,j \notin {\rm{allow}}M\text{。}\end{aligned} \right.$

式中： $\tau {}_{ij}(t)$ 为 $t$ 时刻位置 $i$ 与位置 $j$ 之间路径上的信息素浓度； $\eta {}_{ij}(t)$ 为启发函数， $\eta {}_{ij}(t) = \displaystyle\frac{1}{{{d_{ij}}}}$ 表示蚂蚁 $M$ 从位置 $i$ 到位置 $j$ 的期望程度， ${d_{ij}}$ 为位置 $i$ 到位置 $j$ 的距离；allowM为蚂蚁M待访问位置的集合。

2.1.1 信息素分布的优化

信息素浓度是衡量路径规划过程中路径价值，为蚂蚁的运动作引导依据。信息素只要涉及3个基本概念，分别是初始信息素、信息素更新、信息素挥发。初始信息素由蚂蚁 $M$ 的当然位置来确定，不会有任何其他因素的影响，但是信息素的更新与挥发对信息素浓度影响最大，这也对最优路径的选择起到了至关重要的作用。

1）信息素更新的优化

蚂蚁每移动一次称为一次搜索，当蚂蚁完成一次搜索后，就会对路径上的信息素进行局部更新，更新公式如下：

${\tau _{ij}}(t + 1) = (1 - \varepsilon )\tau (t) + \varepsilon \Delta \tau \text{。}$

式中： $\varepsilon $ 为局部信息素挥发因子； $\Delta \tau = \left\{ \begin{aligned}& {\left( {{D_k}(t)} \right)^{ - 1}},{\text{当前路径已完成}}\\& 0,{\text{其他}}\end{aligned} \right.$ 。式中 ${D_k}(t)$ 为当然运动过程中第 $k$ 只蚂蚁所构造的总代价函数值，而且 ${D_k}(t)$ 只计算两位置点间的总代价。

由此可见，当路径最优时，其总代价函数越小，局部更新中信息素的增量 $\Delta \tau $ 越大，对应路径上信息素的保留就越多，后续迭代中被选择的几率就高^[2]。

当所有蚂蚁完成一次迭代后，对路径的全局信息素进行更新，公式如下：

${\tau _{ij}}(t + 1) = (1 - \rho ){\tau _{ij}}(t) + \rho \Delta {\tau _{ij}}\text{。}$

式中： $\rho $ 为全局信息素挥发因子， $\Delta {\tau _{ij}}$ 由以下公式确定：

$\Delta {\tau _{ij}} = \left\{ \begin{aligned}& \frac{{\left| {{L_B} - L} \right|}}{{{L_B}}},\left| {L - {L_B}} \right| > \sigma \text{；}\\& \frac{{\left| {L - {L_B}} \right|}}{{{L_B}}},\left| {L - {L_B}} \right| < \sigma \text{；}\\& 0,{\text{其他}}\text{。}\end{aligned} \right.$

式中： ${L_B}$ 为当前历史最优路径长度， $L$ 为本次迭代循环的最优路径长度， $\sigma $ 为根据实际路径规划需求与最乱路径的偏移量来确定。

2）信息素挥发的优化

在一次搜素路径结束后，路径还是那个的信息素会进行一定的挥发，这样可以使信息素不会积累过多，对于提高路径规划效率和全局搜索能力有一定的作用。在信息素挥发时最重要的是保留最佳路劲优势的同时也不能影响后续算法的进行，也就是不影响后面蚂蚁对路径的选择。

为了能有更好的最初决策，对路径上的信息素进行不同的挥发，将以通过路径蚂蚁数量 $m$ 和路径长度 $L$ 为参考变量，其公式为：

${\tilde \tau _{ij}} = \left\{ \begin{aligned}& 1 - \frac{{{L_b}}}{{{L_B}}} \rho ,s > \frac{m}{2}\text{；}\\& 1 - \frac{L}{{{L_B}}} \rho ,s < \frac{m}{2}\text{。}\end{aligned} \right.$

式中： $L$ 为蚂蚁搜索得到的路径长度， ${L_b}$ 为当前周期最短路径， $s$ 为通过最短路径的蚂蚁数量， $m$ 为全部的蚂蚁数量， $\rho $ 为挥发权值，根据路径长度的变化改变相应的数值。

2.1.2 启发函数的优化

在路径规划相关模型建立的过程中，可以发现常规蚁群算法中，启发因子 $\eta {}_{ij}(t) = \frac{1}{{{d_{ij}}}}$ 只考虑了目标代价中即两位置之间的距离，并没有考虑下一点与终点和障碍代价、路径代价这些因素的影响，优化后启发因子的公式为：

$\eta {}_{ij}(t) = {\left( {\frac{1}{{{d_{ij}} + {d_{jE}}}}} \right)^{{q_1}}}{f_1}^{{q_2}} {g^{{q_3}}}\text{。}$

式中： ${q_1}$ ， ${q_2}$ ， ${q_3}$ 均为上述因素的相对重要程度， ${d_{jE}}$ 为位置 $j$ 到终点位置的距离。

2.1.3 转移概率的优化

在三维栅格地图中，当一只蚂蚁从当前位置向下移位置运动过程中可能都是遍历过的栅格体时，它将无法选择下一位置进行运动转移，此时，它可能陷入死循环或死锁状态^[3]。因此，在计算转移概率时在考虑距离和信息素浓度的同时也需要考虑栅格体区域的安全性。在一个栅格体的周围障碍体占大多数，那么在运动到这个栅格体后，其运动方向的可能性会变得很小，也就是其栅格体区域内附件的安全性相对较差。那么，优化后的公式为：

${B_{grid}} = \frac{{A{B_{grid}}}}{{{A_{grid}}}}\text{。}$

式中： ${B_{grid}}$ 为与当前栅格体相邻栅格体中障碍栅格体所占的比例， $A{B_{grid}}$ 为与当前栅格体相邻的栅格体中障碍栅格体的数量， ${A_{grid}}$ 为与当前栅格体相邻的栅格体的数量。

在转移概率公式中加入区域安全信因素后的公式为：

${P_{ij}} = \left\{ \begin{aligned}& \frac{{{{\left( {{\tau _{ij}}(t)} \right)}^\alpha } {{\left( {{\eta _{ij}}(t)} \right)}^\beta }{{\left( {1 - {B_{grid \cdot j}}} \right)}^\mu }}}{{\displaystyle\sum\limits_{s \in {\rm{allow}}M} {{{\left( {{\tau _{ij}}(t)} \right)}^\alpha } {{\left( {{\eta _{ij}}(t)} \right)}^\beta }{{\left( {1 - {B_{grid \cdot s}}} \right)}^\mu }} }},j \in {\rm{allow}}M\text{；}\\& 0,j \notin {\rm{allow}}M\text{。}\end{aligned} \right.$

其中， $\mu $ 为安全信息的权重值。注意：尽管在这个栅格体周围障碍占多数，但这个栅格体依然属于可通过性高且有时又是路径的必经之路，所以其值不能过高。

由公式可以看出，当一个栅格体附近的栅格体中障碍数越多， ${B_{grid}}$ 的值就会越大，在路径规划过程中选择的可能性就越小^[4]。

2.2 粒子群算法

粒子群算法是一种群体的迭代算法，通过群体性性能在解空间进行结果的优化计算^[5]。其公式为：

$\!\!\!\!$v_{id}^{k + 1} \!=\! w(k) v_{id}^k \!+\! {c_1}{r_1} \left(p_{id}^k \!-\! x_{id}^k\right)\! +\! {c_2} {r_2}\left(p_{gd}^k \!- \!x_{id}^k\right)\text{，}$$

$x_{id}^{k + 1} = x_{id}^k + v_{id}^{k + 1}\text{。}$

式中： $v_{id}^k$ 为粒子 $i$ 第 $k$ 代速度， $x_{id}^k$ 为粒子 $i$ 第 $k$ 代位置， $w(k)$ 是粒子 $i$ 第 $k$ 代权重， ${c_1}$ 和 ${c_2}$ 为粒子群的学习因子， $p_{id}^k$ 为粒子群进化过程中粒子 $i$ 的最优解， $p_{gd}^k$ 为进化过程中粒子群的最优解。

由公式可以看出，粒子群算法在每次迭代过程中通过粒子跟踪2个最优解来更新自己。

为提高粒子群算法的收敛能力，因为在其公式中存在 $v_{id}^k$ 和 $w(k)$ 两个变量，可以对粒子 $i$ 第 $k$ 代权重 $w(k)$ 在全局路径搜素和局部路径搜索时进行平衡，这样可以增大粒子群算法的搜索效率和收敛速度，且 $w(k)$ 的公式可表示为：

$w(k) = {w_{\max }} - \frac{{{w_{\max }} - {w_{\min }}}}{{{\lambda _{\max }}}} \lambda \text{。}$

式中： ${w_{\max }}$ 为惯性权重的最大值， ${w_{\min }}$ 为惯性权重的最小值， ${\lambda _{\max }}$ 为最大迭代次数， $\lambda $ 为当前迭代次数。

2.3 改进型蚁群——粒子群融合算法

蚁群算法是启发式优化算法中比较常用和经典的算法，具有较强的鲁棒性与正反馈机制，但是由于其初期信息素信息匮乏，搜索比较盲目容易陷入局部最优和过早收敛，为了追求搜索精度使其整个算法的搜索速度慢。而粒子群算法虽然其搜索精度不高，但却具有较强的全局搜索能力和较快的搜索速度。将蚁群算法和粒子群算法进行融合，先利用粒子群算法较强的全局搜索能力和搜索速度对整个区域空间进行路径搜索得到最优解或是次优解。接下来，将得到的最优解或是次优解转化为蚁群算法中的初始信息素，并进行重新分布，利用优化后的蚁群算法的正反馈机制对次优解进行精确搜索，得到当前路径信息素丰富的最优解。这样不仅解决了蚁群算法容易陷入局部最优、搜索盲目的缺点，也通过粒子群算法加快了路径规划搜索的速度。

改进型蚁群—粒子群融合算法中2种算法的衔接点为利用粒子群算法产生蚁群算法的初始信息素分布，也就是利用蚁群算法中根据外界环境信息等确定的信息素的初始值 $\tau {{}_{ij}^C}$ ，利用粒子群算法求得的次优或是最优路径转化为信息素的增量 $\tau {{}_{ij}^P}$ ，最终得到蚁群算法中各路径上的信息素的初始值为 $\tau {}_{ij}$ ，其公式为

$\tau {}_{ij} = \tau {{}_{ij}^c} + \tau {{}_{ij}^p}\text{。}$

算法流程框架图如图2所示。

3 仿真与实验验证分析

在Matlab环境下编程对AUV三维路径规划进行仿真实验，实验环境在跨度为21×21 km的一片海域中搜索从起点到目标终点并且避开所有障碍物及干扰的路径。为了方便问题的求解，取该片区域内最深的点的高度为0，其他高度根据最深点来确定。起点坐标为（1，0，800），目标终点坐标为（21，4，100）。粒子群算法参数设置为： ${c_1}$ 与 ${c_2}$ 都为2， ${w_{\max }}$ 为0.9， ${w_{\min }}$ 为0.6，最大迭代次数为100；蚁群算法参数设置为： ${q_1}$ ， ${q_2}$ ， ${q_3}$ 都设置为1， ${B_{grid}}$ 为0.9，全局信息素挥发因子 $\rho $ 为0.2；分别采用未融合、融合前未优化及优化并融合的蚁群——粒子群融合算法对AUV路径规划进行仿真，结果如图3所示。

可以看出融合后的算法在路径规划时适应度值较小，且响应时间较快，而且在优化后的融合算法中效果更加明显。

4 结　语

本文提出了基于改进型蚁群—粒子群融合算法对AUV在水下三维空间的路径规划进行研究。首先利用粒子群算法较强的全局搜索能力和搜索速度对整个区域空间进行路径搜索得到最优解或是次优解，然后再利用蚁群算法的正反馈机制对次优解进行精确搜索，得到当前路径信息素丰富的最优解。根据仿真结果显示分析可知，改进型蚁群—粒子群融合算法获得的最终结果优于单一以蚁群算法或粒子群算法以及传统蚁群—粒子群融合算法进行路径规划寻优的结果。该算法对于AUV路径规划的应用研究以及多AUV水下环境协调路径规划方法的研究具有一定的参考意义。