一种三维环境的搜救ROV路径规划系统及方法

本发明涉及搜救rov路径规划技术，特别是涉及一种三维环境的搜救rov路径规划系统及方法。

背景技术：

1、rov(遥控水下机器人，remotely operated underwater vehicle)很早就被应用于搜救工作之中，它可以在从海面到海底的各种水深状况下工作，具有不可替代的优势。但在搜索的过程中，rov的运行仍是人工操作，需要操作员保持着极高的专注度，但长时间的工作必然会对操作员的精神至身体造成极大的负担。为了减轻操作员的操作压力，rov机器人已经不能仅仅满足于手动操作的运行方式，而是应该向着自动化的方向发展，为完善rov机器人的应用领域，路径规划至关重要。

2、现有技术中，张旋,皮大伟,王洪亮,谢伯元,王显会,王尔烈,孙晓旺所著一种基于改进a*算法的类三维路径规划方法，具体包括如下步骤：步骤(1)：预处理原始数字高程模型,即dem；步骤(2)：提取分析障碍物因素,建立环境地图模型：建立的环境地图模型基于模糊化障碍物概念，所建模型包含了模糊化障碍物的分布；步骤(3)：优化改进a*算法的代价函数,建立总代价函数；步骤(4)：运用改进a*算法于建立更优的环境地图模型,搜索得到最优路径。该方法仅用二维的地图上就有效的表示了环境信息，极大地缩小了数据存储量；把可行驶区域和模糊化障碍物融合到一起，扩大了路径规划的区域，为缩短规划长度提供了可行性。

3、现有技术方案虽优点很明显，不仅把可行驶区域和模糊化障碍物融合到一起，将复杂的三维环境简化为二维，减少计算量，扩大了路径规划的区域，但对于较为复杂的水下三维环境而言，二维地图难以表示；且对于rov机器人而言，路径的安全性与平滑要求更高，上述技术方案在此方面并无过多要求，不利于rov搜救工作的进行。

技术实现思路

1、发明目的：本发明的一个目的是提供一种三维环境的搜救rov路径规划系统。

2、本发明的另一个目的是提供一种三维环境的搜救rov路径规划方法，从水下搜救任务中大范围扫描需求出发，实现三维路径规划功能，提高搜救效率，使rov更好的应用在救援领域中。

3、技术方案：本发明的一种三维环境的搜救rov路径规划系统，包括：

4、环境建模模块，用于获取rov机器人水下环境信息，将rov机器人运动的区域从真实的环境中抽象出三维空间模型，并细分多个三维区域，且每个区域都用三维信息表示，构建水下环境地图三维栅格模型；

5、算法调用路径生成模块，用于选择路径规划的目标点，并以rov的当前位置作为路径规划的起始点，目标位置为终点；判断所选取目标点是否合理，不合理将重新选择；采用基于jps-cosps的三维路径规划算法，输入水下环境地图三维栅格模型、起始点、目标点，进行路径规划，获得规划路径；

6、路径检测模块，用于通过判断规划路径完整度，保证路径连续；通过检测路径中是否存在相同路径，保证rov的运行顺畅；通过检测路径是否贴近障碍物，保证路径安全性；若检测的规划路径不可行，则反馈算法调用路径生成模块重新进行路径规划，若检测的规划路径可行，则输出可行的规划路径。

7、本发明的另一实施例中，一种三维环境的搜救rov路径规划方法，包括以下步骤：

8、s1、通过激光，声纳扫描或外部输入获取rov机器人水下环境，将rov机器人运动的区域从真实的环境中抽象出三维空间模型，并细分多个三维区域，且每个三维区域都用三维信息表示，建立三维坐标系统，三维区域即为三维路径规划空间，然后把坐标等分，形成水下环境三维栅格地图模型；

9、s2、选择路径规划的目标点，并以rov机器人的当前位置作为路径规划的起始点，目标点终点；

10、s3、判断所选取的目标点是否为障碍物，如果是，则返回步骤s2重新选择目标点；如果不是，则执行步骤s4；

11、s4、采用基于jps-cosps的三维路径规划算法，输入水下环境地图三维栅格模型、起始点、目标点，进行路径规划，获得规划路径；

12、首先，确定临界障碍，并筛选临界障碍的周围点集；

13、其次，设定筛选后的周围点集为跳点，通过裁剪平面的方法，搜寻跳点；将符合要求的跳点加入openlist列表，筛选列表中评价函数f(n)最小的节点作为下一次搜索的父节点，并将其加入closelist列表；

14、最后，删除多余拐点，直接连接前后路径点；

15、s5、对获得的规划路径进行检测，通过判断规划路径完整度，保证路径连续；通过检测路径中是否存在相同路径，保证rov的运行顺畅；通过检测路径是否贴近障碍物，保证路径安全性；若检测的规划路径不可行，则返回步骤s2，若检测的规划路径可行，则输出可行的规划路径。

16、进一步的，步骤s1中三维坐标系统中x轴和y轴分别为纵向和横向增加方向，z轴为高度增加方向，形成三维空间，在水下环境三维栅格地图模型中把rov机器人看作一个质点，用x轴、y轴和z轴三个方向表示rov在模型的位置，每个质点不能超越分割后的小立体栅格的边界，且rov机器人和障碍物在三维空间中运行时不能相互碰撞。

17、进一步的，步骤s4中筛选临界障碍的周围点集的规则为：

18、(ⅰ)判断临界障碍方格一邻格是否存在障碍，若存在，则此邻格不可行；若不存在，则临界障碍邻格为可行点；

19、(ⅱ)判断在临界障碍方格所在一平面对角方向邻格上是否存在障碍，若存在，则此对角邻格不可行；若不存在，则临界障碍对角邻格为可行点；

20、(ⅲ)若一障碍方格存在相邻的三个面方向方格均不存在障碍，则三个面形成的角所对的方格为可行点。

21、更进一步的，筛选临界障碍的周围点集的方法为：

22、(1)确定临界障碍周围点集，集合长度为length(sps)，定义标识符i＝1，j＝1；

23、(2)将点集中编号为i的点，记为pi(xi,yi,zi)；

24、(3)计算点pi距离最近障碍点的距离di；

25、(4)判断di2>＝n，如果是，则记录点pi(xi,yi,zi)，令pi(xi,yi,zi)＝qj(xj,yj,zj)，且j++；然后i++；否则，i++；

26、其中，qj(xj,yj,zj)为筛选后的可行点；n＝2或3，若选用筛选规则(ⅱ)，则n取值为2，即判断语句为di2>＝2；若选用筛选规则(ⅲ)，则n取值为3，判断语句为di2>＝3，若选用筛选规则(ⅰ)，则不执行步骤(4)；

27、(5)判断i<＝length(sps)，如果是，则返回步骤(2)执行，否则，则结束。

28、进一步的，步骤s4中跳点搜寻方法为：

29、(1)在点搜索的过程中，将符合要求的跳点加入openlist列表，筛选列表中评价函数f(n)最小的节点作为下一次搜索的父节点；

30、(2)通过寻找父节点所在三平面存在的可无障碍到达的跳点加入openlist列表；

31、(3)判断列表中是否存在目标点，若存在，寻路结束；若不存在，将父节点向当前方向延伸至下一栅格点；

32、(4)更新延伸点为父节点，判断父节点是否为障碍点，不是则跳至步骤(2)，是则跳至步骤(1)。

33、进一步的，步骤s4中拐点删除方法为：

34、随机选取路径中的路径拐点为可删除点，尝试删除点，并直接连接可删除点前后节点，判断连接后的路径是否接触障碍物，若存在，则取消删除操作，恢复原路径，另寻其他路径拐点；若不存在，保留删除操作，生成新路径。为避免不必要操作，设定节点均为整数坐标的路径点。

35、本发明再一实施例中，一种装置设备，包括存储器和处理器，其中：

36、存储器，用于存储能够在处理器上运行的计算机程序；

37、处理器，用于在运行所述计算机程序时，执行如上述一种三维环境的搜救rov路径规划方法的步骤。

38、本发明的另一实施例中，一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种三维环境的搜救rov路径规划方法的步骤。

39、有益效果：与现有技术相比，本发明显著的技术效果为：本发明在rov控制系统中加入路径规划功能，利用由a*算法衍生的jps算法在三维栅格环境中进行路径规划，通过引入cosps(critical obstacle and around point set，临界障碍与周围点集)降低三维环境复杂性，减低寻路难度；通过设定周围点集的选定方法保障路径安全性；通过改进算法的代价函数,建立总代价函数，保证路径质量；通过删除与曲线优化拐点，保证路径平滑度。最终得到路径较短，安全性高，平滑顺畅的优秀路径，保障搜救rov的运行顺利。