AUV三维航路规划方法.pdf

《AUV三维航路规划方法.pdf》由会员分享，可在线阅读，更多相关《AUV三维航路规划方法.pdf（12页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 202010196626.X (22)申请日 2020.03.19 (71)申请人 哈尔滨工程大学 地址 150001 黑龙江省哈尔滨市南岗区南 通大街145号哈尔滨工程大学科技处 知识产权办公室 (72)发明人 刘厂刘瑞航盛亮张志强 高峰靳光强 (51)Int.Cl. G01C 21/20(2006.01) (54)发明名称 一种AUV三维航路规划方法 (57)摘要 本发明公开了一种AUV三维航路规划方法， 通过改进粒子初始化方法、 进入障碍物的惩罚值 设置以及加入粒子移动位置。

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

3、其特征在于， 包括以下步骤： 步骤1： 三维环境建模和粒子位置的初始化， 根据空间中的起点以及终点的位置规划出 航行空间范围， 并且将粒子随机分布到非障碍物区域内； 所述三维环境建模具体为： 首先将进行AUV航路规划的真实海洋环境空间划分为lon latdepth个网格， 网格划分原则为： 经度方向和纬度方向的网格间距与航行区域网格水 深数据文件的分辨率保持一致， 深度方向的网格间距不应超过航行区域最大深度值的1/ 10， 然后进行AUV航行区域的地形场、 海流场的建模； AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现的， 将 航行空间中从文件中读取的水深值以下的区域设。

4、为禁航区； AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的， 如果 海流数据文件的分辨率低于水深数据文件的分辨率， 则对海流数据文件进行线性插值处 理， 反之， 对海流数据文件进行稀疏化处理， 使得海流数据文件的分辨率与水深数据文件的 分辨率一致， 从而使得每个网格都包含当前位置的海流信息； 深度方向的海流大小设为0； 所述的粒子位置的初始化具体为： 设改进量子粒子群算法的解空间为Pp1,p2,.,pnRmn， 其中n为粒子群的规模， 每一维粒子群代表一条路径， m为粒子群的维度， 对应为航路的节点个数， 令P中的每个向量 其中xj,yj,zjRm， 将pi在X、 Y、。

5、 Z三个方向进行初始化， pi在X、 Y、 Z方向上 随机取点， 具体计算公式如下： rand_x为X方向最小值Xmin到X方向最大值Xmax之间的随机值； rand_y为Y方向最小值Ymin到Y方向最大值Ymax之间的随机值； rand_z为Z方向最小值Zmin到Z方向最大值Zmax之间的随机值； 如果出现粒子初始化在禁航区的情况时， 就将此粒子重新初始化， 直至所有的粒子都 在非障碍物的区域内； 步骤2： 进行障碍物的惩罚值设置， 障碍物内的惩罚值设定为从障碍物中心位置向两端 逐渐降低的方式； 步骤3： 计算目标函数值， 目标函数值由两部分组成， 包括航路能耗和进入障碍物的惩 罚值， 航路。

6、能耗近似为航路节点之间的能量消耗的总和； 步骤4： 设置粒子移动步长的约束条件； 步骤5： 比较粒子群的目标函数值， 进行粒子位置的更新， 并且记录每次粒子更新的最 新位置， 加入步长约束， 按照改进的量子粒子群算法的位置更新公式重新进行粒子位置寻 优； X轴方向位置更新具体为： 所述改进的量子粒子群算法的位置更新公式具体为： Xi,j(t+1)Xi.j(t)+c1r1Pi,j(t)+c2step_s 权利要求书 1/2 页 2 CN 111307158 A 2 其中， step_s为在阻力系数的影响下的移动步长， step_s满足： step_s(Xi,j(t)+Xi,j+2(t)-2Xi,。

7、j+1(t)s s为给定的阻力系数， 同时， 利用步骤4中的约束条件对step_s进行约束计算； 其中， Pi,j为X方向的位置偏移量， Pi,j(t)满足： Pi,j(t)Xi,j(t+1)-Xi,j(t) 其中，pbest 是指粒子历史最优位置， gbest是指粒子全局最优位置， mbest为粒子平均最优位置， r为在 0,1均匀分布的随机变量， 其中b为收缩扩张系数， b(t)bm+(bo-bm)(T-t)/T， u为在0,1 均匀分布的随机变量； c1为取自粒子群算法中的学习因子， 设为： c1c_max-(t/T)(c_max-c_min) 其中c_max为0,1之间的常数且大于c_。

8、min， c_min为0,1之间的常数且小于c_max； r1为在0,1均匀分布的随机变量， 能够防止粒子陷入局部最优位置； c2为收缩位移参数， 体现参数对收缩位移的影响， c2大于零且小于移动步长step_s最大 值； Y轴和Z轴方向位置更新方法与上述X轴方向位置更新方法相同； 步骤6： 当达到设定的最大迭代次数时， 输出全局最优路径， 否则， 返回步骤3。 2.根据权利要求1所述的一种AUV三维航路规划方法， 其特征在于： 步骤3所述目标函数 具体为： 其中n为未进入障碍物粒子数， m为进入障碍物的粒子数， lj为最近非障碍物区域的距 离， K为给定的距离影响系数， fi为航路能耗， f。

9、i满足： 其中， Vx为AUV和速度的X方向上的分向量， Dx为航路距离的X方向上的分向量， Vy为AUV 和速度的Y方向上的分向量， Dy为航路距离的Y方向上的分向量， Vz为AUV和速度的Z方向上的 分向量， Dz为航路距离的Z方向上的分向量。 3.根据权利要求1或2所述的一种AUV三维航路规划方法， 其特征在于： 步骤4所述设置 粒子移动步长的约束条件具体为： 设置最小步长为step_min， 最大步长为step_max， 记录每 次粒子偏移量B， 设置步长约束极限step0为： step0step_max-(t/T)(step_max-step_min) 其中t为当前迭代次数， T为最。

10、大迭代次数； 当Bstep0时， 粒子的移动步长不进行约束； 当Bstep0时， 粒子的移动步长约束方式为： step(t+1)(step(t+1)/B)step0 其中， step为需要进行约束的移动步长。 权利要求书 2/2 页 3 CN 111307158 A 3 一种AUV三维航路规划方法 技术领域 0001 本发明属于水下潜器三维路径规划技术领域， 涉及一种AUV三维航路规划方法， 特 别是一种基于改进量子粒子群算法的水下潜器三维航路规划方法。 背景技术 0002 海洋大约占地球表面积的71， 存储着庞大的资源， 不仅拥有丰富的生物资源和 水资源， 水下石油等矿产资源同样不可估量。 。

11、随着人类的不断发展， 陆地资源呈开采殆尽趋 势， 海洋资源逐渐成为人类开发的重点。 因此， 海洋的开发利用已经成为各国的战略重点， 也成为高科技研究的重要领域。 自主水下航行器(Autonomous Underwater Vehicle简称 AUV)是指能够自主的进行导航和规划的新型水下运载平台， 可以代替人类在危险或者人类 无法到达的区域进行水下作业， 极大地拓展了人类的水下探测范围。 作为一种智能化的海 洋开发工具， AUV己经成为探测和开发海洋资源的重要手段， 同时成为世界各国海洋领域研 究的重点。 由于海洋环境的复杂性， 因此如何有效的利用海洋环境以及避开周围海洋环境 中的威胁区及障碍。

12、物是AUV实现自主航行的关键。 航路规划是实现AUV自主航行和自主作业 的基础， 其水平的高低在一定程度上标志着AUV的智能水平。 因此研究AUV的航路规划方法 对于提高我国的水下智能机器人的自主水平和加快海洋的开发利用均具有重要的意义。 0003 量子行为粒子群优化算法(Quantum-behaved Particle Swarm Optimization， QPSO)是粒子群算法的一个改进版本， 最早由孙俊提出。 在量子粒子群算法中， 粒子被视为 具有波粒二象性， 无法用确定的位置和速度来描述粒子， 即不能使用粒子群算法中的位置 和速度更新方程来表示粒子的状态， 而是利用波函数进行代替。 。

13、由于粒子被假设具有了量 子行为， 当粒子在搜索时， 能够以一定的概率完全覆盖整个解空间， 因此从理论上讲， 相比 粒子群算法， 量子粒子群算法具有更强的全局搜索能力。 专利CN201610887891.6公开了一 种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法， 该专利将粒子群算 法 进 行 改 进 ， 但 是 只 是 考 虑 最 短 路 径 ， 并 没 有 考 虑 海 流 对 A U V 的 影 响 。 专 利 CN201210178003.5公开了一种基于粒子群算法的三维路径规划新方法， 该专利是基于粒子 群算法进行路径规划， 考虑因素仅仅是路径长度， 没有将AUV。

14、能耗作为航路优劣的评价标 准。 本发明是将量子粒子群算法进行改进， 考虑了海流对AUV的影响， 并将改进的量子粒子 群算法应用于三维海洋环境中进行路径规划， 目前暂时还没有公开的文献将量子粒子群算 法应用于三维航路规划。 发明内容 0004 针对上述现有技术， 本发明要解决的技术问题是提供一种将量子粒子群算法进行 改进处理应用其中、 并将海流因素考虑其中并且加入移动步长约束的AUV三维航路规划方 法， 用来提高规划速度和规划质量。 0005 为解决上述技术问题， 本发明的一种AUV三维航路规划方法， 包括以下步骤： 0006 步骤1： 三维环境建模和粒子位置的初始化， 根据空间中的起点以及终点。

15、的位置规 说明书 1/7 页 4 CN 111307158 A 4 划出航行空间范围， 并且将粒子随机分布到非障碍物区域内； 0007 所述三维环境建模具体为： 首先将进行AUV航路规划的真实海洋环境空间划分为 lonlatdepth个网格， 网格划分原则为： 经度方向和纬度方向的网格间距与航行区域网 格水深数据文件的分辨率保持一致， 深度方向的网格间距不应超过航行区域最大深度值的 1/10， 然后进行AUV航行区域的地形场、 海流场的建模； 0008 AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现 的， 将航行空间中从文件中读取的水深值以下的区域设为禁航区； 0009。

16、 AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的， 如果海流数据文件的分辨率低于水深数据文件的分辨率， 则对海流数据文件进行线性插值 处理， 反之， 对海流数据文件进行稀疏化处理， 使得海流数据文件的分辨率与水深数据文件 的分辨率一致， 从而使得每个网格都包含当前位置的海流信息； 深度方向的海流大小设为 0； 0010 所述的粒子位置的初始化具体为： 0011 设改进量子粒子群算法的解空间为Pp1,p2,.,pnRmn， 其中n为粒子群的规 模， 每一维粒子群代表一条路径， m为粒子群的维度， 对应为航路的节点个数， 令P中的每个 向量其中xj,yj,zjRm， 将pi。

17、在X、 Y、 Z三个方向进行初始化， pi在X、 Y、 Z方 向上随机取点， 具体计算公式如下： 0012 0013 rand_x为X方向最小值Xmin到X方向最大值Xmax之间的随机值； 0014 rand_y为Y方向最小值Ymin到Y方向最大值Ymax之间的随机值； 0015 rand_z为Z方向最小值Zmin到Z方向最大值Zmax之间的随机值； 0016 如果出现粒子初始化在禁航区的情况时， 就将此粒子重新初始化， 直至所有的粒 子都在非障碍物的区域内； 0017 步骤2： 进行障碍物的惩罚值设置， 障碍物内的惩罚值设定为从障碍物中心位置向 两端逐渐降低的方式； 0018 步骤3： 计算。

18、目标函数值， 目标函数值由两部分组成， 包括航路能耗和进入障碍物 的惩罚值， 航路能耗近似为航路节点之间的能量消耗的总和； 0019 步骤4： 设置粒子移动步长的约束条件； 0020 步骤5： 比较粒子群的目标函数值， 进行粒子位置的更新， 并且记录每次粒子更新 的最新位置， 加入步长约束， 按照改进的量子粒子群算法的位置更新公式重新进行粒子位 置寻优； 0021 X轴方向位置更新具体为： 0022 所述改进的量子粒子群算法的位置更新公式具体为： 0023 Xi,j(t+1)Xi.j(t)+c1r1Pi,j(t)+c2step_s 0024 其中， step_s为在阻力系数的影响下的移动步长，。

19、 step_s满足： 0025 step_s(Xi,j(t)+Xi,j+2(t)-2Xi,j+1(t)s 说明书 2/7 页 5 CN 111307158 A 5 0026 s为给定的阻力系数， 同时， 利用步骤4中的约束条件对step_s进行约束计算； 0027 其中， Pi,j为X方向的位置偏移量， Pi,j(t)满足： 0028 Pi， j(t)Xi， j(t+1)-Xi， j(t) 0029其中， pbest是指粒子历史最优位置， gbest是指粒子全局最优位置， mbest为粒子平均最优位置， r 为在0,1均匀分布的随机变量， 其中b为收缩扩张系数， b(t)bm+(bo-bm)(。

20、T-t)/T， u为在 0,1均匀分布的随机变量； 0030 c1为取自粒子群算法中的学习因子， 设为： 0031 c1c_max-(t/T)(c_max-c_min) 0032 其中c_max为0,1之间的常数且大于c_min， c_min为0,1之间的常数且小于c_ max； 0033 r1为在0,1均匀分布的随机变量， 能够防止粒子陷入局部最优位置； 0034 c2为收缩位移参数， 体现参数对收缩位移的影响， c2大于零且小于移动步长step_s 最大值； 0035 Y轴和Z轴方向位置更新方法与上述X轴方向位置更新方法相同； 0036 步骤6： 当达到设定的最大迭代次数时， 输出全局最优。

21、路径， 否则， 返回步骤3。 0037 本发明还包括： 0038 1.步骤3的目标函数具体为： 0039 0040 其中n为未进入障碍物粒子数， m为进入障碍物的粒子数， lj为最近非障碍物区域 的距离， K为给定的距离影响系数， fi为航路能耗， fi满足： 0041 0042 其中， Vx为AUV和速度的X方向上的分向量， Dx为航路距离的X方向上的分向量， Vy为 AUV和速度的Y方向上的分向量， Dy为航路距离的Y方向上的分向量， Vz为AUV和速度的Z方向 上的分向量， Dz为航路距离的Z方向上的分向量。 0043 2.步骤4中设置粒子移动步长的约束条件具体为： 设置最小步长为ste。

22、p_min， 最大 步长为step_max， 记录每次粒子偏移量B， 设置步长约束极限step0为： 0044 step0step_max-(t/T)(step_max-step_min) 0045 其中t为当前迭代次数， T为最大迭代次数； 0046 当Bstep0时， 粒子的移动步长不进行约束； 0047 当Bstep0时， 粒子的移动步长约束方式为： 0048 step(t+1)(step(t+1)/B)step0 0049 其中， step为需要进行约束的移动步长。 0050 本发明的有益效果： 本发明提出一种基于改进量子粒子群算法的AUV三维航路规 划方法。 该方法相比粒子群算法航路。

23、规划搜索算法全局性更强， 加入了粒子位置移动步长 约束， 提高了规划速度和规划质量， 并且加入海流影响， 设计能耗环节， 提高在实际海洋环 说明书 3/7 页 6 CN 111307158 A 6 境中实用性。 附图说明 0051 图1为本发明提出的基于改进量子粒子群算法的AUV三维航路规划流程图。 0052 图2为本发明采用的改进量子粒子群算法流程图。 具体实施方式 0053 下面结合附图对本发明具体实施方式进行具体说明。 0054 本发明步骤主要包括： 0055 步骤1： 三维环境建模和粒子位置的初始化。 根据空间中的起点以及终点的位置规 划出航行空间范围， 并且将粒子随机分布到非障碍物区。

24、域内， 以提高收敛速度。 0056 步骤2： 进入障碍物的惩罚值设置。 障碍物内的惩罚值设定为从中心位置向两端逐 渐降低的方式， 以保证粒子在更新过程中能够逃离局部最优的情况。 0057 步骤3： 计算目标函数值。 目标函数值由两部分组成， 即航路能耗和进入障碍物的 惩罚值。 航路能耗近似为航路节点之间的能量消耗的总和。 0058 步骤4： 粒子移动步长约束设置。 在本发明中， 由于量子粒子群的位置变化是依据 概率随机变化的， 通过对粒子位置移动步长进行约束， 可以提高规划航路的质量。 0059 步骤5： 比较粒子群的目标函数值， 进行粒子位置的更新。 比较不同粒子群之间的 目标函数值， 更新。

25、粒子的位置， 并且记录每次粒子更新的最新位置。 加入步长约束， 按照改 进的量子粒子群算法的位置更新公式重新进行粒子位置寻优。 0060 步骤6： 当满足算法迭代停止条件时输出全局最优路径， AUV三维航路规划结束。 0061 结合图1和图2本发明具体实施步骤如下： 0062 步骤1： 三维环境建模和粒子位置的初始化。 0063 步骤1.1航行空间抽象建模 0064 首先将进行AUV航路规划的真实海洋环境空间划分为lonlatdepth个网格。 网 格划分原则为： 经度方向和纬度方向的网格间距与航行区域网格水深数据文件的分辨率保 持一致， 深度方向的网格间距不应超过航行区域最大深度值的1/10。

26、。 其次进行AUV航行区域 的地形场、 海流场的建模。 0065 AUV航行区域的地形场的建模是通过读取航行区域的网格水深数据文件来实现 的， 将航行空间中从文件中读取的水深值以下的区域设为禁航区， 禁航区的目标函数具有 很大的惩罚值。 0066 AUV航行区域的海流场建模是通过读取航行区域的网格海流数据文件来实现的， 如果海流数据文件的分辨率低于水深数据文件的分辨率， 则对海流数据文件进行线性插值 处理， 反之， 对海流数据文件进行稀疏化处理， 使得海流数据文件的分辨率与水深数据文件 的分辨率一致， 从而使得每个网格都包含当前位置的海流信息； 深度方向的海流大小设为 0。 0067 步骤1.。

27、2粒子位置的初始化 0068 设改进量子粒子群算法的解空间为Pp1,p2,.,pnRmn， 其中n为粒子群的规 模， 每一维粒子群代表一条路径， m为粒子群的维度， 对应为航路的节点个数。 由于节点应包 说明书 4/7 页 7 CN 111307158 A 7 含三维位置信息， 因此令P中的每个向量其中xj,yj,zjRm。 将pi在X、 Y、 Z 三个方向进行初始化。 pi在X、 Y、 Z方向上随机取点， 具体计算公式如下： 0069 0070 rand_x为X方向最小值Xmin到X方向最大值Xmax之间的随机值； 0071 rand_y为Y方向最小值Ymin到Y方向最大值Ymax之间的随机。

28、值； 0072 rand_z为Z方向最小值Zmin到Z方向最大值Zmax之间的随机值； 0073 如果出现粒子初始化在禁航区的情况时， 就将此粒子重新初始化， 直至所有的粒 子都在非障碍物的区域内， 从而可以提高粒子群算法的收敛速度。 0074 步骤2： 进入障碍物的惩罚值设置。 0075 由于在航行空间中会存在特别大的障碍物， 因此当出现所有粒子都进入障碍物的 情况时， 对于传统处理方法， 即加一个固定的惩罚值， 就会出现粒子很难逃离障碍物的情 况。 本发明提出了一种从障碍物内部向外部逐渐降低其惩罚值的方法， 不仅防止粒子会陷 入障碍物中， 而且能够提高寻找最优路径的效率。 针对于三维空间中。

29、的山体障碍物， 障碍物 内部点的惩罚值与该点到非障碍物区域的距离成正相关。 0076 步骤3： 计算目标函数值。 0077 算法的目标函数值由航路节点之间能量消耗和进入障碍物的代价值构成的。 其中 为了考虑海流的影响， 将目标函数设置为能耗函数， 和速度Vn为海流速度与AUV速度的和， 能耗函数为： 0078 0079 其中， Vx为AUV和速度的X方向上的分向量， Dx为航路距离的X方向上的分向量， Vy为 AUV和速度的Y方向上的分向量， Dy为航路距离的Y方向上的分向量， Vz为AUV和速度的Z方向 上的分向量， Dz为航路距离的Z方向上的分向量， 对于进入障碍物的粒子加上相应的惩罚值 。

30、作为该航路的目标函数值。 因此得到改进的目标值函数设为： 0080 0081 其中n为未进入障碍物粒子数， m为进入障碍物的粒子数， lj为最近非障碍物区域 的距离。 K表示为距离影响系数， 为体现出系数对距离的影响， 系数的设置原则为大于零小 于lj中的最大值， 此发明设为0.4。 0082 步骤4： 粒子移动步长设置。 0083 为了加快算法的规划进程， 提高规划质量， 使航路更加平滑可行， 要求算法初期能 够进行大范围的搜索， 而在算法优化后期应具备小范围优化的特点。 在本发明中， 改进的量 子粒子群算法对粒子的移动步长进行了设置， 本发明设置最小步长为step_min， 最大步长 为s。

31、tep_max， 记录每次粒子偏移量B， 设置步长约束极限step0为 0084 step0step_max-(t/T)(step_max-step_min) 0085 其中t为当前迭代次数， T为最大迭代次数。 说明书 5/7 页 8 CN 111307158 A 8 0086 当Bstep0时， 粒子的移动步长不进行约束。 0087 当Bstep0时， 将粒子的移动步长约束方式为： 0088 step(t+1)(step(t+1)/B)step0 0089 其中， step为需要进行约束的移动步长。 0090 通过分析可得， 粒子的步长约束随着时间的变化会不断变小， 因此在算法初期在 大范。

32、围搜索阶段有利于快速初始化， 逃离局部最优的情况； 到了算法后期， 算法处于小范围 优化阶段， 为防止粒子大范围移动再次陷入障碍物的情况， 粒子进行小范围优化搜索。 0091 步骤5： 比较粒子群的目标函数值， 进行粒子位置更新。 0092 根据量子粒子群原理， 进行不同粒子群之间的目标函数值比较， 进行更新粒子群 的位置， 从而不断优化粒子群的代价值， 位置偏移量更新公式为： 0093 0094 Pi,j(t)Xi,j(t+1)-Xi,j(t) 0095 其中Pi,j为X方向的位置偏移量， pbest是指粒子历史最优位置， gbest是指粒子全 局最优位置， mbest为粒子平均最优位置， 。

33、r为在0,1均匀分布的随机变量， 其中b为收缩扩 张系数， b(t)bm+(bo-bm)(T-t)/T， u为在0,1均匀分布的随机变量。 0096 此外本发明为了提高路径规划质量， 使得规划出的航路更加光滑， 加入了阻力系 数， 即： 0097 step_s(Xi,j(t)+Xi,j+2(t)-2Xi,j+1(t)s 0098 s为阻力系数， step_s为在阻力系数的影响下的移动步长， 利用步骤4对step_s进 行约束。 0099 进而得到在收缩因子影响下的位置更新公式： 0100 Xi,j(t+1)Xi.j(t)+c1r1Pi,j(t)+c2step_s 0101 其中c1为取自粒子群。

34、算法中的学习因子， 体现了粒子向群体中优秀个体学习的能 力， 使粒子能够向最优位置靠近， 设为： 0102 c1c_max-(t/T)(c_max-c_min) 0103 其中c_max的设置原则为0,1且大于c_min， c_min的设置原则为0,1且小于c_ max。 在具体实施例中设c_max0.8， c_min0.1， r1为在0,1均匀分布的随机变量， 能够 防止粒子陷入局部最优位置。 c2为收缩位移参数， 为体现参数对收缩位移的影响， 参数的设 置原则为大于零小于收缩位移的最大值， 在具体实施例中设c22.5。 0104 同理Y、 Z方向用同样的方法进行位置更新。 0105 步骤6。

35、： 当满足算法迭代停止条件则输出全局最优路径， 多AUV三维协同航路规划 结束。 0106 发明中提出基于改进量子粒子群算法进行三维航路的方法， 通过改进粒子初始化 方法、 进入障碍物的惩罚值设置以及加入粒子移动位置步长的设置， 提高的算法的规划速 度和规划速度。 步骤主要包括： 三维环境抽象建模和粒子位置的初始化、 进入障碍物的惩罚 值设置、 计算代价函数值、 粒子移动步长设置、 比较粒子群的代价值， 进行粒子位置更新、 输 出规划结果。 相比于传统量子粒子群航路规划算法， 本专利提出的三维航路规划方法考虑 了海流因素的影响， 使得算法应用更加广泛； 同时， 三维航路规划的实现相比于二维航路规 说明书 6/7 页 9 CN 111307158 A 9 划更加具有实用性， 能够更好的满足实际航行需要。 说明书 7/7 页 10 CN 111307158 A 10 图1 说明书附图 1/2 页 11 CN 111307158 A 11 图2 说明书附图 2/2 页 12 CN 111307158 A 12 。