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

有效

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

本發明提出一種基于MEMS IMU/磁力計/DVL組合的水下運載體組合導航方法，包括了初始對準過程、慣性解算過程、速度量測卡爾曼濾波和航向量測卡爾曼濾波，初始對準過程利用磁力計和加速度計完成初始航向姿態解算，慣性解算過程利用MEMS IMU的數據進行載體位姿的遞推計算，速度量測卡爾曼濾波首先以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組合導航下的濾波方程狀態變量為：