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

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

0 引言

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

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

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

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

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

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

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

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

(1)

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

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

(2)

(3)

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

(4)

(5)

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

(6)

(7)

(8)

(9)

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

(10)

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

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

一般情況下,室內(nèi)環(huán)境中使用的低成本MEMS慣導系統(tǒng)總是在小范圍內(nèi)運動,因此選擇當?shù)刂苯亲鴺讼底鳛閷Ш絽⒖甲鴺讼?n系),導航起點為坐標原點(O),磁信標兩正交軸的軸向為Oxn和Oyn,垂直xOy平面向上為Ozn,建立慣導系統(tǒng)的誤差方程

(11)

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

(12)

Fk/k-1=

(13)

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

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

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

(14)

Step2 進行Sigma采樣,計算采樣點

(15)

Step3 時間更新

(16)

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

(17)

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

Step4:量測更新

(18)

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

(19)

(20)

2 實驗驗證

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

2.1 基于磁感應(yīng)矢量夾角的觀測模型實驗驗證

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

圖3 磁信標實驗系統(tǒng)Fig.3 Magnetic beacon experimental system

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

實驗1:將磁傳感器分別放置于9個實驗點處,保證發(fā)射線圈和磁傳感器處于同一坐標系下,兩者間無遮擋物,測量此時磁場數(shù)據(jù)。

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

實驗3:在發(fā)射線圈上覆蓋住一個木箱,其他同實驗1。

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

(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)定位模型仿真驗證

圖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)組合導航位置誤差圖Fig.7 Position error diagram of inertial magnetic-induction integrated navigation

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

3 結(jié)論

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

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

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

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