CN108489491A - 一种水下自主航行器的三维航迹智能规划方法 - Google Patents

Abstract

本发明涉及一种水下自主航行器的三维航迹智能规划方法，包括以下步骤：1)海底地形模拟；2)航迹评价指标和优化函数选取，通过对航迹规划问题的分析，建立了综合考虑航程长度及规避动态目标的评价函数模型；3)约束处理和搜索空间划分，制定了一种将约束处理与搜索空间裁剪相结合的机制，用于在缩减搜索空间的同时使得航迹满足各项约束条件；4)采用改进的蚁群算法进行航迹的优化。与现有技术相比，本发明具有提高了最优航迹的搜索效率等优点。

Description

一种水下自主航行器的三维航迹智能规划方法

技术领域

本发明涉及机器人路径规划技术领域，尤其是涉及一种水下自主航行器的三维航迹智能规划方法。

背景技术

航迹规划是水下自主航行器任务规划系统的核心之一，其目的是在综合考虑时间、燃料消耗、避障以及允许工作区域等因素的前提下，为水下自主航行器规划出一条最优、或者是最满意的航行轨迹。

与其他无人系统相比，水下自主航行器的航迹规划具有一些特殊性。一方面，水下自主航行器任务需求复杂，不但要规避静态障碍物，还要规避其他的水下航行器等动态障碍物。由于大多数水下自主航行器属于欠驱动航行器，机动性能较差，响应速度较慢，因此航迹的实时规划对完成工作任务至关重要。另一方面，水下自主航行器工作环境复杂，会受到海流、跃层及海洋内波等多种干扰因素的影响，而任务指定的抵达目标位置也可能会有所改变，给航迹实时规划施加了很强的约束，降低了优化求解的效率，限制了航迹规划的效率。

针对航迹规划的研究，研究人员提出了许多不同的方法。以往主要采用的方法可分为全局航迹规划方法和局部航迹规划方法。全局航迹规划方法有最速下降法、动态规划方法、A*算法、最优控制法和Voronoi图法等；局部航迹规划方法主要有梯度法和人工势场法等。这些方法各有优缺点，其优劣主要取决于方法的计算效率及所求解的最优性。现有的航迹规划算法主要存在以下问题：

1)为了加速搜索进程和减少内存需求，很多算法采用了将环境信息投影到一个二维的C空间中。而二维的C空间不能完全包括战区的所有信息(如地形、障碍、动态目标的三维信息)，因此这种方法生成的航迹不可能有很好的地形跟随和地形规避，也无法在规划的时候综合考虑实际地形条件和动态目标的威胁。

2)尽管有些文献研究三维空间的航迹规划，但这些方法或者不能满足航迹规划的各项约束条件，规划出的航迹无法真正用于实际任务，或者为了提高算法的可行性而牺牲了算法的优化精度和优化效率。

3)航迹规划的区域非常广阔，是一个巨大的搜索空间。通常的搜索算法计算时间随着问题规模的变大往往以几何级数增长，要获得最优航迹往往需要很长的收敛时间和很大的内存空间。很多算法在二维空间下可以较快地得到一条航迹，但一旦上升到三维空间，其时间将显著增加，很难满足水下自主航行器的实时规划需求。

目前，水下自主航行器三维航迹规划研究的重点是向实时性、可实现性和增强寻优能力方向发展。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种水下自主航行器的三维航迹智能规划方法。

本发明的目的可以通过以下技术方案来实现：

一种水下自主航行器的三维航迹智能规划方法，包括以下步骤：

1)海底地形模拟；

2)航迹评价指标和优化函数选取，通过对航迹规划问题的分析，建立了综合考虑航程长度及规避动态目标的评价函数模型；

3)约束处理和搜索空间划分，制定了一种将约束处理与搜索空间裁剪相结合的机制，用于在缩减搜索空间的同时使得航迹满足各项约束条件，提高航迹的可实现性和规划效率；

4)采用改进的蚁群算法进行航迹的优化，可以克服经典蚁群算法收敛时间长和易于陷入局部最优的缺点，提高最优航迹的搜索效率。

优选地，所述的海底地形模拟采用随机地形模拟与山峰地形模拟相结合的方法来模拟航迹规划的地形。

优选地，所述的海底地形模拟具体过程为：

海底地形在高度方向上分解为两个组成部分，首先是产生地形平均高度的地形基准平面高度，其次是地形基准平面上叠加的地形起伏：

H(x,y)＝H₀+Z(x,y)

其中，H(x,y)为坐标(x,y)点的实际海拔高度；H₀为整个地区的平均高度；Z(x,y)为在地形平均高度基础上的地形起伏高度；

对于实际地形的起伏算法是用高斯分布的随机序列来表征的，地形采样点的相关性为指数衰减：

其中，τ_x和τ_y为两个采样点在x和y方向的距离；τ_xc和τ_yc是抗相关距离，对于典型地形，抗相关距离越大地形越平滑；A为衰减系数；

对山峰地形而言，Z(x,y)表示山峰的地形采样点，其模拟采用函数法来产生，函数形式为:

其中，Z₀为基准地形高度；Z_i为第i个山峰在基准地形之上的最高点的高度；x_0i和y_0i为第i个山峰最高点的坐标；x_si和y_si为第i个山峰沿x轴和y轴方向与坡度有关的量，其值越大，相应的山峰就越平坦，反之就越陡峭；n＝1时是孤立的山峰，n＞1是多个山峰组合。

优选地，所述的航迹评价指标和优化函数选取具体为：

采用网格法来划分三维规划空间；

在对规划空间进行合理的网格法离散化后，航迹规划问题表示为寻找从起始点S₀到目标点D之间的最优的航迹点序列{S₀,P₁,P₂,...,P_n,D}，使得水下自主航行器在沿着由这些航迹点连接而成的航迹上航行时航迹的某种代价最小，其中，P₁,...,P_n为某个六面体的某一顶点；

在经过网格法离散化处理后的航迹规划空间中，航迹的总长度由各段航迹边的长度相加得到，其优化目标函数取为：

其中，l_i为第i条航迹边的长度；

假设水下自主航行器在航迹边i时与动态障碍物碰撞的概率为P_i，则在整条航迹上，水下自主航行器规避动态目标的优化目标函数为：

令P_i ^*＝log((1-P_i)^-1)，则水下自主航行器规避动态目标的优化目标表述为：

对航迹长度与动态目标碰撞概率两项评价指标的形式做一致性处理后，就将两者统一于同一次优化过程中，引入加权系数k₁和k₂，分别表征航迹长度优化与规避动态目标优化的权重值，其中k₁+k₂＝1，k₁、k₂＞0，最终的航迹优化目标取为如下形式：

选取了合适的优化目标函数之后，就按照某条航迹对应的目标函数值的大小来对航迹的优劣做一个综合的评价，采用所选取的优化目标函数为和最小化形式，因此，如果组成一条航迹的各航迹边的代价之和越小，则该条航迹性能越好。

优选地，所述的采用网格法来划分三维规划空间具体为：

将三维空间沿着直角坐标系的各方向，按照设定的分辨率分割成六面体，从而得到离散化的规划空间。

优选地，考虑到水下自主航行器在一条航迹边上与动态目标碰撞的概率与该航迹边的长度正相关，同时为了消除优化目标函数中两项优化指标的数量级不匹配，在航迹安全性评价指标项前乘上了航迹长度l_i。

优选地，所述的约束处理和搜索空间划分具体为：

在水下自主航行器三维航迹规划中，必须考虑任务要求和水下自主航行器自身性能约束，其中约束包括：

1)最小安全航行高度

针对水下平坦地形和山地地形对水下自主航行器的最小安全航行高度分别规定为H_pmin与H_Mmin；

2)最大转弯角和最大俯仰角

记第i条航迹边的水平投影为r_i，水下自主航行器经过第i段航迹后在高度上的增值为ΔZ_i，水下自主航行器最大转弯角为θ，最大俯仰角为则有：

3)航迹边矢量方向

记起始点指向目标点的方向矢量为第i段航迹与的夹角为σ_i，规定对于i＝0,1,...,n，有：

σ_i＜π/2

4)最大航程长度

记起始点到目标点的直线距离为L_min，规定最大航程长度为2L_min，即

针对水下自主航行器三维航迹规划搜索空间大的问题，进行搜索空间的裁剪，并结合航迹规划的约束条件，生成新的搜索空间C'。

优选地，所述的新的搜索空间C'满足以下条件：

1)C'是规划空间C的子集，且同C相比，C'的规模有显著减小；在C'中搜寻最优航迹的效率将高于在C中搜寻；

2)航行器在C'中航行时，其位置和转移状态都满足航迹规划的约束条件，即搜索空间C'中的航迹是可行航迹。

优选地，所述的新的搜索空间C'生成具体为：

提出允许后续航迹点的概念，即规定只有属于allowed_i,k集合的点才能成为蚂蚁k当前位置i的允许后续航迹点；按照这样的后续航迹点扩展策略，逐步形成航行器的航迹搜索空间C'；显然，航迹搜索空间C'是动态生成的，不同的航迹搜索过程将产生不同的C'；

其中allowed_i,k集合的定义：

蚂蚁k当前的航迹点为i，航迹点u与航迹点i相邻，如果点u满足下列两个条件，则称航迹点u∈allowed_i,k：

1)蚂蚁k在本次迭代中尚未经过航迹点u。

2)航迹点u和边(i,u)满足航迹约束条件。

优选地，所述的采用改进的蚁群算法进行航迹的优化具体为：

针对水下自主航行器航迹规划的需求，在传统的蚂蚁算法基础上提出一种带自适应阈值的最大最小蚂蚁系统算法MMATSAT，根据蚁群算法搜索情况来自适应调整阈值的大小，并且对各航迹段上的信息量做最大值和最小值的限制，用于解决扩大搜索空间与寻找最优解之间的矛盾；

MMATSAT采用时变函数q₀(t)来代替“伪随机比例”状态转移规则中为常数项的q₀：

式中，s代表经典蚁群算法中蚂蚁k从城市i转移到城市j的概率；tabu_k记录着蚂蚁k已经遍历过的城市；q₀(t)∈(0,1)称为阈值；q为(0,1)间的随机数；S为计算选择的节点；τ_i,j(t)为t时刻城市i到城市j对应的边e_i,j上的信息素浓度，各条路径的初始信息素浓度相同，即τ_i,j(0)＝τ₀，其中τ₀为一正常数；η_i,j(t)为启发式信息，取为1/d_ij，d_ij为城市i与城市j之间的距离；α，β为调节信息素浓度τ和启发式信息η相对重要性的参数；

在迭代初始阶段，蚁群搜索得到的最优解往往离全局最优解较远，因此q₀(t)取较小值，鼓励蚂蚁探索新航迹；随着迭代次数增加，迭代最优解逐步向全局最优解逼近，增大q₀(t)的值，有利于提高算法收敛速度；

根据以上分析，q₀(t)选为如下阶梯函数：

其中，q₃₀＞q₂₀＞q₁₀≥0；T₁～T₃根据期望的优化时间而定；

对各航迹边上的信息素量的上下限做以下规定：

其中，s_max和s_min是由待求解问题的构造图产生的伪可行解，s_max表示有l_max条权重为e_max构成的一个序列，e_max为构造图的最大权值，s_min表示有l_min条权重为e_min构成的一个序列，e_min为构造图的最小权值，s_max和s_min只要能求出其目标函数值即可；Q为一正常数，表示信息素总量。

与现有技术相比，本发明克服了经典蚁群算法收敛时间长和易于陷入局部最优的缺点，提高了最优航迹的搜索效率；并通过制定合理的约束处理及搜索空间裁剪策略，充分利用地形和环境信息，改进了航迹规划算法的寻优效率。

附图说明

图1为航迹表示方法示意图；

图2(a)为二维空间邻接点示意图，图2(b)为三维空间邻接点示意图；

图3为水下自主航行器三维优化航迹图；

图4为水下自主航行器等高线优化航迹图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

本发明提出了一种基于改进蚁群算法的高效三维航迹规划算法，克服了经典蚁群算法收敛时间长和易于陷入局部最优的缺点，提高了最优航迹的搜索效率。并通过制定合理的约束处理及搜索空间裁剪策略，充分利用地形和环境信息，改进了航迹规划算法的寻优效率。本发明着重考虑了下面几个问题：

1)航迹评价指标

一个好的航迹规划方案，不仅需要有较高的规划效率，而且应该具备较小的碰撞风险和较小的航程长度。所以在规划的时候，需要综合考虑地形条件和动态障碍的威胁以及航程长度，确定合理的航迹评价指标。

2)航迹规划约束处理与搜索空间裁剪

航迹规划不仅需要考虑油耗和运行时间等的约束，还需要考虑水下航行器自身物理性能的约束，如最小机动半径和最快响应速度等。在航迹规划中需要对这些约束作适当处理，才能使得规划出的航迹具有可行性。此外，航迹规划还需要结合约束条件对搜索空间作一定裁剪，从而减小搜索空间的规模，提高规划寻优效率。

3)最优航迹的搜索效率

航迹规划算法的搜索效率对于实现航迹的实时规划至关重要。然而采用经典蚁群算法进行航迹规划时往往存在收敛时间长和易于陷入局部最优的缺点，对最优航迹的搜索效率较低，无法满足水下自主航行器的实时规划需求。因此，需要对经典蚁群算法进行改进，克服其缺点，从而提高最优航迹的搜索效率。

具体实施方式：

1.海底地形模拟

航迹规划必然要求获知航行区域的海底地形信息。本发明采用随机地形模拟与山峰地形模拟相结合的方法来模拟航迹规划的地形。

海底地形在高度方向上可以分解为两个组成部分，首先是产生地形平均高度的地形基准平面高度，其次是地形基准平面上叠加的地形起伏：

H(x,y)＝H₀+Z(x,y) (1)

其中，H(x,y)为坐标(x,y)点的实际海拔高度；H₀为整个地区的平均高度；Z(x,y)为在地形平均高度基础上的地形起伏高度。对于实际地形的起伏算法是用高斯分布的随机序列来表征的，地形采样点的相关性为指数衰减：

其中，τ_x，τ_y为两个采样点在x和y方向的距离；τ_xc，τ_yc是抗相关距离，对于典型地形，抗相关距离越大地形越平滑；A为衰减系数。

对山峰地形而言，Z(x,y)表示山峰的地形采样点，其模拟采用函数法来产生。函数形式为:

其中，Z₀为基准地形高度；Z_i为第i个山峰在基准地形之上的最高点的高度；x_0i，y_0i为第i个山峰最高点的坐标；x_si，y_si为第i个山峰沿x轴和y轴方向与坡度有关的量,其值越大，相应的山峰就越平坦，反之就越陡峭；n＝1时是孤立的山峰，n＞1是多个山峰组合。数据在内存单元中以网格形式存储。

2.优化函数的选取

水下自主航行器三维航迹规划问题可定义为，在一定的规划空间中，寻找到从初始位置S₀到终点位置(目标所在位置)D的一条三维航迹δ，该航迹表示了水下自主航行器运动状态的一个集合。其中，δ满足在一定的约束条件R(δ)下，使得航迹性能优化目标函数C(δ)达到最优：

s.t.R(δ) (4)

水下自主航行器三维航迹规划的规划空间是一个比较大的空间区域。设(x,y,z)为规划空间某一点的坐标，其中，x，y表示该点的经纬度，z表示该点距离海底的高度，则航迹规划空间区域C可以表示为：

C＝{(x,y,z)|0≤x≤maxX,0≤y≤maxY,0≤z≤maxZ} (5)

理论上，航迹规划空间是连续的。然而，直接在连续空间随机搜索最优航迹，将产生搜索空间的指数膨胀问题，导致搜索效率极低而无法满足实时规划要求。因此在实际的水下自主航行器三维航迹规划过程中，需要对航迹规划空间区域作离散化。本发明采用易于计算的网格法来划分三维规划空间，即将三维空间沿着直角坐标系的各方向，按照一定的分辨率分割成六面体，从而得到离散化的规划空间。比如将规划空间C均匀划分为M×N×P(M,N,P∈Z⁺)个六面体，每个六面体的大小为离散化所采用的分辨率越高，航迹规划结果的精度就越高，但同时问题的计算规模也会增大，导致效率降低；反之，分辨率低，可以节省一定的计算量，但规划的精度就会降低。

在对规划空间进行合理的网格法离散化后，航迹规划问题可表示为寻找从起始点S₀到目标点D之间的最优的航迹点序列{S₀,P₁,P₂,...,P_n,D}，使得水下自主航行器在沿着由这些航迹点连接而成的航迹上航行时航迹的某种代价最小。其中，P₁,...,P_n为某个六面体的某一顶点，如图1所示：

在经过网格法离散化处理后的航迹规划空间中，航迹的总长度由各段航迹边的长度相加得到，其优化目标函数可取为：

其中，l_i为第i条航迹边的长度。

假设水下自主航行器在航迹边i时与动态障碍物碰撞的概率为P_i，则在整条航迹上，水下自主航行器规避动态目标的优化目标函数为：

令P_i ^*＝log((1-P_i)^-1)，则水下自主航行器规避动态目标的优化目标可表述为：

对航迹长度与动态目标碰撞概率两项评价指标的形式做一致性处理后，就可以将两者统一于同一次优化过程中。引入加权系数k₁和k₂分别表征航迹长度优化与规避动态目标优化的权重值，最终的航迹优化目标取为如下形式：

k₁+k₂＝1，k₁，k₂＞0 (10)

考虑到水下自主航行器在一条航迹边上与动态目标碰撞的概率与该航迹边的长度正相关，同时，也为了在一定程度上消除优化目标函数中两项优化指标的数量级不匹配，在航迹安全性评价指标项前乘上了航迹长度l_i。选取了合适的优化目标函数之后，就可以按照某条航迹对应的目标函数值的大小来对航迹的优劣做一个综合的评价。本发明所选取的优化目标函数为和最小化形式，因此，如果组成一条航迹的各航迹边的代价之和越小，则该条航迹性能越好。

3.约束处理与搜索空间裁剪

水下自主航行器的三维航迹规划是一个带约束的优化问题。在水下自主航行器三维航迹规划中，必须考虑任务要求和水下自主航行器自身性能约束。这些约束包括：

1)最小安全航行高度

针对水下平坦地形和山地地形对水下自主航行器的最小安全航行高度分别规定为H_pmin与H_Mmin。

2)最大转弯角和最大俯仰角

记第i条航迹边的水平投影为r_i，水下自主航行器经过第i段航迹后在高度上的增值为ΔZ_i，水下自主航行器最大转弯角为θ，最大俯仰角为则有：

3)航迹边矢量方向

记起始点指向目标点的方向矢量为第i段航迹与的夹角为σ_i，规定对于i＝0,1,...,n，有：

σ_i＜π/2 (13)

4)最大航程长度

记起始点到目标点的直线距离为L_min，规定最大航程长度为2L_min，即

在三维空间中进行航迹规划，其问题规模远远大于二维空间的情况。在二维空间中，当前航迹点的邻接点有8个，而在三维空间中，当前航迹点的邻接点有26个，如图2(a)和图2(b)所示：

图中，实心圆点表示当前航迹点，空心圆点为其邻接点。当前航迹点的邻接点越多，则对于蚂蚁而言，在当前点可以选择的下一个航迹点也越多。因此，在三维空间中，蚂蚁的搜索空间远大于二维情况，从而问题规模显著增大。

针对水下自主航行器三维航迹规划搜索空间大的问题，首要的任务是裁剪搜索空间，并结合航迹规划的约束条件，生成新的搜索空间C'。C'满足：

1)C'是规划空间C的子集，且同C相比，C'的规模有显著减小。在C'中搜寻最优航迹的效率将高于在C中搜寻。

2)航行器在C'中航行时，其位置和转移状态都满足航迹规划的约束条件，即搜索空间C'中的航迹是可行航迹。

下面将详细阐述如何生成搜索空间C'。

为了便于问题的说明，先给出allowed_i,k集合的定义：

蚂蚁k当前的航迹点为i，航迹点u与航迹点i相邻，如果点u满足下列两个条件，则称航迹点u∈allowed_i,k：

1)蚂蚁k在本次迭代中尚未经过航迹点u。

2)航迹点u和边(i,u)满足航迹约束条件。

本发明提出允许后续航迹点的概念，即规定只有属于allowed_i,k集合的点才能成为蚂蚁k当前位置i的允许后续航迹点。按照这样的后续航迹点扩展策略，逐步形成航行器的航迹搜索空间C'。显然，航迹搜索空间C'是动态生成的，不同的航迹搜索过程将产生不同的C'。

采用这样的航迹空间生成方法，可以将约束条件与搜索空间的裁剪结合起来，合理缩小搜索空间，有利于提高所规划航迹的可用性以及提高航迹规划的效率。

4.改进的蚁群算法

本发明采用改进的蚁群算法进行航迹的优化。经典蚁群算法的信息交互主要通过信息素来完成，其收敛到最优解的过程为信息正反馈的动态过程。正反馈机制旨在强化性能较好的解，却使蚁群算法在求解问题中容易陷入搜索停滞。在“探索”和“利用”之间建立一个平衡点是蚁群算法研究的关键问题之一，既要使得蚁群算法的搜索空间尽可能大，以寻找那些可能存在最优解的解区间，同时又要充分利用蚂蚁群体内当前所具有的有效信息，使得蚁群算法搜索的侧重点放在那些可能具有较高适应值的个体所在的区间内，从而以较大的概率收敛到全局最优解。

由于经典的蚁群算法在进行航迹优化时效率不够理想，本发明针对水下自主航行器航迹规划的需求，在著名的ACS(Ant Colony System)及MMAS(Max-Min Ant System)的基础上提出一种带自适应阈值的最大最小蚂蚁系统算法MMATSAT(Max-Min Ant System withAdaptive Threshold)，其特征是根据蚁群算法搜索情况来自适应调整阈值的大小，并且对各航迹段上的信息量做最大值和最小值的限制，在一定程度上可以解决扩大搜索空间与寻找最优解之间的矛盾。

MMATSAT采用时变函数q₀(t)来代替“伪随机比例”状态转移规则中为常数项的q₀：

式中，s代表经典蚁群算法中蚂蚁k从城市i转移到城市j的概率；tabu_k记录着蚂蚁k已经遍历过的城市；q₀(t)∈(0,1)称为阈值；q为(0,1)间的随机数；S为计算选择的节点；τ_i,j(t)为t时刻城市i到城市j对应的边e_i,j上的信息素浓度，各条路径的初始信息素浓度相同，即τ_i,j(0)＝τ₀(τ₀为一正常数)；η_i,j(t)为启发式信息，一般取为1/d_ij，d_ij为城市i与城市j之间的距离；α，β为调节信息素浓度τ和启发式信息η相对重要性的参数。

在迭代初始阶段，蚁群搜索得到的最优解往往离全局最优解较远，因此q₀(t)取较小值，鼓励蚂蚁探索新航迹。随着迭代次数增加，迭代最优解逐步向全局最优解逼近，增大q₀(t)的值，有利于提高算法收敛速度。

根据以上分析，q₀(t)可选为如下阶梯函数：

其中，q₃₀＞q₂₀＞q₁₀≥0；T₁～T₃根据期望的优化时间而定。

如果一段时间内，蚁群搜索得到的最优解没有改善，说明搜索陷入了某个局部最优解(但未必是全局最优解)，此时可采用强制机制迫使蚂蚁重新探索新的解，比如令q₀(t)＝0，力图使算法从局部极小值中逃脱出来。

由于信息正反馈及信息素随时间衰减这两个因素的存在，在搜索陷入局部最优时，某些航迹上的信息素相对于其他航迹的信息素而言在数量上占绝对优势，这不利于蚂蚁搜索新的航迹。因此，本发明对各航迹边上的信息素量的上下限做以下规定：

其中，s_max，s_min是由待求解问题的构造图产生的伪可行解，s_max表示有l_max条权重为e_max构成的一个序列,e_max为构造图的最大权值，s_min表示有l_min条权重为e_min构成的一个序列,e_min为构造图的最小权值，s_max，s_min不一定要求是待求解的最小化问题的可行解，只要能求出其目标函数值即可；Q为一正常数，表示信息素总量。

基于改进蚁群算法的水下自主航行器三维航迹规划的算法流程如下：

Initial:

设置信息素初值

最优航迹代价＝∞

设置航迹起始点

计算各条边的广义代价

设置蚁群算法各参数

蚂蚁航迹表置空

Repeat:寻找最优航迹

Out：输出最优航迹

本发明采用的算法终止条件有2个：

1)迭代次数N等于最大迭代次数N_max。

2)连续N_isi次迭代最优解没有变化。N_isi∈Z⁺为迭代间隔次数。

5.算例

采用前面介绍的随机地形模拟与山峰地形模拟相结合的地形模拟方法来生成航迹规划模拟海底地形图，规划区域大小为90km×90km×2.5km。部分仿真数据如表1所示：

表1

仿真算例的参数设置如下：最大迭代次数N_max＝50，蚂蚁数M＝40，一次最大搜索步数S_max＝60，N_isi＝15，Q＝50，τ₀＝1，τ_max＝1，τ_min＝1.0e-4，α＝1，β＝5，最大转弯角θ＝60°，最大俯仰角H_Pmin＝25m，H_Mmin＝60m。图3显示了取k₁＝0.8，k₂＝0.2时的三维优化航迹图。图4则是上述情况下以等高线图形式表示的优化航迹图。

本发明提出一种真正可用于三维空间中满足水下自主航行器航迹规划任务要求的航迹规划方案，更好的实现地形跟随、地形回避和动态避障，提高航迹规划的效率及可行性。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种水下自主航行器的三维航迹智能规划方法，其特征在于，包括以下步骤：

1)海底地形模拟；

2)航迹评价指标和优化函数选取，通过对航迹规划问题的分析，建立了综合考虑航程长度及规避动态目标的评价函数模型；

3)约束处理和搜索空间划分，制定了一种将约束处理与搜索空间裁剪相结合的机制，用于在缩减搜索空间的同时使得航迹满足各项约束条件；

4)采用改进的蚁群算法进行航迹的优化。

2.根据权利要求1所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的海底地形模拟采用随机地形模拟与山峰地形模拟相结合的方法来模拟航迹规划的地形。

3.根据权利要求1或2所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的海底地形模拟具体过程为：

海底地形在高度方向上分解为两个组成部分，首先是产生地形平均高度的地形基准平面高度，其次是地形基准平面上叠加的地形起伏：

H(x,y)＝H₀+Z(x,y)

其中，H(x,y)为坐标(x,y)点的实际海拔高度；H₀为整个地区的平均高度；Z(x,y)为在地形平均高度基础上的地形起伏高度；

对于实际地形的起伏算法是用高斯分布的随机序列来表征的，地形采样点的相关性为指数衰减：

其中，τ_x和τ_y为两个采样点在x和y方向的距离；τ_xc和τ_yc是抗相关距离，对于典型地形，抗相关距离越大地形越平滑；A为衰减系数；

对山峰地形而言，Z(x,y)表示山峰的地形采样点，其模拟采用函数法来产生，函数形式为:

其中，Z₀为基准地形高度；Z_i为第i个山峰在基准地形之上的最高点的高度；x_0i和y_0i为第i个山峰最高点的坐标；x_si和y_si为第i个山峰沿x轴和y轴方向与坡度有关的量，其值越大，相应的山峰就越平坦，反之就越陡峭；n＝1时是孤立的山峰，n＞1是多个山峰组合。

4.根据权利要求1所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的航迹评价指标和优化函数选取具体为：

采用网格法来划分三维规划空间；

在对规划空间进行合理的网格法离散化后，航迹规划问题表示为寻找从起始点S₀到目标点D之间的最优的航迹点序列{S₀,P₁,P₂,...,P_n,D}，使得水下自主航行器在沿着由这些航迹点连接而成的航迹上航行时航迹的某种代价最小，其中，P₁,...,P_n为某个六面体的某一顶点；

在经过网格法离散化处理后的航迹规划空间中，航迹的总长度由各段航迹边的长度相加得到，其优化目标函数取为：

其中，l_i为第i条航迹边的长度；

假设水下自主航行器在航迹边i时与动态障碍物碰撞的概率为P_i，则在整条航迹上，水下自主航行器规避动态目标的优化目标函数为：

令P_i ^*＝log((1-P_i)^-1)，则水下自主航行器规避动态目标的优化目标表述为：

对航迹长度与动态目标碰撞概率两项评价指标的形式做一致性处理后，就将两者统一于同一次优化过程中，引入加权系数k₁和k₂，分别表征航迹长度优化与规避动态目标优化的权重值，其中k₁+k₂＝1，k₁、k₂＞0，最终的航迹优化目标取为如下形式：

选取了合适的优化目标函数之后，就按照某条航迹对应的目标函数值的大小来对航迹的优劣做一个综合的评价，采用所选取的优化目标函数为和最小化形式，因此，如果组成一条航迹的各航迹边的代价之和越小，则该条航迹性能越好。

5.根据权利要求4所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的采用网格法来划分三维规划空间具体为：

将三维空间沿着直角坐标系的各方向，按照设定的分辨率分割成六面体，从而得到离散化的规划空间。

6.根据权利要求4所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，考虑到水下自主航行器在一条航迹边上与动态目标碰撞的概率与该航迹边的长度正相关，同时为了消除优化目标函数中两项优化指标的数量级不匹配，在航迹安全性评价指标项前乘上了航迹长度l_i。

7.根据权利要求1所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的约束处理和搜索空间划分具体为：

在水下自主航行器三维航迹规划中，必须考虑任务要求和水下自主航行器自身性能约束，其中约束包括：

1)最小安全航行高度

针对水下平坦地形和山地地形对水下自主航行器的最小安全航行高度分别规定为H_pmin与H_Mmin；

2)最大转弯角和最大俯仰角

记第i条航迹边的水平投影为r_i，水下自主航行器经过第i段航迹后在高度上的增值为ΔZ_i，水下自主航行器最大转弯角为θ，最大俯仰角为则有：

3)航迹边矢量方向

记起始点指向目标点的方向矢量为第i段航迹与的夹角为σ_i，规定对于i＝0,1,...,n，有：

σ_i＜π/2

4)最大航程长度

记起始点到目标点的直线距离为L_min，规定最大航程长度为2L_min，即

针对水下自主航行器三维航迹规划搜索空间大的问题，进行搜索空间的裁剪，并结合航迹规划的约束条件，生成新的搜索空间C'。

8.根据权利要求7所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的新的搜索空间C'满足以下条件：

1)C'是规划空间C的子集，且同C相比，C'的规模有显著减小；在C'中搜寻最优航迹的效率将高于在C中搜寻；

2)航行器在C'中航行时，其位置和转移状态都满足航迹规划的约束条件，即搜索空间C'中的航迹是可行航迹。

9.根据权利要求7或8所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的新的搜索空间C'生成具体为：

提出允许后续航迹点的概念，即规定只有属于allowed_i,k集合的点才能成为蚂蚁k当前位置i的允许后续航迹点；按照这样的后续航迹点扩展策略，逐步形成航行器的航迹搜索空间C'；显然，航迹搜索空间C'是动态生成的，不同的航迹搜索过程将产生不同的C'；

其中allowed_i,k集合的定义：

蚂蚁k当前的航迹点为i，航迹点u与航迹点i相邻，如果点u满足下列两个条件，则称航迹点u∈allowed_i,k：

1)蚂蚁k在本次迭代中尚未经过航迹点u。

2)航迹点u和边(i,u)满足航迹约束条件。

10.根据权利要求1所述的一种水下自主航行器的三维航迹智能规划方法，其特征在于，所述的采用改进的蚁群算法进行航迹的优化具体为：

针对水下自主航行器航迹规划的需求，在传统的蚂蚁算法基础上提出一种带自适应阈值的最大最小蚂蚁系统算法MMATSAT，根据蚁群算法搜索情况来自适应调整阈值的大小，并且对各航迹段上的信息量做最大值和最小值的限制，用于解决扩大搜索空间与寻找最优解之间的矛盾；

MMATSAT采用时变函数q₀(t)来代替“伪随机比例”状态转移规则中为常数项的q₀：

式中，s代表经典蚁群算法中蚂蚁k从城市i转移到城市j的概率；tabu_k记录着蚂蚁k已经遍历过的城市；q₀(t)∈(0,1)称为阈值；q为(0,1)间的随机数；S为计算选择的节点；τ_i,j(t)为t时刻城市i到城市j对应的边e_i,j上的信息素浓度，各条路径的初始信息素浓度相同，即τ_i,j(0)＝τ₀，其中τ₀为一正常数；η_i,j(t)为启发式信息，取为1/d_ij，d_ij为城市i与城市j之间的距离；α，β为调节信息素浓度τ和启发式信息η相对重要性的参数；

在迭代初始阶段，蚁群搜索得到的最优解往往离全局最优解较远，因此q₀(t)取较小值，鼓励蚂蚁探索新航迹；随着迭代次数增加，迭代最优解逐步向全局最优解逼近，增大q₀(t)的值，有利于提高算法收敛速度；

根据以上分析，q₀(t)选为如下阶梯函数：

其中，q₃₀＞q₂₀＞q₁₀≥0；T₁～T₃根据期望的优化时间而定；

对各航迹边上的信息素量的上下限做以下规定：

其中，s_max和s_min是由待求解问题的构造图产生的伪可行解，s_max表示有l_max条权重为e_max构成的一个序列，e_max为构造图的最大权值，s_min表示有l_min条权重为e_min构成的一个序列，e_min为构造图的最小权值，s_max和s_min只要能求出其目标函数值即可；Q为一正常数，表示信息素总量。