CN116088516A - 一种基于改进粒子群算法的水下航行器三维路径规划方法 - Google Patents

Abstract

本发明涉及路径规划领域，尤其涉及一种基于改进粒子群算法的水下航行器三维路径规划方法。包括如下步骤：设置任务以及获取原始数据：设置任务海区范围，导入任务海区的地形数据；在三维空间中，用节点的三维坐标建立从起点到终点的路径模型；考虑航行的安全性、可执行性、经济性建立评价函数；对传统粒子群算法进行改进，利用算法对路径进行优化；设定算法参数，进行路径规划，得出规划结果。本发明以三维坐标描述水下航行器的路径，并对传统粒子群优化算法进行改进，提高优化效果，利用该方法进行水下航行器的路径规划，从而得到最优的规划路径，可以显著提升水下航行器路径的优化质量，提高路径的适用性。

Description

一种基于改进粒子群算法的水下航行器三维路径规划方法

技术领域

本发明涉及路径规划领域，尤其涉及一种基于改进粒子群算法的水下航行器三维路径规划方法。

背景技术

水下航行器路径规划问题就是在特定约束条件下，构造合理的路径，以满足水下航行器的航行需求。目前常用的路径规划方法主要包括：遗传算法、模拟退火算法、粒子群算法、蚁群算法以及启发式算法等。水下航行器执行任务时，通常具有明确出发位置和到达位置，而且航行器的航行受其机动性能的限制，需要综合考虑水平方向转向角、爬升/下降角度，水下航行器的续航能力，航行路径的安全性、平滑度等诸多因素。而目前已有的路径规划方法主要基于二维路径进行规划，综合考虑因素较少，采用的环境模型并未完全贴近实际，导致算法优化效果不佳，应用受限。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种基于改进粒子群算法的水下航行器三维路径规划方法，以使其可以根据任务区域海底地形，结合水下航行器运动特性，综合考虑航行的安全性、可执行性、经济性，据此进行水下航行器的路径规划，以提高规划路径的适用性。

为实现上述目的，本发明采用如下技术方案。

一种基于改进粒子群算法的水下航行器三维路径规划方法，包括如下步骤：

S1.设置任务以及获取原始数据：设置任务海区范围，导入任务海区的地形数据，所述地形数据包含对应海区范围内海底高程的测量信息；

S2.建立路径模型：在三维空间中，用节点的三维坐标建立从起点到终点的路径模型；

S3.建立路径评价模型：考虑航行的安全性、可执行性、经济性；基于路径所经过区域的水深建立安全性评价函数；基于路径水平和垂直方向上转角大小建立可执行性评价函数；基于路径的路径长度建立经济性评价函数，通过加权得到路径的综合评价函数；

S4.对传统粒子群算法进行改进，利用该算法对路径进行优化；

用下标i区分不同的路径，第i个粒子表示第i条路径；

第i条路径表示为

第i条路径的第j个节点用三维坐标p_i,j＝[x_i,j,y_i,j,z_i,j]^T表示；

第i条路径的速度矢量表示为

以连续的三个节点的位置关系计算中间节点的优化方向，改进粒子位置的调整方式；用p_i,j-1、p_i,j和p_i,j+1表示路径上的三个连续节点，以中间节点为起点，作其指向前后节点的矢量，即(p_i,j-1-p_i,j)和(p_i,j+1-p_i,j)，设置加权因子分别为c₃和c₄，c₃和c₄为0～1之间的随机数，以两矢量加权和的方向为该点的位置调整方向，确定调整矢量的方向，调整比例因子为h，确定调整矢量的大小；

则节点p_i,j的速度变化值表示为：

则所有粒子的速度调整矩阵表示为u_i＝[u_i,1,u_i,2,...,u_i,N]；

基于速度调整矩阵进行位置调整，采用的粒子位置更新方法为：

以上标k表示优化更新的迭代次数

则

更新只与当前粒子位置有关，如下式：

粒子速度更新方式：

S5.设定粒子群算法的相关参数，进行路径的规划，得出规划结果。

对前述基于改进粒子群算法的水下航行器三维路径规划方法的进一步补充或完善，所述步骤S1具体包括：确定所需搜索的任务海区范围，包括X方向范围[X_min,X_max]和Y方向范围[Y_min,Y_max]，X_min和X_max分别表示任务海区X方向的最小值和最大值，Y_min和Y_max分别表示任务海区Y方向的最小值和最大值；导入该区域内的海底地形数据，包含对应海区范围内海底高程的测量信息，用于路径的评价。

对前述基于改进粒子群算法的水下航行器三维路径规划方法的进一步补充或完善，所述X方向为海区的经度方向，Y方向为海区的纬度方向。

对前述基于改进粒子群算法的水下航行器三维路径规划方法的进一步补充或完善，所述步骤S3具体包括：

S31.建立路径水平方向可执行性评价函数模型

通过路径上的节点确定各段航行路径水平方向和垂直方向的转角，整条路径分为N-1段，第n和n+1个节点间路径为第n段路径，1≤n≤N-1，第n段路径的方位角和俯仰角为：

设水平方向角度变化门限为Th_α，水平方向第n段和第n+1段之间的水平转角为|α_n+1-α_n|，水平方向转角应小于相应门限，|α_n+1-α_n|<Th_α；则用水平转角变化的最大值来表达路径水平方向可执行性评价函数，即：f_α＝max(|α_n+1-α_n|)/2π；

S32.建立路径垂直方向角度大小和角度变化大小的可执行性评价函数模型

设垂直方向爬升或下降的门限值为Th_β1；角度变化量的门限值为Th_β2；第n段路径垂直方向的角度为β_n，则第n段和第n+1段之间的垂直方向角度变化为β_n+1-β_n，则垂直方向角度大小和角度变化大小的可执行性评价函数为：

f_β2＝max(|β_n+1-β_n|)/2π；

S33.建立路径垂直方向可执行性评价函数模型

垂直方向可执行性即通过评价函数为：

其中，k₁表示水平方向执行权重，k₂表示垂直方向角度大小和角度变化大小对可执行性的权重；

S34.建立路径的经济性评价函数模型

以整条路径的航行距离来评价路径的经济性，整条路径的航行距离通过下式计算：

利用整条路径的距离与起点终点的直线距离比作为经济性评价函数：

路径距离越短，f_L越接近1；

S35.建立路径的安全性评价函数模型

用Dpt表示航道覆盖区域地形的内最小海底的海拔高度，用Th_DptL表示距海底深度余量Th_DptL≥0，Th_DptH表示距水面的深度余量Th_DptH≥0。PathL和PathH分别表示该段路径的最小高程和最大高程；用c_Dpt1和c_Dpt2表示该段路径距离海底和海面是否在安全范围内，则：

对于N个节点的路径，

下标n表示路径段；则路径的安全性评价函数表示为：

k₃和k₄表示两个指标的权重，根据该路径距离海底和海面安全程度的影响严重程度确定，影响程度越大，权重越大，取值范围为0～1；

S36.建立路径的综合评价函数模型

水下航行器路径的综合评价函数模型用下式计算：

f＝ω₁f_a+ω₂f_β+ω₃f_L+ω₄f_γ

其中，ω₁,ω₂,ω₃,ω₄分别为可执行性中水平角度限制、垂直角度限制，经济性，安全性对路径评价的权重。

其有益效果在于：

本发明的基于改进粒子群算法的水下航行器路径规划方法，针对目前水下航行器路径规划优化效果不佳，应用范围局限的问题，直接结合海洋水深数据，以三维坐标描述水下航行器的路径，并对传统粒子群优化算法进行改进，提高优化效果，利用该方法进行水下航行器的路径规划，从而得到最优的规划路径，本方法可以显著提升水下航行器路径的优化质量，提高路径的适用性。

附图说明

图1是本发明实施例的总体流程图；

图2是本方法和传统方法规划路径结果；

图3是本方法和传统方法规划结果适应度的统计直方图。

具体实施方式

以下结合具体实施例对本发明作详细说明。

本发明的基于改进粒子群算法的水下航行器路径规划方法，主要用于提高水下航行器的路径规划优化效果，拓展其应用范围，其具体包括如下步骤：

步骤1)，根据任务要求，确定所需搜索的任务海区范围，包括X方向范围[X_min,X_max]和Y方向范围[Y_min,Y_max]，X_min和X_max分别表示任务海区X方向的最小值和最大值，Y_min和Y_max分别表示任务海区Y方向的最小值和最大值，一般情况下X方向为海区的经度方向，Y方向为海区的纬度方向。

基于“数字水深数据库”(Digital Bathymetric DataBase)等数据路内容，导入该区域内的海底地形数据，该数据至少包含对应海区范围内海底高程的测量信息，用于路径的评价。

步骤2)中，建立路径模型，设置路径评价方法；

(1)路径表达方式；用一系列节点的三维坐标表示从起点到终点的路径。如对一条N个节点的路径，即可表示为：

矩阵的第n列就表示路径上第n个节点的三维坐标。x，y，z分别为三个方向的坐标值。

(2)路径评价方法；路径的评价指标包括：主要考虑航行的安全性、可执行性、经济性等，安全性通过路径所经过区域的水深评价，可执行性通过路径水平和垂直方向上角度大小及角度变化大小进行评价，经济性通过路径的路径长度来评价，通过加权得到路径的综合评价结果。

通过路径上的节点，可以确定各段航行路径水平方向和垂直方向的转角及其变化情况。整条路径可分为N-1段，第n和n+1个节点间路径为第n段路径，1≤n≤N-1。第n段路径的方位角和俯仰角可由节点坐标进行计算：

A)可执行性

可执行性通过路径上水下航行器的水平和垂直方向上的转角来评价，航行器受自身运动性能的影响，水平和垂直方向上的转角不能过大。

设水平方向角度变化门限为Th_α，水平方向第n段和第n+1段之间的水平转角可以用|α_n+1-a_n|表示，水平方向转角小于相应门限，|a_n+1-a_n|<Th_α。一条路径水平方向可执行性用水平转角变化的最大值来衡量：

f_α＝max(|α_n+1-α_n|)/2π

同时，航行器垂直方向上的角度及其变化量也有一定的限制。首先，垂直方向爬升或下降不能超过一定门限，用Th_β1表示；其次，角度变化量也不能超过一定限度，用Th_β2表示。β_n表示第n段路径垂直方向的角度，则第n段和第n+1段之间的垂直方向角度变化可以用β_n+1-β_n表示，则用下式描述垂直方向角度大小和角度变化大小对可执行性的评价方法。

f_β2＝max(|β_n+1-β_n|)/2π

那么，垂直方向可执行即可通过两方面进行综合评价：

其中，k₁和k₂表示两个指标的权重。根据垂直方向角度大小和角度变化大小对航行的影响程度，确定k₁和k₂的值。

B)经济性

以整条路径的航行距离来评价路径的经济性，整条路径的航行距离可以通过如下计算：

用整条路径的距离与起点终点的直线距离比作为评价标准：

路径距离越短，f_L越接近1。

C)安全性

路径的安全性主要考虑三个方面：①路径离海底的距离；②路径离水面的距离；③路径侧向离障碍物的距离。距海底过近可能导致触底事故，距水面较近则可能暴露自身位置，同时设定航道的安全距离，否则也导致航行绕过障碍物时触壁。通过比较路径高程和对应区域航道内水深，确定是否满足航行安全性需求。

对于某一段路径，用Dpt表示航道覆盖区域地形的内海底最小海拔高度，可能是负值，用Th_DptL表示距海底深度余量(Th_DptL≥0)，Th_DptH表示距水面的深度余量(Th_DptH≥0)。PathL和PathH分别表示该段路径的最小高程和最大高程。用c_Dpt1和c_Dpt2表示该段路径距离海底和海面是否在安全范围内：

对于N个节点的路径，

下标n表示路径段。路径安全性的评价指标可表示为：

k₃和k₄表示两个指标的权重，根据该路径距离海底和海面安全程度的影响严重程度确定，影响程度越大，权重越大，取值范围为0～1。

通过上述分析，即可对水下航行器路径实现综合评价，可用下式计算：

f＝ω₁f_a+ω₂f_β+ω₃f_L+ω₄f_γ

其中，ω₁,ω₂,ω₃,ω₄分别为可执行性中水平角度限制、垂直角度限制、经济性、安全性对路径评价的权重。该权重根据各因素的重要程度和实际影响需求设定。

步骤3)中，对传统粒子群速度更新方式进行了改进，并用于路径的规划中。

用下标i区分不同的路径，则第i条路径可表示为p_i：

第i条路径的第j个节点可用三维坐标p_i,j＝[x_i,j,y_i,j,z_i,j]^T表示。对应的，速度矢量也有相同的维度。第i条路径(即第i个粒子)的速度可表示为：

特别的，以连续的三个节点的位置关系计算中间节点的优化方向，改进粒子位置的调整方式。用p_i,j-1、p_i,j和p_i,j+1表示路径上的三个连续节点，以中间节点为起点，作其指向前后节点的矢量，即(p_i,j-1-p_i,j)和(p_i,j+1-p_i,j)，设置加权因子分别为c₃和c₄，c₃和c₄为0～1之间的随机数，以两矢量加权和的方向为该点的位置调整方向，确定调整矢量的方向，调整比例因子为h，确定调整矢量的大小。节点p_i,j的速度变化值可表示为：

则所有粒子的速度调整矩阵表示为u_i＝[u_i,1,u_i,2,...,u_i,N]。基于上述速度的新的位置调整方法，采用了新的粒子位置更新方法：上标k表示优化更新的迭代次数。

更新只与当前粒子位置有关，如下式

粒子速度更新采用相同的方式：

步骤4)中，得到规划结果。

设定粒子群算法的相关参数，进行路径的规划，得出规划结果。

为了验证本发明的有效应，以某海域的路径规划为例，进行了仿真实验。本发明基于粒子群算法进行设计。实验中，任务海区范围为X方向为[0,100]km，Y方向为[0,120]km；设置路径起点(37.42，91.58，-0,12)km。设置路径节点数为16，粒子的数量为40，迭代次数为200，惯性因子w＝0.4、学习因子c₁＝1、c₂＝0.2。图2给出了利用本方法和传粒子群算法的路径规划结果，可以明显看出两种方法规划的路径都可准确避开障碍，而本方法规划的路径到达目标位置路径更短，起伏更小，且更加平滑。同时进行了500次仿真，得到不同初始路径下规划结果的适应度值，其分布直方图如图3所示。统计两种方法适应度值的均值和标准差，传统方法均值为0.586，标准差0.134，本方法均值为0.193，标准差0.036，可以看出，本方法适应度明显优于传统方法，且更加稳健。

上述说明示出并描述了发明应用的实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是其他实施例的排除，而可用于各种其他组合、修改和环境，并能在本文所述发明构想范围内，通过上述教导或相关领域的技术进行改动。而本领域人员所进行的改动和变化不脱离发明的精神和范围，则都应在发明所附权利要求的保护范围内。

Claims (4)

1.一种基于改进粒子群算法的水下航行器三维路径规划方法，其特征在于，包括如下步骤：

S1.设置任务以及获取原始数据：设置任务海区范围，导入任务海区的地形数据，所述地形数据包含对应海区范围内海底高程的测量信息；

S2.建立路径模型：在三维空间中，用节点的三维坐标建立从起点到终点的路径模型；

S3.建立路径评价模型：考虑航行的安全性、可执行性、经济性；基于路径所经过区域的水深建立安全性评价函数；基于路径水平和垂直方向上转角大小建立可执行性评价函数；基于路径的路径长度建立经济性评价函数，通过加权得到路径的综合评价函数；

S4.对传统粒子群算法进行改进，利用该算法对路径进行优化；

用下标i区分不同的路径，第i个粒子表示第i条路径；

第i条路径表示为

第i条路径的第j个节点用三维坐标p_i,j＝[x_i,j,y_i,j,z_i,j]^T表示；

第i条路径的速度矢量表示为

以连续的三个节点的位置关系计算中间节点的优化方向，改进粒子位置的调整方式；用p_i,j-1、p_i,j和p_i,j+1表示路径上的三个连续节点，以中间节点为起点，作其指向前后节点的矢量，即(p_i,j-1-p_i,j)和(p_i,j+1-p_i,j)，设置加权因子分别为c₃和c₄，c₃和c₄为0～1之间的随机数，以两矢量加权和的方向为该点的位置调整方向，确定调整矢量的方向，调整比例因子为h，确定调整矢量的大小；

则节点p_i,j的速度变化值表示为：

则所有粒子的速度调整矩阵表示为u_i＝[u_i,1,u_i,2,...,u_i,N]；

基于速度调整矩阵进行位置调整，采用的粒子位置更新方法为：

以上标k表示优化更新的迭代次数

则

更新只与当前粒子位置有关，如下式：

粒子速度更新方式：

S5.设定粒子群算法的相关参数，进行路径的规划，得出规划结果。

2.根据权利要求1所述的基于改进粒子群算法的水下航行器三维路径规划方法，其特征在于，所述步骤S1具体包括：确定所需搜索的任务海区范围，包括X方向范围[X_min,X_max]和Y方向范围[Y_min,Y_max]，X_min和X_max分别表示任务海区X方向的最小值和最大值，Y_min和Y_max分别表示任务海区Y方向的最小值和最大值；导入该区域内的海底地形数据，包含对应海区范围内海底高程的测量信息，用于路径的评价。

3.根据权利要求2所述的基于改进粒子群算法的水下航行器三维路径规划方法，其特征在于，所述X方向为海区的经度方向，Y方向为海区的纬度方向。

4.根据权利要求1所述的基于改进粒子群算法的水下航行器三维路径规划方法，其特征在于，所述步骤S3具体包括：

S31.建立路径水平方向可执行性评价函数模型

通过路径上的节点确定各段航行路径水平方向和垂直方向的转角，整条路径分为N-1段，第n和n+1个节点间路径为第n段路径，1≤n≤N-1，第n段路径的方位角和俯仰角为：

设水平方向角度变化门限为Th_α，水平方向第n段和第n+1段之间的水平转角为α_n+1-α_n，水平方向转角小于相应门限，α_n+1-α_n<Th_α；则用水平转角变化的最大值来表达路径水平方向可执行性评价函数，即：f_α＝max(α_n+1-α_n)/2π；

S32.建立路径垂直方向角度大小和角度变化大小的可执行性评价函数模型

设垂直方向爬升或下降的门限值为Th_β1；角度变化量的门限值为Th_β2；第n段路径垂直方向的角度为β_n，则第n段和第n+1段之间的垂直方向角度变化为β_n+1-β_n，则垂直方向角度大小和角度变化大小的可执行性评价函数为：

f_β2＝max(β_n+1-β_n)/2π；

S33.建立粒径直方向执行即通过评价函数模型

垂直方向执行即通过评价函数为：f_β＝k₁·f_β1+k₂·f_β2；

其中，k₁表示水平方向执行权重，k₂表示垂直方向角度大小和角度变化大小对可执行性的权重；

S34.建立路径的经济性评价函数模型

以整条路径的航行距离来评价路径的经济性，整条路径的航行距离通过下式计算：

利用整条路径的距离与起点终点的直线距离比作为经济性评价函数：

路径距离越短，f_L越接近1；

S35.建立路径的安全性评价函数模型

用Dpt表示航道覆盖区域地形的内最小海底的海拔高度，用Th_DptL表示距海底深度余量Th_DptL≥0，Th_DptH表示距水面的深度余量Th_DptH≥0。PathL和PathH分别表示该段路径的最小高程和最大高程；用c_Dpt1和c_Dpt2表示该段路径距离海底和海面是否在安全范围内，则：

对于N个节点的路径，

下标n表示路径段；则路径的安全性评价函数表示为：

f_γ＝k₃·f_Dpt1+k₄·f_Dpt2

k₃和k₄表示两个指标的权重，根据该路径距离海底和海面安全程度的影响严重程度确定，影响程度越大，权重越大，取值范围为0～1；

S36.建立路径的综合评价函数模型

水下航行器路径的综合评价函数模型用下式计算：

f＝ω₁f_α+ω₂f_β+ω₃f_L+ω₄f_γ

其中，ω₁,ω₂,ω₃,ω₄分别为可执行性中水平角度限制、垂直角度限制、经济性、安全性对路径评价的权重。

Images