面向室內(nèi)場(chǎng)景的慣性磁感應(yīng)定位方法

夏子權(quán), 李新年, 聞 帆, 于謙璽, 馮學(xué)光, 李清華

(1.哈爾濱工業(yè)大學(xué)空間控制與慣性技術(shù)研究中心, 哈爾濱 150001;2.中國(guó)人民解放軍32380部隊(duì), 北京 100072)

0 引言

近年來(lái),隨著智能無(wú)人系統(tǒng)、物聯(lián)網(wǎng)等技術(shù)在室內(nèi)環(huán)境的應(yīng)用,人們對(duì)室內(nèi)導(dǎo)航定位技術(shù)提出了更高的要求,低成本、高精度且易于維護(hù)的室內(nèi)導(dǎo)航定位方案是室內(nèi)導(dǎo)航技術(shù)領(lǐng)域重點(diǎn)研究方向之一[1-2]。在衛(wèi)星導(dǎo)航拒止的室內(nèi)環(huán)境中,UWB、視覺(jué)、激光雷達(dá)及慣性導(dǎo)航等方法由于環(huán)境約束和自身因素,無(wú)法長(zhǎng)時(shí)間提供高精度的可靠導(dǎo)航定位服務(wù),利用低頻旋轉(zhuǎn)磁場(chǎng)的高穿透力和特征矢量方向不變的特性,可為室內(nèi)等復(fù)雜場(chǎng)景提供一種高精度、無(wú)積累誤差的導(dǎo)航定位方案[3-4]。通過(guò)使用通電線圈或旋轉(zhuǎn)永磁體等人工磁信標(biāo)可生成規(guī)律性的低頻旋轉(zhuǎn)磁場(chǎng),結(jié)合磁傳感器測(cè)量值與相對(duì)位置間的數(shù)值關(guān)系,可以獲得高精度的定位解算結(jié)果[5-7]。

目前,針對(duì)室內(nèi)環(huán)境下的磁信標(biāo)定位技術(shù),國(guó)內(nèi)外學(xué)者已開(kāi)展了大量研究,研究?jī)?nèi)容主要圍繞基于磁場(chǎng)強(qiáng)度、磁場(chǎng)矢量和指紋匹配的定位技術(shù)3個(gè)方面展開(kāi)[8-14]。Darmindra D. Arumugam等人[8-9]開(kāi)發(fā)了一種遠(yuǎn)程低頻磁場(chǎng)靜態(tài)定位技術(shù),通過(guò)解耦磁場(chǎng)強(qiáng)度和距離間的關(guān)系進(jìn)行測(cè)距,在室內(nèi)環(huán)境下利用兩個(gè)磁信標(biāo)實(shí)現(xiàn)了高精度的二維定位;Bo We等人[10-11]利用在空間中選取一定數(shù)量的指紋特征匹配點(diǎn),記錄對(duì)應(yīng)的感應(yīng)磁場(chǎng)強(qiáng)度,在實(shí)際導(dǎo)航中通過(guò)匹配測(cè)量點(diǎn)和指紋點(diǎn)處的磁場(chǎng)強(qiáng)度來(lái)實(shí)現(xiàn)目標(biāo)定位,并結(jié)合慣性導(dǎo)航開(kāi)發(fā)了一套iMag+SLAM系統(tǒng),以滿足室外和室內(nèi)環(huán)境中精確定位和快速設(shè)置的要求;李新年等人[12-14]根據(jù)旋轉(zhuǎn)磁場(chǎng)具有唯一方向不變的特征矢量的特點(diǎn),建立了特征矢量與位置間的數(shù)學(xué)關(guān)系,僅依靠磁場(chǎng)矢量便可實(shí)現(xiàn)定位功能,在此基礎(chǔ)上提出了一種不受傳感器姿態(tài)誤差影響的慣性磁感應(yīng)定位方法,有效解決了定位過(guò)程中磁傳感器姿態(tài)對(duì)結(jié)果的影響。然而,上述3種方法都存在共同的缺點(diǎn):需提前確定信標(biāo)的磁矩信息,對(duì)于大小呈正弦變化的磁矩,只有獲得整個(gè)周期的數(shù)據(jù)才能確定其大小,這大大降低了定位結(jié)果的輸出頻率,增加了磁信標(biāo)部署的難度。

為了降低人工磁信標(biāo)定位系統(tǒng)的部署難度、提高系統(tǒng)的輸出頻率,本文建立了一種不受磁信標(biāo)磁矩信息和傳感器姿態(tài)影響的磁感應(yīng)矢量夾角觀測(cè)模型;結(jié)合慣性導(dǎo)航的誤差模型,提出了一種的慣性磁感應(yīng)動(dòng)態(tài)定位方法,并利用無(wú)跡卡爾曼濾波[15,17](unscented Kalman filter, UKF)算法實(shí)現(xiàn)了磁場(chǎng)數(shù)據(jù)和慣導(dǎo)數(shù)據(jù)的同步融合,有效提高了導(dǎo)航精度。

1 磁感應(yīng)定位模型

1.1 基于磁感應(yīng)矢量夾角的觀測(cè)模型

人工磁信標(biāo)定位系統(tǒng)主要由磁場(chǎng)信號(hào)發(fā)射機(jī)、接收機(jī)與導(dǎo)航解算模塊3部分構(gòu)成,如圖1所示,磁場(chǎng)信號(hào)發(fā)射機(jī)由兩個(gè)通入不同頻率正弦激勵(lì)電流的正交線圈構(gòu)成;接收機(jī)由3個(gè)正交的感應(yīng)線圈構(gòu)成,利用3個(gè)線圈上感應(yīng)電動(dòng)勢(shì)的變化實(shí)現(xiàn)對(duì)發(fā)射機(jī)磁場(chǎng)矢量的測(cè)量;導(dǎo)航解算模塊則根據(jù)接收機(jī)的磁場(chǎng)矢量測(cè)量值進(jìn)行信號(hào)處理,結(jié)合導(dǎo)航算法實(shí)現(xiàn)對(duì)目標(biāo)點(diǎn)處的快速定位。

圖1 人工磁信標(biāo)定位系統(tǒng)示意圖Fig.1 Schematic diagram of artificial magnetic beacon positioning system

將由雙軸正交螺線管構(gòu)成的磁信標(biāo)作為磁場(chǎng)信號(hào)發(fā)射機(jī),分析其磁場(chǎng)分布規(guī)律。根據(jù)我們之前的研究[13-14],當(dāng)接收機(jī)與發(fā)射機(jī)之間的距離r大于發(fā)射機(jī)線圈半徑R的3倍時(shí),可將發(fā)射機(jī)的線圈等效為兩個(gè)正交的磁偶極子,根據(jù)畢奧-薩伐爾定律,磁偶極子的磁場(chǎng)矢量可以表示為

(1)

式中,μ0=4π×10-7H/m,表示空間環(huán)境的磁導(dǎo)率;r表示接收機(jī)與發(fā)射機(jī)之間的距離;m表示磁矩矢量。

雙軸磁信標(biāo)的兩個(gè)發(fā)射線圈Tx1和Tx2的線圈匝數(shù)為Nc和Ns,磁通面積為Sc和Ss,分別通入幅值為Ic和Is、信號(hào)頻率為ωC和ωS,且相位相差60°的正弦激勵(lì)電流,根據(jù)圖1和式(1),可以得到線圈Tx1和Tx2在空間中任意一點(diǎn)處P(r,φ,θ)磁場(chǎng)矢量BC和BS的表達(dá)式

(2)

(3)

式(2)和(3)中,MC=ICNCSCsin(ωCt)和MS=ISNSSSsin(ωSt+60°)分別表示發(fā)射線圈Tx1和Tx2的磁矩;φ和θ分別為接收機(jī)相對(duì)于發(fā)射機(jī)的俯仰角和方位角。

(4)

(5)

圖2 旋轉(zhuǎn)磁場(chǎng)示意圖Fig.2 Schematic diagram of rotating magnetic field

(6)

(7)

(8)

(9)

基于式(8)和式(9),可以構(gòu)建如下觀測(cè)模型

(10)

分析式(10)可知,該觀測(cè)模型直接利用測(cè)量得到的磁感應(yīng)矢量夾角余、正弦值作為量測(cè)量,可以配合其他傳感器如慣性傳感器進(jìn)行組合導(dǎo)航。

1.2 基于UKF的慣性磁感應(yīng)定位模型

一般情況下,室內(nèi)環(huán)境中使用的低成本MEMS慣導(dǎo)系統(tǒng)總是在小范圍內(nèi)運(yùn)動(dòng),因此選擇當(dāng)?shù)刂苯亲鴺?biāo)系作為導(dǎo)航參考坐標(biāo)系(n系),導(dǎo)航起點(diǎn)為坐標(biāo)原點(diǎn)(O),磁信標(biāo)兩正交軸的軸向?yàn)镺xn和Oyn,垂直xOy平面向上為Ozn,建立慣導(dǎo)系統(tǒng)的誤差方程

(11)

在慣導(dǎo)系統(tǒng)誤差方程和磁感應(yīng)矢量夾角觀測(cè)模型的基礎(chǔ)上,構(gòu)建慣性磁感應(yīng)定位系統(tǒng)的狀態(tài)空間模型

(12)

Fk/k-1=

(13)

根據(jù)式(12)可知,利用磁感應(yīng)矢量夾角輔助慣導(dǎo)對(duì)動(dòng)態(tài)目標(biāo)進(jìn)行定位的模型是非線性的。目前,針對(duì)非線性模型的濾波方法,常用的有擴(kuò)展卡爾曼濾波(EKF)和無(wú)跡卡爾曼濾波(UKF)[17]。EKF算法基本思想是先對(duì)非線性函數(shù)進(jìn)行泰勒展開(kāi),然后保存展開(kāi)式的一階項(xiàng),忽略其余高階項(xiàng),將非線性系統(tǒng)近似為線性系統(tǒng),最后再完成卡爾曼濾波估計(jì);UKF是一種對(duì)后驗(yàn)概率密度進(jìn)行近似而得到的濾波算法,可直接對(duì)系統(tǒng)導(dǎo)航參數(shù)進(jìn)行估計(jì),從而避免對(duì)非線性量測(cè)方程線性化,保證了系統(tǒng)具有較高的定位精度,因此本文采用UKF完成對(duì)慣導(dǎo)和磁場(chǎng)數(shù)據(jù)的融合。

針對(duì)本文建立的慣性磁感應(yīng)定位模型,給出以下UKF濾波流程。

Step1 初始化狀態(tài)量和協(xié)方差矩陣

(14)

Step2 進(jìn)行Sigma采樣,計(jì)算采樣點(diǎn)

(15)

Step3 時(shí)間更新

(16)

式中,Wi,e和Wi,c分別為UT變換中計(jì)算Sigma采樣點(diǎn)對(duì)應(yīng)的均值和協(xié)方差權(quán)值,定義為

(17)

其中,τ=α2(n+γ)-n為縮放比例參數(shù);n為狀態(tài)量的維數(shù),本文中n=15;α控制由非線性函數(shù)帶來(lái)的高階影響,一般取e-4≤α≤1,γ=3-n,β=2。

Step4:量測(cè)更新

(18)

Step5:狀態(tài)和協(xié)方差估計(jì)更新

(19)

(20)

2 實(shí)驗(yàn)驗(yàn)證

通過(guò)上述分析,本文建立了基于雙軸磁信標(biāo)的磁感應(yīng)矢量夾角觀測(cè)模型和基于UKF的慣性磁感應(yīng)定位模型,本章將通過(guò)實(shí)驗(yàn)的方式分別對(duì)兩個(gè)模型的有效性和精度進(jìn)行驗(yàn)證。由于實(shí)際實(shí)驗(yàn)過(guò)程中很難給出運(yùn)動(dòng)目標(biāo)的真實(shí)動(dòng)態(tài)軌跡,因此本章將采用數(shù)值仿真的方式對(duì)基于UKF的慣性磁感應(yīng)定位模型進(jìn)行仿真驗(yàn)證,通過(guò)對(duì)比真實(shí)軌跡和實(shí)際運(yùn)動(dòng)軌跡,從而達(dá)到驗(yàn)證該模型精度的目的。

2.1 基于磁感應(yīng)矢量夾角的觀測(cè)模型實(shí)驗(yàn)驗(yàn)證

本節(jié)將利用如圖3所示的雙軸磁信標(biāo)實(shí)驗(yàn)系統(tǒng)測(cè)試基于磁感應(yīng)矢量夾角觀測(cè)模型的精度和穩(wěn)定性,該系統(tǒng)由激勵(lì)電流生成器、功率放大器、雙軸磁場(chǎng)發(fā)射機(jī)、三軸矢量磁傳感器和磁場(chǎng)數(shù)據(jù)處理模塊5個(gè)單元組成。

圖3 磁信標(biāo)實(shí)驗(yàn)系統(tǒng)Fig.3 Magnetic beacon experimental system

實(shí)驗(yàn)時(shí),首先確定導(dǎo)航參考坐標(biāo)系(n)和測(cè)量點(diǎn)坐標(biāo),本實(shí)驗(yàn)以雙軸磁場(chǎng)發(fā)射機(jī)的幾何中心為坐標(biāo)原點(diǎn)(O),兩正交軸指向分別為x和y方向,垂直xOy平面指向下為z方向,在該坐標(biāo)系下按照接收機(jī)與發(fā)射機(jī)間距離由近及遠(yuǎn)的原則選擇9個(gè)實(shí)驗(yàn)點(diǎn),測(cè)得其坐標(biāo)值,位置點(diǎn)和坐標(biāo)如圖3所示;其次,磁場(chǎng)發(fā)射機(jī)的兩個(gè)線圈中分別通入幅值2 A、頻率20 Hz和30 Hz、相位相差60°的激勵(lì)電流,此時(shí)空間中生成旋轉(zhuǎn)磁場(chǎng);然后,將感知范圍為0.1～10 000 nT 的三軸矢量磁傳感器分別放置于9個(gè)實(shí)驗(yàn)點(diǎn),測(cè)得實(shí)驗(yàn)點(diǎn)處的磁場(chǎng)數(shù)據(jù);最后,利用式(8)和(9)分別計(jì)算出磁感應(yīng)矢量夾角余、正弦值的理論值和實(shí)驗(yàn)值。為了測(cè)試傳感器姿態(tài)和環(huán)境中遮擋物對(duì)精度的影響以及實(shí)驗(yàn)結(jié)果的穩(wěn)定性,設(shè)計(jì)以下3組實(shí)驗(yàn),并且每組實(shí)驗(yàn)重復(fù)10次。

實(shí)驗(yàn)1:將磁傳感器分別放置于9個(gè)實(shí)驗(yàn)點(diǎn)處,保證發(fā)射線圈和磁傳感器處于同一坐標(biāo)系下,兩者間無(wú)遮擋物,測(cè)量此時(shí)磁場(chǎng)數(shù)據(jù)。

實(shí)驗(yàn)2:控制磁傳感器相對(duì)于發(fā)射線圈的俯仰角、橫滾角和航向角分別為20°,30°和40°,其他同實(shí)驗(yàn)1。

實(shí)驗(yàn)3:在發(fā)射線圈上覆蓋住一個(gè)木箱,其他同實(shí)驗(yàn)1。

實(shí)驗(yàn)結(jié)果如圖4所示,從誤差圖中可知,不同位置處磁感應(yīng)矢量夾角余弦值和正弦值的理論值與3組實(shí)驗(yàn)值之間的最大誤差值均小于0.1,且3組實(shí)驗(yàn)值之間的誤差值小于0.05,說(shuō)明傳感器姿態(tài)以及環(huán)境中遮擋物對(duì)磁感應(yīng)定位結(jié)果并無(wú)影響;從誤差棒圖中可以看出,每組實(shí)驗(yàn)重復(fù)10次的標(biāo)準(zhǔn)差小于0.08,離散程度較小,說(shuō)明實(shí)驗(yàn)結(jié)果具有較高的穩(wěn)定性,且變化的磁矩對(duì)磁感應(yīng)定位并無(wú)影響。需注意的是隨著接收機(jī)和發(fā)射機(jī)之間距離的增大,數(shù)據(jù)的離散程度有增大的趨勢(shì),這是由于磁場(chǎng)信號(hào)強(qiáng)度隨距離衰減導(dǎo)致信噪比下降引起的,說(shuō)明距離是影響測(cè)量精度的重要因素。

(a)

(b)圖4 不同位置處磁感應(yīng)矢量夾角余弦值和正弦值的誤差大小及穩(wěn)定性分析結(jié)果圖Fig.4 Error and stability analysis result of the cosine and sine values of the included angle of the magnetic-induction vector at different positions

2.2 慣性磁感應(yīng)定位模型仿真驗(yàn)證

圖5 基于UKF的慣性磁感應(yīng)定位結(jié)果Fig.5 Results of inertial magnetic-induction positioning based on UKF

圖6 純慣性位置誤差圖Fig.6 Inertia position error diagram

圖7 慣性磁感應(yīng)組合導(dǎo)航位置誤差圖Fig.7 Position error diagram of inertial magnetic-induction integrated navigation

從仿真的結(jié)果可知,相較于僅依靠慣性導(dǎo)航,無(wú)人車使用基于UKF的慣性磁感應(yīng)定位方法具有更高的定位精度:全過(guò)程中x、y、z方向上的位置誤差均小于0.75 m,有效地抑制了慣導(dǎo)的發(fā)散。

3 結(jié)論

針對(duì)衛(wèi)星拒止條件下的室內(nèi)導(dǎo)航問(wèn)題,本文結(jié)合磁信標(biāo)定位系統(tǒng)及MEMS慣性傳感器提出了一種不受傳感器姿態(tài)和磁信標(biāo)磁矩影響的慣性磁感應(yīng)定位方法,并通過(guò)實(shí)驗(yàn)與仿真結(jié)合的方式驗(yàn)證了方法的有效性。主要內(nèi)容有:

1)在雙軸人工磁信標(biāo)磁場(chǎng)分布規(guī)律的基礎(chǔ)上詳細(xì)推導(dǎo)并建立了不受傳感器姿態(tài)和磁信標(biāo)磁矩影響的磁感應(yīng)矢量夾角觀測(cè)模型,通過(guò)實(shí)驗(yàn)驗(yàn)證了模型的有效性。

2)提出了一種基于UKF的慣性磁感應(yīng)定位方法,實(shí)現(xiàn)了慣導(dǎo)數(shù)據(jù)與磁感應(yīng)數(shù)據(jù)的融合。利用數(shù)值仿真的方式進(jìn)行驗(yàn)證,結(jié)果表明,該方法能有效地抑制慣性導(dǎo)航的發(fā)散,適用于較高精度的室內(nèi)導(dǎo)航定位。

本文的研究成果對(duì)慣性磁感應(yīng)組合導(dǎo)航系統(tǒng)在室內(nèi)場(chǎng)景下的應(yīng)用具有較好的理論價(jià)值和實(shí)踐價(jià)值,但受限于磁信標(biāo)有效作用域的大小,目前該套系統(tǒng)的使用范圍有限,后期可通過(guò)提高磁信標(biāo)磁場(chǎng)覆蓋范圍和磁傳感器精度的方式擴(kuò)大慣性磁感應(yīng)組合導(dǎo)航系統(tǒng)的使用范圍。