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

有效

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

1.一種地磁場模型輔助的載體姿態和航向計算方法，其特征在于，包括以下步驟：

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

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

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

其中，采用以下公式構建所述磁傳感器量測值的自適應權重：

不存在環境磁干擾時；

存在環境磁干擾時；

其中，為k時刻所述磁傳感器第i個量測值的自適應權重，p_ki為量測值協方差矩陣的第i個主對角線元素，c為常數，V_i,k′為磁傳感器量測值的歸一化殘差；

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

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

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

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

其中步驟(2)又具體包括：根據慣性傳感器誤差模型和姿態誤差四元數更新模型，構建Kalman濾波的狀態方程；根據所述磁傳感器的輸出值以及根據世界地磁場模型估計的所述磁傳感器的輸出值，構建第二量測方程；

根據所述Kalman濾波的狀態方程、所述第二量測方程、所述磁傳感器量測值以及所述磁傳感器量測值的等價權矩陣，更新狀態更新值，得到狀態估計值，具體為：

采用以下公式更新狀態更新值，得到狀態估計值：

為k時刻的狀態估計值，為k時刻的狀態更新值，L_m,k為k時刻的所述磁傳感器量測值，為的協方差矩陣，為k時刻的協方差矩陣，T為轉置，為從導航坐標系到載體坐標系的轉化矩陣，K_m,k為k時刻的增益矩陣，I為單位陣,A_m,k為k時刻的量測矩陣，為k時刻的所述磁傳感器量測值的等價權矩陣，為所述姿態四元數的估計值。

2.如權利要求1所述的方法，其特征在于，根據所述加速度計量測值更新狀態預測值，得到狀態更新值，具體包括：

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

根據所述Kalman濾波的狀態方程、所述第一量測方程以及所述加速度計量測值更新狀態預測值，得到狀態更新值。

3.如權利要求2所述的方法，其特征在于，所述Kalman濾波的狀態方程，具體為：

其中，X(t)＝[q_e b_ω b_a]^T∈R^9×1為狀態量，T為轉置，F(t)為狀態轉移矩陣，W(t)為過程噪聲，為陀螺儀輸出值的反對稱矩陣，v_ω為陀螺儀的隨機漂移噪聲，為陀螺儀隨機常值模型噪聲，為加速度計隨機常值模型噪聲，q_e為誤差四元數的矢量部分，b_ω為陀螺儀隨機常值，b_a為加速度計偏差。

4.如權利要求2所述的方法，其特征在于，所述第一量測方程，具體為：

其中，L_a為所述加速度計的輸出值與根據重力場參考矢量估計的所述加速度計的輸出值之間的差值，為加速度計的輸出值，為從導航坐標系到載體坐標系的轉化矩陣，為姿態四元數的估計值，gⁿ為重力加速度在導航坐標系下的投影，v_a為加速度計的隨機誤差。