基于船载UWB的无人潜航器水面定位方法及系统与流程

本发明涉及无人潜航器回收，具体地，涉及水下航行器的水面导航定位方法及系统，更为具体地，涉及一种基于船载uwb的无人潜航器水面定位方法及系统

背景技术：

1、随着探索的范围不断扩大，海洋机器人成为了探索海洋必不可少的重要工具，其中无人潜航器提供了极大便利，能够代替人类完成各种潜航作业，避免了下潜对研究人员造成的安全隐患，也拓宽了获取海洋信息的渠道。无人潜航器发射后水下完成一系列任务上浮水面后，由于所搭载的单频gps位置误差较大，所搭载的惯性测量仪imu和多普勒测速仪dvl定位结果累计误差大，因而无法得到准确的位置信息。导致无人潜航器无法通过动力定位实现原地等待回收，也无法通过主动运动到指定位置配合回收。目前在回收时主要采用人工反复抛钩连接无人潜航器的方式进行回收，该方式效率低，耗时长且难以应对多无人潜航器回收的任务要求。而受制于体积、重量和水密要求，高精度gps可以搭载在母船上，难以搭载在无人潜航器上。因此可以借助母船上高精度的gps，设计一种无人潜航器水面精准定位的方法，为无人潜航器在水面上的精准运动控制提供基准。

2、超宽带(ultra wide band，uwb)技术是一种无线载波通信技术，它不采用正弦载波，而是利用纳秒级的非正弦波窄脉冲传输数据，因此其所占的频谱范围很宽。uwb技术具有系统复杂度低，发射信号功率谱密度低，对信道衰落不敏感，截获能力低，定位精度高等优点，可达到厘米级的相对定位。通过在母船上安装uwb基站，在无人潜航器上安装uwb标签，无人潜航器可获取与母船相对位置信息，结合母船发送自身的高精度gps定位信息，可以得到较为精准的无人潜航器位置信息，其再将imu姿态信息和dvl速度信息融合，可以准确计算自身的位置和姿态，从而执行后续的回收任务。

3、专利文献cn114459476a(申请号：)公开了基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法，首先，获取安装在水下无人潜航器上的多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据；然后，利用dvl的输出、imu的输出以及sins捷联解算获得的历史速度数据构建一组虚拟速度量测，该虚拟速度量测为对地速度；最终，将所构建的虚拟速度量测与imu、dvl传感器的输出一同输入至卡尔曼滤波器中，完成多传感器融合后的对水下无人潜航器的定位与导航。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于船载uwb的无人潜航器水面定位方法及系统。

2、根据本发明提供的一种基于船载uwb的无人潜航器水面定位方法，包括：

3、步骤s1：无人潜航器上浮，其搭载的uwb标签与母船的uwb基站进行相互定位得到uwb标签的位置信息；

4、步骤s2：母船将自身gps信息发送给无人潜航器，无人潜航器根据母船的gps信息及uwb标签的位置信息计算无人潜航器的位置量测值；

5、步骤s3：将dvl测得的以自身为中心的速度消除比例误差，得到无人潜航器的速度测量值；

6、步骤s4：构建卡尔曼滤波器，并根据构建的卡尔曼滤波器进行时间更新；

7、步骤s5：无人潜航器进行量测更新，输出自身的位置和姿态。

8、优选地，所述步骤s1采用：

9、以母船的位置为零点，母船上的三个uwb基站安装在同一个水平面，位置分别为a1(x1,y1,zbase)，a2(x2,y2,zbase)，a3(x3,y3,zbase)；无人潜航器上uwb标签的位置用(xtag,ytag,ztag)表示；根据测得的uwb距离值，标签的位置表示为：

10、

11、其中，d1，d2，d3分别表示测得的uwb距离值。

12、优选地，所述步骤s2采用：

13、

14、其中，(xship,yship)表示母船的gps信息；(xtag,ytag)表示uwb标签的位置信息；(xauv,yauv)表示无人潜航器的位置量测值。

15、优选地，所述步骤s3采用：

16、设dvl测得的速度将其乘上比例因子δk进行修正，得到无人潜航器的量测速度

17、

18、优选地，所述步骤s4采用：

19、由于水下航行器上浮至水面，高度方向固定，因而选取卡尔曼滤波器的状态变量为：

20、x＝[x y vx vy vz]t

21、其中，x,y为无人潜航器导航坐标系下x轴和y轴方向的位置坐标，vx,vy,vz为无人潜航器载体坐标系下的三轴方向速度；根据无人潜航器的运动方程，得到卡尔曼滤波器的状态转移方程：

22、

23、其中，

24、

25、其中，w表示过程噪声，服从正态分布w～(0,q)；ψ表示航向角，θ表示俯仰角，φ表示横滚角，δt表示时间间隔；

26、将卡尔曼滤波器状态转移方程离散化：

27、xk＝φk-1xk-1+wk-1

28、其中，k表示卡尔曼滤波器离散化后第k时刻；

29、

30、卡尔曼滤波器进行时间更新计算水下航行器的位置和速度：向前推算状态变量

31、xk＝φk-1xk-1+wk-1

32、向前推算误差协方差矩阵：

33、

34、其中，q表示过程噪声的协方差矩阵；上标t表示矩阵转置，pk-1|k-1表示先验协方差估计。

35、优选地，所述步骤s5采用：

36、以位置和速度为量测量可得卡尔曼滤波器的量测方程为

37、

38、其中，

39、

40、计算卡尔曼滤波增益：

41、kk＝pk|k-1ht(hpk|k-1ht+r))

42、其中，r表示量测噪声的协方差矩阵；

43、更新卡尔曼滤波最优估计：

44、

45、其中，表示量测向量；

46、更新误差协方差矩阵：

47、pk|k＝(i-kkh)pk|k-1；

48、其中，i表示单位矩阵。

49、根据本发明提供的一种基于船载uwb的无人潜航器水面定位系统，包括：

50、模块m1：无人潜航器上浮，其搭载的uwb标签与母船的uwb基站进行相互定位得到uwb标签的位置信息；

51、模块m2：母船将自身gps信息发送给无人潜航器，无人潜航器根据母船的gps信息及uwb标签的位置信息计算无人潜航器的位置量测值；

52、模块m3：将dvl测得的以自身为中心的速度消除比例误差，得到无人潜航器的速度测量值；

53、模块m4：构建卡尔曼滤波器，并根据构建的卡尔曼滤波器进行时间更新；

54、模块m5：无人潜航器进行量测更新，输出自身的位置和姿态。

55、优选地，所述模块m1采用：

56、以母船的位置为零点，母船上的三个uwb基站安装在同一个水平面，位置分别为a1(x1,y1,zbase)，a2(x2,y2,zbase)，a3(x3,y3,zbase)；无人潜航器上uwb标签的位置用(xtag,ytag,ztag)表示；根据测得的uwb距离值，标签的位置表示为：

57、

58、其中，d1，d2，d3分别表示测得的uwb距离值；

59、所述模块m2采用：

60、

61、其中，(xship,yship)表示母船的gps信息；(xtag,ytag)表示uwb标签的位置信息；(xauv,yauv)表示无人潜航器的位置量测值。

62、优选地，所述模块m3采用：

63、设dvl测得的速度将其乘上比例因子δk进行修正，得到无人潜航器的量测速度

64、

65、所述模块m4采用：

66、由于水下航行器上浮至水面，高度方向固定，因而选取卡尔曼滤波器的状态变量为：

67、x＝[x y vx vy vz]t

68、其中，x,y为无人潜航器导航坐标系下x轴和y轴方向的位置坐标，vx,vy,vz为无人潜航器载体坐标系下的三轴方向速度；根据无人潜航器的运动方程，得到卡尔曼滤波器的状态转移方程：

69、

70、其中，

71、

72、其中，w表示过程噪声，服从正态分布w～(0,q)；ψ表示航向角，θ表示俯仰角，φ表示横滚角，δt表示时间间隔；

73、将卡尔曼滤波器状态转移方程离散化：

74、xk＝φk-1xk-1+wk-1

75、其中，k表示卡尔曼滤波器离散化后第k时刻；

76、

77、卡尔曼滤波器进行时间更新计算水下航行器的位置和速度：向前推算状态变量

78、xk＝φk-1xk-1+wk-1

79、向前推算误差协方差矩阵：

80、

81、其中，q表示过程噪声的协方差矩阵；上标t表示矩阵转置，pk-1|k-1表示先验协方差估计。

82、优选地，所述模块m5采用：

83、以位置和速度为量测量可得卡尔曼滤波器的量测方程为

84、

85、其中，

86、

87、计算卡尔曼滤波增益：

88、kk＝pk|k-1ht(hpk|k-1ht+r))

89、其中，r表示量测噪声的协方差矩阵；

90、更新卡尔曼滤波最优估计：

91、

92、其中，表示量测向量；

93、更新误差协方差矩阵：

94、pk|k＝(i-kkh)pk|k-1；

95、其中，i表示单位矩阵。

96、与现有技术相比，本发明具有如下的有益效果：

97、1、本发明采用uwb设备提供的相对位置信息结合船载gps全局位置信息，得到无人潜航器全局位置信息；

98、2、本发明融合uwb设备提供的位置信息、imu设备提供的姿态信息、dvl设备提供的速度信息，进一步得到无人潜航器的位置、速度和姿态的最优估计，为下一步运动控制提供基准。