基于MEMSIMU和DVL组合的水下航行器导航定位方法和系统与流程

本发明涉及导航定位，具体地，涉及一种基于mems imu和dvl组合的水下航行器导航定位方法和系统。

背景技术：

1、陆地资源的匮乏严重制约着人类社会的发展，越来越多的国家把注意力集中到了海洋资源的开发利用和保护上。海洋资源必将成为国家发展的决定性因素之一，成为国与国之间必须争夺的战略高地。由于海洋环境的特殊限制，人类的开发活动无法在水下自由进行，无需人类参与的水下航行器被广泛地应用于海洋资源的开发利用中。

2、水下航行器的导航定位能力是衡量潜航器性能的重要指标之一，也是制约着潜航器发展的关键技术之一。同时，为了保护国家合法的海洋资源，各国都更加注重海军的投入，促成了现代海战向信息化、多元化和复杂化发展。复杂的现代海战对各种水下兵器攻防能力提出了越来越高的要求。水下兵器的精确导航定位能力成为决定现代海战胜负的关键因素之一。为了充分利用好和保护好国家海洋资源，水下导航定位技术得到了广泛关注和深入研究。由于水下环境复杂，常采用组合导航的方式对水下航行器进行导航定位，组合导航系统的设计是保证导航精度的关键。

3、专利文献cn115019412a(申请号：cn202210618561.2)公开了一种基于多传感器的水下auv海缆巡检系统及方法。构建了一种包括电源模块、auv控制模块、auv动力模块、海缆探测模块、组合导航定位模块以及无线传输模块的水下海缆巡检系统。通过多传感器配合进行海缆探测，能够提高在水下复杂环境中对海缆的识别准确度，由磁力仪探测海缆方向和位置，侧扫声纳和水下摄像机基于图像信息进行海缆识别，记录海缆破损状态等，弥补单一传感器探测的局限性，提高探测准确度。通过组合导航定位算法计算系统的姿态和位置，解决探测设备在水下定位不准确的问题。采用滤波增益补偿算法，克服了定位误差累积。实时上传海缆巡检系统在水下的精确位置信息，提高了海缆探测巡检过程的安全性。

4、惯性测量单元mems imu能够连续地提供载体的姿态、速度等信息，数据更新频率快，在短时间内具有较高的精度，但是随着系统工作时间的延长，mems imu的导航误差将会随之积累增长，此时可利用外部传感器的观测信息来修正导航信息，从而抑制导航累计误差的增长。

5、多普勒计程仪dvl是广泛用于水下及水面航行器组合系统的速度测量装置，能够实时输出载体的三维速度。dvl通过安装在auv底部的四个换能器求得声波的频移，进而计算载体在载体坐标系下的三维速度信息，具有精度较高、可靠性较强等优点。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种基于mems imu和dvl组合的水下航行器导航定位方法和系统。

2、根据本发明提供的基于mems imu和dvl组合的水下航行器导航定位方法，包括：

3、步骤s1：在水下航行器静止时，取一段时间内mems imu的姿态角均值作为水下航行器的初始航向姿态信息；

4、步骤s2：在水下航行器航行时，采用mems imu输出的姿态角和加速度信息进行卡尔曼滤波的时间更新，计算水下航行器的位置和速度；

5、步骤s3：将dvl测得的速度乘上比例因子消除比例误差；

6、步骤s4：将步骤s3得到的dvl速度消除杆臂误差，进一步校正dvl测量值；

7、步骤s5：采用步骤s2得到的水下航行器速度与步骤s4得到的校正后的dvl速度作差，作为卡尔曼滤波器的测量差，进行卡尔曼滤波的量测更新，得到水下航行器位置和速度的最优估计；

8、步骤s6：采用步骤s5得到的水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。

9、优选的，步骤s1包括：

10、mems imu安装在水下航行器的重心位置，以mems imu的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系，以载体轴向向前为x轴，载体向左为y轴，载体向上为z轴建立载体坐标系，则无人潜航器的初始位置坐标为x0(0,0)，初始速度为v0(0,0,0)，初始姿态角为

11、所述步骤s2包括：选取卡尔曼滤波器的状态变量为：

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

13、其中，x,y为水下航行器x轴和y轴方向的位置坐标；vx,vy,vz为水下航行器x轴、y轴和z轴方向的速度；

14、根据水下航行器的运动方程可以得到卡尔曼滤波器的状态转移方程：

15、

16、其中，表示卡尔曼滤波器的状态变量；f表示卡尔曼滤波器的状态转移矩阵；

17、

18、其中，δt是水下航行器前后时刻的时间间隔；

19、以速度为量测量，得卡尔曼滤波器的量测方程为：

20、

21、其中，z表示卡尔曼滤波器的测量值；h表示卡尔曼滤波器的量测矩阵；vx表示水下航行器载体坐标系下的x轴方向速度；vy表示水下航行器载体坐标系下的y轴方向速度；vz表示水下航行器载体坐标系下的z轴方向速度；w是过程噪声，服从正态分布w-(0,q),u是测量噪声，服从正态分布u～(0，r)；q表示过程噪声的协方差矩阵；r表示量测噪声的协方差矩阵；

22、将上述卡尔曼滤波器离散化得：

23、

24、其中，xk表示第k时刻的卡尔曼滤波器的状态变量；zk表示第k时刻的卡尔曼滤波器的测量向量；hk表示第k时刻的卡尔曼滤波器的测量方程；φk-11表示第k-1时刻的卡尔曼滤波器离散化后的状态转移矩阵；wk-1表示第k-1时刻的卡尔曼滤波器的过程噪声；uk-1表示第k-1时刻的卡尔曼滤波器的量测噪声；k表示卡尔曼滤波器离散化后的第k时刻；

25、其中：

26、

27、其中，i表示单位矩阵；fk-1表示第k时刻的卡尔曼滤波器离散化前的状态转移矩阵；

28、向前推算状态变量，表达式为：

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

30、向前推算误差协方差矩阵，表达式为：

31、

32、优选的，所述步骤s3包括：若dvl中心测得的速度为则将其乘上比例因子δk进行修正，得到dvl以自身为中心测得的速度为

33、所述步骤s4包括：若mems imu和dvl安装好后固定不动，记mems imu中心与dvl中心的位置杆臂为l，mems imu输出的角加速度为ωk，则根据刚体动力学，dvl测速的杆臂误差为ω×l，将dvl以自身为中心测得的速度减去杆臂误差，得到imu中心的速度测量值：

34、优选的，所述步骤s5包括：

35、卡尔曼滤波器的测量值为：

36、

37、其中，表示imu测得的航行器速度；

38、计算卡尔曼滤波增益，表达式为：

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

40、更新卡尔曼滤波最优估计，表达式为：

41、

42、更新误差协方差矩阵，表达式为：

43、pk|k＝(i-kkh)pk|k-1。

44、优选的，所述步骤s6包括：

45、直接将mems imu输出的俯仰角θ和滚动角φ作为真值，利用水下航行器前后两次的位置计算偏航角ψ，表达式为：

46、

47、其中，xk表示第k时刻水下航行器导航坐标系下的x轴位置坐标；yk表示第k时刻水下航行器导航坐标系下的y轴位置坐标。

48、根据本发明提供的基于mems imu和dvl组合的水下航行器导航定位系统，包括：

49、模块m1：在水下航行器静止时，取一段时间内mems imu的姿态角均值作为水下航行器的初始航向姿态信息；

50、模块m2：在水下航行器航行时，采用mems imu输出的姿态角和加速度信息进行卡尔曼滤波的时间更新，计算水下航行器的位置和速度；

51、模块m3：将dvl测得的速度乘上比例因子消除比例误差；

52、模块m4：将模块m3得到的dvl速度消除杆臂误差，进一步校正dvl测量值；

53、模块m5：采用模块m2得到的水下航行器速度与模块m4得到的校正后的dvl速度作差，作为卡尔曼滤波器的测量差，进行卡尔曼滤波的量测更新，得到水下航行器位置和速度的最优估计；

54、模块m6：采用模块m5得到的水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。

55、优选的，模块m1包括：

56、mems imu安装在水下航行器的重心位置，以mems imu的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系，以载体轴向向前为x轴，载体向左为y轴，载体向上为z轴建立载体坐标系，则无人潜航器的初始位置坐标为x0(0,0)，初始速度为v0(0,0,0)，初始姿态角为

57、所述模块m2包括：选取卡尔曼滤波器的状态变量为：

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

59、其中，x,y为水下航行器x轴和y轴方向的位置坐标；vx,vy,vz为水下航行器x轴、y轴和z轴方向的速度；

60、根据水下航行器的运动方程可以得到卡尔曼滤波器的状态转移方程：

61、

62、其中，表示卡尔曼滤波器的状态变量；f表示卡尔曼滤波器的状态转移矩阵；

63、

64、其中，δt是水下航行器前后时刻的时间间隔；

65、以速度为量测量，得卡尔曼滤波器的量测方程为：

66、

67、其中，z表示卡尔曼滤波器的测量值；h表示卡尔曼滤波器的量测矩阵；vx表示水下航行器载体坐标系下的x轴方向速度；vy表示水下航行器载体坐标系下的y轴方向速度；vz表示水下航行器载体坐标系下的z轴方向速度；w是过程噪声，服从正态分布w-(0,q),u是测量噪声，服从正态分布u～(0，r)；q表示过程噪声的协方差矩阵；r表示量测噪声的协方差矩阵；

68、将上述卡尔曼滤波器离散化得：

69、

70、其中，xk表示第k时刻的卡尔曼滤波器的状态变量；zk表示第k时刻的卡尔曼滤波器的测量向量；hk表示第k时刻的卡尔曼滤波器的测量方程；φk-1表示第k-1时刻的卡尔曼滤波器离散化后的状态转移矩阵；wk-1表示第k-1时刻的卡尔曼滤波器的过程噪声；uk-1表示第k-1时刻的卡尔曼滤波器的量测噪声；k表示卡尔曼滤波器离散化后的第k时刻；

71、其中：

72、

73、其中，i表示单位矩阵；fk-1表示第k时刻的卡尔曼滤波器离散化前的状态转移矩阵；

74、向前推算状态变量，表达式为：

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

76、向前推算误差协方差矩阵，表达式为：

77、

78、优选的，所述模块m3包括：若dvl中心测得的速度为则将其乘上比例因子δk进行修正，得到dvl以自身为中心测得的速度为

79、所述模块m4包括：若mems imu和dvl安装好后固定不动，记mems imu中心与dvl中心的位置杆臂为l，mems imu输出的角加速度为ωk，则根据刚体动力学，dvl测速的杆臂误差为ω×l，将dvl以自身为中心测得的速度减去杆臂误差，得到imu中心的速度测量值：

80、优选的，所述模块m5包括：

81、卡尔曼滤波器的测量值为：

82、

83、其中，表示imu测得的航行器速度；

84、计算卡尔曼滤波增益，表达式为：

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

86、更新卡尔曼滤波最优估计，表达式为：

87、

88、更新误差协方差矩阵，表达式为：

89、pk|k＝(i-kkh)pk|k-1。

90、优选的，所述模块m6包括：

91、直接将mems imu输出的俯仰角θ和滚动角φ作为真值，利用水下航行器前后两次的位置计算偏航角ψ，表达式为：

92、

93、其中，xk表示第k时刻水下航行器导航坐标系下的x轴位置坐标；yk表示第k时刻水下航行器导航坐标系下的y轴位置坐标。

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

95、(1)本发明采用mems imu直出的姿态角和加速度信息构建5维卡尔曼滤波器，有效克服了常规水下导航定位算法计算量大、计算效率低的缺点；

96、(2)本发明分别消除了dvl测速的比例误差和杆臂误差，校正了dvl的测速结果，进一步提高了水下航行器的导航定位精度。