[發(fā)明專利]一種室內(nèi)環(huán)境磁異常實(shí)時(shí)修正與導(dǎo)航方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202010794600.5
• 申請(qǐng)日: 2020-08-10
• 公開（公告）號(hào): CN112033405B
• 公開（公告）日: 2022-06-17
• 發(fā)明（設(shè)計(jì)）人: 薛原;黃雄;張挺;劉宇;劉善濤;孟來泉;張南;張韻;孟騰飛;姜偉
• 申請(qǐng)（專利權(quán)）人: 北京摩高科技有限公司
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/18;G01C21/16;G01C21/08;G01C21/06;G01C21/00;G01C25/00;G01C17/38
• 代理公司: 北京知聯(lián)天下知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11594
• 代理人: 張陸軍;張迎新

本發(fā)明提出一種室內(nèi)環(huán)境磁異常實(shí)時(shí)修正與導(dǎo)航方法及裝置，其中方法包括以下步驟：確定t時(shí)刻的磁異常量Δm(t)；根據(jù)所述t時(shí)刻的磁異常量Δm(t)對(duì)地磁矢量進(jìn)行修正，獲得t時(shí)刻的地磁矢量修正量Mb_修正(t)；利用所述t時(shí)刻的地磁矢量修正量Mb_修正(t)確定旋轉(zhuǎn)矩陣C(t)；根據(jù)旋轉(zhuǎn)矩陣C(t)確定航向。本發(fā)明對(duì)磁異常進(jìn)行在線估計(jì)，并實(shí)時(shí)對(duì)磁異常進(jìn)行補(bǔ)償，因而在磁異常區(qū)域也可利用磁場(chǎng)強(qiáng)度輸出對(duì)航向進(jìn)行修正，避免了以往算法在室內(nèi)磁異常情況下的航向發(fā)散問題。

技術(shù)領(lǐng)域

本發(fā)明屬于室內(nèi)導(dǎo)航技術(shù)領(lǐng)域，特別涉及一種室內(nèi)環(huán)境磁異常實(shí)時(shí)修正與導(dǎo)航方法及裝置。

背景技術(shù)

在室內(nèi)環(huán)境中，建筑結(jié)構(gòu)會(huì)對(duì)地磁場(chǎng)強(qiáng)度和磁偏角造成一定的、較穩(wěn)定的影響。這種影響的來源通常有：家用電器、鋼筋、電線等。由于地磁強(qiáng)度相對(duì)較小，因而室內(nèi)磁異常經(jīng)常會(huì)導(dǎo)致無法對(duì)航向進(jìn)行修正，造成MEMS導(dǎo)航在航向通道的發(fā)散。

現(xiàn)有技術(shù)通常在磁異常存在情況下，不對(duì)航向做修正，因此，長(zhǎng)時(shí)間處于磁異常環(huán)境，會(huì)導(dǎo)致航向發(fā)散。本專利針對(duì)這種問題，在長(zhǎng)時(shí)間處于室內(nèi)磁異常環(huán)境時(shí)，也能有效利用修正后的磁矢量對(duì)航向進(jìn)行修正，從而極大地減緩航向發(fā)散的速度。

發(fā)明內(nèi)容

針對(duì)上述問題，本發(fā)明提出一種室內(nèi)環(huán)境磁異常實(shí)時(shí)修正與導(dǎo)航方法，所述方法包括以下步驟：

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

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

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

根據(jù)旋轉(zhuǎn)矩陣C(t)確定航向。

進(jìn)一步的，所述方法具體包括以下步驟：

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

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

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

步驟d、利用修正后的地磁矢量Mb修正(t)，結(jié)合加速度輸出、陀螺輸出，重新計(jì)算旋轉(zhuǎn)矩陣C新(t)，確定航向。

進(jìn)一步的，所述步驟b中t時(shí)刻磁異常量Δm(t)計(jì)算包括以下步驟：

步驟b1、計(jì)算初始時(shí)刻的地磁矢量Me(0)＝C(0)*Mb(0)，并置磁異常量ΔM＝0，其中，C(0)表示為初始時(shí)刻從體坐標(biāo)系到大地坐標(biāo)系的旋轉(zhuǎn)矩陣，所述初始時(shí)刻從體坐標(biāo)系到大地坐標(biāo)系的旋轉(zhuǎn)矩陣C(0)通過四元數(shù)獲??；

步驟b2、獲取9軸傳感器數(shù)據(jù)，計(jì)算t時(shí)刻大地坐標(biāo)下的磁矢量Me(t)＝C(t)*Mb(t)，C(t)表示為t時(shí)刻只利用陀螺數(shù)據(jù)計(jì)算的旋轉(zhuǎn)矩陣，Mb(t)表示為地磁矢量在體坐標(biāo)的投影，即MEMS傳感器在t時(shí)刻的磁輸出值；

步驟b3、計(jì)算t時(shí)刻磁異常量Δm(t)＝Me(t)-Me(0)，并存入緩存。

進(jìn)一步的，所述N個(gè)周期磁異常的平均值若tmp大于設(shè)定闕值，則更新磁異常ΔM_t＝tmp。