基于粒子濾波聯(lián)合算法的地磁室內(nèi)定位

黃 鶴 ，仇凱悅 ，李 維 ，羅德安

（1.北京建筑大學(xué)測(cè)繪與城市空間信息學(xué)院，北京 102616；2.北京建筑大學(xué)北京未來(lái)城市設(shè)計(jì)高精尖創(chuàng)新中心，北京 100044）

在諸多建筑物的室內(nèi)環(huán)境中，獲得高精度的位置信息是一項(xiàng)具有挑戰(zhàn)性的任務(wù).由于衛(wèi)星信號(hào)的衰減，傳統(tǒng)方法（如GPS）在建筑物內(nèi)部無(wú)法正常工作，這促進(jìn)了室內(nèi)定位系統(tǒng)的快速發(fā)展，例如基于WiFi[1]，ZigBee[2]和RFID[3]的室內(nèi)定位系統(tǒng).然而，當(dāng)使用無(wú)線電技術(shù)來(lái)解決室內(nèi)定位問(wèn)題時(shí)，物理障礙物產(chǎn)生的影響無(wú)法避免，會(huì)對(duì)定位系統(tǒng)接收信號(hào)產(chǎn)生干擾.此外，上述提出的室內(nèi)定位解決方案的另一個(gè)需要解決的問(wèn)題是都需要部署特定設(shè)備或在定位過(guò)程中使用額外的硬件，并且基礎(chǔ)設(shè)備需要定期維護(hù)，這些都需要高昂費(fèi)用.

因此，無(wú)需額外設(shè)備投入的地磁定位技術(shù)備受關(guān)注.地磁場(chǎng)通常用于確定設(shè)備的航向[4].然而，室內(nèi)環(huán)境中的重大磁擾動(dòng)往往會(huì)對(duì)定向估計(jì)的準(zhǔn)確性造成不利影響.通常需要校準(zhǔn)，但這不是一件容易的事情.另一方面，建筑物中的混凝土與鋼結(jié)構(gòu)以及磁性鐵質(zhì)設(shè)備對(duì)室內(nèi)地磁場(chǎng)造成的形變，可以被視為信息源.因此可以利用室內(nèi)磁場(chǎng)的局部異常來(lái)獲取室內(nèi)各個(gè)位置的獨(dú)特標(biāo)記，采用類似“指紋”的方式構(gòu)建參考圖，而且室內(nèi)地磁場(chǎng)在幾周甚至幾個(gè)月的時(shí)間尺度上具有穩(wěn)定性[5-6].

早在2000年，Suksakulchai 等[7]認(rèn)識(shí)到磁場(chǎng)干擾可以用于室內(nèi)定位.在進(jìn)行室內(nèi)地磁定位時(shí)，需要已知的地磁參考圖.雖然同時(shí)定位和建圖（SLAM）技術(shù)克服了這個(gè)問(wèn)題[8]，但大多數(shù)相關(guān)工作主要集中在解決匹配定位上，匹配算法通常是采用粒子濾波算法（particle filter，PF）.文獻(xiàn)[8]結(jié)合智能手機(jī)中的慣性傳感器，以航跡推算算法作為粒子濾波器的運(yùn)動(dòng)模型，利用測(cè)量磁場(chǎng)的總強(qiáng)度更新粒子權(quán)重，兩條走廊之間的角落則被用作位置矯正的地標(biāo)，同時(shí)對(duì)匹配區(qū)域的范圍進(jìn)行約束，在實(shí)驗(yàn)中實(shí)現(xiàn)了小于1 m 的定位精度.文獻(xiàn)[9]中，利用現(xiàn)有的地磁參考圖和地磁傳感器的測(cè)量信息用粒子濾波器進(jìn)行定位.實(shí)驗(yàn)表明，相比于只用磁場(chǎng)總強(qiáng)度，若同時(shí)考慮磁場(chǎng)的3 個(gè)分量會(huì)使得定位精度更高，在小區(qū)域的室內(nèi)定位中可以達(dá)到厘米級(jí).文獻(xiàn)[10]提出了一種在粒子濾波器的實(shí)現(xiàn)中使用慣性傳感器并應(yīng)用磁場(chǎng)圖和室內(nèi)地圖的方法，以提高定位和跟蹤的準(zhǔn)確性；使用梯度下降法來(lái)糾正由于磁擾動(dòng)引起的用戶航向估計(jì)問(wèn)題；使用磁場(chǎng)三分量加室內(nèi)地圖的實(shí)驗(yàn)實(shí)現(xiàn)了0.75 m的平均定位精度，標(biāo)準(zhǔn)偏差為0.52 m.

現(xiàn)有基于粒子濾波算法的室內(nèi)地磁定位中大多是以點(diǎn)為匹配單元，基于點(diǎn)的匹配模式一共有3 種組合（磁場(chǎng)強(qiáng)度；水平分量和垂直分量；x、y、z軸分量），一個(gè)點(diǎn)的地磁匹配特征最多只有3 個(gè).如果只依靠磁場(chǎng)測(cè)量值，在地磁匹配中會(huì)導(dǎo)致定位結(jié)果模棱兩可[11].增加匹配特征的數(shù)量是一個(gè)解決方法，因此本文提出基于路徑的匹配模式，由多點(diǎn)構(gòu)成的路徑具有足夠的匹配特征數(shù)量，文獻(xiàn)[12]已經(jīng)提出在一維路線中采用DTW （dynamic time warp）進(jìn)行路徑匹配，精度接近1 m.本文提出的匹配算法結(jié)合了DTW 與PF，構(gòu)造基于路徑的二維匹配模式，與一般的基于點(diǎn)匹配的模式相比，在匹配特征數(shù)量方面，具有更大的優(yōu)勢(shì).同時(shí)通過(guò)計(jì)算地磁序列之間的相似度作為輔助定位手段.最后通過(guò)實(shí)驗(yàn)對(duì)本文提出的匹配方法進(jìn)行評(píng)估.

1 地磁室內(nèi)定位

地磁室內(nèi)定位系統(tǒng)可分為離線構(gòu)圖與在線匹配兩個(gè)階段.離線構(gòu)圖是利用磁傳感器遍歷匹配區(qū)域，利用收集的地磁數(shù)據(jù)采用類似“指紋”的方法建立參考圖；在線匹配是在目標(biāo)運(yùn)動(dòng)階段實(shí)時(shí)收集所在位置的地磁數(shù)據(jù)，并利用匹配算法將其與參考圖進(jìn)行最優(yōu)匹配，得出目標(biāo)的最佳估計(jì)位置，整個(gè)過(guò)程如圖1.

1.1 粒子濾波（PF）算法

PF 算法是基于蒙特卡羅思想的一種最優(yōu)貝葉斯估計(jì)，在分析非線性、非高斯的動(dòng)態(tài)時(shí)變系統(tǒng)問(wèn)題上具有突出優(yōu)勢(shì).一般而言，后驗(yàn)概率分布p(xt|zt)十分復(fù)雜不容易獲得，當(dāng)系統(tǒng)的先驗(yàn)概率分布和似然函數(shù)已知時(shí)，粒子濾波器使用蒙特卡洛定位方法對(duì)其近似，原理就是利用粒子的狀態(tài)作為先驗(yàn)概率，目標(biāo)觀測(cè)值作為參數(shù)與粒子所在位置對(duì)應(yīng)的參考值做匹配，最后推導(dǎo)目標(biāo)的最大似然估計(jì).整個(gè)粒子濾波算法包括三個(gè)步驟：預(yù)測(cè)，更新和重采樣.預(yù)測(cè)與更新方法如式（1）.

式中：xt為目標(biāo)狀態(tài)；為目標(biāo)預(yù)測(cè)狀態(tài)；xi,t為第i個(gè)粒子的粒子狀態(tài)；zt為觀測(cè)值；t為時(shí)間；c為歸一化常數(shù).

為了解決迭代過(guò)程中粒子衰減問(wèn)題，粒子濾波器加入重采樣（sampling importance resampling）算法.重采樣的基本思想是去除權(quán)重小的粒子，并對(duì)權(quán)重大的粒子進(jìn)行復(fù)制.在二維定位中，以行人運(yùn)動(dòng)軌跡為粒子濾波算法的運(yùn)動(dòng)模型，如式（2）.

式中：l～U(0,L)；L為目標(biāo)移動(dòng)步幅長(zhǎng)度；

式中：設(shè)定行人當(dāng)前所在位置的方向?yàn)槌跏贾?，θ為行進(jìn)過(guò)程中與初始方向之間的偏移量.

似然函數(shù)是基于單變量高斯概率密度函數(shù)，如式（4）.

式中：σr為 觀測(cè)值z(mì)t的協(xié)方差；函數(shù)f(·)返回粒子xi,t在參考圖中對(duì)應(yīng)的參考值.

式中：N為采樣粒子個(gè)數(shù)；wi,t為時(shí)刻t第i個(gè)粒子的權(quán)重.

并且，定位誤差定義為

1.2 動(dòng)態(tài)時(shí)間規(guī)整算法（DTW）

在進(jìn)行地磁路徑匹配（地磁序列匹配）時(shí)，收集地磁數(shù)據(jù)的行走速度不同，可能會(huì)使得由于速度不同產(chǎn)生的兩個(gè)序列的單個(gè)樣本不準(zhǔn)確.用DTW 進(jìn)行比較的好處是能夠允許兩個(gè)序列在時(shí)間尺度上變化.DTW 算法通過(guò)實(shí)現(xiàn)兩個(gè)序列之間的度量距離最小值，將一個(gè)序列的樣本映射到另一個(gè)序列.算法定義的距離度量具有時(shí)間尺度上的不變性.地磁路徑匹配是對(duì)室內(nèi)兩條不同路徑上每個(gè)點(diǎn)的地磁強(qiáng)度進(jìn)行對(duì)準(zhǔn)，確定兩條路徑是否存在偏差，以及偏差的大小，這是室內(nèi)地磁DTW 算法的概念.假設(shè)目標(biāo)走過(guò)的路徑為Q，長(zhǎng)度為K=q，某個(gè)粒子走過(guò)的路徑為P，長(zhǎng)度為K=p.DTW 算法的度量距離定義為

式中：D(q,p)為長(zhǎng)度分別為q、p的路徑Q、P之間的度量距離；d(q,p)為路徑中對(duì)應(yīng)點(diǎn)的歐幾里得距離，d(·)=‖·‖.

同時(shí)地磁序列之間的相似度可以作為輔助匹配手段，通過(guò)計(jì)算Spearman 相關(guān)系數(shù)確定相似度，該系數(shù)定義為

式中：xm、ym分別為兩條序列中第m個(gè)數(shù)據(jù)；M為序列中數(shù)據(jù)總數(shù)；、分別為兩條序列數(shù)據(jù)的平均值.

系數(shù)越高地磁序列相似度越高，并且該系數(shù)要求兩條序列的長(zhǎng)度要一致.

聯(lián)合算法整體上還是遵循粒子濾波器的框架，整個(gè)聯(lián)合算法的流程如圖2.它與一般的粒子濾波器相比，優(yōu)勢(shì)在于：

圖2 聯(lián)合算法流程Fig.2 Process flow of the joint algorithm

（1）在目標(biāo)運(yùn)動(dòng)一定時(shí)間段后進(jìn)行匹配，進(jìn)而增加了匹配元素的數(shù)量，提高了匹配精度.

（2）利用DTW 算法，通過(guò)計(jì)算粒子與目標(biāo)的距離度量值代替粒子權(quán)重，無(wú)需重采樣.

（3）減少了對(duì)粒子群數(shù)量的需求.

（4）利用地磁序列相似度來(lái)輔助定位.

2 實(shí)驗(yàn)數(shù)據(jù)采集及參考圖構(gòu)建

實(shí)驗(yàn)地點(diǎn)是北京建筑大學(xué)測(cè)繪學(xué)院二樓走廊，長(zhǎng)68.0 m，寬1.8 m，平面圖如圖3（a）.本文采用“指紋”模式的方法，采集二樓走廊的地磁數(shù)據(jù)用于構(gòu)建參考圖（圖3（b））.

圖3 走廊平面圖以及地磁參考圖Fig.3 Corridor plan and geomagnetic reference map

為了能快速收集室內(nèi)地磁數(shù)據(jù)，團(tuán)隊(duì)（城市測(cè)繪研究所）開發(fā)了一款測(cè)量機(jī)器人，上面搭載的地磁傳感器HMC5983 裝載位置距離地面1.3 m，能夠避免機(jī)器人自身的影響，如圖4[13].HMC5983 體積小，具有自動(dòng)偏置補(bǔ)償以及溫度補(bǔ)償功能，攜帶I2C 或者SPI 的數(shù)字接口，最大輸出功率為220 Hz，航向角的精確度為1°～2°.

實(shí)驗(yàn)中，控制裝有地磁傳感器的測(cè)量機(jī)器人沿著走廊收集了4 行間隔0.6 cm 的地磁數(shù)據(jù).機(jī)器人步長(zhǎng)為0.2 m，每25 Hz 測(cè)量一次磁感應(yīng)強(qiáng)度，產(chǎn)生三維矢量B=(Bx,By,Bz)，單位為μT，每個(gè)點(diǎn)采集時(shí)間10 s.為了進(jìn)行定位，本文使用磁感應(yīng)強(qiáng)度‖B‖作為匹配特征，計(jì)算方法如式（9）所示.因?yàn)榇鸥袘?yīng)強(qiáng)度是一個(gè)旋轉(zhuǎn)不變量，避免了設(shè)備在運(yùn)動(dòng)過(guò)程中輕微抖動(dòng)對(duì)實(shí)驗(yàn)的影響.

采集的參考地磁數(shù)據(jù)，利用克里金插值法進(jìn)行處理，并且賦予網(wǎng)格坐標(biāo)，構(gòu)造的參考圖如圖3（b）.從圖3中可知，受建筑物鋼筋混凝土結(jié)構(gòu)的影響，走廊地磁擾動(dòng)隨空間位置的改變而發(fā)生明顯變化，因此能夠用于室內(nèi)定位.而圖中紅色圓圈則表示存在地磁強(qiáng)度相似現(xiàn)象，這導(dǎo)致了室內(nèi)地磁特異性的減弱，同時(shí)給定位結(jié)果帶來(lái)誤差.

圖4 測(cè)量機(jī)器人結(jié)構(gòu)Fig.4 Structure of a measurement robot

3 實(shí)驗(yàn)結(jié)果與分析

本文首先通過(guò)實(shí)驗(yàn)分析在采樣粒子分布相同的情況下N與K對(duì)定位精度的影響.使用測(cè)量機(jī)器人隨機(jī)從某點(diǎn)（如點(diǎn)（1.2，15.4）m）開始沿著Y軸直線運(yùn)動(dòng)15 次，L設(shè)為0.6 m.在線采集地磁數(shù)據(jù)，獲取15 個(gè)位置點(diǎn)，路徑長(zhǎng)度分為 1、2、···、14，如圖5，圖中所示相鄰兩個(gè)點(diǎn)位之間的路徑長(zhǎng)度為1.另外基于路徑匹配只能從點(diǎn)2 開始，而且在每個(gè)位置進(jìn)行匹配，所能使用的特征數(shù)量與點(diǎn)號(hào)的值相等.

圖5 點(diǎn)位與路徑長(zhǎng)度關(guān)系Fig.5 Relationship between points and path length

地磁數(shù)據(jù)采集完后，利用聯(lián)合算法將它與地磁參考圖進(jìn)行匹配，其中粒子濾波算法中粒子分布的方式采用隨機(jī)分布，匹配次數(shù)設(shè)定為300 次，聯(lián)合算法通過(guò)MATLAB 計(jì)算機(jī)語(yǔ)言編寫實(shí)現(xiàn)，平均定位誤差與誤差占比（誤差e小于1 m）分別如圖6、7 所示.

圖6 不同粒子數(shù)隨著路徑增加的定位結(jié)果Fig.6 Positioning results of different particle numbers increasing with path

圖7 誤差占比（e ＜ 1 m）Fig.7 Proportion of error（e ＜ 1 m）

從圖6可知，當(dāng)粒子稀疏時(shí)會(huì)出現(xiàn)定位不穩(wěn)定的情況，如N= 100 和N= 200，這與粒子的分布有關(guān)系.當(dāng)N＜ 500 時(shí)，K= 5 m 和K= 7 m 出現(xiàn)波動(dòng)情況，這與路徑的選擇有關(guān)，但不會(huì)影響實(shí)驗(yàn)的整體定位結(jié)果，后續(xù)實(shí)驗(yàn)會(huì)進(jìn)一步說(shuō)明.

充足的粒子數(shù)可以保證粒子分布密集，因此粒子隨機(jī)分布在目標(biāo)周圍的概率增大，使誤差小于1 m的定位結(jié)果出現(xiàn)的概率也增大，如圖7.而且當(dāng)粒子數(shù)一定，誤差小于1 m 的百分比隨著K的增加而提高；當(dāng)N＞ 300 后，誤差區(qū)百分比的變化趨勢(shì)（隨著K值推移）基本保持一致.

總而言之，當(dāng)N＞ 100，整體的定位精度隨著K增大而增加，尤其是當(dāng)N＞ 300、K＞ 7 m 時(shí)，定位精度優(yōu)于1 m.同時(shí)表1顯示了不同粒子數(shù)對(duì)聯(lián)合算法的影響，表示粒子數(shù)與匹配時(shí)間近似成正比.并且確定后期實(shí)驗(yàn)粒子數(shù)N= 400.

表1 不同粒子數(shù)匹配時(shí)間Tab.1 Time required to match different particle counts

傳統(tǒng)粒子濾波定位結(jié)果如圖8.從圖8中能看出，采用粒子濾波對(duì)同一條路徑進(jìn)行單點(diǎn)定位的結(jié)果大致在點(diǎn)5 處開始收斂，但是收斂結(jié)果時(shí)好時(shí)壞，隨機(jī)匹配20 次中，最大收斂誤差能接近20 m，同樣最小收斂誤差也能小于1 m.在重復(fù)匹配300 次后，得出在第15 號(hào)點(diǎn)的各定位誤差范圍所占百分比情況，同時(shí)與本文提出的方法進(jìn)行比較，如圖9（a）.圖中能充分說(shuō)明在基于單點(diǎn)匹配的模式下，會(huì)導(dǎo)致粒子濾波器的穩(wěn)定性下降，很顯然這樣的穩(wěn)定性不足以用于室內(nèi)定位，相比之下本文提出的方法具有良好的穩(wěn)定性，1 m 以內(nèi)的定位精度接近100%，但是匹配時(shí)間方面，如圖9（b）所示，單點(diǎn)匹配所耗時(shí)間基本不變，且低于路徑匹配.

圖8 同一路徑基于粒子濾波器單點(diǎn)定位結(jié)果（N = 400）Fig.8 Positioning results of single point for the same path based on particle filter （N = 400）

圖9 粒子濾波器基于單點(diǎn)匹配和聯(lián)合算法基于路徑匹配的誤差占比以及匹配時(shí)間Fig.9 Error ratio and matching time of the particle filter based on single point matching and joint algorithm based on path matching

本文提出的方法中還加入了序列相似度作為輔助手段，實(shí)驗(yàn)在計(jì)算DTW 算法后不能直接確定最優(yōu)位置估計(jì)，只能通過(guò)平均計(jì)算位置估計(jì)較好的前幾組來(lái)得出最終位置.因?yàn)镈TW 算法定義的度量距離是指計(jì)算兩條序列的地磁強(qiáng)度差異，使得D(q,p) 值小的粒子不一定靠近目標(biāo).而加入Spearman相關(guān)系數(shù)做為相似度輔助是為了讓 ρ 與D(q,p) 進(jìn)行互補(bǔ).ρ 為序列間的梯度變化，當(dāng)它取最大以及D(q,p) 取最小時(shí)就可以得出最優(yōu)估計(jì)位置.為了驗(yàn)證輔助手段的有效性，取N= 400 分別進(jìn)行匹配，結(jié)果如圖10.從圖10中可知，從K= 1 m 到K= 14 m，采用地磁序列相似度作為輔助手段都能夠提高路徑匹配過(guò)程的定位精度，路徑長(zhǎng)度小于4 m 效果最好，路徑長(zhǎng)度在4～8 m 之間其次.為了驗(yàn)證方法的魯棒性，又隨機(jī)從不同任意點(diǎn)開始收集4 條路徑的地磁序列（一共5 條），移動(dòng)步長(zhǎng)不變.粒子數(shù)N設(shè)定為400，其中包括2 條直角轉(zhuǎn)彎路徑，結(jié)果如圖11.圖11中1、2、3 表示直線路徑，4、5 表示包含直角轉(zhuǎn)彎的路徑，很明顯當(dāng)K＞ 9 m后，最優(yōu)定位精度在1 m 以內(nèi)，而且不存在定位誤差突然劇增現(xiàn)象，說(shuō)明本文所提出的算法性能可靠，具有良好的魯棒性.另外本文實(shí)驗(yàn)是驗(yàn)證基于路徑模式的室內(nèi)地磁定位的可能性，同時(shí)得出該系統(tǒng)的定位精度，所以測(cè)量機(jī)器人的移動(dòng)步長(zhǎng)和航向角都屬于人工控制.

圖10 定位結(jié)果對(duì)比（N = 400）Fig.10 Comparison of positioning results （N = 400）

圖11 不同路徑定位誤差比較Fig.11 Comparison of positioning errors for different paths

4 結(jié) 論

在基于位置服務(wù)的發(fā)展中，室內(nèi)定位是前提.各種定位方法中，基于地磁定位的方法因不需其他基礎(chǔ)設(shè)施投入而受到廣泛關(guān)注.然而在大型建筑中，地磁場(chǎng)的特異性會(huì)減弱，一般的基于粒子濾波器的定位方法會(huì)導(dǎo)致定位結(jié)果出現(xiàn)模糊現(xiàn)象.在本文中，提出了一種新的基于地磁的室內(nèi)定位技術(shù).該技術(shù)中有3 個(gè)創(chuàng)新點(diǎn)：（1）提出一種基于粒子濾波器與DTW的聯(lián)合算法；（2）一種新的基于路徑匹配的地磁定位模式；（3）使用地磁序列相似度進(jìn)行輔助，后期實(shí)驗(yàn)也證明了輔助手段的有效性.最后實(shí)驗(yàn)證明，與傳統(tǒng)的基于粒子濾波算法的室內(nèi)地磁定位方法相比，該算法有效地解決了定位模糊問(wèn)題，當(dāng)路徑長(zhǎng)度和粒子數(shù)足夠時(shí)（K≥ 9 m，N≥ 400），總體定位精度優(yōu)于1 m，而且定位精度小于1 m 出現(xiàn)的概率接近100%，體現(xiàn)了該算法具有良好的穩(wěn)定性與可靠性.