一种基于多点位姿融合解算的船舶辅助靠泊方法及装置

本发明涉及船舶辅助靠泊，尤其涉及一种基于多点位姿融合解算的船舶辅助靠泊方法及装置。

背景技术：

1、随着世界航运经济的不断推进，船舶逐渐向大型化、快速化发展，安全靠泊成为船长和引航员迫切关心的问题。在船舶靠泊过程中，由于解算得到精度不足的船舶关键靠泊参数、错误判断船舶离岸距离以及不完善的预警措施导致操纵船舶失误，从而造成船舶碰撞码头岸线的事故时有发生。因此，对船舶靠泊过程中的关键靠泊参数进行实时高精度解算以及提供合理的预警措施是完成船舶安全靠泊的前提和保障。

2、传统的船舶辅助靠泊系统存在单点定位导致解算的船舶关键靠泊参数精度不足、针对不同环境测算船舶离岸距离不可靠的问题。现有的船舶辅助靠泊系统单点定位获取的参数数据量小、优化空间不足、误差大，导致船舶定位定姿精度低，无法满足大型船舶对高精度的需求。且不同环境下测量船舶离岸距离时可靠性较差，无法保证在各种情况下提供准确的信息，特别是在极端天气或复杂港口环境中，其表现尤为不佳。

技术实现思路

1、本发明的目的是克服现有技术中存在的船舶关键靠泊参数解算精度低导致船舶靠泊可靠性低的缺陷与问题，提供一种船舶关键靠泊参数解算精度高且船舶靠泊可靠性高的基于多点位姿融合解算的船舶辅助靠泊方法及装置。

2、为实现以上目的，本发明的技术解决方案是：一种基于多点位姿融合解算的船舶辅助靠泊方法，包括：

3、在船舶上设置多个硬件部署点，并在硬件部署点布置包括北斗接收机和惯性导航系统的硬件系统；利用硬件系统得到包括船舶的航向、航速、转心率和离岸距离的参数单点解算结果；

4、构建基于最小二乘的多点解算模型，对多个硬件系统得到的参数单点解算结果进行融合得到参数多点解算结果；通过硬件系统获取的船舶横向速度和船舶转向速率计算船舶转心位置与各硬件部署点的距离，以各硬件部署点为圆心，以求解的各距离为半径作圆，并通过圆分布的几何特征确定船舶实际转心位置的搜索区域，遍历搜索区域得到船舶实际转心位置；将参数多点解算结果和船舶实际转心位置作为关键靠泊参数；

5、当船舶根据规划的靠泊路径驶过航路点时，将关键靠泊参数与关键靠泊参数的参考值对比得到参量偏差率，根据参量偏差率做出预警并修正靠泊路径。

6、在船舶的船首、船尾以及垂直于船舶中轴且经过驾驶台中心的船舶左右两弦处分别布设硬件系统。

7、所述利用硬件系统得到包括船舶的航向、航速、转心率和离岸距离的参数单点解算结果，包括：

8、建立船基坐标系，将硬件系统获取的位置坐标信息转换至船基坐标系中，并解算船舶的航向、航速、转心率和离岸距离；

9、所述航速为：

10、；

11、式中，为船舶时刻的位置坐标；为船舶时刻的位置坐标；

12、所述转心率为：

13、；

14、式中，为船舶时刻的航向角；为船舶时刻的航向角；

15、当存在高精度电子海图信息时，将高精度电子海图上的实际岸线信息转换至船基坐标系中，得到岸线方程：

16、；

17、式中，、、为实际岸线信息转换至船基坐标系后岸线方程的参数；

18、将船体补至矩形，并选取e、f为船体俯视矩形图中靠近岸线的两个端点，则e、f两点的离岸距离为：

19、；

20、式中，为e点的位置坐标；为f点的位置坐标；为e点的离岸距离；为f点的离岸距离；

21、选取离岸距离的最小值作为船舶的离岸距离：

22、。

23、所述构建基于最小二乘的多点解算模型，对多个硬件系统得到的参数单点解算结果进行融合得到参数多点解算结果，包括：

24、根据硬件系统得到的船舶的航向、航速、转心率和离岸距离，通过线性拟合得到如下方程：

25、；

26、式中，为参数多点解算结果；、、、为四个参数单点解算结果；为误差扰动项；、、、为待求未知参数；

27、利用历史测量数据得到如下方程：

28、；

29、式中，为1至时刻的历史参数多点解算结果；为1至时刻的个历史参数单点解算结果；为待求未知参数；为1至时刻的误差扰动项；

30、构建高斯-马尔可夫模型：

31、；

32、；；；；

33、利用最小二乘法求解得到多点融合参数：

34、；

35、；

36、则航向、航速、转心率、离岸距离的多点解算结果为：

37、；

38、式中，、、、分别为航向、航速、转心率、离岸距离的多点解算结果；、、、分别为航向的多点融合参数；、、、分别为航速的多点融合参数；、、、分别为转心率的多点融合参数；、、、分别为离岸距离的多点融合参数；、、、分别为四个航向单点解算结果；、、、分别为四个航速单点解算结果；、、、分别为四个转心率单点解算结果；、、、分别为四个离岸距离单点解算结果。

39、所述通过硬件系统获取的船舶横向速度和船舶转向速率计算船舶转心位置与各硬件部署点的距离，以各硬件部署点为圆心，以求解的各距离为半径作圆，并通过圆分布的几何特征确定船舶实际转心位置的搜索区域，遍历搜索区域得到船舶实际转心位置，包括：

40、计算船舶转心位置与各硬件部署点的距离：

41、；

42、式中，为船舶转心位置与各硬件部署点的距离；为船舶靠泊时各硬件部署点的横向速度；为船舶转向速率；

43、以各硬件部署点为圆心，以求解的各距离为半径作圆，并通过圆分布的几何特征确定船舶实际转心位置的搜索区域；利用网格搜索法，分别对船基坐标系的x轴和y轴取搜索区域坐标范围的最小值，并以设定间隔值向上取垂直于坐标轴的网格线至搜索区域边界，网格线交点即为待遍历的搜索点，以此确定搜索网格；对网格中的每个搜索点进行遍历，取误差平方和最小值的坐标为船舶实际转心位置；

44、所述误差平方和最小值的数学模型为：

45、；

46、式中，为船舶实际转心位置坐标；为第个硬件部署点的位置坐标；为第个硬件部署点与船舶转心位置的距离。

47、若船舶不存在高精度电子海图信息，则在船舶驾驶台处安装激光雷达，并通过激光雷达得到船舶的离岸距离；

48、计算激光雷达至岸线各位置点的距离，取其中最小值作为离岸距离，并将该离岸距离作为关键靠泊参数。

49、所述当船舶根据规划的靠泊路径驶过航路点时，将关键靠泊参数与关键靠泊参数参考值对比得到参量偏差率，根据参量偏差率做出预警并修正靠泊路径，包括：

50、所述参量偏差率的计算公式为：

51、；

52、式中，为关键靠泊参数的参量偏差率；为关键靠泊参数；为关键靠泊参数的参考值；

53、当参量偏差率大于等于10％且小于40％时，发出警报并根据不同关键靠泊参数的参量偏差率来修正靠泊路径；对于航向，调整航向角直到船舶的航向与规划的航向匹配；对于航速、转心率和船舶实际转心位置，调整船舶的航速、转心率和船舶实际转心位置直至参量偏差率降低至小于10％；对于离岸距离，优先控制船舶行驶至规划的航路点，再重新开启监测其他关键靠泊参数；

54、当参量偏差率大于等于40％时，发出警报并根据当前船舶关键靠泊参数规划新的靠泊路径；若根据当前船舶关键靠泊参数无法规划新的靠泊路径，则调整船舶的航向、航速、转心率和船舶实际转心位置直至寻找到新的靠泊路径。

55、一种基于多点位姿融合解算的船舶辅助靠泊装置，该装置应用于上述所述一种基于多点位姿融合解算的船舶辅助靠泊方法，所述装置包括：

56、硬件系统，包括北斗接收机和惯性导航系统，布置在船舶的多个硬件部署点，用于得到包括船舶的航向、航速、转心率和离岸距离的参数单点解算结果；

57、关键靠泊参数获取模块，用于构建基于最小二乘的多点解算模型，对多个硬件系统得到的参数单点解算结果进行融合得到参数多点解算结果；通过硬件系统获取的船舶横向速度和船舶转向速率计算船舶转心位置与各硬件部署点的距离，以各硬件部署点为圆心，以求解的各距离为半径作圆，并通过圆分布的几何特征确定船舶实际转心位置的搜索区域，遍历搜索区域得到船舶实际转心位置；将参数多点解算结果和船舶实际转心位置作为关键靠泊参数；

58、预警与靠泊路径修正模块，用于当船舶根据规划的靠泊路径驶过航路点时，将关键靠泊参数与关键靠泊参数的参考值对比得到参量偏差率，根据参量偏差率做出预警并修正靠泊路径。

59、一种基于多点位姿融合解算的船舶辅助靠泊设备，包括存储器和处理器；

60、所述存储器，用于存储计算机程序代码，并将所述计算机程序代码传输给所述处理器；

61、所述处理器，用于根据所述计算机程序代码中的指令执行上述所述一种基于多点位姿融合解算的船舶辅助靠泊方法。

62、一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述一种基于多点位姿融合解算的船舶辅助靠泊方法。

63、与现有技术相比，本发明的有益效果为：

64、1、本发明一种基于多点位姿融合解算的船舶辅助靠泊方法及装置中，方法利用多点部署得到的位姿信息，通过数学模型解算得到船舶关键靠泊参数，如航向、航速、转心率、离岸距离，然后利用最小二乘法得到多点信息融合的输出结果；针对船舶靠泊过程中对实际转心位置难以确定的问题，提出一种基于多点几何特征的解算模型，能够有效获取实际转心位置的变化规律，保持或改变船舶的运动状态；上述设计能够有效提高船舶靠泊参数的解算精度，从而提高船舶靠泊可靠性。

65、2、本发明一种基于多点位姿融合解算的船舶辅助靠泊方法及装置中，针对目前不同环境测算船舶离岸距离不可靠的问题，提出一种多点解算模型与激光雷达的选用策略；若存在高精度电子海图信息，则利用船舶的多点位姿信息以及多点解算模型计算的船舶离岸距离，否则通过激光雷达得到船舶离岸距离，以提高船舶离岸距离测算的可靠性。