一种考虑安全距离的跨域场景下潜浮式无人艇路径规划方法

本发明涉及船舶路径规划，具体而言，尤其涉及一种考虑安全距离的跨域场景下潜浮式无人艇路径规划方法。

背景技术：

1、目前随着船舶行业的不断发展，无人船的研究越来越成为行业内的热点内容。潜浮式无人船是在半潜式无人船的基础上设计出的一种船舶。其既能实现在水面上的低阻力高速运动，又可以实现跨不同潜深的水下航行。在潜浮式无人船研究中，针对其航行的跨域环境下提供一条在运动空间中从起始状态到目标状态、可以避开障碍物的最优或者接近最优的路径，对于缩短行驶距离，节省船舶行驶的时间等方面有十分重要的意义。

2、蚁群算法是一种基于蚁群寻路模型的仿生路径规划算法，其原理是蚁群在寻路时，较优路径上的蚁群会分布信息素，后续蚂蚁寻路时受信息素影响，继续选择较优路径，使该路径上的信息素浓度不断叠加，对后续的蚂蚁的吸引力越来越高，因此越来越多的蚂蚁聚集到最短路径上来，最终形成一种正反馈机制。蚁群算法作为一种全局性路径规划算法具有全局性好、收敛速度块、便于迅速准确地找到最优解的优点，易于拓展到三维路径规划。但同时其具有算法随机性较大，无法确定收敛所需时间的缺点。

3、目前，无人艇路径规划通常针对水面水下单域进行，针对船舶跨越水面及水下的跨域场景进行路径规划的研究项目较少。在路径避障上，现有三维路径规划研究在船舶与障碍物间预留安全距离的方面仍有改进空间。

技术实现思路

1、根据上述提出的技术问题，提供一种考虑安全距离的跨域场景下潜浮式无人艇路径规划方法，本发明采用的技术手段如下：

2、一种考虑安全距离的跨域场景下潜浮式无人艇路径规划方法，包括：

3、s1、根据路径规划需要，获取起点位置、终点位置和障碍物中心坐标，采用栅格法构建环境场景，并将障碍物膨胀为三维正方体组；

4、s2、令船舶路径在危险点向障碍物反方向偏移特定距离进行避障；

5、s3、采用提取关键点的策略，优化最短路径；

6、s4、通过蚁群算法程序多次迭代运算，生成三维场景下的以路径长度最短为指标的最优路径，并给出最短路径值，为船舶实际航行提供参考。

7、进一步地，所述步骤s1中，采用栅格法构建环境场景，并将障碍物膨胀为三维正方体组，具体包括：

8、s11、以所需地图的长度、宽度、高度建立对应行、列、层数的环境矩阵；

9、s12、根据获取的起点位置、终点位置和障碍物中心坐标，将对应的矩阵单元坐标设为起点位置、终点位置和障碍物位置；

10、s13、将包含障碍物的栅格对应的矩阵值设为“1”，在视图中用黑色方格表示，表示不可通过栅格；将无障碍物的栅格对应的矩阵值设为“0”，在视图中用白色方格表示，表示可通过栅格。

11、进一步地，所述步骤s1中，还包括：

12、s14、对环境矩阵中的每个单元赋予序号、矩阵坐标、绘图坐标三个坐标，其中，序号为按层、行、列的顺序依次标号，矩阵坐标为线性代数中的三维矩阵的编号，绘图坐标为正方体栅格的中点所对应的三维空间直角坐标系坐标；

13、s15、设矩阵坐标为(x，y，z)，绘图坐标为(rx，ry，rz)，采用如下公式对坐标进行转换：

14、s151、矩阵坐标和绘图坐标的相互转换：

15、

16、s152、序号和绘图坐标的相互转换：

17、

18、式中，l为横向栅格数量，b为纵向栅格数量，n为序号法所规定的序号，mod表示整除后取余函数，ceil表示向正无穷舍入函数；

19、s16、使用if函数寻找所有纵坐标为b+0.5的点，人为将y坐标更改为y＝y-b。

20、进一步地，所述步骤s2，具体包括：

21、s21、使用for循环从第二个节点搜素至最后一个节点；

22、s22、使用if函数进行判定，设前一个点坐标为(x1，y1，z1)，后一个点坐标为(x2，y2，z2)；若(x1，y1，z2),(x1，y2，z2),(x1，y2，z1),(x2，y1，z2),(x2，y2，z1),(x2，y2，z2)六个节点中至少有一点所对应的地图矩阵为“1”(即该栅格为障碍物)，判定该组点为危险点，将该组点记录并保存；

23、s23、找寻所有符合条件的几组点，在每组点间插入中值点，中值点的坐标为：

24、

25、s24、将中值点向正方体中心点反方向偏移一段距离，使路径距离障碍物间的最短距离，即危险点处距离障碍物顶点的长度大于等于一个船宽的距离；

26、s25、对于碰撞障碍物边的情况，将绘图坐标的x、y、z值分别对0.5取余，令余数不为0的两个坐标同时向反方向偏移一个值，以x、y坐标为例，将x，y坐标同时向反方向偏移：

27、

28、s26、对于碰撞障碍物角点的情况，将x，y，z坐标同时向反方向偏移：

29、

30、式中，b表示船宽。

31、进一步地，所述步骤s3，包括：删除初始路径规划的冗余点和可优化点，具体包括：

32、s31、将规划好的路径中，在同一条直线上的相邻节点，将除首尾两点之外的所有点定义为冗余点；

33、s32、使用for循环功能从第二个节点开始判定至倒数第二个节点，若目标节点和前一节点连线以及目标节点和后一节点的连线斜率相同，则标记目标节点为冗余点，删除冗余点并更新路径；

34、s33、连接目标节点前一节点和后一节点，若连线距离障碍物保持安全距离，则将目标节点定义为关键可优化点；

35、s34、设删除冗余点后的路径点为pi(i＝1，2，3，...，n)，连接p1p3，若p1p3不经过障碍物，即线段p1p3到障碍物的距离大于预设阈值，则继续连接p1p4，直到p1pk(k＝3，4，....，n)经过障碍物，则把pk-1与p1连接起来，同时删除中间的可优化点，更新路径；

36、s35、从节点p2重复步骤s34中的操作，直到路径中没有可优化点，剩下的节点即为关键点。

37、进一步地，所述步骤s4，具体包括：

38、s41、蚁群算法迭代开始前，使用函数建立邻接矩阵，邻接矩阵为边长为栅格个数的方阵；

39、s42、找到每个节点到下一个节点的可能性和两个节点间的距离，其中，上下左右前后六个自由度距离为1，改变两个坐标的12个自由度距离为三个坐标都改变的8个自由度距离为

40、s43、邻接矩阵为一个分块矩阵，主对角线块上的矩阵块表示同层移动，主对角线块上下两条对角线块上的矩阵块表示跨层移动，其余矩阵部分为0值，表示不可移动；

41、s44、每移动一步后，将对应的邻接矩阵对应点的距离存储到总距离中并累加，每次迭代按下述信息素的更新公式进行更新：

42、

43、式中，τij为i节点至j节点路径上的信息素，δτijk为第k只蚂蚁分布的信息素，其公式如下：

44、

45、式中，lk为第k只蚂蚁在本次循环中所走路径的长度；

46、s45、计算蚁群移向各个自由度的概率，公式如下：

47、

48、式中，etaij为i节点至j节点路径的启发式信息值，公式如下：

49、

50、式中，ix、iy、iz分别为当前节点x、y、z坐标，stopx、stopy、stopz分别为终点的x、y、z坐标。

51、s46、执行步骤s41至步骤s45，得到跨域场景下规划出的路径及路径最短长度。

52、进一步地，所述方法还包括：

53、s5、以航行时间最短为目标的路径规划，对每个节点给予对应的速度，具体如下：

54、s51、导入预设的速度，将蚁群算法邻接矩阵累加的路程除以该节点对应的速度，将累加计算最短路程改为累加计算最短时间；

55、s52、设置船舶水面航行速度为1.2m/s，使用if函数判定前后两点的z坐标相同，将邻接矩阵对应前后两点移动值除以1.2；

56、s53、设置船舶上浮下潜航行速度为0.5m/s，使用if函数判定前后两点的z坐标相同，将邻接矩阵对应前后两点移动值除以0.5；

57、s54、执行步骤s51至s53，得到跨域场景下以用时最短为目标规划出的路径及路径最短长度。

58、本发明还提供了一种存储介质，所述存储介质包括存储的程序，其中，所述程序运行时，执行所述考虑安全距离的跨域场景下潜浮式无人艇路径规划方法。

59、较现有技术相比，本发明具有以下优点：

60、1、本发明提供的考虑安全距离的跨域场景下潜浮式无人艇路径规划方法，通过对场景构建方法、蚁群算法公式和邻接矩阵等因素的三维化，能够获得跨域场景下的蚁群算法路径规划最优解。

61、2、本发明提供的考虑安全距离的跨域场景下潜浮式无人艇路径规划方法，通过与潜浮式无人船的特性(船宽、速度等参数)相结合，确保了船舶在实际航行时路径的安全性和快速性。

62、3、本发明提供的考虑安全距离的跨域场景下潜浮式无人艇路径规划方法，可在更改部分参数后，推广到任意移动机器人的三维场景路径规划。

63、基于上述理由本发明可在船舶路径规划等领域广泛推广。