基于改进A*算法的UUV冰下避障航迹规划算法

程建华，李鹏程，管 行，葛靖宇

(哈尔滨工程大学智能科学与工程学院，哈尔滨 150001)

0 引言

北极拥有极为丰富的自然资源，还是亚、欧、北美三大洲的延伸汇集地，有联系三大洲的最短大弧航线，具有极其重要的战略意义。由于北极地区被较厚冰盖覆盖，冰下航行是重要的战略手段，因此开展水下无人潜航器(Unmanned Underwater Vehicle，UUV)北极冰下航迹自主规划技术研究，对于提高我国载人/无人潜航器极区冰下安全航行能力，具有重要的实际意义和应用价值[1]。

UUV在实际的航行过程中，其运动过程较为复杂，所以规划出一条符合UUV运动状态的航迹难度较高。国内外虽有多项研究成果，如蚁群优化算法[2]、遗传算法[3]和人工势场算法[4]等，但这些航迹规划算法各有其局限性[5]。在复杂环境下，UUV的航迹规划是一个复杂的多约束问题[6]，且冰下航迹规划领域研究几乎为空白。航迹规划需要综合考虑地形碰撞约束、海冰碰撞约束、潜航器航行性能以及任务需求等各种因素，规划出一条符合上述所有约束条件的航迹。忽略约束条件或单纯强调目标搜索的规划方法很难适应UUV的真实运动情况[7]。

本文结合传统人工势场航迹规划算法的思想，将海底地形碰撞约束、海冰碰撞约束和UUV巡航高度约束重新编排为人工势场斥力约束，并给出了这三种约束的人工势场设计方法。将这些约束因素与A*搜索算法的代价函数组合，得到改进的A*冰下航迹规划算法。最后，通过仿真验证了该算法可生成满足任务需求的冰下航迹。

1 UUV航迹规划约束条件

1.1 动力学约束

UUV在水下正常航行过程中，可将其视为一个满足动力学约束条件的六自由度运动刚体结构。由于横滚角与航迹无直接联系，因此本文主要考虑UUV的纵倾与艏摇约束条件[8]。设其纵倾角速度为q，纵倾角为θ，最大纵倾安全角速度为qmax，最大纵倾安全角为θmax，所以UUV的纵倾约束为[6]

(1)

同理，可以得知UUV的艏摇约束为

(2)

其中，艏摇角速度为r；艏摇角为ψ；最大艏摇安全角速度为rmax；最大艏摇安全角为ψmax。若UUV运动角速度或者角度超出安全范围，则会出现失控状态。

1.2 防碰撞约束

在进行UUV冰下航迹规划时，需要使航迹与海底地形以及海冰保持安全距离，防止发生碰撞。设UUV距地形与海冰的垂直安全距离分别为εobs和εice，距地形与海冰的实际垂直距离分别为xobs和xice，在整个航迹规划过程中，需满足如下约束

(3)

1.3 定深巡航高度约束

UUV在执行勘测或巡逻任务时，为了获得更好的隐蔽效果，减小被声纳发现的概率，需要使UUV在某一确定高度从起点保持定深巡航至终点。定深巡航高度约束是整个航迹规划过程中必须考虑的问题。设UUV执行任务的目标深度为htask，其实际巡航深度为h，任务可接受的巡航深度最大偏差为he。定深巡航约束应满足如下公式

|h-htask|≤he

(4)

除此之外，UUV还会受材料、工艺技术以及密封性的限制，存在一个最大工作深度H，所以其巡航深度应同时满足

h≤H

(5)

1.4 能量约束

因为UUV是携带燃料或者蓄能电池的水下无人潜航器，所以其航行距离s不能超过UUV的最大航程S，其约束为

s≤S

(6)

2 基于改进的A*航迹规划算法

2.1 传统A*航迹搜索算法

A*航迹搜索算法是一种常见的航迹搜索算法，适用于较大范围地形的航迹搜索。该算法为基于迪杰斯特拉(Dijkstra)航迹搜索算法，同时结合广度优先搜索(Breadth-First-Search，BFS)算法的启发式航迹搜索算法[9]。所以，A*航迹搜索算法同时具备Dijkstra搜索算法与BFS算法的特点，既可以按照最短路径进行搜索，同时又能在搜索过程中添加启发函数和代价函数，使得航迹按照任务进行规划。A*航迹规划算法的代价函数如下所示

f(n)=g(n)+h(n)

(7)

其中，n代表当前待搜索节点；g(n)代表从航迹初始点到当前待搜索节点的实际代价；h(n)代表对从当前待搜索节点到航迹规划目标点的实际代价的估计值，又被称为启发函数[6]。这样，f(n)就可以代表从航迹规划初始点经过点n到达目标点的最小代价估计值[10-11]，上述代价一般指路径长度，可使用欧氏距离或者曼哈顿距离进行计算。

利用A*搜索算法进行三维航迹规划之前，需要将空间三维网格化，利用网格点完成航迹规划。在相邻3×3×3共27个三维网格点中，除去当前节点n，可对剩余26个网格点进行搜索。在使用A*搜索算法进行航迹规划时，需要建立2个数据表：Open表和Close表,Open表用于储存还未进行搜索的节点，Close表用于储存已经完成搜索的节点[12-15]。

2.2 改进A*搜索算法代价函数设计

冰下航迹规划需要从地形与海冰避障约束、航程约束、定深巡航约束以及能量约束4个方面展开。传统的代价函数很难适应该任务需求，所以本文将A*航迹规划算法的代价函数进行了如下修改

f(n)=αg(n)+βh1(n)+μh2(n)+γh3(n)

(8)

其中，g(n)代表从航迹初始点到当前待搜索节点的距离约束代价;h1(n)代表对从当前待搜索节点到航迹规划目标点的距离约束代价的估计值；h2(n)代表对从当前待搜索节点到航迹规划目标点的地形约束代价的估计值；h3(n)代表对从当前待搜索节点到航迹规划目标点的定深巡航约束代价的估计值。α、β、μ、γ为各项的权值系数，且满足α，β，μ，γ≥0。

1)航程约束代价设计

航程约束代价主要分为两部分：从航迹初始点到当前待搜索节点的欧氏距离g(n)与当前待搜索节点到终点的曼哈顿距离h1(n)。

2)地形与海冰避障约束代价设计

在实际航行过程中，当UUV距离障碍物较近时，危险程度会随着与地形(海冰)距离的缩短而逐渐增大；而当UUV距离地形(海冰)较远时，UUV则处于安全状态(图1)。可依据UUV航行时距地形与海冰的垂直安全距离εobs、εice和实际垂直距离xobs、xice对地形与海冰避障约束代价进行定义

h2(n)=hobs(n)+hice(n)

(9)

其中，hobs(n)为地形避障约束代价；hice(n)为海冰避障约束代价，它们的表达式分别为

hobs(n)=

(10)

(11)

式中，kobs与kice分别为地形避障约束代价与海冰避障约束代价的调整参数。

图1 UUV水下航行剖面图Fig.1 UUV underwater navigation profile

3)定深巡航约束代价设计

当UUV在执行任务时，会有定深巡航高度的要求，在任务允许的范围内，调整UUV航行深度不会增加定深巡航代价;当UUV航行深度超出允许范围时，定深巡航高度约束代价会快速增加，可定义定深巡航高度约束代价为

h3(n)=

(12)

式中，kh为定深巡航约束代价的调整参数。

4)能量约束代价设计

当UUV在能量耗尽之前完成任务可不受到约束的影响;当UUV未能在能量耗尽之前完成任务，则判定航迹规划失败。

经过前期仿真验证可得：当α=β=1，γ=1，μ≤0.035时，规划出的航迹会不受距离约束代价的约束；当α=β=1，μ=1，γ≥62时，规划出的航迹会不受距离约束代价与地形约束代价的约束。综上所述，可得本文的权值系数取值范围为：当α=β=1时，需满足如下不等式

(13)

3 仿真验证

以UUV穿越某一覆盖有海冰的复杂地形这一情形为例，进行多次仿真验证。地形范围为100km×100km，格网精度为(1km，1km，1m)，仿真步长为r=1。

任务1：UUV 要安全穿越某一被海冰完全覆盖的复杂地形，任务要求UUV在深度-60m保持定深航行，且要完成避障。航迹规划的起点为(5km，5km，-60m)，航迹规划的终点为(95km，95km，-60m)。设定定深巡航高度htask=-60m，可接受巡航深度最大偏差he=0，地形安全距离εobs=5m，海冰安全距离εice=10m，仿真中各项权值系数α=1，β=1，μ=3，γ=2。UUV既需要保证航行安全，又需要完成定深航行任务，所以地形(海冰)避障约束权值系数与定深巡航高度约束权值系数需取较大值，并且前者大于后者。仿真结果如图2～图5所示。

从图2与图3的航迹三维图可以看出，UUV具有较好的地形回避能力，且可以顺利完成从起点至终点的航迹规划，航迹总长度为142.65km。从图4与图5的航迹高度示意图与安全距离示意图可以看出，UUV在整个航迹规划过程中，航迹大部分时间处于设定的航行高度htask=-60m且基本保持高度稳定，距离地形的实际距离始终大于或者等于设定的地形安全距离，符合任务需求。

图2 任务1地形高度及三维航迹Fig.2 Terrain height and 3D track of task 1

图3 任务1冰层下表面高度及三维航迹Fig.3 The height of the lower surface of the ice layer and its 3D track of task 1

图4 任务1航迹高度及地形与冰层下表面高度Fig.4 Track height, terrain and ice surface height of task 1

图5 任务1航迹上安全距离与实际距离Fig.5 Safe distance and actual distance on track of task 1

任务2：UUV 要安全穿越某一被海冰覆盖的复杂地形，任务要求UUV在整个航行过程中需要完成避障，并且尽可能缩短航程。航迹规划的起点为(5km，5km，-40m)，航迹规划的终点为(95km，95km，-40m)。设定定深巡航高度htask=-40m，可接受巡航深度最大偏差he=10m，地形安全距离εobs=15m，海冰安全距离εice=10m，各项的权值系数α=2，β=2，μ=3，γ=1。UUV既需要保证航行安全且完成避障，又需要缩短航程，所以初始点到当前待搜索节点的距离约束权值系数、当前待搜索节点到航迹规划目标点的距离约束权值系数与地形(海冰)避障约束权值系数需要取较大值，并且后者大于前两者。仿真结果如图6～图9所示。

从图6与图7的航迹三维图可以看出，UUV具有良好的地形回避能力，可以顺利完成从起点至终点的航迹规划，且规划出的航迹较为平直，航迹总长度为135.72km。从图8与图9的航迹高度示意图与安全距离示意图可以看出，UUV在整个航迹规划过程中，由于定深巡航约束代价取值较小，所以初始设定的航行高度htask=-40m对航迹规划约束不强。规划的航迹距离地形的实际距离始终大于或者等于设定的地形安全距离，相较于任务1，航程缩短，符合任务需求。

图6 任务2地形高度及三维航迹Fig.6 Terrain height and 3D track of task 2

图7 任务2冰层下表面高度及三维航迹Fig.7 The height of the lower surface of the ice layer and its 3D track of task 2

图8 任务2航迹高度及地形与冰层下表面高度Fig.8 Track height, terrain and ice surface height of task 2

图9 任务2航迹上安全距离与实际距离Fig.9 Safe distance and actual distance on track of task 2

4 结论

本文针对冰下避障航迹规划问题，结合传统人工势场航迹规划算法的思想，设计了一种基于改进A*算法的UUV冰下避障航迹规划算法，充分考虑了海冰避障约束、地形避障约束、定深巡航高度约束以及能量约束,设计了针对上述约束的代价函数，将代价函数与A*航迹搜索算法结合获得改进A*航迹搜索算法，并完成了三维航迹仿真。

通过仿真验证了该算法规划的路径能够有效完成地形与海冰避障、航行深度控制以及航行距离控制。通过调整代价函数的权值系数，仿真所得航迹能够契合任务要求。

本文仿真只是在已知的环境下进行的，而UUV实际航行时的环境是动态变化的，针对动态变化环境的航迹规划是下一步工作的重点。