[發明專利]一種室內環境磁異常實時修正與導航方法及裝置

有效

• 申請號: 202010794600.5
• 申請日: 2020-08-10
• 公開（公告）號: CN112033405B
• 公開（公告）日: 2022-06-17
• 發明（設計）人: 薛原;黃雄;張挺;劉宇;劉善濤;孟來泉;張南;張韻;孟騰飛;姜偉
• 申請（專利權）人: 北京摩高科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/18;G01C21/16;G01C21/08;G01C21/06;G01C21/00;G01C25/00;G01C17/38
• 代理公司: 北京知聯天下知識產權代理事務所(普通合伙) 11594
• 代理人: 張陸軍;張迎新

1.一種室內環境磁異常實時修正與導航方法，其特征在于，所述方法包括：

確定t時刻的磁異常量Δm(t)；

根據所述t時刻的磁異常量Δm(t)對地磁矢量進行修正，獲得t時刻的地磁矢量修正量Mb_修正(t)；

利用所述t時刻的地磁矢量修正量Mb_修正(t)確定旋轉矩陣C(t)；

根據旋轉矩陣C(t)確定航向；

所述方法中還包括對MEMS傳感器中加速度計的修正方法，所述對MEMS傳感器中加速度計的修正方法包括以下步驟：

步驟e1、判斷加速度是否異常，當三軸加速度計模值sqrt(ax²+ay²+az²)與1g相差較大時，則不進行加速度計修正，其中，1g表示一個重力加速度；

步驟e2、加速度歸一化，A＝[ax ay az]^T，A_新＝A/norm(A)，其中，ax表示為x軸的加速度，ay表示為y軸的加速度，az表示為z軸的加速度，norm表示矩陣范數；

步驟e3、計算加速度偏差Ea＝A_新×V，其中，V＝C^T[0 0 1]^T表示為重力加速度在體坐標系的投影；

所述方法中還包括對MEMS傳感器中磁力計的修正方法，所述對MEMS傳感器中磁力計的修正方法包括以下步驟：

步驟f1、磁力計歸一化：M＝[mx my mz]T，M_新＝M/norm(M)，其中，mx表示為x軸的磁力計，my表示為y軸的磁力計，mz表示為z軸的磁力計；

步驟f2、計算磁場偏差Em＝M_新×W，其中，W＝C^TMe(t)表示為磁力計在體坐標系的投影；

所述方法中包括對人體姿態捷聯解算的修正方法，所述對人體姿態捷聯解算的修正方法包括以下步驟：

步驟g1、初始化四元數q，并初始化陀螺偏差δ＝[δ_x δ_y δ_z]^T＝[0 0 0]^T，其中，δ_x表示為x軸的陀螺偏差，δ_y表示為y軸的陀螺偏差，δ_z表示為z軸的陀螺偏差；

步驟g2、計算xyz三軸的修正速率，w＝w₀+δ，其中，w₀＝[wx wy wz]^T，表示為三軸角速率，wx表示為x軸的角速率，wy表示為y軸的角速率，wz表示為z軸的角速率；

步驟g3、計算修正后的其中q＝[q₀ q₁q₂ q₃]^T。

2.根據權利要求1所述的室內環境磁異常實時修正與導航方法，其特征在于，所述方法具體包括以下步驟：

步驟a、將MEMS傳感器靜止，利用磁輸出Mb(0)確定初始航向，利用加速度計輸出確定初始橫滾和俯仰，其中，Mb(0)表示為地磁矢量在體坐標的投影，即MEMS傳感器在初始時刻的磁輸出值；

步驟b、通過基本四元數積分算法，計算t時刻磁異常量Δm(t)；

步驟c、對t時刻的地磁矢量進行修正得到修正后的地磁矢量Mb_修正，Mb_修正(t)＝Mb(t)-C^T(t)ΔM_t，其中，C(t)表示為t時刻只利用陀螺數據計算的旋轉矩陣，C^T(t)表示C(t)在t時刻的轉置矩陣，Mb(t)表示為地磁矢量在體坐標的投影，即MEMS傳感器在t時刻的磁輸出值，ΔM_t表示為大于設定闕值的N個周期磁異常的平均值；

步驟d、利用修正后的地磁矢量Mb_修正(t)，結合加速度輸出、陀螺輸出，重新計算旋轉矩陣C_新(t)，確定航向。