基于MEMSIMU和DVL组合的水下航行器导航定位方法和系统技术方案

本发明专利技术提供了一种基于MEMS IMU和DVL组合的水下航行器导航定位方法和系统，包括：取MEMS IMU的姿态角均值作为水下航行器的初始航向姿态信息；进行卡尔曼滤波的时间更新，计算水下航行器的位置和速度；将DVL测得的速度乘上比例因子消除比例误差；将DVL测得的速度消除杆臂误差，进一步校正DVL测量值；采用水下航行器速度与校正后的DVL速度作差，作为卡尔曼滤波器的测量差，进行卡尔曼滤波的量测更新，得到水下航行器位置和速度的最优估计；采用水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。本发明专利技术分别消除了DVL测速的比例误差和杆臂误差，进一步提高了水下航行器的导航定位精度。的导航定位精度。的导航定位精度。

【技术实现步骤摘要】
基于MEMS IMU和DVL组合的水下航行器导航定位方法和系统


[0001]本专利技术涉及导航定位
，具体地，涉及一种基于MEMS IMU和DVL组合的水下航行器导航定位方法和系统。

技术介绍

[0002]陆地资源的匮乏严重制约着人类社会的发展，越来越多的国家把注意力集中到了海洋资源的开发利用和保护上。海洋资源必将成为国家发展的决定性因素之一，成为国与国之间必须争夺的战略高地。由于海洋环境的特殊限制，人类的开发活动无法在水下自由进行，无需人类参与的水下航行器被广泛地应用于海洋资源的开发利用中。
[0003]水下航行器的导航定位能力是衡量潜航器性能的重要指标之一，也是制约着潜航器发展的关键技术之一。同时，为了保护国家合法的海洋资源，各国都更加注重海军的投入，促成了现代海战向信息化、多元化和复杂化发展。复杂的现代海战对各种水下兵器攻防能力提出了越来越高的要求。水下兵器的精确导航定位能力成为决定现代海战胜负的关键因素之一。为了充分利用好和保护好国家海洋资源，水下导航定位技术得到了广泛关注和深入研究。由于水下环境复杂，常采用组合导航的方式本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于MEMSIMU和DVL组合的水下航行器导航定位方法，其特征在于，包括：步骤S1：在水下航行器静止时，取一段时间内MEMSIMU的姿态角均值作为水下航行器的初始航向姿态信息；步骤S2：在水下航行器航行时，采用MEMSIMU输出的姿态角和加速度信息进行卡尔曼滤波的时间更新，计算水下航行器的位置和速度；步骤S3：将DVL测得的速度乘上比例因子消除比例误差；步骤S4：将步骤S3得到的DVL速度消除杆臂误差，进一步校正DVL测量值；步骤S5：采用步骤S2得到的水下航行器速度与步骤S4得到的校正后的DVL速度作差，作为卡尔曼滤波器的测量差，进行卡尔曼滤波的量测更新，得到水下航行器位置和速度的最优估计；步骤S6：采用步骤S5得到的水下航行器的位置计算姿态角，输出导航位置、速度和姿态角。2.根据权利要求1所述的基于MEMSIMU和DVL组合的水下航行器导航定位方法，其特征在于，步骤S1包括：MEMSIMU安装在水下航行器的重心位置，以MEMSIMU的初始位置为坐标原点，以东向为x轴，北向为y轴，天向为z轴建立导航坐标系，以载体轴向向前为x轴，载体向左为y轴，载体向上为z轴建立载体坐标系，则无人潜航器的初始位置坐标为X0(0,0)，初始速度为V0(0,0,0)，初始姿态角为所述步骤S2包括：选取卡尔曼滤波器的状态变量为：X＝[x y v
x v
y v
z
]
T
其中，x,y为水下航行器x轴和y轴方向的位置坐标；v
x
,v
y
,v
z
为水下航行器x轴、y轴和z轴方向的速度；根据水下航行器的运动方程可以得到卡尔曼滤波器的状态转移方程：其中，表示卡尔曼滤波器的状态变量；F表示卡尔曼滤波器的状态转移矩阵；其中，Δt是水下航行器前后时刻的时间间隔；以速度为量测量，得卡尔曼滤波器的量测方程为：其中，Z表示卡尔曼滤波器的测量值；H表示卡尔曼滤波器的量测矩阵；v
x
表示水下航行器载体坐标系下的x轴方向速度；v
y
表示水下航行器载体坐标系下的y轴方向速度；v
z
表示水下航行器载体坐标系下的z轴方向速度；W是过程噪声，服从正态分布W～(0，Q)，U是测量噪声，服从正态分布U～(0，R)；Q表示过程噪声的协方差矩阵；R表示量测噪声的协方差矩阵；
将上述卡尔曼滤波器离散化得：其中，X
k
表示第k时刻的卡尔曼滤波器的状态变量；Z
k
表示第k时刻的卡尔曼滤波器的测量向量；H
k
表示第k时刻的卡尔曼滤波器的测量方程；Φ
k
‑
11
表示第k
‑
1时刻的卡尔曼滤波器离散化后的状态转移矩阵；W
k
‑1表示第k
‑
1时刻的卡尔曼滤波器的过程噪声；U
k
‑1表示第k
‑
1时刻的卡尔曼滤波器的量测噪声；k表示卡尔曼滤波器离散化后的第k时刻；其中：其中，I表示单位矩阵；F
k
‑1表示第k时刻的卡尔曼滤波器离散化前的状态转移矩阵；向前推算状态变量，表达式为：X
k
＝Φ
k
‑1X
k
‑1+W
k
‑1向前推算误差协方差矩阵，表达式为：3.根据权利要求2所述的基于MEMSIMU和DVL组合的水下航行器导航定位方法，其特征在于，所述步骤S3包括：若DVL中心测得的速度为则将其乘上比例因子δk进行修正，得到DVL以自身为中心测得的速度为所述步骤S4包括：若MEMSIMU和DVL安装好后固定不动，记MEMSIMU中心与DVL中心的位置杆臂为l，MEMSIMU输出的角加速度为ω
k
，则根据刚体动力学，DVL测速的杆臂误差为ω
×
l，将DVL以自身为中心测得的速度减去杆臂误差，得到IMU中心的速度测量值：4.根据权利要求3所述的基于MEMSIMU和DVL组合的水下航行器导航定位方法，其特征在于，所述步骤S5包括：卡尔曼滤波器的测量值为：其中，表示IMU测得的航行器速度；计算卡尔曼滤波增益，表达式为：K
k
＝P
k|k
‑1H
T
(HP
k|k
‑1H
T
+R))更新卡尔曼滤波最优估计，表达式为：更新误差协方差矩阵，表达式为：P
k|k
＝(I
‑
K
k
H)P
k|k
‑1。5.根据权利要求4所述的基于MEMSIMU和DVL组合的水下航行器导航定位方法，其特征在于，所述步骤S6包括：
直接将MEMSIMU输出的俯仰角θ和滚动角φ作为真值，利用水下航行器前后两次的位置计算偏航角ψ，表达式为：其中，x
k
表示第k时刻水下航行器导航坐标系下的x轴位置坐标；y
k
表示第k时刻水下航行器导航坐标系下的y轴位置坐标。6.一种...

【专利技术属性】
技术研发人员：刘佳鹏，于特，曾青山，卢地华，周畅，
申请(专利权)人：上海船舶电子设备研究所中国船舶集团有限公司第七二六研究所，
类型：发明
国别省市：