[發明專利]一種基于MEMS IMU/磁力計/DVL組合的水下運載體組合導航方法

有效

• 申請號: 202010889071.7
• 申請日: 2020-08-28
• 公開（公告）號: CN112097763B
• 公開（公告）日: 2022-07-05
• 發明（設計）人: 張福斌;廖偉飛;潘光;宋保維;高劍
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/18;G01C21/20;G01C21/04
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 高凌君

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慣導，得到高精度的水下運載體導航定位信息。