CN113341998A - 一种改进蚁群算法的三维水下欠驱动auv路径规划方法 - Google Patents

Abstract

一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，涉及水下无人航行器路径规划方法。先建立海底三维地形空间模型；通过对各个平面的等距切割形成多个平面，根据实际需要将水下空间的三维网格序列坐标通过矩阵运算转换成实际位置坐标，完成水下欠驱动AUV的路径规划；给出水下机器人视觉区域的定义，得出下一路径点可行域范围；改进三维离散空间下的优化启发式函数；根据给定的数学模型和优化启发式函数，推导欠驱动AUV三维路径规划的改进蚁群算法，确定不同位置空间信息素的更新程度，实现复杂海底情况下的三维水下欠驱动AUV路径规划方法。有效避免训练过程中陷入局部最优的可能性，能提高搜索质量和搜索精度,输出稳定性更好。

Description

一种改进蚁群算法的三维水下欠驱动AUV路径规划方法

技术领域

本发明涉及水下无人航行器的路径规划方法，尤其是涉及一种改进蚁群算法的三维水下欠驱动AUV路径规划方法。

背景技术

水下无人航行器(Autonomous underwater vehicle,AUV)的三维空间路径规划能力是实现海洋资源开发、海底地形勘测等领域重要技术前提。由于其潜在的技术优势，水下航行器已广泛应用于商业、科学和军事领域，如海洋石油和避让鱼雷等。在这些应用中，通常需要高精度来完成特定的任务。水下完全未知环境下的路径规划问题是水下机器人研究的一个重要分支。在复杂的海底环境下，水下机器人从起始点到目标点需要有一个避障的最优路径来解决路径规划问题。

现有的路径规划方法大多在二维平面上，三维平面路径规划算法计算过程复杂，存储信息量大，设计难度大。难以直接执行全局规划，以及其他问题。许多算法被提出用以解决三维空间中的路径规划问题，并取得良好的效果，例如人工势场算法、A*算法(Carrollet al.1992)、遗传算法(Hao and Zhang 2003)和粒子群优化算法(Chen et al.2013a)。然而，它们也有一些局限性。人工势场算法由于优化过程复杂，容易陷入局部最优路径。当维数增加时，A*算法不能满足时间与空间要求。当环境比较复杂时，遗传算法很难找到可行路径。粒子群算法是一种拟三维规划算法。蚁群算法是一种新的智能优化算法(Blum 2005；Narasimha et al.2013；Reed et al.2014；Arora et al.2019)。蚁群算法具有群智能、正反馈机制和分布式计算等特点。它可以用于二维或三维路径规划。传统蚁群算法很容易存在陷入局部最优、三维路径规划质量差、精度低等问题。

发明内容

本发明的目的是提供利用粒子群算法对传统蚁群方法中过早出现的局部最优问题进行全局寻优的一种改进蚁群算法的三维水下欠驱动AUV路径规划方法。

本发明包括以下步骤：

1)欠驱动自主水下航行器根据当前任务，利用均匀网格法沿坐标系建立复杂水下空间模型；

2)利用复杂水下空间模型，通过对各个平面的等距切割形成多个平面，根据实际需要将水下空间的三维网格序列坐标通过矩阵运算转换成实际位置坐标，完成水下欠驱动AUV的路径规划；

3)根据水下欠驱动AUV的运动特点和模拟实际海底环境下的能见度，给出水下机器人视觉区域的定义，得出下一路径点的可行域范围；

4)改进三维离散空间下的优化启发式函数，由三部分组成，包括下一目标点的可行性、当前路径函数与全局路径函数，其中全局路径函数由基于粒子群的优化算法得出；

5)根据给定的数学模型和步骤4)中所得优化启发式函数，推导欠驱动AUV三维路径规划的改进蚁群算法，通过多次的迭代，确定不同位置空间信息素的更新程度，实现复杂海底情况下的三维水下欠驱动AUV路径规划方法。

在步骤1)中，所述利用均匀网格法沿坐标系建立复杂水下空间模型的具体方法为：欠驱动自主水下航行器(AUV)根据当前任务，利用均匀网格法沿坐标系O-xyz建立复杂水下空间OABC-O′A′B′C′模型，其中x轴遵循经度增量方向，y轴遵循纬度增量方向，z轴垂直于x、y轴所形成的平面，方向向上(指向地心相反方向)，三轴垂直相交于O点。

在步骤2)中，所述完成水下欠驱动AUV的路径规划的具体步骤可为：利用步骤1)中建立完成的复杂水下空间模型，通过对各个平面的等距切割形成多个平面，这些平面组成多个有限数量的三维网格序列坐标N(i,j,k)，根据实际需要将水下空间的三维网格序列坐标N(i,j,k)通过矩阵运算转换成实际位置坐标n(x_i,y_j,z_k)，水下欠驱动AUV的路径规划在此空间中完成；

网格序列坐标通过如下方法转换为实际坐标：

其中，λ是序列坐标到位置坐标的转移向量，λ₁,λ₂,λ₃为空间变换向量的值；i,j,k分别表示延x轴、y轴、z轴所切割的当前平面上的序列坐标；x_i,y_i,z_i分别表示在x轴、y轴、z轴上的实际位置坐标。

在步骤3)中，所述得出下一路径点的可行域范围的具体步骤可为：根据步骤2)水下欠驱动AUV的运动特点和模拟实际海底环境下的能见度，给出水下机器人视觉区域的定义，默认沿x轴方向为前进主方向，单位步进视距为L_x，通过与俯仰角与偏航角的正弦值相乘可求得在y轴与z轴上的可视最大距离为L_y与L_z，因此得出下一路径点的可行域范围是Γ＝L_y·L_z；

水下欠驱动AUV的水下可视区域规则表达式为：

其中，θ是欠驱动AUV的俯仰角，ψ是欠驱动AUV的偏航角；L_y和L_z分别是在y轴方向和在x轴方向的可视距离；Γ为欠驱动AUV下一路径点在x-yz平面的可行域。

在步骤4)中，所述改进三维离散空间下的优化启发式函数是三维空间路径规划方法的重要组成部分，该函数搜寻在可视区域内，每一个蚂蚁从平面上的当前点到下一路径平面的所有可行点，以保证改进蚁群算法在合理的时间内搜索全局最优解；

所述改进三维离散空间下的优化启发式函数主要包括以下：

优化启发式函数：

其中，D_present(t)、D_global分别是基于粒子群优化算法的改进局部与全局路径长度函数，使整个路径长度尽可能短；r₁，r₂是学习率；η₁，η₂是0和1之间的随机数；S(t)为路径可行函数；

路径可行函数：

局部与全局路径函数：

其中：其中N_nextp是蚂蚁k在此迭代中搜索的最优点；N_nextg是最后一次迭代中所有蚂蚁搜索的最优点；w是权重；D_Nx,Ny表示从点N_x到点N_y的距离。

在步骤5)中，所述欠驱动AUV三维路径规划的改进蚁群算法，包括局部信息素轨迹更新方法、全局信息素轨迹更新方法以及最优路径价值系数；

不同位置空间的信息素更新方法包括以下三种，分别是选择概率函数、本地信息素更新函数和全局轨迹信息素更新函数，其表达式分别为：

选择概率函数：

局部信息素轨迹更新函数：

全局信息素轨迹更新函数：

其中：σ是常系数，ρ是局部信息素衰减系数，

是下一可行位置点的信息素的值，Δτ^k(t)是蚂蚁k经过N_next点时释放的信息素的值。

本发明针对水下欠驱动无人航行器三维路径规划中的位置、环境等随时间变化的要求设计一种改进的蚁群优化算法，用于求解水下机器人在三维复杂海底空间中的路径规划问题。有别于传统的蚁群算法，有效的避免训练过程中陷入局部最优的可能性，并利用优化的启发式函数来调节局部信息素在全局信息素中的优先度，提高算法的性能。首先，建立海底三维地形空间模型。然后改进将蚁群算法应用于水下三维空间的路径规划。目标是在三维环境下找到起始点和目标点之间的最优路径。给出改进蚁群算法的具体实现过程，并与传统蚁群算法进行仿真比较，结果表明改进算法能提高搜索质量和搜索精度。输出稳定性更好。

相比已有的二维平面路径规划方法，本发明不仅将其扩展到三维空间中，而且还能针对局部最优解与全局最优解做出分析，判断每个局部解是否陷入局部寻优，是否对障碍点有较强的躲避能力。这些对于欠驱动水下无人航行器不仅具有科学的研究意义，而且还具有工程应用价值，其更接近实际使用，有利于加快本发明的技术转化。

本发明突出的有益效果主要体现在：

1、定义空间坐标变换方程，将位置信息转化成有限的离散位置点，简化计算过程；

2、定义优化的启发式函数，其中为避免局部寻优时陷入局部寻优的死穴，加入基于粒子群算法的局部路径函数，有效解决上述问题；

3、优化的启发式函数通入到优化的蚁群算法中，经过多次迭代，实现对复杂海底环境下水下无人航行器的路径规划问题。

附图说明

图1改进蚁群算法的三维水下欠驱动AUV路径规划流程图。

图2路径规划空间。

图3路径规划空间的平面分割图。

图4无人水下航行器可视区域。

图5基于改进蚁群算法的路径规划在三维路径下的仿真结果。

图6基于传统蚁群算法的路径规划在三维路径下的仿真结果。

图7基于改进蚁群算法的最佳适应度变化趋势。

图8基于蚁群算法的最佳适应度变化趋势。

具体实施方式

以下实施例将结合附图对本发明做更详细的描述。本发明包括以下步骤：

步骤1欠驱动无人自主水下航行器根据当前任务，如图3所示，首先利用均匀网格法沿坐标系O-xyz建立复杂水下空间OABC-O′A′B′C′模型，其中x轴遵循经度增量方向，y轴遵循纬度增量方向，z轴垂直于x、y轴所形成的平面，方向向上(指向地心相反方向)，三轴垂直相交于O点。沿x轴以最大长度|OA|作为立方体的边长，沿y轴以最大长度|OC|作为立方体的宽，沿z轴以最大长度|OO’|作为立方体的高，从而得到立方体空间OABC-O′A′B′C′。

将路径规划空间划分为网格，生成抽象的海底模型。将路径规划空间沿直线OA绘制成n个部分，得到n+1个平面Pi(i＝0,1,…,n)，任意平面Pi沿直线v绘成m个部分，沿直线h绘成l个部分。

通过单元网格法，路径规划空间OABC-O′A′B′C′被分散成许多三维网格点，P*就是这些点组成的集合。

随后建立序列坐标和位置坐标，集合P*中的任何一点都对应两个坐标，即序列坐标N(i,j,k)(i＝1,2,3…,n；j＝1,2,3…,m；k＝1,2,3…,l)和位置坐标n(x_i,y_j,z_k)。序列坐标N(i,j,k)为绘制空间过程切割各个平面所形成区域的序号。位置坐标n(x_i,y_j,z_k)为O点对应位置的经度偏移距离、纬度偏移距离、深度偏移距离。

序列坐标与位置坐标的关系满足如下坐标变换方程:

令λ＝[λ₁λ₂λ₃]，则上述方程可描述为

n＝λ·N； (2)

其中，λ是序列坐标到位置坐标的转移向量。

步骤2利用步骤1中的信息，三维海底环境模型将整个搜索空间划分为一系列包含障碍点的离散区域。当AUV从当前点移动到下一个点时，搜索区域被限制在视觉域空间内，该可以提高改进蚁群算法的搜索效率。

假设路径规划的主方向是沿x轴方向。欠驱动AUV沿x方向运动。为降低路径规划的复杂性，水下机器人的运动通过三个方向进行简化：前向，横向和纵向运动。

根据水下机器人的运动特点，给出水下机器人视觉域的定义如下：

定义1、如图4所示，当AUV从平面P_i的当前点N_now向前移动一个单元的长度L_x时，它允许蚂蚁进行最大横向运动距离为L_y，和最大纵向运动距离L_z。其中，L_x表示蚂蚁在x轴方向上的最大可视距离。Γ是P_i+1矩形平面的选择域，它包含下一个路径可行点的集合allowP*_i+1。

矩形平面选择域Γ满足如下关系式：

θ是欠驱动AUV的俯仰角，且满足关系θ_min≤θ≤θ_max。其中，θ_max和分θ_min别表示俯仰角的上下界值，且|θ_max|＝|θ_min|。Φ是欠驱动AUV的偏航角，且满足关系Φ_min≤Φ≤Φ_max。其中，Φ_max和分Φ_min别表示偏航角的上下界值，且|Φ_max|＝|Φ_min|。

根据公式(1)以及公式(3)，推导出可视矩形区域点集N(i，j，k)如下：

在改进的蚁群算法中，路径搜索是根据概率进行的。在此使用一种与传统决策不同的规则，称其为伪随机决策规则。该规则可以使用当前点之间的距离启发式信息以及先前已经存储的信息素，并且有探索的倾向。下面的过程显示在平面P_i上的当前点N_now中的蚂蚁如何选择平面P_i+1上的下一个路径点N_next。根据三维海底环境模型选取P_i+1平面空间中的可行路径点。计算平面P_i+1选择空间中任意点N_next的改进启发式信息值。

根据步骤4的要求，改进启发式函数是三维空间路径规划方法的重要组成部分，这一步也是本发明的主要创新点。该函数搜寻在可视区域内，每一个蚂蚁从平面P_i上的当前点N_now到下一路径平面P_i+1的所有可行点。启发式函数是表征未来信息的载体，也是三维路径规划算法的重要组成部分。启发式函数必须保证改进蚁群算法在合理的时间内搜索全局最优解。

基于粒子群算法的改进启发式函数如下：

其中：N_next属于allowP*_i+1，allowP*_i+1是蚂蚁k下一选择P_i+1平面上的所有可行点的集合。D_present(t)、D_global(t)是基于粒子群优化算法改进的路径长度函数，使整个路径长度尽可能短；r₁，r₂是学习率；η₁，η₂是0和1之间的随机数；S(t)为路径可行函数；

路径可行函数：

其中，1表示点N_next是可行的且N_next∈allowP*_i+1；0表示点N_next是障碍点。

在改进蚁群算法的启发式函数中，D_present(t)和D_global(t)是非常重要的因素，也是本方法的核心创新点。引入粒子群算法中速度解的介质来构造路径长度函数。它们的给出如下：

其中：N_nextp是蚂蚁k当前迭代中搜寻到的最优点；N_nextg是最后一次迭代中所有蚂蚁搜寻到的最优点；w是权重；

表示从点N_now到点N_nextp的距离；

表示从点N_start，到点N_nextp的距离；

表示从点N_nextp到点N_end的距离；

表示从点N_now，到点N_nextg的距离；

表示从点N_start到点N_nextg的距离；

表示从点N_nextg到点N_end的距离；

根据步骤5所述，信息素更新包括局部信息素轨迹更新和全局信息素轨迹更新两部分。在蚂蚁通过这个点之后，本地信息素路径会被更新。信息素的更新可以提高蚂蚁搜索到全局搜索目标的概率。

在更新信息素之前，蚂蚁k首先根据选择概率函数选择P_i+1平面上的下一个点N_next。

洗择概率函数表达式：

其中，σ是常系数。

局部信息素轨迹更新方法表示为：

其中ρ是当前信息素的衰减系数，

是可行点N_next的信息素的值，Δr^k(t)是蚂蚁k在经过N_next释放的信息素的值。

全局信息素轨迹更新方法表示为：

f_best＝min(fⁱ) (11)

其中，f^k为适应度值，表示蚂蚁k构造其最优路径的代价，Δδ是限制条件与蚂蚁k运动有关。ρ′是全局轨迹信息素的衰减系数。在所有蚂蚁都构建好它们的路径之后，全球信息素路径会被更新。

三维路径规划过程的求解如图1所述：

建立三维海底环境模型，确定蚂蚁运动的主要方向后，首先确定水下环境模型中的起始点和目标点。

基于启发式信息和信息素权重值，根据公式(8)确定蚂蚁搜索的下一个点。

然后根据公式(9)更新局部信息素轨迹。

确定是否所有的蚂蚁都完成创建路径的工作。如果没有，则继续规划当前路径。

根据式(12)更新全局信息素轨迹，确定算法是否满足停止条件，最优输出结果是否满足条件。否则，返回步骤3。

仿真实验与验证分析：下面举例说明并验证本方法的有效性；

为验证所提供路径规划方法的优点和有效性，及避免陷入局部最优的效果，并从实际工程的角度考虑设计一种复杂海底环境下三维路径规划的仿真环境。

仿真基于20km*20km*2000m的模拟三维复杂水下地形环境视图，如图2所示，起始位置为(1，10，4)，对应真实距离n_start为(1km，10km，0.8km)；终点位置为(21，8，5)，对应真实距离为n_end为(21km，8km，1km)；步进距离L_x为1，对应转移坐标系数λ为(1km，1km，0.2km)；蚂蚁数量为10只，迭代次数为100次。

模型参数如下：仿真中所用到的模型参数为：学习率r₁＝2，r₂＝2；路径长度函数中权重w＝2；俯仰角最大值θ＝[-22°，22°]；偏航角Φ＝[-22°，22°]；前信息素的衰减系数ρ＝0.5；全局轨迹信息素的衰减系数ρ′＝0.2；常系数σ＝1；MATLAB的仿真结果如图5所示。图6为传统蚁群方法在三维复杂海底环境下欠驱动AUV的路径规划的仿真结果，与图5的改进后的方法明显优于传统蚁群方法；图7与图8分别是改进方法与传统方法的最佳个体适应度变化趋势，从中不难发现，相较于传统方法，改进方法在刚开始迭代时适应度值下降速度更快，且最终稳定结果的适应度值也更小，有效避免陷入局部最优。

Claims (6)

1.一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于包括以下步骤：

1)欠驱动自主水下航行器根据当前任务，利用均匀网格法沿坐标系建立复杂水下空间模型；

2)利用复杂水下空间模型，通过对各个平面的等距切割形成多个平面，根据实际需要将水下空间的三维网格序列坐标通过矩阵运算转换成实际位置坐标，完成水下欠驱动AUV的路径规划；

3)根据水下欠驱动AUV的运动特点和模拟实际海底环境下的能见度，给出水下机器人视觉区域的定义，得出下一路径点的可行域范围；

4)改进三维离散空间下的优化启发式函数，由三部分组成，包括下一目标点的可行性、当前路径函数与全局路径函数，其中全局路径函数由基于粒子群的优化算法得出；

5)根据给定的数学模型和步骤4)中所得优化启发式函数，推导欠驱动AUV三维路径规划的改进蚁群算法，通过多次的迭代，确定不同位置空间信息素的更新程度，实现复杂海底情况下的三维水下欠驱动AUV路径规划方法。

2.如权利要求1所述一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于在步骤1)中，所述利用均匀网格法沿坐标系建立复杂水下空间模型的具体方法为：欠驱动自主水下航行器(AUV)根据当前任务，利用均匀网格法沿坐标系O-xyz建立复杂水下空间OABC-O′A′B′C′模型，其中x轴遵循经度增量方向，y轴遵循纬度增量方向，z轴垂直于x、y轴所形成的平面，方向向上(指向地心相反方向)，三轴垂直相交于O点。

3.如权利要求1所述一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于在步骤2)中，所述完成水下欠驱动AUV的路径规划的具体步骤为：利用步骤1)中建立完成的复杂水下空间模型，通过对各个平面的等距切割形成多个平面，这些平面组成多个有限数量的三维网格序列坐标N(i,j,k)，根据实际需要将水下空间的三维网格序列坐标N(i,j,k)通过矩阵运算转换成实际位置坐标n(x_i,y_j,z_k)，水下欠驱动AUV的路径规划在此空间中完成；

网格序列坐标通过如下方法转换为实际坐标：

其中，λ是序列坐标到位置坐标的转移向量，λ₁,λ₂,λ₃为空间变换向量的值；i,j,k分别表示延x轴、y轴、z轴所切割的当前平面上的序列坐标；x_i,y_i,z_i分别表示在x轴、y轴、z轴上的实际位置坐标。

4.如权利要求1所述一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于在步骤3)中，所述得出下一路径点的可行域范围的具体步骤为：根据步骤2)水下欠驱动AUV的运动特点和模拟实际海底环境下的能见度，给出水下机器人视觉区域的定义，默认沿x轴方向为前进主方向，单位步进视距为L_x，通过与俯仰角与偏航角的正弦值相乘可求得在y轴与z轴上的可视最大距离为L_y与L_z，得出下一路径点的可行域范围是Γ＝L_y·L_z；

具体的，水下欠驱动AUV的水下可视区域规则表达式为：

其中，θ是欠驱动AUV的俯仰角，ψ是欠驱动AUV的偏航角；L_y和L_z分别是在y轴方向和在x轴方向的可视距离；Γ为欠驱动AUV下一路径点在x-yz平面的可行域。

5.如权利要求1所述一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于在步骤4)中，所述改进三维离散空间下的优化启发式函数是三维空间路径规划方法的重要组成部分，该函数搜寻在可视区域内，每一个蚂蚁从平面上的当前点到下一路径平面的所有可行点，以保证改进蚁群算法在合理的时间内搜索全局最优解；

所述改进三维离散空间下的优化启发式函数主要包括以下：

优化启发式函数：

其中，D_present(t)、D_global分别是基于粒子群优化算法的改进局部与全局路径长度函数，使整个路径长度尽可能短；r₁，r₂是学习率；η₁，η₂是0和1之间的随机数；S(t)为路径可行函数；

路径可行函数：

局部与全局路径函数：

其中：其中N_nextp是蚂蚁k在此迭代中搜索的最优点；N_nextg是最后一次迭代中所有蚂蚁搜索的最优点；w是权重；D_Nx,Ny表示从点N_x到点N_y的距离。

6.如权利要求1所述一种改进蚁群算法的三维水下欠驱动AUV路径规划方法，其特征在于在步骤5)中，所述欠驱动AUV三维路径规划的改进蚁群算法，包括局部信息素轨迹更新方法、全局信息素轨迹更新方法以及最优路径价值系数；

不同位置空间的信息素更新方法包括以下三种，分别是选择概率函数、本地信息素更新函数和全局轨迹信息素更新函数，其表达式分别为：

选择概率函数：

局部信息素轨迹更新函数：

全局信息素轨迹更新函数：

其中：σ是常系数，ρ是局部信息素衰减系数，

是下一可行位置点的信息素的值，Δτ^k(t)是蚂蚁k经过N_next点时释放的信息素的值。

Images