[發明專利]一種地磁場模型輔助的載體姿態和航向計算方法和系統

有效

• 申請號: 201610059817.5
• 申請日: 2016-01-28
• 公開（公告）號: CN105716610B
• 公開（公告）日: 2018-09-04
• 發明（設計）人: 高楠;趙龍
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京慕達星云知識產權代理事務所(特殊普通合伙) 11465
• 代理人: 苗青盛

本發明公開一種地磁場模型輔助的載體姿態和航向計算方法和系統，該方法包括以下步驟：根據載體的位置信息和地磁場模型，計算理想磁傾角；根據加速度計的輸出值和磁傳感器的輸出值，計算估計磁傾角；根據理想磁傾角和估計磁傾角，判斷是否存在環境磁干擾；當存在環境磁干擾時，根據磁傳感器量測值的自適應權重，構建磁傳感器量測值的等價權矩陣；根據加速度計量測值、磁傳感器量測值和磁傳感器量測值的等價權矩陣，計算載體姿態信息和航向信息。本發明根據理想磁傾角和估計磁傾角檢測環境磁干擾，并在檢測到環境磁干擾時，構建磁傳感器量測值的等價權矩陣，并實現磁傳感器量測值對狀態估計值貢獻的自適應調整，使計算得到的姿態信息和航向信息具有抗差性。

技術領域

本發明涉及通信技術領域，特別涉及一種地磁場模型輔助的載體姿態和航向計算方法和系統。

背景技術

姿態和航向參考系統(Attitude and Heading Reference System，簡稱AHRS)是一種可以精確測量載體在空間坐標系下三軸姿態的測量裝置，由三軸磁強計、三軸陀螺儀和三軸加速度計等硬件和相應的軟件構成，為載體提供實時的姿態和航向信息，在航空航天、機器人、汽車工業以及行人導航定位領域得到廣泛的應用。隨著微機電技術的發展，基于微機電系統傳感器的姿態和航向參考系統應用更廣泛。但由于陀螺儀存在漂移，導致在計算載體姿態和航向信息，其誤差不斷累積。為解決此問題，在姿態和航向參考系統中引入加速度計和磁傳感器的信息，并以重力場和地磁場作為參考矢量，利用加速計和磁強計輸出值與陀螺儀的輸出值實時計算載體的姿態和航向信息。

雖然姿態和航向參考系統可為載體實時提供精確的姿態和航向信息，但在近地運動的載體經常受到外部環境的干擾，特別是磁傳感器受環境磁干擾明顯。當受到外部磁干擾時，利用磁傳感器輸出值作為量測值估計載體的姿態和航向信息存在誤差，甚至不能正確計算載體的姿態和航向信息，從而給載體的自主控制帶來很大的風險。

發明內容

本發明提供了一種地磁場模型輔助的載體姿態和航向計算方法和系統，以解決存在環境磁干擾時載體姿態和航向實時計算的技術問題。

本發明提供了一種地磁場模型輔助的載體姿態和航向計算方法，包括以下步驟：

根據載體的位置信息和地磁場模型，計算理想磁傾角；根據加速度計的輸出值和磁傳感器的輸出值，計算估計磁傾角；

根據所述理想磁傾角和所述估計磁傾角，判斷是否存在環境磁干擾；

當存在環境磁干擾時，根據磁傳感器量測值的歸一化殘差和量測值協方差矩陣，構建所述磁傳感器量測值的自適應權重，并根據所述磁傳感器量測值的自適應權重，構建所述磁傳感器量測值的等價權矩陣；

根據所述加速度計量測值、所述磁傳感器量測值和所述磁傳感器量測值的等價權矩陣，計算載體姿態信息和航向信息。

可選地，根據所述加速度計量測值、所述磁傳感器量測值和所述磁傳感器量測值的等價權矩陣，計算所述載體姿態信息和航向信息，具體包括：

根據所述加速度計量測值更新狀態預測值，得到狀態更新值，并使用所述狀態更新值更新姿態四元數的估計值；

根據所述磁傳感器量測值和所述磁傳感器量測值的等價權矩陣，更新所述狀態更新值，得到狀態估計值，并使用所述狀態估計值更新所述姿態四元數的估計值；

根據所述姿態四元數的估計值以及從導航坐標系到載體坐標系的轉化矩陣，計算所述載體姿態信息和航向信息。

可選地，根據所述加速度計量測值更新狀態預測值，得到狀態更新值，具體包括：

根據慣性傳感器誤差模型和姿態誤差四元數更新模型，構建Kalman濾波的狀態方程；根據所述加速度計的輸出值以及根據重力場參考矢量估計的所述加速度計的輸出值，構建第一量測方程；