快速行进平方法与虚拟障碍势场法相结合的船舶编队算法

本发明涉及一种能有效建立编队结构并保证所有船舶安全性的船舶编队算法，尤其是提供一种快速行进平方法与虚拟障碍势场法相结合的船舶编队算法，属于船舶智能安全航行。

背景技术：

1、船舶编队对保持多船协调作业稳定性具有重要意义。领导—跟随的编队控制方法因具备原理简单、易于实现的优势深受学者的喜爱。为通过该方法实现海上多船协调作业需要提前为队伍中的领导者进行路径规划。目前常用的全局路径规划算法中dijkstra算法和a*算法是求解最优路径的算法，但dijkstra算法和a*算法存在着以下不足：首先，dijkstra算法原理简单，但是计算流程过于复杂且占用较多的内存，只适用于较小规模的路径规划；a*算法比dijkstra算法运算更快，但是其对启发函数非常依赖，这就导致该算法计算量巨大。其次，由于二者的主要目标是规划出最短的路线，因此无法保证航行时的安全性。

2、并且在通过领导—跟随的编队控制方法实现跟随者船舶对领导者船舶的路径跟踪时，可能有航行路线中出现静态障碍物的情况，因此迫切需要提出一种算法解决跟随者在跟踪过程中可能出现发生碰撞的问题。在传统的避碰方法中，人工势场法具有实时性强、数学计算简单、规划的路径较为平滑、易于编程等优点因此在解决局部避碰问题中广泛使用，但它会出现陷入局部极小点导致船舶停滞的情况。

技术实现思路

1、本发明公开一种快速行进平方法与虚拟障碍势场法相结合的船舶编队算法，通过将领导—跟随的编队控制方法与快速行进平方法的路径规划方法、虚拟障碍势场法的局部避碰方法相结合，实现了一种有效建立编队结构并保证所有船舶安全性的算法。

2、本发明所述的快速行进平方法与虚拟障碍势场法相结合的船舶编队算法，技术解决方案如下：

3、1)基于快速行进平方法的路径规划

4、快速行进平方法的步骤如下：

5、step1：对航行环境进行建模，将环境转换成二进制网格图；分别标记属于障碍的单元格与可通行区域对应的单元格；

6、step2：快速行进平方法将地图中每个被标记为障碍的单元视作波源，同时扩展多个波，地图中每个单元格的结果值表示波需要到达最近的障碍物的时间，它与距离障碍物的距离成正比；将这一步得到的势图为速度势，记为w(x)，速度势图中的每个网格都有一个值，表示其与最近障碍物的距离，取值范围为0-1；

7、step3：在势图w(x)的基础上，将目标点当作唯一的波源，以确保一个全局最小值，在地图上扩展波，直到到达初始点。在可行区域中的每个单元，波的扩展速度从上一步计算的速度势图w(x)中提取；

8、step4：最后，从船舶的起始点开始，在整个的到达时间地图上进行梯度下降，并向其目标点(结果地图的全局最小值)移动，得到综合考虑了到达时间、平滑性和安全性方面的路径；

9、2)基于laplace矩阵的队形控制

10、考虑领导—跟随多船舶编队系统，其中系统中的每艘船舶均可看作网络中的节点，船舶间的信息传递环节可看作连接不同节点的边；由此，多船舶系统及其通信网络可被建模为一个图；在编队中由于有信息流的存在，因此将船舶编队系统构建成为一个有向图，如图1所示，其中箭头方向就代表了信息传递的方向。

11、以g＝{v,e}表示该有向图，其中v＝{v1,v2,...,vn}表示图中的n个顶点，也就是系统中的n艘船舶，则表示由顶点构成的边的集合。其拉普拉斯矩阵l被定义为：l＝d-a，其中d为图的入度矩阵，它对角线上的元素为节点的入度之和；a为图的权重矩阵，如果顶点v是边e的头部，则a矩阵中的元素aij＝1，反之aij＝0。

12、由此可以得到该系统的权重矩阵为：

13、

14、入度矩阵为：

15、

16、laplace矩阵为：

17、

18、定义x＝[x1,x2,...x5]t为每个船舶的当前坐标值，η为相邻两点的期望相对位置。

19、由此对于拓扑图上的每一条边可得：

20、点xj相对于点xi距离期望值的差值为：eij＝xi-xj-η

21、点xj相对于所有点距离期望值的差值为：

22、其中α为相对位置增益系数。

23、为了能够让跟随者实现对于领导者轨迹的跟踪，我们需要将ej的值减小为0，因此每一个跟随者的速度将会受到ej的影响，通过消除该误差量实现队形的稳定。

24、3)基于虚拟障碍势场法的局部避障

25、编队中的跟随者对领导者路径追踪的过程中，如果在跟踪的路径上出现了距离障碍物过近的情况，就很有可能会出现碰撞风险。因此每一个跟随者能够独立的实现局部避碰以应对这种情况是十分必要的。传统的人工势场法具有实时性强、数学计算简单、规划的路径较为平滑、易于编程等优点因此在解决局部避碰问题中广泛使用，但它会出现陷入局部极小点导致船舶停滞的情况。为解决该问题提出了一种虚拟障碍势场法，当跟随者船舶进入局部极小点的位置时，首先通过判断障碍物的分布来增设一个虚拟障碍物，此时通过增设的虚拟障碍物，船舶受到的合力就会发生改变，由虚拟障碍物为船舶提供额外的逃逸力逃离局部极小值点。而且由于该斥力的存在可以避免船舶再次陷入该极小点。

26、假设船舶的实时坐标位置为p＝(x,y)，目标点的位置为pg＝(xg,yg)，那么引力场函数为：

27、

28、其中，系数k是引力场的增益系数，通过实验选择合适的值。

29、通过引力与引力场的梯度关系可知，引力的表达式为：

30、fa＝-grad(ua)＝-k(p-pg)                  (5)

31、斥力场的数学表达式为：

32、

33、其中，系数β是斥力场的增益系数，ρ是船舶到障碍物的距离，ρ0是斥力场的影响半径，超出该范围的斥力大小为零。

34、斥力的表达式为：

35、

36、当船舶进入局部极小点的位置时，引入一个虚拟障碍物，该虚拟斥力场函数为：

37、

38、其中，β′为大于零的虚拟斥力势场常数，ρ′为船舶与虚拟障碍物之间距离，ρ0′是虚拟障碍物对船舶的影响距离。

39、虚拟斥力为：

40、

41、由此得到合力表达式为：

42、

43、虚拟障碍势场法在船舶编队航行中的应用步骤如下：

44、step1：输入每艘船的当前位置、航行速度、方向等参数；

45、step2：确定跟随者船舶是否已进入障碍物影响范围，若已进入障碍物影响范围，计算斥力和引力，否则继续航行；

46、step3：当跟随者已经受到障碍物影响时，判断受到的合力是否为0。如果为0，则添加虚拟障碍物然后重新计算合力，判断运动方向；如果不为0，则直接计算运动方向。重复此步骤，直到它离开障碍物的影响范围；

47、step4：当跟随者船舶脱离碰撞风险后，继续按原轨迹航行。

48、本发明的积极效果在于：提供了一种快速行进平方法与虚拟障碍势场法相结合的船舶编队算法。首先采用快速行进平方法进行全局静态路径规划，然后通过领导—跟随的编队控制方法实现跟随者船舶对领导者船舶的路径跟踪，最后，采用改进的虚拟障碍势场法进行局部避碰，并通过增加虚拟障碍物的方式改进了船舶可能会进入局部极小值点位置的情况；实验结果显示算法为船舶中的领导者规划出一条同时具有时间、平滑度和安全度优势的路线，并快速形成稳定队形；对于一个n×n的网格，该方法的总算术复杂度为o(nlogn)，dijkstra算法和a*算法的总算术复杂度为o(n2)，由此可见该方法相较于dijkstra算法和a*算法的运行速度上有显著的提升；当船舶遇到碰撞风险的障碍物时可以及时进行避碰操作，并解决了传统的人工势场法无法解决的脱离局部极小值点的问题。