一种多约束优化的UUV粒子群三维路径规划方法

本发明涉及无人潜器(unmanned underwater vehicle,uuv)路径规划领域，尤其涉及一种多约束优化的uuv粒子群三维路径规划方法。

背景技术：

1、随着技术的发展，世界对uuv的应用越来越重视，海洋勘探：海底地形测绘、海洋生物调查和海洋资源勘探；海洋环境监测：uuv可用于海洋环境参数的实时监测和数据采集，以帮助科学家们更好地了解海洋生态系统的变化和演化；海底资源开发：uuv可用于辅助海底矿产资源的开发与勘测，为海底资源的开发提供重要的数据支持；海洋安全与救援：uuv可以进行海底搜索和目标定位，此外，uuv还可用于海洋事故救援，如油污染事件的监测和清理；海洋科学研究：uuv在海洋科学研究中，能够进行海洋生物学、地质学、地球物理学等领域的实地观测与数据采集，为科学家们提供珍贵的海洋研究数据；水下文化遗产保护：uuv通过搭载摄像头和激光扫描仪等设备，uuv能够获取水下文物的高清图像和三维模型，帮助考古学家们保护和研究这些宝贵的文化遗产，除了以上应用领域，uuv还可以在军事方面、海洋工程施工、海洋能源开发、海洋交通管理等方面发挥作用，而这些应用实现的基础是uuv的路径规划，能在有限的时间，消耗有限的能源安全到达指定地点。

2、相较于二维平面下的路径规划，uuv所处的三维环境路径规划更具难度，主要原因是水下环境的复杂性，鱼群、岛礁、洋流等复杂环境条件都给uuv的自主性应用带来了相当大的难度。uuv本身也存在诸多限制，如本身能源限制、通讯限制、抗压强度限制等，这些限制了uuv的进一步发展。规划出一条合理的航线，可以最大程度的节省能源、缩短航程、规避危险、提高任务成功率。然而不论传统算法还是新兴的智能算法，都存在一定的局限性，如路径搜索效率低或搜索成本高等，规划的质量直接关系着uuv执行命令的效率和成本。同时，大多数算法适用于二维空间，忽略了高度对于路径规划的影响，而实际的海洋环境极其复杂，海山海岭等地形地貌在执行任务过程中是不可避免的，这样使得传统的路径规划方法不能很好地满足复杂三维空间环境下的避障要求，因此对三维环境中的uuv进行路径规划是不可避免的。

3、常用的路径规划方法有图论算法：dijkstra算法、a*算法等；搜索算法：深度优先搜索(dfs)、广度优先搜索(bfs)等；遗传算法；强化学习算法：q-learning、深度强化学习等；其他启发式算法：禁忌搜索算法、粒子群优化算法等。粒子群算法较其他算法而言，在三维环境中具有简单易实现、全局搜索能力强、鲁棒性好等方面的优势。但是粒子群算法在路径规划领域的应用有如下问题：

4、(1)对参数设置敏感：粒子群算法中的参数设置对算法的性能和收敛速度有很大影响，例如，粒子数量、惯性权重和加速因子等参数需要经过适当的调整才能达到最佳效果，不合适的参数选择可能导致算法收敛过慢或无法找到较好的解；

5、(2)容易陷入局部最优解：粒子群算法依赖于粒子之间的信息交流和协作，这种协作机制可能导致算法在局部最优解附近徘徊，难以跳出局部最优解的束缚，特别是在复杂的问题空间中，粒子群算法可能无法全面搜索整个解空间。

6、同时，相较于本发明，以下方法存在区别如下：

7、论文《一种基于单纯形搜索的粒子群优化算法》提供的改进方法与本发明所述方法区别如下：该论文在粒子群初始化未进行改进优化，初始种群并不优质；该论文在进行搜索时粒子未进行速度和位置的限制，粒子易出现跑偏和停滞；

8、论文《基于模拟退火机制的自适应粘性粒子群算法》提供的改进方法与本发明所述方法区别如下：该论文未改进粒子群初始化，初始种群不优质；该论文在进行模拟退火准则后，未进行变异，粒子探索性不高；该论文未进行粒子速度和位置的限制修正，粒子易跑偏；

9、专利cn 117075471 a《一种智能无人船两阶段多目标点全局路径规划方法》提供的改进方法与本发明所述方法区别如下：该专利未对粒子群初始种群优化；该专利在进行模拟退火搜索后，未进行变异粒子探索性偏低；该专利对多个指标未进行加权，不符合现实情况。

10、本发明为了解决粒子群对参数设置敏感、易出现局部最优解等问题，提出一种多约束优化的uuv粒子群三维路径规划方法，对种群初始化进行改进，将多约束加权更满足现实uuv航行时减少能源消耗，并且进行局部搜索，帮助粒子跳出局部最优解。

技术实现思路

1、本发明为了解决上述路径规划出现的问题，提出以下技术方案：设计了一种多约束优化的uuv粒子群三维路径规划方法，在传统粒子群算法的基础上，对粒子群初始化时进行改进，利用预先寻优对部分粒子进行高质量初始化，然后在起点和终点直线之间生成初始种群进一步优化，提高了初始解的质量；此外，在寻优过程中，还引入模拟退火准则作为一种局部搜索机制，以避免陷入局部最优解，并在位置更新时根据模拟退火原理进行调整；当模拟退火无法更新个体时，通过一次变异操作增加了搜索空间；并且在粒子进行速度和位置更新时，对速度和位置的上下界进行限制并进行了修正，避免速度过大或过小导致粒子跑偏或停滞，同时也避免粒子位置出现偏差而偏离地图；最后，在一定概率下让一些个体进行局部搜索，以进一步提高收敛性能。具体包括以下步骤：

2、步骤1：

3、建立uuv航行环境下的三维海底地图，并且设置起点和终点的位置坐标，并且生成移动方向的偏移量和解的维度。

4、步骤2：

5、进行粒子群参数初始化，及初始化种群popsize，维度dim，基因值popmax和popmin，总迭代次数iterations，学习因子c1和c2，惯性权重w以及速度范围vmax和vmin。

6、步骤3：

7、对粒子群的种群进行初始化。

8、步骤4：

9、计算初始适应度值，首先线性插值和坐标重塑：根据起点和终点的范围对输入变量进行线性插值，得到坐标cor，公式如下：

10、cori,1＝cori,1*(end(1)-start(1))+start(1)               (1)

11、cori,2＝cori,2*size(2)                      (2)

12、cori,3＝cori,3*size(3)                       (3)

13、其中，cori,j为第i个点在第j个坐标轴上的值；start(1)为地图的起点在x轴上的位置；end(1)为地图的终点在x轴上的位置；size(2)为地图在y轴上的长度；size(3)为地图在z轴上的长度。

14、随后进行归一化和映射：将坐标cor的第一维度(x坐标)进行归一化操作，将其映射到[0,1]范围内，根据起点和终点的范围进行线性映射，将归一化后的x坐标还原为实际坐标值，将坐标cor的第二维度(y坐标)和第三维度(z坐标)根据缩放因子进行缩放。

15、然后进行b样条插值：对坐标cor进行b样条插值处理，得到插值后的点的坐标corall，根据b样条插值的系数计算速度vall，根据速度的差分和插值的断点计算加速度aall，计算公式如下：

16、cori＝[xi,yi,zi],xi∈r,yi∈r,zi∈r                   (4)

17、cor＝{cor1,cor2,...,corn}                       (5)

18、cor′＝pointn(x′,cor)                        (6)

19、

20、cor″′＝[start,cori″,end]                       (8)

21、corall″′,vall″′,aall″′＝b-splineinterpolation(cor″′)           (9)

22、corall,i＝[xall,i,yall,i,zall,i],i＝1,2,...,m                 (10)

23、其中，x′为参数向量；cor为原始坐标点；cor′为调整后的坐标点；cori″为将坐标点缩放后的坐标点；cor″′为在坐标点首尾添加起点和终点后的坐标点；corall″′为插值得到的所有点的位置信息；vall″′为插值得到的所有点的速度信息；aall″′为插值得到的所有点的加速度信息；corall为插值后的所有点的位置坐标。

24、然后计算曲率：根据加速度和速度计算曲率k，公式如下：

25、

26、其中，ki为曲线上第i个点的曲率；vi为插值处理后曲线上第i个点处的速度；ai为插值处理后曲线上第i个点处的加速度。

27、继续进行约束处理：将坐标corall的z坐标与场地地图进行二维插值计算，得到插值后的高度zinterp，统计约束条件的惩罚项，包括插入高度不能低于场地地图和坐标不能超过边界，公式如下：

28、

29、

30、

31、其中，m为插值后路径上的点数；zinterp为插值点在地图上的高度信息；restraint(1)为不能碰撞约束；restraint(2)为不能超过边界约束；restraint(3)为不能超过场地大小约束；将所有约束合并为总约束res，取约束restraint(1)，restraint(2)和restraint(3)的和。

32、对于四个目标进行优化后的目标函数计算：计算总距离dis，即所有点之间的欧氏距离之和，计算总高度height，即所有点的z坐标之和，计算曲率均值meank，以及约束条件的惩罚项res，组合以上计算结果，并使用加权系数得到目标函数值fitness，公式如下：

33、

34、

35、

36、fitness＝0.5*dis+0.2*height+0.3*meank+res*103     (18)

37、其中，dis为路径的总长度；height为航行过程中uuv升沉的总高度；meank为路径的平均曲率；res为所有约束的总和；fitness为多目标优化的函数值。

38、步骤5：

39、寻找初始极值，fitnessbest为最优适应度值，表示当前已经发现的最好的适应度值，初始化最fitnessbest为正无穷大，indexbest为最优个体索引，表示对应于最优适应度值的个体在种群中的位置，初始化indexbest为0，遍历种群中的每个个体，计算其适应度值，并与当前最优适应度值fitnessbest进行比较，如果该个体的适应度值小于当前最优适应度值fitnessbest，则更新fitnessbest为该个体的适应度值，并记录该个体的索引为indexbest，最后根据indexbest找到对应的zbest为群体极值位置，表示整个种群中具有最优适应度值的个体的位置，和对应的为群体极值适应度值，表示整个种群中具有最优适应度值的个体的适应度值。

40、步骤6：

41、设置模拟退火温度下降过程，其中起始值为200，终止值为0，表示温度会从起始值下降到0，公式如下：

42、

43、其中，tempi为第i个温度值；iterations为总共需要生成的温度值数量。

44、由于初始温度为200，因此当i＝0时，temp0＝200；当i＝iterations-1时，tempiterations-1＝0；当i取取任意值时，tempi都会在200和0之间变化，随着迭代次数的增加，i的取值不断变化，从而实现了温度的不断下降。

45、步骤7：

46、记录历史最优解，定义并初始化向量history用于存储每次迭代的历史最优解，将当前迭代的最优解添加到history向量中。

47、步骤8：

48、进行更新速度，公式如下：

49、vi,j＝wvi,j+c1r1(pbesti,j-xi,j)+c2r2(gbestj-xi,j)           (20)

50、其中，vi,j为第i个粒子在第j个维度上的速度；w为惯性权重；c1和c2为加速度常数；r1和r2为[0,1]之间的随机数；pbesti,j为第i个体极值在第j个维度上的位置；gbestj为群体极值在第j个维度上的位置；xi,j为第i个粒子在第j个维度上的位置。

51、为了保证速度的范围在vmin和vmax之间，对超过范围的速度进行修正，公式如下：

52、

53、其中，vj,i为粒子j在第i个维度上的速度；vmin为速度的下界；vmax为速度的上界。

54、步骤9：

55、根据更新后的速度，更新粒子的位置，公式如下：

56、xj,i＝xj,i+vj,i                         (22)

57、其中，xj,i为粒子j在第i个维度上的位置；vj,i为粒子j在第i个维度上的速度。

58、对新的位置进行修正，确保位置在指定范围内，公式如下：

59、

60、其中，xj,i为粒子j在第i个维度上的位置；ubi为第i个维度上的上界；lbi为第i个维度上的下界。

61、步骤10：

62、对种群进行更新变异。

63、步骤11：

64、通过随机数判断是否进行局部搜索优化，若随机数小于0.5，则进入步骤12，否则进入步骤13。

65、步骤12：

66、进行进一步局部搜索优化。

67、步骤13：

68、记录每次迭代最优值。

69、步骤14：

70、判断是否达到整个种群迭代次数100次，是则进入步骤15；否则回到步骤8。

71、步骤15：

72、输出最优值。

73、本发明具有如下有益效果：

74、(1)本发明所述方法将uuv航行路径长度、航行时升沉高度以及航行转弯幅度多约束进行线性加权进行优化，本发明产生的路径在升沉高度上波动更小处于-0.15到+0.11之间，而传统粒子群算法产生的路径波动在-0.31到+0.62之间波动，说明本发明所述方法在航行高度上更稳定，更利于减少uuv频繁的上浮和下潜，进而减少uuv能源消耗。本发明产生的路径在转弯幅度上具有更小的波动，通过曲率变化来体现其最大值为0.31，而传统粒子群算法产生的路径曲率变化最大值为3.78，相比于传统粒子群算法最大曲率减少91.7％，说明本发明所述方法在转弯幅度上具有更小的波动，进而产生的路径更平滑且更满足航行时转角约束。

75、(2)本发明所述方法在传统粒子群初始化种群的基础上，利用预先寻优对部分粒子进行高质量初始化，并且在起点和终点直线间进行初始化一个种群，增加了种群的多样性，有助于更好地探索搜索空间。由于本发明在初始化进行改进后，本发明最初的函数值为3.16e4而传统粒子群算法最初的函数值为1.02e5，相比于传统粒子群算法最初函数值减少69％，由此说明本发明所述方法粒子群初始种群更优。

76、(3)本发明所述方法在传统粒子群的基础上，引入了模拟退火准则，在更新个体解时，根据适应度值的变化和当前温度，采取一定的概率接受较差的解，以避免陷入局部最优解，模拟退火无法更新个体时，通过一次变异操作增加了搜索空间，提高算法的全局搜索能力，增加了算法的多样性和探索性。

77、(4)本发明所述方法在更新速度和位置时，对速度和位置的上下界进行限制，并进行了修正，避免速度过大或过小导致粒子跑偏或停滞，同时也避免粒子位置出现偏差而偏离地图，并且在群体中选取一部分个体进行局部搜索，可以更快地收敛到局部最优解并且粒子群。传统粒子群算法出现明显的“转圈”路径，导致路径长度明显偏长，而本发明所述方法产生的路径在平滑程度和合理程度都更优。