不受姿態(tài)誤差影響的慣性/磁感應(yīng)融合定位方法

李新年，李清華，王常虹，鄭元?jiǎng)祝跽窕?，夏子?quán)

（1.哈爾濱工業(yè)大學(xué) 空間控制與慣性技術(shù)研究中心，哈爾濱 150001；2.中國(guó)兵器工業(yè)集團(tuán)航空彈藥研究院，哈爾濱 150030）

科技的高速發(fā)展在便捷人類(lèi)生活、拓展人類(lèi)生存空間的同時(shí)，對(duì)現(xiàn)有技術(shù)提出了更高的功能與指標(biāo)。導(dǎo)航定位技術(shù)作為一種基礎(chǔ)技術(shù)，在物聯(lián)、機(jī)器人等新興研究領(lǐng)域中起著重要的作用[1,2]，然而由于室內(nèi)等復(fù)雜場(chǎng)景存在非視距、多徑等影響因素，現(xiàn)有GPS、UWB等技術(shù)難以實(shí)現(xiàn)無(wú)累積誤差的高精度定位服務(wù)。根據(jù)電磁信號(hào)的波粒二象性可知，磁場(chǎng)穿透能力與頻率成反比，因此基于高穿透低頻磁場(chǎng)的定位方法是實(shí)現(xiàn)室內(nèi)場(chǎng)景高精度定位的有效方案之一[3-5]。低頻磁場(chǎng)定位信號(hào)一般由通電螺線管、旋轉(zhuǎn)永磁體等人工磁信標(biāo)產(chǎn)生，經(jīng)分析，其在空間的磁場(chǎng)分布與相對(duì)位置存在確定性數(shù)值關(guān)系，基于此，本文提出了一種磁感應(yīng)室內(nèi)定位方法。

基于人工磁信標(biāo)的磁感應(yīng)定位方法主要圍繞磁感應(yīng)強(qiáng)度、磁感應(yīng)矢量[6-17]兩個(gè)角度開(kāi)展研究。磁感應(yīng)強(qiáng)度所提供的定位信息少于磁感應(yīng)矢量，因此基于磁感應(yīng)矢量的人工磁信標(biāo)定位系統(tǒng)的復(fù)雜度通常要低于基于磁感應(yīng)強(qiáng)度的定位系統(tǒng)。然而基于磁場(chǎng)矢量的磁信標(biāo)定位系統(tǒng)受傳感器姿態(tài)影響，通常需要結(jié)合其它輔助信息來(lái)獲取姿態(tài)信息，進(jìn)一步解算目標(biāo)位置。基于慣性傳感器[6-11]、多磁信標(biāo)/傳感器的幾何信息[12-17]常被用來(lái)輔助磁信標(biāo)定位系統(tǒng)，如Pasku等人[9]利用磁強(qiáng)計(jì)與慣性傳感器設(shè)計(jì)了一種組合導(dǎo)航系統(tǒng)，慣性傳感器通過(guò)敏感姿態(tài)數(shù)據(jù)輔助磁傳感器轉(zhuǎn)換磁感應(yīng)矢量，并結(jié)合磁場(chǎng)分布模型與空間幾何構(gòu)型建立磁信標(biāo)定位模型，實(shí)現(xiàn)對(duì)目標(biāo)位置的估計(jì)；鄭元?jiǎng)椎热薣12-14]采用多個(gè)位置姿態(tài)已知的人工磁信標(biāo)構(gòu)建出具有明確幾何信息的磁感應(yīng)定位系統(tǒng)，達(dá)到對(duì)目標(biāo)位置姿態(tài)的估計(jì)。然而輔助信息源的設(shè)計(jì)不僅增加了系統(tǒng)的復(fù)雜度，而且直接從其它信息源獲取的姿態(tài)信息被用來(lái)轉(zhuǎn)換磁感應(yīng)矢量時(shí)，其姿態(tài)誤差會(huì)對(duì)磁感應(yīng)定位精度造成影響。

為解決磁信標(biāo)定位系統(tǒng)受傳感器姿態(tài)誤差影響的問(wèn)題，本文建立了一種與傳感器姿態(tài)無(wú)關(guān)的磁信標(biāo)定位模型；并基于此，結(jié)合慣導(dǎo)模型提出了一種改進(jìn)的慣性/磁感應(yīng)的定位方法，通過(guò)無(wú)跡卡爾曼濾波[18-20]（Unscented Kalman Filter,UKF）算法和磁信標(biāo)磁場(chǎng)數(shù)據(jù)實(shí)現(xiàn)了對(duì)慣導(dǎo)一步預(yù)測(cè)結(jié)果的修正，提高了定位精度、定位結(jié)果的輸出速度和穩(wěn)定性。

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

1.1 雙軸磁信標(biāo)定位模型

人工磁信標(biāo)系統(tǒng)主要由磁源、傳感器與導(dǎo)航解算單元三部分構(gòu)成，而由磁源產(chǎn)生的低頻磁場(chǎng)又是定位信息的傳播媒介，其分布規(guī)律是分析定位方法的理論基礎(chǔ)。本節(jié)將結(jié)合畢奧-薩伐爾定律與磁偶極子模型對(duì)雙軸磁信標(biāo)的磁場(chǎng)分布進(jìn)行理論分析。依據(jù)磁偶極子模型將通電螺線管視為圓形載流回路，當(dāng)環(huán)境中的磁導(dǎo)率為0μ，通電螺線管的激勵(lì)電流幅值為I，信號(hào)頻率為ω，線圈匝數(shù)為N，則根據(jù)畢奧-薩伐爾定律，磁偶極子磁矩指向與信標(biāo)坐標(biāo)系中z軸的指向相同時(shí)，空間中任意一點(diǎn)處P（x,y,z）單軸螺線管的磁感應(yīng)矢量的表達(dá)式為：

其中r為目標(biāo)點(diǎn)P到坐標(biāo)原點(diǎn)O的相對(duì)距離，為俯仰角，為相對(duì)方位角[5]，為通電螺線管磁矩，人工磁信標(biāo)與目標(biāo)點(diǎn)之間的相對(duì)位置如圖1所示。

圖1 信標(biāo)中心與目標(biāo)點(diǎn)之間的相對(duì)方位Fig.1 Relative position from center of beacon to the target

圖中1φ、2φ、0φ分別為磁信標(biāo)與目標(biāo)點(diǎn)之間相對(duì)于x軸、y軸、z軸的俯仰角，1θ、2θ、0θ分別為磁信標(biāo)與目標(biāo)點(diǎn)之間相對(duì)于x軸、y軸、z軸的偏航角，根據(jù)長(zhǎng)方體中的邊角關(guān)系，可以獲得如下轉(zhuǎn)換關(guān)系：

根據(jù)螺線管軸數(shù)，常見(jiàn)的磁信標(biāo)可以分為單軸磁信標(biāo)、雙軸磁信標(biāo)和三軸磁信標(biāo)；基于磁場(chǎng)矢量的單軸磁信標(biāo)不足以實(shí)現(xiàn)定位，而三軸磁信標(biāo)存在成本高、空間結(jié)構(gòu)復(fù)雜等缺點(diǎn)，因此本系統(tǒng)采用雙軸正交螺線管磁信標(biāo)作為磁源，其空間結(jié)構(gòu)如圖2所示。

圖2 雙軸磁信標(biāo)結(jié)構(gòu)形式Fig.2 The structure of dual-axis magnetic beacon

基于單軸螺線管的磁感應(yīng)矢量表達(dá)式(1)、相對(duì)方位角之間的轉(zhuǎn)換式(2)以及雙軸磁信標(biāo)結(jié)構(gòu)形式，雙軸磁信標(biāo)中兩個(gè)通電螺線管在空間中點(diǎn)產(chǎn)生的磁場(chǎng)可以分別表示為：

依據(jù)雙軸磁信標(biāo)在空間中任意一點(diǎn)的磁場(chǎng)矢量表達(dá)式和高維空間中橢圓的基本方程可以分析出，空間中任意一點(diǎn)的磁場(chǎng)矢量隨時(shí)間呈橢圓平面變化，該橢圓平面具有唯一方向不變的特征矢量，該特征矢量可以通過(guò)式(5)進(jìn)行提取。

圖3 傳感器坐標(biāo)轉(zhuǎn)換過(guò)程Fig.3 Process of magnetic sensor attitude translation

為避免傳感器載體坐標(biāo)系與信標(biāo)坐標(biāo)系不一致對(duì)定位精度的影響，根據(jù)幾何理論中矢量?jī)?nèi)積相關(guān)知識(shí)，可以得到如下不受影響的磁場(chǎng)矢量與位置相關(guān)表達(dá)式：

根據(jù)磁場(chǎng)傳播特性，式(9)-(11)可以簡(jiǎn)化為雙對(duì)數(shù)表達(dá)式：

基于式(12)，可以構(gòu)建如下定位模型：

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

基于磁場(chǎng)分布規(guī)律，本文在1.1節(jié)分析了一種不受傳感器姿態(tài)影響的雙軸磁信標(biāo)定位模型，能夠以較高的精度實(shí)現(xiàn)對(duì)靜態(tài)目標(biāo)位置的估計(jì)，然而磁信標(biāo)定位方法往往需要整周期的磁場(chǎng)數(shù)據(jù)解算目標(biāo)位置，輸出速度不能滿(mǎn)足動(dòng)態(tài)目標(biāo)需求，因此本文基于無(wú)跡卡爾曼濾波算法提出了一種融合慣性數(shù)據(jù)和磁感應(yīng)矢量的定位方法，慣性/磁感應(yīng)定位模型可以表示為：

改進(jìn)的慣性/磁感應(yīng)定位模型是一種不存在累計(jì)誤差的定位模型，慣性元件的一步預(yù)測(cè)誤差是由器件自身的零偏、隨機(jī)游走、標(biāo)定誤差等因素造成的，是一種隨時(shí)間逐漸增加的累計(jì)誤差。短時(shí)間內(nèi)，慣性元件的一步預(yù)測(cè)結(jié)果接近真實(shí)位置，能夠?yàn)榇鸥袘?yīng)定位方法求解目標(biāo)位置提供參考值，而低頻磁場(chǎng)定位模型能夠?qū)崿F(xiàn)對(duì)慣性元件的一步預(yù)測(cè)結(jié)果的修正，二者的融合使慣性/磁感應(yīng)定位方法的誤差在一定范圍上下波動(dòng)。

根據(jù)式(14)可以分析出，利用慣性/磁感應(yīng)技術(shù)對(duì)動(dòng)態(tài)目標(biāo)進(jìn)行定位的過(guò)程可以被建模成典型的非線性動(dòng)態(tài)模型。EKF、UKF及智能優(yōu)化（群智能）算法是解決非線性動(dòng)態(tài)系統(tǒng)問(wèn)題的主流方法，EKF是一種利用函數(shù)近似的思路進(jìn)行非線性濾波的方法，由于EKF對(duì)非線性強(qiáng)度高的系統(tǒng)濾波存在較大模型誤差；智能優(yōu)化算法是利用多個(gè)智能體的自身狀態(tài)與目標(biāo)狀態(tài)之間的差距不斷調(diào)整、優(yōu)化自身以實(shí)現(xiàn)對(duì)目標(biāo)狀態(tài)進(jìn)行最優(yōu)估計(jì)的一種搜索方法，存在尋優(yōu)速度較慢，每一次尋優(yōu)需要遍歷所有智能體后才能完成，計(jì)算資源需求高等問(wèn)題。UKF是一種對(duì)后驗(yàn)概率密度進(jìn)行近似的次優(yōu)濾波算法，降低了系統(tǒng)模型誤差，且能夠以較少的計(jì)算資源和較高的計(jì)算效率逼近非線性系統(tǒng)的結(jié)果。因此，采用UKF算法對(duì)慣性/磁感應(yīng)模型進(jìn)行濾波估計(jì)，在保證定位精度的同時(shí)，提升計(jì)算效率。

根據(jù)慣性/磁感應(yīng)定位模型(14)，系統(tǒng)狀態(tài)噪聲kV與量測(cè)噪聲kW無(wú)關(guān)，且為零均值高斯白噪聲，則一步預(yù)測(cè)誤差協(xié)方差為：

進(jìn)行Sigma采樣，并對(duì)磁感應(yīng)矢量進(jìn)行一步預(yù)測(cè)：

磁感應(yīng)矢量的一步預(yù)測(cè)均值、量測(cè)預(yù)測(cè)誤差協(xié)方差以及狀態(tài)預(yù)測(cè)誤差和量測(cè)預(yù)測(cè)誤差的互協(xié)方差為：

基于UKF的慣性/磁感應(yīng)定位方法流程圖如圖4所示。

圖4 慣性/磁感應(yīng)定位流程圖Fig.4 Inertial magneto-i nductivepositioning flow chart

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

不受傳感器姿態(tài)影響的磁感應(yīng)定位模型與基于UKF的慣性/磁感應(yīng)融合定位方法為本文的兩個(gè)主要研究目標(biāo)，本節(jié)將通過(guò)實(shí)驗(yàn)的方式對(duì)磁感應(yīng)定位模型有效性和精度進(jìn)行驗(yàn)證，由于實(shí)際測(cè)試過(guò)程中目標(biāo)的真實(shí)動(dòng)態(tài)軌跡很難給出，因此本節(jié)將采用數(shù)值仿真的方式對(duì)慣性/磁感應(yīng)定位方法進(jìn)行仿真驗(yàn)證，將文獻(xiàn)[9]中IMU解算的姿態(tài)輔助磁信標(biāo)定位方法與本文提出的改進(jìn)慣性/磁感應(yīng)定位方法對(duì)動(dòng)態(tài)目標(biāo)的運(yùn)動(dòng)軌跡估計(jì)結(jié)果與真實(shí)軌跡進(jìn)行對(duì)比，從而達(dá)到驗(yàn)證基于UKF的慣性/磁感應(yīng)定位方法精度的目的。

2.1 磁感應(yīng)定位模型實(shí)驗(yàn)驗(yàn)證

本節(jié)將利用如圖5所示的雙軸磁信標(biāo)實(shí)驗(yàn)系統(tǒng)測(cè)試磁感應(yīng)定位模型，整個(gè)系統(tǒng)由信號(hào)生成單元、線性功率放大器、雙軸磁信標(biāo)、磁傳感器探頭與數(shù)據(jù)處理模塊、定位解算計(jì)算機(jī)5部分構(gòu)成，激勵(lì)電流幅值均為4 A、頻率均為20Hz，相位相差90°，磁傳感器感知范圍為0.1 nT-10000 nT，磁信標(biāo)軸線長(zhǎng)度為35cm，螺線管直徑為15cm。系統(tǒng)導(dǎo)航坐標(biāo)系以磁信標(biāo)幾何中心為原點(diǎn)，磁信標(biāo)坐標(biāo)系與全局坐標(biāo)系重合，磁傳感器在全局坐標(biāo)系第一象限內(nèi)，坐標(biāo)系指向與圖5中坐標(biāo)軸一致。實(shí)驗(yàn)過(guò)程中磁信標(biāo)底面與傳感器之間的高度相差0.82m，傳感器位置每次向x軸或y軸移動(dòng)0.6m，在同一位置處傳感器采集3次磁場(chǎng)矢量，磁傳感器所在的坐標(biāo)系與磁信標(biāo)所在坐標(biāo)系的偏航角分別成0°、60°和120°角度，輸入定位解算計(jì)算機(jī)中進(jìn)行解算，以測(cè)試不受傳感器姿態(tài)影響的磁感應(yīng)定位模型有效性與定位效果。

圖5 實(shí)驗(yàn)系統(tǒng)Fig.5 Experimental system

根據(jù)式(13)的非線性磁感應(yīng)定位模型對(duì)磁傳感器位置進(jìn)行估計(jì)，利用Levenberg-Marquardt方法通過(guò)迭代的方式估計(jì)非線性方程組最優(yōu)解，實(shí)驗(yàn)結(jié)果如圖6所示，第1組磁場(chǎng)矢量解算結(jié)果為傳感器坐標(biāo)系與信標(biāo)坐標(biāo)系一致時(shí)（即偏航角夾角為0°）的位置估計(jì)，第2、3組為傳感器坐標(biāo)系與磁信標(biāo)坐標(biāo)系夾角分別為60°和120°時(shí)的估計(jì)結(jié)果。根據(jù)以上實(shí)驗(yàn)的結(jié)果可以看出，利用磁感應(yīng)定位模型能夠準(zhǔn)確地估計(jì)出傳感器與磁信標(biāo)之間的相對(duì)位置，且位置估計(jì)結(jié)果不受傳感器姿態(tài)的影響。

圖6 靜態(tài)目標(biāo)定位結(jié)果Fig.6 Positioning resultsof static tar get

2.2 慣性/磁感應(yīng)定位方法數(shù)值仿真驗(yàn)證

在前一節(jié)實(shí)驗(yàn)中驗(yàn)證了本文提出的磁感應(yīng)定位模型的可行性，本節(jié)將通過(guò)數(shù)值仿真的方式驗(yàn)證基于UKF的慣性/磁感應(yīng)定位方法。設(shè)IMU和磁傳感器是坐標(biāo)系一致的剛性連接，兩者之間質(zhì)心重合，磁場(chǎng)數(shù)據(jù)包括磁信標(biāo)磁場(chǎng)、地磁場(chǎng)和環(huán)境中干擾磁場(chǎng)，磁信標(biāo)磁場(chǎng)數(shù)據(jù)由磁偶極子模型和估計(jì)的雙軸螺線管磁矩參數(shù)產(chǎn)生，短時(shí)間內(nèi)磁場(chǎng)數(shù)據(jù)可以視為常值，根據(jù)實(shí)驗(yàn)室測(cè)量地磁場(chǎng)強(qiáng)度約為40000 nT，干擾磁場(chǎng)可視為白噪聲約為0.5 nT，磁傳感器信號(hào)采樣率為1000 Hz，慣性元件指標(biāo)為：（1）加速度計(jì)的零偏不穩(wěn)定性為9.8 ×10-2m/s2；（2）陀螺儀的零偏不穩(wěn)定性為8 °/h；（3）慣導(dǎo)數(shù)據(jù)輸出速率為1000 Hz。依據(jù)上述數(shù)據(jù)模型和相關(guān)參數(shù)，以給出的實(shí)際路徑為基礎(chǔ)生成磁場(chǎng)數(shù)據(jù)和慣性導(dǎo)航系統(tǒng)數(shù)據(jù)。該仿真將文獻(xiàn)[9]中的傳統(tǒng)慣性/磁感應(yīng)定位方法，即通過(guò)IMU解算的姿態(tài)信息對(duì)磁場(chǎng)矢量進(jìn)行轉(zhuǎn)換再進(jìn)行融合定位的方法，與本文提出的不受傳感器姿態(tài)影響的改進(jìn)慣性/磁感應(yīng)定位方法進(jìn)行對(duì)比分析，以達(dá)到驗(yàn)證磁感應(yīng)定位模型有效性和測(cè)試慣性/磁感應(yīng)定位方法性能的目的。

仿真結(jié)果表明，結(jié)果輸出速度方面：由于僅依靠磁感應(yīng)定位方法需要整周期的磁場(chǎng)數(shù)據(jù)進(jìn)行位置解算，位置結(jié)果的輸出頻率低于20 Hz；本文提出的慣性/磁感應(yīng)定位方法結(jié)果解算時(shí)間低于8 ms，即輸出頻率高于125 Hz，慣性/磁感應(yīng)融合定位方法提高了磁感應(yīng)定位方法的位置估計(jì)結(jié)果輸出速度。定位精度方面：數(shù)值仿真誤差如圖7所示，兩種定位方法誤差的累積概率分布如圖8所示。

圖7 動(dòng)態(tài)目標(biāo)定位誤差Fig.7 Positioning errors of moving target

動(dòng)態(tài)目標(biāo)定位誤差和誤差累積概率分布的結(jié)果表明，慣性/磁感應(yīng)融合定位方法能夠避免累計(jì)誤差，且本文提出的改進(jìn)慣性/磁感應(yīng)定位方法與文獻(xiàn)[9]采用的傳統(tǒng)慣性/磁感應(yīng)定位方法相比，傳統(tǒng)的慣性/磁感應(yīng)融合方法定位誤差隨著相對(duì)距離的增加而增大，由式(13)分析可得出：隨著距離的增長(zhǎng)，磁場(chǎng)信號(hào)信噪比降低，姿態(tài)解算誤差對(duì)磁信標(biāo)定位方法的影響增大。在磁信標(biāo)有效作用域內(nèi)，本文提出的方法定位性能優(yōu)于傳統(tǒng)慣性/磁感應(yīng)方法，不會(huì)受傳感器姿態(tài)解算誤差的影響，定位結(jié)果在一定范圍內(nèi)成上下波動(dòng)的變化。誤差累積概率分布如圖8所示。

圖8 動(dòng)態(tài)目標(biāo)定位誤差累積概率分布Fig.8 The cumulative probability distribution of moving target positioning errors

文獻(xiàn)[9]和本文提出方法的定位誤差低于0.5 m的

概率為95%，文獻(xiàn)[9]方法最大估計(jì)誤差為7.3 m，本文提出的基于UKF慣性/磁感應(yīng)融合方法的最大估計(jì)誤差為0.77 m。由此看出在磁場(chǎng)信號(hào)信噪比低于一定程度時(shí)，傳感器姿態(tài)誤差對(duì)磁信標(biāo)定位方法造成的影響增大，而本文提出的慣性/磁感應(yīng)定位方法一方面避免了傳感器姿態(tài)對(duì)系統(tǒng)定位的影響，另一方面實(shí)現(xiàn)了慣導(dǎo)數(shù)據(jù)與磁感應(yīng)數(shù)據(jù)的融合，提升了系統(tǒng)定位性能。

3 結(jié)論

針對(duì)基于磁感應(yīng)矢量的電磁信標(biāo)定位系統(tǒng)定位精度受限于傳感器姿態(tài)的問(wèn)題，本文提出了一種改進(jìn)的慣性/磁感應(yīng)定位方法，有效避免了傳感器姿態(tài)誤差對(duì)磁信標(biāo)定位系統(tǒng)造成的影響，并通過(guò)實(shí)驗(yàn)與仿真結(jié)合的方式驗(yàn)證了方法的有效性。主要內(nèi)容如下：

（1）基于磁信標(biāo)的磁場(chǎng)分布規(guī)律和特征矢量建立了不受傳感器姿態(tài)影響的磁感應(yīng)模型，給出了該模型的詳細(xì)分析過(guò)程，并通過(guò)實(shí)驗(yàn)驗(yàn)證了模型的有效性。

（2）提出了一種基于UKF的改進(jìn)慣性/磁感應(yīng)融合定位方法，實(shí)現(xiàn)了慣導(dǎo)數(shù)據(jù)與磁感應(yīng)數(shù)據(jù)的融合，提升了系統(tǒng)估計(jì)目標(biāo)位置的速率和定位性能，并通過(guò)仿真驗(yàn)證了方法的性能。

本文提出的改進(jìn)慣性/磁感應(yīng)定位方法實(shí)現(xiàn)了慣性導(dǎo)航與磁感應(yīng)定位的優(yōu)勢(shì)互補(bǔ)，為室內(nèi)等復(fù)雜場(chǎng)景的高精度定位系統(tǒng)的開(kāi)發(fā)提供了思路。