[發明專利]一種附有運動加速度補償的MARG姿態計算方法

有效

• 申請號: 202011427992.8
• 申請日: 2020-12-07
• 公開（公告）號: CN112683269B
• 公開（公告）日: 2022-05-03
• 發明（設計）人: 周澤波;張澤亮
• 申請（專利權）人: 電子科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 電子科技大學專利中心 51203
• 代理人: 陳一鑫

本發明公開了本發明提出一種附有運動加速度補償的MARG姿態計算方法，屬于導航領域。該方法首先利用陀螺儀進行姿態一步預測，然后利用一步預測值對加速度計與磁力計進行檢測，最后針對不同檢測結果執行相應的數據融合算法。通過使用本方法進行MARG傳感器姿態估計，有效對載體運動加速度與磁場干擾進行檢測；針對運動載體的動態特征，引入滑動窗口數據擬合運動加速度變化規律并進行相應的模型補償；推導重磁(加速度和磁感應強度測量值)矢量對的在Wahba問題下的解析解從而加快姿態求解速度，顯著提高MARG姿態估計效率。

技術領域

本發明屬于導航領域，具體是附有滑動遞推窗口運動加速度補償的重磁矢量對快速解析的MARG姿態估計方法。

背景技術

近幾十年來，隨著微電子機械(Micro-Electro-Mechanical System,MEMS)技術的發展，MEMS慣性傳感器被廣泛應用于工業領域和科學研究，用以提供位置姿態信息。由三軸陀螺儀和三軸加速度計構成的MEMS慣性傳感器，搭配三軸磁傳感器的組合模塊，被稱為MARG(magnetic,angular rate and gravity,MARG)傳感器。為了最佳地融合MARG傳感器數據，發展了眾多數據融合方法，主要分為兩大類。其中一類是加速度計和陀螺儀互補濾波的方法，基于加速度計和陀螺儀輸出信息在頻域具有互補特性。偏航角可通過磁傳感器進行進一步修正。互補濾波具有計算時間短，易于在硬件平臺應用等特點，然而其只能提供次優估計。為獲得更加準確的姿態，另一類方法——線性卡爾曼濾波(Kalman filter,KF)被用于數據融合，其可獲得最小均方差意義下的最優估計。此外，針對非線性模型，發展了各類非線性濾波器如擴展KF、無跡KF和粒子KF，用以估計載體位置與姿態。然而，MARG傳感器姿態估計易受非重力加速度及磁場干擾的影響。這些干擾將導致系統不再滿足濾波假設模型，引起濾波估計有偏甚至發散。為解決這些問題，需建立加速度計與磁力計檢測準則，進行異常數據檢測。

發明內容

針對上述問題，本發明提出一種附有運動加速度補償的MARG姿態計算方法；該方法首先利用陀螺儀進行姿態一步預測，然后利用一步預測值對加速度計與磁力計進行檢測，最后針對不同檢測結果執行相應的數據融合算法。通過使用本方法進行MARG傳感器姿態估計，有效對載體運動加速度與磁場干擾進行檢測；針對運動載體的動態特征，引入滑動窗口數據擬合運動加速度變化規律并進行相應的模型補償；推導重磁(加速度和磁感應強度測量值)矢量對的在Wahba問題下的解析解從而加快姿態求解速度，顯著提高MARG姿態估計效率。

本發明的技術方案為：一種附有運動加速度補償的MARG姿態計算方法，具體包括以下步驟：

步驟1：利用陀螺儀進行一步預測；

步驟2：利用一步預測值，對加速度計與磁力計進行檢測，通過滑動窗口多項式擬合，對運動加速度進行補償；

步驟3：針對不同的檢測結果執行相應的融合算法：

無磁場干擾，無運動加速度(或運動加速度已補償)：轉至步驟4；

存在磁干擾，無運動加速度(或運動加速度已補償)：轉步驟5；

無磁干擾，存在運動加速度且運動加速度未補償，轉步驟6；

存在磁干擾，存在運動加速度且運動加速度未補償：將陀螺推算的一步預測結果作為姿態估計值，等待下一時刻的傳感器數據。

步驟4：建立加速度和磁場強度的矢量對方程，通過快速線性四元數估計方法求解獲得量測信息，基于卡爾曼濾波進行姿態估計。

步驟5：利用加速度計數據建立量測模型，基于非線性濾波實現狀態估計，并對磁干擾進行估計。

步驟6：利用磁力計數據建立量測模型，基于非線性濾波實現狀態估計。