CN112097763B - 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 - Google Patents

Abstract

本发明提出一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，包括了初始对准过程、惯性解算过程、速度量测卡尔曼滤波和航向量测卡尔曼滤波，初始对准过程利用磁力计和加速度计完成初始航向姿态解算，惯性解算过程利用MEMS IMU的数据进行载体位姿的递推计算，速度量测卡尔曼滤波首先以MEMS IMU和DVL输出的速度之差作为滤波器一次量测，利用滤波估计出航向、姿态等导航参数，通过反馈修正，提高了水平姿态等测量精度；其次结合获得的水平姿态信息和磁力计测量值，计算磁航向信息，航向量测卡尔曼滤波以一次反馈修正得到的航向和磁航向之差作为滤波器二次量测，再进行滤波，进而获得高精度的航向、姿态、速度等导航参数，使得系统的定位精度大幅度提高。

Description

一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航 方法

技术领域

本发明涉及水下运载体的导航定位技术领域，特别是涉及一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，通过多级滤波模式，减小水下运载体长时间水下航行的导航定位误差。

背景技术

海洋是人类发展的重要战略空间，实现对海洋资源的合理、可持续性开发利用对人类未来的发展至关重要。水下运载体是目前进行海洋调查、海洋资源勘探、水下搜救、潜水支援、军事侦探的关键性装备，可以在无人控制的状态下全天候、多航时的自主完成水下任务。由于水下环境的特殊性，实现水下运载体的精准导航仍很困难，导航问题是水下运载体所面临的主要技术挑战之一。

现阶段的高精度导航系统主要以激光或光纤捷联惯导系统为主，辅以GPS和DVL等手段，这类系统普遍体积较大，价格昂贵；考虑到系统的便携性和经济性，基于MEMS的导航定位系统具有体积小、成本低的特点，但动态条件下的航向、姿态测量精度通常较为有限，若水下运载体长时间处于机动状态，此导航系统得到的航向、姿态信息将存在一定误差。

发明内容

为提高MEMS惯导水下运载体导航定位精度，尤其是水下运载体水下长航时运行的导航定位精度，本发明提出了一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，通过多级组合滤波模式，将MEMS IMU/磁力计/DVL的原始测量数据进行融合，获得更加准确的航向、姿态估计，为水下运载体水下作业提供较高精度的导航信息，提高MEMS IMU水下运载体水下长航时运行的导航定位精度。

本发明技术方案为：

所述一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，包括以下步骤：

步骤1：水下运载体入水后，采用MEMS IMU的加速度计数据解算水下运载体的姿态角，用加速度解算的姿态角对磁力计数据进行坐标转换，采用航向角计算公式，得到水下运载体的初始航向姿态信息；

步骤2：水下运载体开始水下航行时，采用MEMS IMU/DVL/磁力计组合导航，用步骤1得到的航向姿态信息作为水下运载体初始航向姿态，采用MEMS IMU输出的数据进行姿态速度位置的递推计算，用卡尔曼滤波估计出系统的导航参数，得到实时的水下运载体航向姿态速度信息；

步骤3：采用步骤2得到的水下运载体速度和DVL输出的速度之差作为卡尔曼滤波器一次量测，用卡尔曼滤波器估计出航向、姿态参数，校正MEMS IMU后，得到较高精度的水平姿态等测量信息；

步骤4：采用步骤3得到的较高精度的水平姿态信息以及磁力计测量值，得到水下运载体的磁航向信息；用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测，用卡尔曼滤波器估计出航向、姿态参数，再次对MEMS惯导进行校正，得到高精度的水下运载体导航定位信息。

进一步的，步骤3中水下运载体水下航行时采用步骤2得到的速度信息和DVL输出的速度之差作为卡尔曼滤波器一次量测，得到较高精度的水平姿态信息的过程为：

步骤3.1：建立MEMS IMU/磁力计/DVL组合导航下的卡尔曼滤波模型；

MEMS IMU/磁力计/DVL组合导航下的滤波方程状态变量为：

其中，Φ为载体的姿态角误差；δvⁿ为速度误差；δP为位置误差；ε^b为陀螺常值漂移；

为加速度计常值零偏；δv_d为DVL测速误差，此处仅考虑速度偏移误差；

MEMS IMU/磁力计/DVL组合导航下的系统状态方程为：

式中，F为系统状态转移矩阵，G为系统噪声分配矩阵，W^b为系统噪声矩阵；

系统状态转移矩阵F为：

式中：M_ij为惯性系统的状态转移矩阵，下标i和j分别取a、v、p；

为姿态转换矩阵；M_dvl为多普勒测速仪的反相关时间矩阵；

系统噪声分配矩阵G为：

系统噪声矩阵W^b为：

式中：

为陀螺仪测量中的白噪声；

为加速度计测量中的白噪声；

为多普勒测速误差驱动的白噪声；

步骤3.2：水下运载体水下航行中以MEMS IMU和DVL输出的速度之差作为滤波器一次量测：

由MEMS IMU解算值和DVL输出的速度之差形成的量测值Z_INS/DVL为：

第一级滤波器的量测方程为：

Z_INS/DVL＝H₁X+V

其中:

V＝V_D

式中，H₁为系统第一级滤波器量测矩阵，Vⁿ为运载体在导航下的速度，V_D为DVL量测噪声向量；

当获得速度量测更新时，通过卡尔曼滤波器对导航参数进行估计，将导航参数反馈给MEMS惯导，得到较高精度的水平姿态信息。

进一步的，步骤4用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测，得到高精度的水下运载体导航定位信息的过程为：

步骤4.1：采用步骤3得到的较高精度的水平姿态信息以及磁力计的测量值，得到水下运载体的磁航向信息：

磁力计计算航向公式为：

式中，b_x、b_y、b_z为载体坐标系下的磁力计输出，γ为一次反馈修正后的载体横滚角，θ为一次反馈修正后的载体俯仰角；

步骤4.2：用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测：

由一次反馈修正得到的航向和磁航向之差形成的量测值Z_INS/mag为：

其中

为载体递推航向误差，

为载体磁力计计算航向误差；

T₁₂、T₂₂、T₃₂为姿态转换矩阵

中的对应位置元素，φ_E为载体姿态东向失准角，φ_N为载体姿态北向失准角，φ_U为载体姿态天向失准角；

其中

式中，δθ为载体俯仰角误差，δγ为载体横滚角误差，T₁₁、T₁₃、T₂₁、T₂₃、T₃₁、T₃₃为姿态转换矩阵

中的对应位置元素；

第二级滤波器的量测方程为：

Z_INS/mag＝H₂X+V

＝[A B C 0_1×14]X+V

其中：

C＝-1

V＝V_M

式中，H₂为系统第二级滤波器量测矩阵，V_M为磁力计计算航向量测噪声向量；

当获得磁航向量测更新时，通过卡尔曼滤波器对导航参数进行估计，将导航参数反馈给MEMS惯导，得到高精度的水下运载体导航定位信息。

有益效果

使用本发明的方法后，水下运载体在水下航行时，以陀螺仪和加速度计数据进行系统状态更新，分别使用DVL和磁力计来对两级滤波器进行量测修正，以陀螺仪进行姿态解算并通过DVL速度信息对水平姿态角进行修正，提高了水下运载体动态条件下水平姿态角的测量精度。

使用磁力计进行航向解算时，通过水平姿态角信息对磁力计测量进行坐标转换，精度较高的水平姿态信息可以提高磁力计计算航向精度。

将磁力计计算航向引入二次量测方程，利用了磁力计长期稳定性好且不存在累积误差的特点，能够一定程度上解决水下运载体长时间运行时的航向漂移问题，提高水下运载体水下长航时运行的导航定位精度。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明的算法框架示意图。

图2为本发明的算法滤波过程示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明提出一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，它包括了初始对准过程、惯性解算过程、速度量测卡尔曼滤波过程和航向量测卡尔曼滤波过程。水下运载体入水后，利用磁力计和加速度计得到水下运载体初始航向姿态，完成初始对准过程。此处引入卫星导航数据作为惯导递推解算的初始位置。

水下运载体在水下航行时，采用MEMS IMU/磁力计/DVL组合导航方式，用初始对准过程得到的航向姿态信息作为水下运载体初始航向姿态，引入的卫星导航数据作为水下运载体的初始位置信息。采用MEMS IMU输出的数据进行姿态速度位置的递推计算，用卡尔曼滤波估计出系统的导航参数，得到水下运载体航向姿态速度以及位置信息。以MEMS IMU和DVL输出的速度之差作为卡尔曼滤波器一次量测，用卡尔曼滤波器估计出航向、姿态等导航参数校正MEMS惯导后，得到较高精度的水平姿态等测量信息。结合反馈修正的水平姿态信息和磁力计测量值，得到水下运载体的磁航向信息。用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测，用卡尔曼滤波器估计出航向、姿态等导航参数，再次对MEMS惯导进行校正，使得水下运载体的导航定位精度大幅度提高。

水下运载体在水下航行过程中建立了多级卡尔曼滤波模式，第一级滤波模式(速度量测卡尔曼滤波)用于减小水下运载体航行过程中的水平姿态和速度误差，此时水下运载体的导航方式为MEMS IMU/DVL组合导航方式，即水下运载体在水下航行过程建立的第一级滤波模式为MEMS IMU/DVL组合导航下的滤波方式。第二级滤波模式(航向量测卡尔曼滤波)用于减小水下运载体航行过程中的航向姿态误差，此时水下运载体的导航方式为MEMSIMU/磁力计组合导航方式，即水下运载体在水下航行过程建立的第二级滤波模式为MEMSIMU/磁力计组合导航下的滤波方式。

所述速度量测卡尔曼滤波，通过建立MEMS惯导与DVL组合导航下的滤波模型，以MEMS IMU和DVL输出的速度之差作为量测值，用卡尔曼滤波器估计出的导航参数校正MEMS惯导后，提高了水下运载体的水平姿态信息的精度，使用磁力计进行航向解算时，需要通过水平姿态角信息对磁力计测量进行坐标转换，精度较高的水平姿态信息可以提高磁力计计算航向精度。

所述航向量测卡尔曼滤波，通过建立MEMS惯导与磁力计组合导航下的滤波模型，将磁力计计算航向引入二次量测方程，利用了磁力计长期稳定性好且不存在累积误差的特点，能够一定程度上解决水下运载体的航向漂移问题，提高水下运载体长航时运行的导航定位精度。

具体包括以下步骤：

步骤1：水下运载体入水后，采用MEMS IMU中加速度计数据解算水下运载体的姿态角，用加速度解算的姿态角对磁力计数据进行坐标转换，采用航向角计算公式，得到水下运载体的初始航向姿态信息。

步骤2：水下运载体开始水下航行时，采用MEMS IMU/磁力计/DVL组合导航，用步骤1得到的航向姿态信息作为水下运载体初始航向姿态，引入卫星导航数据作为水下运载体的初始位置信息。采用MEMS IMU中陀螺和加速度计的数据进行姿态速度位置的递推计算(卡尔曼滤波状态更新)，通过卡尔曼滤波估计出系统的导航参数，得到实时的水下运载体航向姿态速度信息。

步骤3：采用步骤2得到的水下运载体速度和DVL输出的速度之差作为卡尔曼滤波器一次量测，用卡尔曼滤波器估计出航向、姿态等导航参数校正MEMS惯导后，得到较高精度的水平姿态等测量信息：

步骤3.1：建立MEMS IMU与DVL组合导航下的卡尔曼滤波模型；

MEMS IMU/磁力计/DVL组合导航下的滤波方程状态变量为：

其中，Φ为水下运载体的姿态角误差；δvⁿ为速度误差；δP为位置误差；ε^b为陀螺常值漂移；

为MEMS IMU中加速度计常值零偏；δv_d为DVL测速误差，此处仅考虑速度偏移误差。

MEMS IMU/磁力计/DVL组合导航下的系统状态方程为：

式中，F为系统状态转移矩阵，G为系统噪声分配矩阵，W^b为系统噪声矩阵。

系统状态转移矩阵F为：

式中：

M_ij—惯性系统的状态转移矩阵，与系统误差方程对应；

—姿态转换矩阵；

M_dvl—多普勒测速仪DVL的反相关时间矩阵。

系统噪声分配矩阵G为:

系统噪声矩阵W^b为：

式中：

—陀螺仪测量中的白噪声；

—加速度计测量中的白噪声；

—多普勒测速误差驱动的白噪声。

步骤3.2：水下运载体水下航行中以MEMS IMU和DVL输出的速度之差作为滤波器一次量测，利用卡尔曼滤波估计出航向、姿态等导航参数校正MEMS惯导后，提高了水平姿态等测量精度；

对MEMS IMU与DVL组合导航下的滤波模型先进行离散化得到离散状态方程，再进行离散卡尔曼滤波，当获得速度量测更新时，通过卡尔曼滤波器对导航参数(状态向量X(t))进行估计，将导航参数反馈给MEMS IMU，得到较高精度的水平姿态信息。

由MEMS IMU解算值和DVL输出的速度之差形成的量测值Z_INS/DVL为：

第一级滤波器的量测方程为：

Z_INS/DVL＝H₁X+V

其中:

V＝V_D

式中，H₁为系统第一级滤波器量测矩阵，Vⁿ为水下运载体在导航下的速度，V_D为DVL量测噪声向量。

步骤4：采用步骤3得到的较高精度的水平姿态信息和磁力计测量值，得到水下运载体的磁航向信息。用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测，用卡尔曼滤波器估计出航向、姿态等导航参数，再次对MEMS惯导进行校正，得到高精度的水下运载体导航定位信息。

步骤4.1：采用步骤3得到的较高精度的水平姿态信息和磁力计测量值，得到水下运载体的磁航向信息；

磁力计计算航向公式为：

式中，b_x、b_y、b_z为载体坐标系下的磁力计输出，γ为一次反馈修正后的载体横滚角，θ为一次反馈修正后的载体俯仰角。

步骤4.2：水下运载体水下航行中以一次反馈修正得到的航向和磁航向之差作为滤波器二次量测，利用卡尔曼滤波估计出航向、姿态等导航参数校正MEMS惯导后，得到高精度的水下运载体导航定位信息；

对MEMS惯导与磁力计组合导航下的滤波模型先进行离散化得到离散状态方程，再进行离散卡尔曼滤波，当获得磁航向量测更新时，通过卡尔曼滤波器对导航参数进行估计，将导航参数反馈给MEMS惯导，得到高精度的水下运载体导航定位信息。

第二级滤波器的量测方程推导如下：

式中,

为一次反馈修正后的载体航向角，

为磁力计计算航向，

载体惯性系统递推航向角误差，

为磁力计计算航向误差。

已知基于惯性系统递推载体航向误差与载体姿态误差角的关系为：

式中，

为载体递推航向误差，T₁₂、T₂₂、T₃₂为姿态矩阵

中的对应位置元素，φ_E为载体姿态东向失准角，φ_N为载体姿态北向失准角，φ_U为载体姿态天向失准角。

利用载体的俯仰角和横滚角信息辅助解析磁航向值，根据一阶偏微分定义，对磁计算航向公式求解关于俯仰角θ和横滚角γ的偏导数，得到磁航向值的误差模型为：

其中:

式中，

为载体磁力计计算航向误差，δθ为载体俯仰角误差，δγ为载体横滚角误差，T_ij为姿态矩阵

中的对应位置元素。

由一次反馈修正得到的航向和磁航向之差形成的量测值Z_INS/mag为：

第二级滤波器的量测方程为：

Z_INS/mag＝H₂X+V

＝[A B C 0_1×14]X+V

其中：

C＝-1

V＝V_M

式中，H₂为系统第二级滤波器量测矩阵，T_ij为姿态矩阵

中的对应位置元素，V_M为磁力计计算航向量测噪声向量。

上述方法用到的系统误差模型为：

姿态误差方程：

其中，

R_Mh＝R_M+h

R_Nh＝R_N+h

式中，φ_E为载体姿态东向失准角，φ_N为载体姿态北向失准角，φ_U为载体姿态天向失准角，V_E为载体东向速度，V_N为载体北向速度，V_U为载体天向速度，ε_E为陀螺常值误差在东向的投影，ε_N为陀螺常值误差在北向的投影，ε_U为陀螺常值误差在天向的投影，ω_ie为地球自转角速率，R_M为子午圈主曲率半径，R_N为卯酉圈主曲率半径，δL为纬度误差。

速度误差方程：

其中，

式中，f_E为加速度计输出在东向的投影，f_N为加速度计输出在北向的投影，f_U为加速度计输出在天向的投影，

为加速度计常值误差在东向的投影，

为加速度计常值误差在北向的投影，

为加速度计常值误差在天向的投影

位置误差方程：

陀螺仪测量误差：

式中，ε_b为常值漂移，ε_w为均值为零的高斯白噪声。

加速度计测量误差：

式中，

为加速度计零偏误差，

为均值为零的高斯白噪声。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种基于MEMS IMU/磁力计/DVL组合的水下运载体组合导航方法，其特征在于：包括以下步骤：

步骤1：水下运载体入水后，采用MEMS IMU的加速度计数据解算水下运载体的姿态角，用加速度解算的姿态角对磁力计数据进行坐标转换，采用航向角计算公式，得到水下运载体的初始航向姿态信息；

步骤2：水下运载体开始水下航行时，采用MEMS IMU/DVL/磁力计组合导航，用步骤1得到的航向姿态信息作为水下运载体初始航向姿态，采用MEMS IMU输出的数据进行姿态速度位置的递推计算，用卡尔曼滤波估计出系统的导航参数，得到实时的水下运载体航向姿态速度信息；

步骤3：采用步骤2得到的水下运载体速度和DVL输出的速度之差作为卡尔曼滤波器一次量测，用卡尔曼滤波器估计出航向、姿态参数，校正MEMS IMU后，得到较高精度的水平姿态测量信息；具体为：

步骤3.1：建立MEMS IMU/磁力计/DVL组合导航下的卡尔曼滤波模型；

MEMS IMU/磁力计/DVL组合导航下的滤波方程状态变量为：

其中，Φ为载体的姿态角误差；δvⁿ为速度误差；δP为位置误差；ε^b为陀螺常值漂移；

为加速度计常值零偏；δv_d为DVL测速误差，此处仅考虑速度偏移误差；

MEMS IMU/磁力计/DVL组合导航下的系统状态方程为：

式中，F为系统状态转移矩阵，G为系统噪声分配矩阵，W^b为系统噪声矩阵；

系统状态转移矩阵F为：

式中：M_ij为惯性系统的状态转移矩阵，下标i和j分别取a、v、p；

为姿态转换矩阵；M_dvl为多普勒测速仪的反相关时间矩阵；

系统噪声分配矩阵G为：

系统噪声矩阵W^b为：

式中：

为陀螺仪测量中的白噪声；

为加速度计测量中的白噪声；

为多普勒测速误差驱动的白噪声；

步骤3.2：水下运载体水下航行中以MEMS IMU和DVL输出的速度之差作为滤波器一次量测：

由MEMS IMU解算值和DVL输出的速度之差形成的量测值Z_INS/DVL为：

第一级滤波器的量测方程为：

Z_INS/DVL＝H₁X+V

其中:

V＝V_D

式中，H₁为系统第一级滤波器量测矩阵，Vⁿ为运载体在导航下的速度，V_D为DVL量测噪声向量；

当获得速度量测更新时，通过卡尔曼滤波器对导航参数进行估计，将导航参数反馈给MEMS惯导，得到较高精度的水平姿态信息；

步骤4：采用步骤3得到的较高精度的水平姿态信息以及磁力计测量值，得到水下运载体的磁航向信息；用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测，用卡尔曼滤波器估计出航向、姿态参数，再次对MEMS惯导进行校正，得到高精度的水下运载体导航定位信息；具体为：

步骤4.1：采用步骤3得到的较高精度的水平姿态信息以及磁力计的测量值，得到水下运载体的磁航向信息：

磁力计计算航向公式为：

式中，b_x、b_y、b_z为载体坐标系下的磁力计输出，γ为一次反馈修正后的载体横滚角，θ为一次反馈修正后的载体俯仰角；

步骤4.2：用一次反馈修正得到的航向和磁航向之差作为卡尔曼滤波器二次量测：由一次反馈修正得到的航向和磁航向之差形成的量测值Z_INS/mag为：

其中

为载体递推航向误差，

为载体磁力计计算航向误差；

T₁₂、T₂₂、T₃₂为姿态转换矩阵

中的对应位置元素，φ_E为载体姿态东向失准角，φ_N为载体姿态北向失准角，φ_U为载体姿态天向失准角；

其中

式中，δθ为载体俯仰角误差，δγ为载体横滚角误差，T₁₁、T₁₃、T₂₁、T₂₃、T₃₁、T₃₃为姿态转换矩阵

中的对应位置元素；

第二级滤波器的量测方程为：

Z_INS/mag＝H₂X+V

＝[A B C 0_1×14]X+V

其中：

C＝-1

V＝V_M

式中，H₂为系统第二级滤波器量测矩阵，V_M为磁力计计算航向量测噪声向量；

当获得磁航向量测更新时，通过卡尔曼滤波器对导航参数进行估计，将导航参数反馈给MEMS惯导，得到高精度的水下运载体导航定位信息。

Images