CN111307158B - 一种auv三维航路规划方法 - Google Patents

Abstract

本发明公开了一种AUV三维航路规划方法，通过改进粒子初始化方法、进入障碍物的惩罚值设置以及加入粒子移动位置步长的设置，提高的算法的规划速度和规划速度。步骤主要包括：三维环境抽象建模和粒子位置的初始化、进入障碍物的惩罚值设置、计算代价函数值、粒子移动步长设置、比较粒子群的代价值，进行粒子位置更新、输出规划结果。相比于传统量子粒子群航路规划算法，本发明提出的三维航路规划方法考虑了海流因素的影响，使得算法应用更加广泛；同时，三维航路规划的实现相比于二维航路规划更加具有实用性，能够更好的满足实际航行需要。

Description

一种AUV三维航路规划方法

技术领域

本发明属于水下潜器三维路径规划技术领域，涉及一种AUV三维航路规划方法，特别是一种基于改进量子粒子群算法的水下潜器三维航路规划方法。

背景技术

海洋大约占地球表面积的71％，存储着庞大的资源，不仅拥有丰富的生物资源和水资源，水下石油等矿产资源同样不可估量。随着人类的不断发展，陆地资源呈开采殆尽趋势，海洋资源逐渐成为人类开发的重点。因此，海洋的开发利用已经成为各国的战略重点，也成为高科技研究的重要领域。自主水下航行器(Autonomous Underwater Vehicle简称AUV)是指能够自主的进行导航和规划的新型水下运载平台，可以代替人类在危险或者人类无法到达的区域进行水下作业，极大地拓展了人类的水下探测范围。作为一种智能化的海洋开发工具，AUV己经成为探测和开发海洋资源的重要手段，同时成为世界各国海洋领域研究的重点。由于海洋环境的复杂性，因此如何有效的利用海洋环境以及避开周围海洋环境中的威胁区及障碍物是AUV实现自主航行的关键。航路规划是实现AUV自主航行和自主作业的基础，其水平的高低在一定程度上标志着AUV的智能水平。因此研究AUV的航路规划方法对于提高我国的水下智能机器人的自主水平和加快海洋的开发利用均具有重要的意义。

量子行为粒子群优化算法(Quantum-behaved Particle Swarm Optimization，QPSO)是粒子群算法的一个改进版本，最早由孙俊提出。在量子粒子群算法中，粒子被视为具有波粒二象性，无法用确定的位置和速度来描述粒子，即不能使用粒子群算法中的位置和速度更新方程来表示粒子的状态，而是利用波函数进行代替。由于粒子被假设具有了量子行为，当粒子在搜索时，能够以一定的概率完全覆盖整个解空间，因此从理论上讲，相比粒子群算法，量子粒子群算法具有更强的全局搜索能力。专利CN201610887891.6公开了一种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法，该专利将粒子群算法进行改进，但是只是考虑最短路径，并没有考虑海流对AUV的影响。专利CN201210178003.5公开了一种基于粒子群算法的三维路径规划新方法，该专利是基于粒子群算法进行路径规划，考虑因素仅仅是路径长度，没有将AUV能耗作为航路优劣的评价标准。本发明是将量子粒子群算法进行改进，考虑了海流对AUV的影响，并将改进的量子粒子群算法应用于三维海洋环境中进行路径规划，目前暂时还没有公开的文献将量子粒子群算法应用于三维航路规划。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种将量子粒子群算法进行改进处理应用其中、并将海流因素考虑其中并且加入移动步长约束的AUV三维航路规划方法，用来提高规划速度和规划质量。

为解决上述技术问题，本发明的一种AUV三维航路规划方法，包括以下步骤：

步骤1：三维环境建模和粒子位置的初始化，根据空间中的起点以及终点的位置规划出航行空间范围，并且将粒子随机分布到非障碍物区域内；

所述三维环境建模具体为：首先将进行AUV航路规划的真实海洋环境空间划分为lon×lat×depth个网格，网格划分原则为：经度方向和纬度方向的网格间距与航行区域网格水深数据文件的分辨率保持一致，深度方向的网格间距不应超过航行区域最大深度值的1/10，然后进行AUV航行区域的地形场、海流场的建模；

AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现的，将航行空间中从文件中读取的水深值以下的区域设为禁航区；

AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的，如果海流数据文件的分辨率低于水深数据文件的分辨率，则对海流数据文件进行线性插值处理，反之，对海流数据文件进行稀疏化处理，使得海流数据文件的分辨率与水深数据文件的分辨率一致，从而使得每个网格都包含当前位置的海流信息；深度方向的海流大小设为0；

所述的粒子位置的初始化具体为：

设改进量子粒子群算法的解空间为P＝[p₁,p₂,...,p_n]∈R^m×n，其中n为粒子群的规模，每一维粒子群代表一条路径，m为粒子群的维度，对应为航路的节点个数，令P中的每个向量

其中x_j,y_j,z_j∈R^m，将p_i在X、Y、Z三个方向进行初始化，p_i在X、Y、Z方向上随机取点，具体计算公式如下：

rand_x为X方向最小值X_min到X方向最大值X_max之间的随机值；

rand_y为Y方向最小值Y_min到Y方向最大值Y_max之间的随机值；

rand_z为Z方向最小值Z_min到Z方向最大值Z_max之间的随机值；

如果出现粒子初始化在禁航区的情况时，就将此粒子重新初始化，直至所有的粒子都在非障碍物的区域内；

步骤2：进行障碍物的惩罚值设置，障碍物内的惩罚值设定为从障碍物中心位置向两端逐渐降低的方式；

步骤3：计算目标函数值，目标函数值由两部分组成，包括航路能耗和进入障碍物的惩罚值，航路能耗近似为航路节点之间的能量消耗的总和；

步骤4：设置粒子移动步长的约束条件；

步骤5：比较粒子群的目标函数值，进行粒子位置的更新，并且记录每次粒子更新的最新位置，加入步长约束，按照改进的量子粒子群算法的位置更新公式重新进行粒子位置寻优；

X轴方向位置更新具体为：

所述改进的量子粒子群算法的位置更新公式具体为：

X_i,j(t+1)＝X_i.j(t)+c₁·r₁·P_i,j(t)+c₂·step_s

其中，step_s为在阻力系数的影响下的移动步长，step_s满足：

step_s＝(X_i,j(t)+X_i,j+2(t))-2·X_i,j+1(t)·s

s为给定的阻力系数，同时，利用步骤4中的约束条件对step_s进行约束计算；

其中，P_i,j为X方向的位置偏移量，P_i,j(t)满足：

P_i，j(t)＝X′_i，j(t+1)-X_i，j(t)

其中，

pbest是指粒子历史最优位置，gbest是指粒子全局最优位置，mbest为粒子平均最优位置，r为在[0,1]均匀分布的随机变量，其中b为收缩扩张系数，b(t)＝b_m+(b_o-b_m)(T-t)/T，u为在[0,1]均匀分布的随机变量；

c₁为取自粒子群算法中的学习因子，设为：

c₁＝c_max-(t/T)·(c_max-c_min)

其中c_max为[0,1]之间的常数且大于c_min，c_min为[0,1]之间的常数且小于c_max；

r₁为在[0,1]均匀分布的随机变量，能够防止粒子陷入局部最优位置；

c₂为收缩位移参数，体现参数对收缩位移的影响，c₂大于零且小于移动步长step_s最大值；

Y轴和Z轴方向位置更新方法与上述X轴方向位置更新方法相同；

步骤6：当达到设定的最大迭代次数时，输出全局最优路径，否则，返回步骤3。

本发明还包括：

1.步骤3的目标函数具体为：

其中n为未进入障碍物粒子数，m为进入障碍物的粒子数，l_j为最近非障碍物区域的距离，K为给定的距离影响系数，f_i为航路能耗，f_i满足：

其中，V_x为AUV和速度的X方向上的分向量，D_x为航路距离的X方向上的分向量，V_y为AUV和速度的Y方向上的分向量，D_y为航路距离的Y方向上的分向量，V_z为AUV和速度的Z方向上的分向量，D_z为航路距离的Z方向上的分向量。

2.步骤4中设置粒子移动步长的约束条件具体为：设置最小步长为step_min，最大步长为step_max，记录每次粒子偏移量B，设置步长约束极限step0为：

step0＝step_max-(t/T)·(step_max-step_min)

其中t为当前迭代次数，T为最大迭代次数；

当B＜step0时，粒子的移动步长不进行约束；

当B＞step0时，粒子的移动步长约束方式为：

step(t+1)＝(step(t+1)/B)·step0

其中，step为需要进行约束的移动步长。

本发明的有益效果：本发明提出一种基于改进量子粒子群算法的AUV三维航路规划方法。该方法相比粒子群算法航路规划搜索算法全局性更强，加入了粒子位置移动步长约束，提高了规划速度和规划质量，并且加入海流影响，设计能耗环节，提高在实际海洋环境中实用性。

附图说明

图1为本发明提出的基于改进量子粒子群算法的AUV三维航路规划流程图。

图2为本发明采用的改进量子粒子群算法流程图。

具体实施方式

下面结合附图对本发明具体实施方式进行具体说明。

本发明步骤主要包括：

步骤1：三维环境建模和粒子位置的初始化。根据空间中的起点以及终点的位置规划出航行空间范围，并且将粒子随机分布到非障碍物区域内，以提高收敛速度。

步骤2：进入障碍物的惩罚值设置。障碍物内的惩罚值设定为从中心位置向两端逐渐降低的方式，以保证粒子在更新过程中能够逃离局部最优的情况。

步骤3：计算目标函数值。目标函数值由两部分组成，即航路能耗和进入障碍物的惩罚值。航路能耗近似为航路节点之间的能量消耗的总和。

步骤4：粒子移动步长约束设置。在本发明中，由于量子粒子群的位置变化是依据概率随机变化的，通过对粒子位置移动步长进行约束，可以提高规划航路的质量。

步骤5：比较粒子群的目标函数值，进行粒子位置的更新。比较不同粒子群之间的目标函数值，更新粒子的位置，并且记录每次粒子更新的最新位置。加入步长约束，按照改进的量子粒子群算法的位置更新公式重新进行粒子位置寻优。

步骤6：当满足算法迭代停止条件时输出全局最优路径，AUV三维航路规划结束。

结合图1和图2本发明具体实施步骤如下：

步骤1：三维环境建模和粒子位置的初始化。

步骤1.1航行空间抽象建模

首先将进行AUV航路规划的真实海洋环境空间划分为lon×lat×depth个网格。网格划分原则为：经度方向和纬度方向的网格间距与航行区域网格水深数据文件的分辨率保持一致，深度方向的网格间距不应超过航行区域最大深度值的1/10。其次进行AUV航行区域的地形场、海流场的建模。

AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现的，将航行空间中从文件中读取的水深值以下的区域设为禁航区，禁航区的目标函数具有很大的惩罚值。

AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的，如果海流数据文件的分辨率低于水深数据文件的分辨率，则对海流数据文件进行线性插值处理，反之，对海流数据文件进行稀疏化处理，使得海流数据文件的分辨率与水深数据文件的分辨率一致，从而使得每个网格都包含当前位置的海流信息；深度方向的海流大小设为0。

步骤1.2粒子位置的初始化

设改进量子粒子群算法的解空间为P＝[p₁,p₂,...,p_n]∈R^m×n，其中n为粒子群的规模，每一维粒子群代表一条路径，m为粒子群的维度，对应为航路的节点个数。由于节点应包含三维位置信息，因此令P中的每个向量

其中x_j,y_j,z_j∈R^m。将p_i在X、Y、Z三个方向进行初始化。p_i在X、Y、Z方向上随机取点，具体计算公式如下：

rand_x为X方向最小值X_min到X方向最大值X_max之间的随机值；

rand_y为Y方向最小值Y_min到Y方向最大值Y_max之间的随机值；

rand_z为Z方向最小值Z_min到Z方向最大值Z_max之间的随机值；

如果出现粒子初始化在禁航区的情况时，就将此粒子重新初始化，直至所有的粒子都在非障碍物的区域内，从而可以提高粒子群算法的收敛速度。

步骤2：进入障碍物的惩罚值设置。

由于在航行空间中会存在特别大的障碍物，因此当出现所有粒子都进入障碍物的情况时，对于传统处理方法，即加一个固定的惩罚值，就会出现粒子很难逃离障碍物的情况。本发明提出了一种从障碍物内部向外部逐渐降低其惩罚值的方法，不仅防止粒子会陷入障碍物中，而且能够提高寻找最优路径的效率。针对于三维空间中的山体障碍物，障碍物内部点的惩罚值与该点到非障碍物区域的距离成正相关。

步骤3：计算目标函数值。

算法的目标函数值由航路节点之间能量消耗和进入障碍物的代价值构成的。其中为了考虑海流的影响，将目标函数设置为能耗函数，和速度V_n为海流速度与AUV速度的和，能耗函数为：

其中，V_x为AUV和速度的X方向上的分向量，D_x为航路距离的X方向上的分向量，V_y为AUV和速度的Y方向上的分向量，D_y为航路距离的Y方向上的分向量，V_z为AUV和速度的Z方向上的分向量，D_z为航路距离的Z方向上的分向量，对于进入障碍物的粒子加上相应的惩罚值作为该航路的目标函数值。因此得到改进的目标值函数设为：

其中n为未进入障碍物粒子数，m为进入障碍物的粒子数，l_j为最近非障碍物区域的距离。K表示为距离影响系数，为体现出系数对距离的影响，系数的设置原则为大于零小于l_j中的最大值，此发明设为0.4。

步骤4：粒子移动步长设置。

为了加快算法的规划进程，提高规划质量，使航路更加平滑可行，要求算法初期能够进行大范围的搜索，而在算法优化后期应具备小范围优化的特点。在本发明中，改进的量子粒子群算法对粒子的移动步长进行了设置，本发明设置最小步长为step_min，最大步长为step_max，记录每次粒子偏移量B，设置步长约束极限step0为

step0＝step_max-(t/T)·(step_max-step_min)

其中t为当前迭代次数，T为最大迭代次数。

当B＜step0时，粒子的移动步长不进行约束。

当B＞step0时，将粒子的移动步长约束方式为：

step(t+1)＝(step(t+1)/B)·step0

其中，step为需要进行约束的移动步长。

通过分析可得，粒子的步长约束随着时间的变化会不断变小，因此在算法初期在大范围搜索阶段有利于快速初始化，逃离局部最优的情况；到了算法后期，算法处于小范围优化阶段，为防止粒子大范围移动再次陷入障碍物的情况，粒子进行小范围优化搜索。

步骤5：比较粒子群的目标函数值，进行粒子位置更新。

根据量子粒子群原理，进行不同粒子群之间的目标函数值比较，进行更新粒子群的位置，从而不断优化粒子群的代价值，位置偏移量更新公式为：

P_i,j(t)＝X′_i,j(t+1)-X_i,j(t)

其中P_i,j为X方向的位置偏移量，pbest是指粒子历史最优位置，gbest是指粒子全局最优位置，mbest为粒子平均最优位置，r为在[0,1]均匀分布的随机变量，其中b为收缩扩张系数，b(t)＝b_m+(b_o-b_m)(T-t)/T，u为在[0,1]均匀分布的随机变量。

此外本发明为了提高路径规划质量，使得规划出的航路更加光滑，加入了阻力系数，即：

step_s＝(X_i,j(t)+X_i,j+2(t))-2·X_i,j+1(t)·s

s为阻力系数，step_s为在阻力系数的影响下的移动步长，利用步骤4对step_s进行约束。

进而得到在收缩因子影响下的位置更新公式：

X_i,j(t+1)＝X_i.j(t)+c₁·r₁·P_i,j(t)+c₂·step_s

其中c₁为取自粒子群算法中的学习因子，体现了粒子向群体中优秀个体学习的能力，使粒子能够向最优位置靠近，设为：

c₁＝c_max-(t/T)·(c_max-c_min)

其中c_max的设置原则为[0,1]且大于c_min，c_min的设置原则为[0,1]且小于c_max。在具体实施例中设c_max＝0.8，c_min＝0.1，r₁为在[0,1]均匀分布的随机变量，能够防止粒子陷入局部最优位置。c₂为收缩位移参数，为体现参数对收缩位移的影响，参数的设置原则为大于零小于收缩位移的最大值，在具体实施例中设c₂＝2.5。

同理Y、Z方向用同样的方法进行位置更新。

步骤6：当满足算法迭代停止条件则输出全局最优路径，多AUV三维协同航路规划结束。

发明中提出基于改进量子粒子群算法进行三维航路的方法，通过改进粒子初始化方法、进入障碍物的惩罚值设置以及加入粒子移动位置步长的设置，提高的算法的规划速度和规划速度。步骤主要包括：三维环境抽象建模和粒子位置的初始化、进入障碍物的惩罚值设置、计算代价函数值、粒子移动步长设置、比较粒子群的代价值，进行粒子位置更新、输出规划结果。相比于传统量子粒子群航路规划算法，本专利提出的三维航路规划方法考虑了海流因素的影响，使得算法应用更加广泛；同时，三维航路规划的实现相比于二维航路规划更加具有实用性，能够更好的满足实际航行需要。

Claims (3)

1.一种AUV三维航路规划方法，其特征在于，包括以下步骤：

步骤1：三维环境建模和粒子位置的初始化，根据空间中的起点以及终点的位置规划出航行空间范围，并且将粒子随机分布到非障碍物区域内；

所述三维环境建模具体为：首先将进行AUV航路规划的真实海洋环境空间划分为lon×lat×depth个网格，网格划分原则为：经度方向和纬度方向的网格间距与航行区域网格水深数据文件的分辨率保持一致，深度方向的网格间距不应超过航行区域最大深度值的1/10，然后进行AUV航行区域的地形场、海流场的建模；

AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现的，将航行空间中从文件中读取的水深值以下的区域设为禁航区；

AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的，如果海流数据文件的分辨率低于水深数据文件的分辨率，则对海流数据文件进行线性插值处理，反之，对海流数据文件进行稀疏化处理，使得海流数据文件的分辨率与水深数据文件的分辨率一致，从而使得每个网格都包含当前位置的海流信息；深度方向的海流大小设为0；

所述的粒子位置的初始化具体为：

设改进量子粒子群算法的解空间为P＝[p₁,p₂,...,p_n]∈R^m×n，其中n为粒子群的规模，每一维粒子群代表一条路径，m为粒子群的维度，对应为航路的节点个数，令P中的每个向量

其中x_j,y_j,z_j∈R^m，将p_i在X、Y、Z三个方向进行初始化，p_i在X、Y、Z方向上随机取点，具体计算公式如下：

rand_x为X方向最小值X_min到X方向最大值X_max之间的随机值；

rand_y为Y方向最小值Y_min到Y方向最大值Y_max之间的随机值；

rand_z为Z方向最小值Z_min到Z方向最大值Z_max之间的随机值；

如果出现粒子初始化在禁航区的情况时，就将此粒子重新初始化，直至所有的粒子都在非障碍物的区域内；

步骤2：进行障碍物的惩罚值设置，障碍物内的惩罚值设定为从障碍物中心位置向两端逐渐降低的方式；

步骤3：计算目标函数值，目标函数值由两部分组成，包括航路能耗和进入障碍物的惩罚值，航路能耗近似为航路节点之间的能量消耗的总和；

步骤4：设置粒子移动步长的约束条件；

步骤5：比较粒子群的目标函数值，进行粒子位置的更新，并且记录每次粒子更新的最新位置，加入步长约束，按照改进的量子粒子群算法的位置更新公式重新进行粒子位置寻优；

X轴方向位置更新具体为：

所述改进的量子粒子群算法的位置更新公式具体为：

X_i,j(t+1)＝X_i.j(t)+c₁·r₁·P_i,j(t)+c₂·step_s

其中，step_s为在阻力系数的影响下的移动步长，step_s满足：

step_s＝(X_i,j(t)+X_i,j+2(t))-2·X_i,j+1(t)·s

s为给定的阻力系数，同时，利用步骤4中的约束条件对step_s进行约束计算；

其中，P_i,j为X方向的位置偏移量，P_i,j(t)满足：

P_i,j(t)＝X′_i,j(t+1)-X_i,j(t)

其中，

pbest是指粒子历史最优位置，gbest是指粒子全局最优位置，r为在[0,1]均匀分布的随机变量，其中b为收缩扩张系数，b(t)＝b_m+(b_o-b_m)(T-t)/T，u为在[0,1]均匀分布的随机变量；

c₁为取自粒子群算法中的学习因子，设为：

c₁＝c_max-(t/T)·(c_max-c_min)

其中c_max为[0,1]之间的常数且大于c_min，c_min为[0,1]之间的常数且小于c_max，t为当前迭代次数，T为最大迭代次数；r₁为在[0,1]均匀分布的随机变量，能够防止粒子陷入局部最优位置；

c₂为收缩位移参数，体现参数对收缩位移的影响，c₂大于零且小于移动步长step_s最大值；

Y轴和Z轴方向位置更新方法与上述X轴方向位置更新方法相同；

步骤6：当达到设定的最大迭代次数时，输出全局最优路径，否则，返回步骤3。

2.根据权利要求1所述的一种AUV三维航路规划方法，其特征在于：步骤3所述目标函数具体为：

其中n为未进入障碍物粒子数，m为进入障碍物的粒子数，l_j为最近非障碍物区域的距离，K为给定的距离影响系数，f_i为航路能耗，f_i满足：

其中，V_x为AUV和速度的X方向上的分向量，D_x为航路距离的X方向上的分向量，V_y为AUV和速度的Y方向上的分向量，D_y为航路距离的Y方向上的分向量，V_z为AUV和速度的Z方向上的分向量，D_z为航路距离的Z方向上的分向量。

3.根据权利要求1或2所述的一种AUV三维航路规划方法，其特征在于：步骤4所述设置粒子移动步长的约束条件具体为：设置最小步长为step_min，最大步长为step_max，记录每次粒子偏移量B，设置步长约束极限step0为：

step0＝step_max-(t/T)·(step_max-step_min)

其中t为当前迭代次数，T为最大迭代次数；

当B＜step0时，粒子的移动步长不进行约束；

当B＞step0时，粒子的移动步长约束方式为：

step(t+1)＝(step(t+1)/B)·step0

其中，step为需要进行约束的移动步长。

Images