基于改进蚁群算法的水下自主航行机器人路径规划*

刘雨青，向 军，曹守启

(上海海洋大学工程学院，上海 201306)

1 引言

水下机器人AUV(Autonomous Underwater Vehicle)路径规划是在环境已知或者部分已知的情况下，依据能耗最低、用时最少和路程最短等指标为AUV规划出一条从起始点到目标点的无碰撞最优路径，路径规划是AUV能够独立自主完成各种任务的首要前提。目前国内外已有很多用于路径规划的算法，如人工势场法[1]、A*搜索法[2]、粒子群优化算法[3-5]和遗传算法[6,7]等。

蚁群算法ACO(Ant Colony Algorithm)[8-14]是一种基于模型的构造型随机算法，具有自组织性、较强的鲁棒性与寻优能力，通过与其他启发式算法相结合，可很容易地改善算法的求解能力。该算法可以通过施加约束条件，求解水下三维空间路径规划的优化问题。武小年等[7]针对启发式算法在数字高程模型DEM(Digital Elevation Model)路径规划中数据量巨大、效率较低等问题，提出一种基于遗传和蚁群的混合路径规划算法。封声飞等[9]针对传统蚁群算法在路径规划中存在收敛速度和寻优能力不平衡，算法易陷入局部最优等问题，提出一种自适应改进蚁群算法。金飞虎等[10]采用根据学习次数和与最近障碍物的距离来调节信息激素物质的自适应蚁群算法，弥补了传统路径规划方法缺乏足够鲁棒性的问题，修改了信息激素物质的更新方法，解决了算法在计算初期出现停滞的现象。柳长安等[11]在改进蚁群算法时，用粒子群优化算法对重要参数进行优化选择，借鉴狼群分配原则对信息素进行更新，改进后的算法可以避免搜索陷入局部最优，但改进后的算法运行时间并未有明显缩短。史恩秀等[12]详细分析了蚁群算法的几个重要参数对规划路径长度及效率的影响，但仅限于找出具体环境下的最佳匹配参数组，仍未解决因信息素挥发系数等参数取值不准而影响规划效果的问题。

为提高AUV三维路径规划效率，优化规划路径，本文针对具有复杂时空要求的水下AUV路径规划问题，使用蚁群算法为基本优化搜索算法，在多重约束条件下以能耗最低为优化目标，建立了AUV运动的能耗模型并获得AUV运动的总能耗，通过改进的蚁群算法，实现了路径规划能耗最低的优化目标。仿真实验结果表明，采用改进的蚁群算法进行水下自主航行机器人的路径规划，能够提升路径规划的实时性，规划出的路径便于AUV控制跟踪，符合能耗最优的目标。

2 水下机器人数学建模

2.1 水下环境建模

AUV水下环境模型直接影响到路径规划算法所规划出的路径优劣，因此合理地将三维环境抽象表达出来是AUV在三维环境中自主航行和进行作业的关键。本文将三维环境空间模型的3个轴依次设为经度、纬度和海拔增加的方向，采用栅格法将二维空间扩展到三维空间，在三维海底地形数据的基础上进行建模，以栅格为单位离散化水下机器人工作区域。

2.2 搜索模式

从二维平面到三维空间中，由于选择节点数量成倍增加，导致算法搜索最优解变得异常复杂，而且很容易出现死锁现象，使得算法收敛性差，全局搜索能力降低。为此，在搜索最优的过程中采用一种分层前进与栅格平面法相结合的搜索模式(如图1所示)。以y轴为主前进方向，设AUV起始点为PS,目标点为PG，规定机器人最大横向移动距离为Xmax，最大纵向移动距离为Zmax。首先蚂蚁从PS出发，在第1个平面搜索到可行节点P1(x1,y1,z1)，接着在第2个平面搜索到第2个可行节点P2(x2,y2,z2)，蚂蚁依次在各个平面选择路径节点，直到搜索到目标点PG结束，即可搜索到一条最优路径。

Figure 1 Search mode of AUV path

2.3 能耗模型

为了求解AUV跟踪所规划出的路径总能耗，首先根据航行状态分别建立AUV水下移动速度模型，然后利用水阻力和推进器推力的计算公式依据功能关系求得AUV跟踪规划出的路径的总能耗。

2.3.1 巡航速度模型

为了降低计算过程的复杂度，本文忽略AUV在航行过程中的加减速过程，并作如下假设：(1)AUV以匀速直线状态开始航行，中间转弯根据转角大小采用相应的低速匀速航行，最终以匀速直线航行状态到达终点；(2)路径关键点前后各一个单位长度路程均为匀速航行状态；(3)匀速直线航行状态与匀速转弯航行状态间存在一个单位长度的加减速航行状态。

故AUV的航行状态为匀速直线航行状态、匀速转弯航行状态和加减速航行状态的叠加，如图2所示。设匀速直线航行状态的速度为v=μm/s；AUV在转弯时转弯速度低于做直线运动时的速度，同时在不同转弯处，转弯半径不一样速度会不同，转弯半径越大，转弯航行状态的速度也就越大；从匀速直线航行状态到转弯航行状态为加减速航行状态，取其平均速度作为该过程的行驶速度。不同航行状态下的速度计算方法如式(1)所示：

(1)

Figure 2 Model of AUV speed

2.3.2 水阻力的计算

AUV在水下运动时必定受到水阻力，由于AUV是一个非线性动力学系统，计算阻力需要确定的参数较多，本文根据水动力计算公式，将AUV受到的水阻力计算简化为式(2)所示:

(2)

其中，F为AUV在航行过程中受到的水阻力；C为水动力系数，该系数的取值与AUV形状、迎流面等一系列要素有关；ρ为水的密度；v为AUV的航行速度，S为AUV的横截面积。

2.3.3 能耗计算

AUV在作匀速运动时加速度为零，水阻力F与推进器产生的推力相等，故由推进器的推力根据式(2)可求得匀速航行状态下AUV的航行速度。AUV在水下巡航时克服水阻力做功，为简化计算，本文忽略AUV推进器之外的所有能耗，故在计算能量消耗时所有能耗均用来克服水阻力做功。

AUV在匀速航行过程中消耗的能量如式(3)所示：

E=Pt/η=Fvt/η=FL/η

(3)

将式(2)代入式(3)中得到:

E=Cρv2SL/2η

(4)

其中，L代表AUV航行的路径长度，η为机械效率，P为效率，由式(4)可知，AUV运动消耗的能量与其运行的速度和工作的路径正相关。

2.4 规划数学模型

AUV路径规划问题可以定义为：在某个规划空间中，满足一定约束条件g(γ)=0的前提下(γ为约束条件变量)，根据某种航行性能评价指标J，规划得到AUV的航行路线，即为从起始点PS到目标点PG且满足一定约束条件的一系列路径节点Γ={PS,P1,P2,…,Pn,PG}。

minγJ=Cρv2SL/2η

(5)

s.t. ∀(x,y)∉Rff(x,y)=0

(6)

Lg+δg

(7)

(8)

(9)

在AUV路径规划中，主要的约束条件包括：

(1)地形与威胁约束。AUV在航行过程中，必须避开一些地形障碍和威胁区域，以保证AUV的安全性，在水下三维环境中，地形障碍和威胁区域均可表示为AUV不可穿越的区域。假设在规划区域中，AUV不可穿越区域集合为R′f，由于AUV具有一定的尺寸,所规划出的路径还要经过平滑处理，为保证路径的安全性，需要对障碍物和威胁区域进行一定的膨化处理，经膨化处理的集合Rf=εR′f，即扩大原始障碍物及威胁区域，膨化系数ε根据AUV的实际尺寸给定。所规划出的路径曲线为f(x,y)=0，其中，(x,y)表示路径上任意一点的经纬度坐标，则规划区域中的地形与威胁约束如式(6)所示。

(2)最大航程约束。AUV自身携带的能源决定了其最远航行距离，故在路径规划时必须将其加入约束条件。AUV航程约束如式(7)所示，式中Lmax为AUV所携带能量能航行的最大路程；Lg为算法规划路径长度;δg为实际航行路程与算法规划路程的偏差，该偏差取决于AUV路径跟踪算法。

3 改进蚁群算法

3.1 基于Dijkstra算法的初始信息素分配

在蚁群算法中，蚂蚁通常根据路径上信息素浓度的差异来寻找最优路径，在信息素的引导下选择一条从起始点到目标点的完整路径。由于传统蚁群算法初始信息素浓度是均匀分配的，这样算法初期搜索具有较强的盲目性进而导致收敛速度慢，容易使算法陷入局部最优，影响算法的寻优能力[8]。

为了避免因信息素导致算法陷入局部最优，减少错误的启发信息对蚂蚁的误导，提高算法路径规划能力，本文在Dijkstra算法[15-17]所规划出的最短路径基础上改进初始信息素的分配。

由于水下自主航行机器人所消耗的能量在很大程度上取决于所需跟踪路径的长度，使用Dijkstra算法能规划出最短路径。如图3所示，将在最短路径上的点的信息素设置为τ0，最短路径两边的点的信息素设置为τ=τ0-λn，其中λ为信息素递减系数，n为路径点距离最短路径点的栅格数，即最短路径周围的点的信息素浓度随着与最短路径点距离增大逐渐减少。

Figure 3 Initial pheromone allocation for improved ant colony algorithm

3.2 启发式函数改进

启发式函数对于算法快速、高效地规划出一条安全合理的路径起着重要的作用，是路径规划算法中的重要组成部分。构造函数时所考虑的各项因素综合体现了算法构建路径的优化目标。本文路径规划算法的优化目标是安全、能耗最低、路径尽可能最短的等深航行路线。根据这一目标设计的环境抽象模型中任意点(x,y,z)的启发式函数H(x,y,z)如式(10)所示：

H(x,y,z)=

en·[E(x,y,z)]ζ·[S(x,y,z)]ξ+uc

(10)

其中，ζ和ξ为相关影响因子；考虑到机器人路径中的安全因素，在构造三维环境模型时，将栅格分为安全区域K和禁止航行区域N，安全区域系数en的取值如式(11)所示：

(11)

E(x,y,z)为路径点(x,y,z)所需要消耗的能量，由式(4)可知，选择下一个路径点所需的能量与选择该路径点后的路径长度和AUV在该点处的速度相关，AUV在实际的运动过程中会根据路径的转弯半径∂(x,y,z)调节速度，转弯处转弯半径越大，相应运动速度会加快，故下一个路径点处所消耗的能量如式(8)所示：

E(x,y,z)=K1/(L1+L2)+K2*∂(x,y,z)

(12)

其中,L1表示下一步将要选择的点与上一个路径点的距离;L2表示下一步将要选择的点与规划路径目标点的距离，若下一步选择的点处于上一个点与目标点所连线段上，则L1+L2的值达到最小，该点的启发信息会较大；K1和K2分别为路程影响因子和转弯半径影响因子。

考虑规划路径平滑度因素S(x,y,z)，使得规划出来的航线尽可能地平滑以利于控制器求解跟踪。S(x,y,z)表示考虑环境障碍时可行点(x,y,z)的平滑程度，其计算如式(13)所示:

(13)

其中，yg为下一个路径点的坐标值,K3为路径平滑系数。

海洋往往存在海流，海流随深度的不同而有所变化，在同一深度，一般表现为水平方向上的均匀流。海流的流速和流向均会对AUV的能量消耗产生巨大的影响，海流模型如图4所示。顺海流方向航行时AUV可以节省部分能耗，顶流时AUV需要额外耗费能量以保持航速，侧流时不但需要消耗能量以保持航速，而且海流还会使AUV偏离原来的航向，加大了控制难度。设海底存在渐变的速度为Vc，流向为δ的无旋海流，uc和vc分别为海流投影到水下机器人横向和纵向的速度，其计算分别如式(14)和式(15)所示：

(14)

(15)

Figure 4 AUV current model

3.3 基于线性回归的信息素更新

信息素是蚂蚁寻路的重要依据，指导蚂蚁向更优的路径前进。本文通过改进信息素更新方式优化算法收敛速度与求解质量。

3.3.1 基于线性回归更新全局信息素

由于算法具有随机性，每一次迭代的m只蚂蚁选择的路径点都有可能距离最优路径点较远，甚至有些蚂蚁在搜索路径的过程中遇到死锁，无法找到一条可行的路径，因此合理的路径更新方式是蚂蚁能够寻找到一条可达路径的保证。本文采用线性回归模型[18,19](如式(16)所示)将本轮迭代最具有代表性的路径点的信息更新一次，强化下一轮蚂蚁搜索路径的方向性，从而提高算法的收敛性能，改善解的质量。

(16)

(17)

(18)

(19)

以上各式中，(xi,yi)为第i只蚂蚁所选择路径点的坐标，hω(x)为线性回归模型，ω为线性回归模型中的权重参数向量，x表示路径点的横坐标向量，J(ω)为采用梯度下降法求解线性回归模型的目标函数，η′为学习率。

3.3.2 自适应调节信息素挥发因子

蚁群算法中的人工蚂蚁具有记忆功能，随着时间的推移，以前留下的信息素会逐渐消失，信息素挥发因子ρi的大小直接影响到蚁群算法的全局搜索能力及其收敛速度。若求解问题的规模比较大时，会使得那些从未被搜索到的路径上的信息素减少到接近于零，降低了算法的全局搜索能力。本文采用前后2次迭代后线性回归路径的长度来自动调节信息素挥发因子，当回归路径长度差较大时，增强信息素的启发作用，适当调小ρi，当回归路径长度差较小时，为提升蚂蚁寻找新路径的能力，增强全局搜索能力，适当增大ρi，具体调节如式(20)所示：

(20)

其中，ΔL为前后2次线性回归路径的长度差;σ为路径长度系数，表征路径长度对信息素的影响程度，为防止ρi过大和过小影响算法收敛速度和求解质量，将其限制在[ρmin,ρmax]之间，以此来优化蚁群算法。

3.4 路径平滑

在栅格地图中规划出来的路径是根据规划出的路径点坐标序列在栅格的中心依序连接而成的折线段，在线段的连接处存在尖峰，容易损坏AUV上的零部件，不利于AUV进行路径的跟踪，为了解决该问题，本文采用Bezier曲线对尖峰进行平滑处理[20]。

3.5 求解流程

改进蚁群算法求解流程如下所示：

(1)首先进行参数初始化，主要包括2类参数，环境信息参数和蚁群算法本身的参数，这2类参数对仿真的结果有重要的影响。环境信息参数包括各个栅格点上的初始信息素、路径的起止点和障碍物信息的表达等；与算法相关的参数主要包括算法迭代次数、同时寻找路径的蚂蚁只数、信息素影响因子、信息素常量和启发函数因子等。

(2)开始进行迭代，一群蚂蚁同步或异步地在问题的相邻状态之间转移，利用关联在每个状态中的信息素和启发信息，采用状态转移规则选择移动的方向，逐步构造出问题的可行解；每只蚂蚁在构造可行解时，可以局部地构造信息素；每一代的所有蚂蚁都完成了解的构造后，依据获得的解对信息素进行全局更新。

(3)同时，完成一次迭代后，根据优化准则在当代中选择一条最优路径，保留该路径直到下一次迭代产生更优的路径。

(4)该迭代过程持续到完成规定的迭代次数或者连续多代迭代结果一致为止。满足停止寻优条件后，表明已经得到一条满足能耗最低的路径，再通过平滑路径得到一条适合AUV跟踪的平滑路径。

4 AUV路径规划仿真实验

4.1 实验参数设置

蚁群算法作为一种启发式算法，性能优化在很大程度上取决于参数的设置，参数是影响蚁群算法求解效率和全局收敛性能的关键因素之一。由于蚁群算法参数空间的庞大性以及各参数之间的关联性，如何确定一组优质参数组合使得算法求解性能最佳是一个复杂的优化问题，本文通过大量的实验和经验，选取了如表1所示的参数。

Table 1 Parameter setting of improved ant colony algorithm

4.2 规划路径分析

本文根据建立的三维环境模型和目标函数(式(5))，利用传统蚁群算法、改进蚁群算法和遗传算法分别对 AUV 基于能耗最优进行等深路径规划仿真实验，最后利用本文所改进的蚁群算法对 AUV 定深航行进行了路径规划仿真实验。

本文将AUV航行高度设为300 m，使用传统蚁群算法、遗传算法和改进蚁群算法分别进行路径规划仿真实验，各算法分别进行80次，其中将改进蚁群算法中所需的参数设置成表1所示，其统计结果如表2所示。图5～图10分别为使用传统蚁群算法、遗传算法和改进蚁群算法所规划出的路径，图11～图13是3种算法目标函数的收敛趋势图。

仿真实验统计结果表明，采用改进蚁群算法规划出的路径其消耗的能量较传统蚁群算法和遗传算法的低，其长度较传统蚁群算法和遗传算法的短。改进蚁群算法具有比遗传算法和传统蚁群算法更强的求解能力，得到最优解的概率更大，能够规划出航程较短、能耗最小的全局路径,满足AUV等深度、低能耗航行的要求。

图5和图6分别给出了基于传统蚁群算法所规划出的路径的三维立体视图和俯视图。该路径并非全局最优路径，其原因在于蚁群算法是一种随机搜索算法，每只蚂蚁要搜索的解空间规模较大，为了加快算法构造解的速度，在算法中增加了启发信息所占的比重，即增加了算法的确定性，牺牲了算法的全局寻优能力。

Figure 5 3D view of path planning using traditional ant colony algorithm

Figure 6 Vertical view of path planning using traditional ant colony algorithm

Table 2 Statistical results of simulation experiment

算法在迭代的初期，信息素均匀分配导致其正反馈作用不强，路径启发信息与当前所在路径点到下一个路径点的距离有关，在启发信息的引导下蚂蚁更倾向于选择使得整条路径长度更短的路径点，启发信息的方向性较弱，故规划出的路径转折点较多，路径较长，AUV消耗的能量较多。

与传统蚁群算法相比，遗传算法从串集开始搜索，覆盖面广，利于全局择优，路径搜索不易陷入局部最优，由图7和图8可知道，由遗传算法规划出的路径还是不够平滑，AUV难以跟踪该路径。同时，由于遗传算法的局部搜索能力较差，导致未经改进的遗传算法比较费时，在进化后期搜索效率较低，求解时间较长。

Figure 7 3D view of path planning using genetic algorithm

Figure 8 Vertical view of path planning using genetic algorithm

改进蚁群算法所规划的路径如图9和图10所示，该路径转折点较少，使用Bezier曲线改善路径的平滑性后，便于AUV跟踪该路径，AUV依靠惯性就能很好地跟踪该路径，从而减少了能量的消耗，保护了AUV的机械结构和电子元器件。

Figure 9 3D view of path planning using improved ant colony algorithm

Figure 10 Vertical view of path planning using improved ant colony algorithm

本文通过对启发函数、信息素初始化及更新方式的改进，从而保证了规划出的路径平滑且能耗较低，使该算法更适合运用到水下机器人的路径规划上。

4.3 能耗分析

从3组算法所规划的路径中分别随机地选择一次实验结果，其收敛过程如图11～图13所示。从路径寻优的过程中不难发现，在寻求能耗最优的路径时，路径的总长度并非持续缩短，这就意味着能耗与路径长度在某些情况下会存在矛盾，影响AUV能耗的因素除路径长度外还有AUV转弯幅度和水流等因素。

Figure 11 Convergence process of traditional ant colony algorithm

Figure 12 Convergence process of genetic algorithm

Figure 13 Convergence process of improved ant colony algorithm

遗传算法同传统蚁群算法相比，规划出的路径虽然在能量消耗和路径长度方面有所改善，但需要更长的时间去优化路径，路径规划实时性不高。改进蚁群算法的求解能力和求解速度相比传统蚁群算法和遗传算法都有了很大的提升，较传统蚁群算法求解时间缩短了10.74%，较遗传算法求解时间缩短了28.05%。

同传统蚁群算法规划出的路径(如图5和图6所示)相比，在改进蚁群算法初始化前基于Dijkstra算法分配信息素，这些信息素集中分配在最短路径附近，在算法的初期，为寻找路径的蚂蚁指明了方向，同时在每一轮迭代完成后，基于线性回归模型进行全局信息素更新，线性回归模型路径上的路径点更能代表本次迭代的最优路径，加快了算法的收敛速度，提高了算法求解质量。最终规划出的路径(如图9和图10所示)，能量消耗比传统蚁群算法降低了20.44%，路径长度减少了11.51%。

基于传统蚁群算法、遗传算法和改进蚁群算法3种路径规划算法的能耗分析见表3。

Table 3 Energy consumption analysis of path planning

由表3可知改进蚁群算法求解效率较高，所规划出的机器人路径符合能量消耗最少的优化目标，改进蚁群算法用于水下机器人路径规划具有较强的工程意义。

5 结束语

本文基于蚁群算法对AUV路径规划问题进行了研究，详细设计了三维空间环境建模以及蚁群算法在该模型中的搜索模式，结合能耗最低的优化目标改进了蚁群算法的启发函数、信息素初始分配方式及更新方式，在规划出的路径点上采用贝塞尔曲线改善了路径的平滑性，便于AUV跟踪该路径，最后给出了改进蚁群算法的具体流程。实验结果表明，改进蚁群算法在一定的约束条件下，基于能耗最优所规划出的路径较传统蚁群算法能耗降低了20.44%，较遗传算法能耗降低了15.20%，适合用于水下三维环境中AUV的路径规划。