基于智能手機(jī)的地磁/WiFi/PDR的室內(nèi)定位算法

阮 琨，王 玫，羅麗燕，熊璐琦，宋浠瑜

(1.認(rèn)知無(wú)線電與信息處理省部共建教育部重點(diǎn)實(shí)驗(yàn)室(桂林電子科技大學(xué))，廣西 桂林 541004；2.桂林電子科技大學(xué) 廣西信息科學(xué)實(shí)驗(yàn)中心,廣西 桂林 541004)

0 引言

導(dǎo)航定位技術(shù)在物聯(lián)網(wǎng)中處于基礎(chǔ)地位[1]。在現(xiàn)今廣泛發(fā)展的普適計(jì)算技術(shù)中，用戶位置是使用頻率最高、應(yīng)用最廣泛的基本感知信息，滲透于日常生活、醫(yī)護(hù)、火險(xiǎn)救助、交通管理等各領(lǐng)域[2]。精確而快速的室內(nèi)定位技術(shù)能明顯改善生活質(zhì)量和工作效率，如在超市中快速尋找到自己所需的商品，在不熟悉的環(huán)境下到達(dá)目的地，在火災(zāi)現(xiàn)場(chǎng)迅速找到待解救的人員等。

高精度、低成本的室內(nèi)定位系統(tǒng)一直是研究的熱點(diǎn)課題，已經(jīng)提出的有基于超寬帶(Ultra Wideband, UWB)[3]、紅外線、WiFi[4]、ZigBee[5]等各類室內(nèi)定位系統(tǒng)。其中，UWB、藍(lán)牙、紅外線定位精度高，但需要額外架設(shè)基礎(chǔ)設(shè)備，且遇障礙物衰耗嚴(yán)重。WiFi信號(hào)廣泛分布于各種室內(nèi)環(huán)境，免于部署基礎(chǔ)設(shè)施，但多徑效應(yīng)明顯，定位精度有限[6]，并且持續(xù)的WiFi掃描是非常耗能的[7]。

地磁場(chǎng)在室內(nèi)受到建筑材料的影響會(huì)發(fā)生不同程度扭曲，這些異常形成了十分豐富的地磁信息，而且若建筑結(jié)構(gòu)不發(fā)生大改變，室內(nèi)地磁場(chǎng)就會(huì)保持相對(duì)穩(wěn)定[8]，這使得利用地磁信息進(jìn)行室內(nèi)定位成為可能。地磁指紋定位與WiFi指紋定位原理相同，但具有如下幾個(gè)優(yōu)勢(shì)：1)穩(wěn)定，不隨時(shí)間改變，不受移動(dòng)物體干擾；2)無(wú)處不在，無(wú)需架設(shè)基礎(chǔ)設(shè)施就可獲得；3)耗能低。文獻(xiàn)[9]提出了利用室內(nèi)地磁場(chǎng)干擾造成的多樣化來(lái)進(jìn)行室內(nèi)定位和軌跡追蹤的Magicol系統(tǒng)，在每一步的基礎(chǔ)上向量化連續(xù)的磁場(chǎng)信號(hào)處理磁場(chǎng)強(qiáng)度的不唯一性。若室內(nèi)環(huán)境存在WiFi，可以結(jié)合WiFi信號(hào)提高室內(nèi)定位精度。

以上諸多工作都是致力于粒子濾波算法中粒子對(duì)狀態(tài)的精確估計(jì)及粒子多樣性的保持，并且初始位置一般由WiFi直接確定[8]，但是WiFi信號(hào)的不穩(wěn)定導(dǎo)致解算的結(jié)果可能有較大誤差?？紤]智能手機(jī)上已經(jīng)配置各種傳感器，利用加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行位置解算，采用改進(jìn)的粒子濾波算法融合行人航跡推算(Pedestrian Dead Reckoning, PDR)和室內(nèi)地磁場(chǎng)，可以克服地磁大范圍內(nèi)不唯一性以及慣性傳感器(Inertial Measurement Unit, IMU)的累計(jì)誤差，同時(shí)針對(duì)WiFi信號(hào)跳躍，引入了隨機(jī)采樣一致性(RANdom SAmple Consensus, RANSAC)算法，用于初始位置確定，提高粒子濾波的收斂速度，可達(dá)到高精度、低成本且低能耗的室內(nèi)定位目的。

1 定位系統(tǒng)

系統(tǒng)框圖如圖1所示，考慮資源分配問(wèn)題，將整個(gè)定位過(guò)程分為兩個(gè)部分：一個(gè)是數(shù)據(jù)采集和預(yù)處理部分，在移動(dòng)端完成，包括行人移動(dòng)檢測(cè)和慣性導(dǎo)航；另一個(gè)是地圖存儲(chǔ)和定位，在云端完成，包括存儲(chǔ)建筑平面圖、地磁指紋地圖及通過(guò)粒子濾波完成用戶的定位。在定位之前，首先需要采集定位區(qū)域的地磁強(qiáng)度，形成地磁強(qiáng)度分布圖，即指紋地圖，并存儲(chǔ)在服務(wù)器中。當(dāng)用戶發(fā)送定位請(qǐng)求時(shí)，智能手機(jī)上的傳感器會(huì)不斷采集用戶行走過(guò)程中加速度、移動(dòng)方向、步幅和磁場(chǎng)強(qiáng)度并進(jìn)行預(yù)處理，再將這些數(shù)據(jù)上傳至云端。在服務(wù)器中執(zhí)行RANSAC算法確定初始位置及關(guān)鍵地磁數(shù)據(jù)與指紋地圖數(shù)據(jù)的匹配，經(jīng)由粒子濾波算法進(jìn)行地理位置估計(jì)，最后將定位結(jié)果返回手機(jī)客戶端，完成定位。若檢測(cè)到WiFi信號(hào)，可以用WiFi信號(hào)與RANSAC算法聯(lián)合進(jìn)行初始位置的確定，以及行走過(guò)程中WiFi信號(hào)作為路標(biāo)校準(zhǔn)路徑。

圖1 定位系統(tǒng)結(jié)構(gòu)

2 多傳感器融合及粒子濾波算法模型

多傳感器信息融合是模擬大腦對(duì)客觀世界的綜合認(rèn)知。通過(guò)多源信息之間的相互輔助、補(bǔ)充，可彌補(bǔ)彼此間的劣勢(shì)，有效提高系統(tǒng)性能。因此，本文融合智能手機(jī)自配置的加速度計(jì)、陀螺儀、磁力計(jì)等傳感器采集到的信息，不需額外布置基礎(chǔ)設(shè)施就能完成行人導(dǎo)航，具體的流程如圖2所示。

圖2 系統(tǒng)定位算法流程

2.1 計(jì)步算法

行走過(guò)程的加速度是隨著每一步的起落而周期變化的，呈正弦波形變化趨勢(shì)。檢測(cè)步數(shù)，可以通過(guò)檢測(cè)波峰的個(gè)數(shù)來(lái)獲得，分下列幾步來(lái)完成：1)初始值的設(shè)定，如波峰閾值、波峰波谷之間差值的初始值；2)濾除因手機(jī)隨機(jī)抖動(dòng)引入的無(wú)效波峰；3)動(dòng)態(tài)閾值的計(jì)算?？梢詫⒎逯?，波峰波谷之間的差值超過(guò)初始閾值的先算作一步，同時(shí)，若波峰波谷之間的差值大于預(yù)定值，將差值保存在序列中，當(dāng)序列滿后，將均值作為新的峰值閾值。當(dāng)有新的值進(jìn)來(lái)時(shí)，頂替最前面的值，這樣閾值將會(huì)隨著運(yùn)動(dòng)狀態(tài)實(shí)時(shí)更新。確定有效步后，零點(diǎn)對(duì)應(yīng)的地磁強(qiáng)度將作為后面粒子濾波算法的觀測(cè)變量。

2.2 動(dòng)態(tài)步長(zhǎng)計(jì)算

在實(shí)驗(yàn)中發(fā)現(xiàn)，人行走的步長(zhǎng)是和行走的速度成正比的，走得越快步長(zhǎng)就越長(zhǎng)，幾乎是線性的關(guān)系，因此用下列模型來(lái)進(jìn)行描述：

L=a×f+b

(1)

其中:L是步長(zhǎng)，f是行走頻率，在這里a和b分別是根據(jù)不同用戶得到的個(gè)性化參數(shù)。通過(guò)參數(shù)a和b的動(dòng)態(tài)調(diào)整，計(jì)算出每一步的步長(zhǎng)。

2.3 位移估計(jì)

用戶位置可以根據(jù)式(2)更新:

我目前從事的工作為勞動(dòng)管理。而我本科學(xué)習(xí)的專業(yè)為經(jīng)濟(jì)學(xué)和工業(yè)工程。也許在常人眼中，我的專業(yè)和崗位是相當(dāng)對(duì)口的，工作起來(lái)會(huì)非常地得心應(yīng)手，但事實(shí)并非如此。

(2)

2.4 粒子濾波算法

粒子濾波模型廣泛應(yīng)用于非線性、非高斯環(huán)境下的狀態(tài)估計(jì)[14]，本文中分為四個(gè)步驟，分別是粒子初始化采樣、賦權(quán)值、粒子狀態(tài)更新和重采樣[15]。粒子濾波的每一次迭代建立在用戶行走的每一步基礎(chǔ)上。粒子的狀態(tài)為當(dāng)前粒子在地理位置上的坐標(biāo)p={xk,yk}，k表示步數(shù)，觀測(cè)量則是每一步對(duì)應(yīng)的地磁指紋強(qiáng)度Magk。

1)粒子初始化采樣：當(dāng)用戶打開手機(jī)軟件發(fā)送定位請(qǐng)求時(shí)，慣性傳感器中的加速度計(jì)不斷收集數(shù)據(jù)，分析加速度數(shù)據(jù)判斷用戶是否發(fā)生運(yùn)動(dòng)。若檢測(cè)到用戶開始運(yùn)動(dòng)，在初始位置附近隨機(jī)采樣N個(gè)粒子，分別對(duì)應(yīng)一個(gè)指紋地圖上的地磁指紋。

2)粒子賦權(quán)值：粒子的權(quán)重由式(3)計(jì)算得到：

(3)

其中：di是每個(gè)粒子對(duì)應(yīng)的指紋地圖上的地磁強(qiáng)度與實(shí)際行走狀態(tài)下采集到的特征地磁強(qiáng)度差值，σ是室內(nèi)地磁場(chǎng)對(duì)位置估計(jì)的影響因子。如果粒子的位置超出了地圖邊界，則那個(gè)位置不可能是下一步的位置，可直接將其權(quán)重設(shè)為0。

3)粒子狀態(tài)更新：所有粒子權(quán)重確定后，將所有粒子加權(quán)和的中心點(diǎn)作為新的位置估計(jì)，同時(shí)根據(jù)上文的步長(zhǎng)計(jì)算得到的每一步步長(zhǎng)Lk和轉(zhuǎn)角θk，利用式(4)進(jìn)行粒子狀態(tài)更新。由于陀螺儀的累積誤差過(guò)大，所以本實(shí)驗(yàn)只利用陀螺儀數(shù)據(jù)檢測(cè)是否發(fā)生了轉(zhuǎn)向，并不參與到每步的計(jì)算中，若發(fā)生轉(zhuǎn)向，具體方向由粒子濾波算法確定。粒子狀態(tài)更新如下所示：

p′.pos=p.pos+Lk+δ

(4)

4)粒子重采樣：經(jīng)過(guò)若干次迭代，粒子逐漸向高權(quán)值靠攏，只剩下幾個(gè)權(quán)值很高的粒子，從而失去粒子多樣性，無(wú)法有效表達(dá)狀態(tài)的后驗(yàn)概率密度分布。

室內(nèi)環(huán)境下物體擺放密集，行人運(yùn)動(dòng)的范圍十分受限，而且大部分情況下遵循路徑的主要方向而不是無(wú)規(guī)律隨意行走，如超市、圖書館、辦公室。基于此，可以利用地圖約束進(jìn)行自適應(yīng)粒子重采樣，采樣模型如圖3所示。該算法的主要思想是根據(jù)當(dāng)前環(huán)境下路徑的主要方向以及陀螺儀計(jì)算的用戶轉(zhuǎn)角有目的性地重采樣粒子。在一維路徑下，陀螺儀檢測(cè)用戶是否發(fā)生轉(zhuǎn)向，在前進(jìn)方向以及路徑主要方向采樣粒子，其他方向保留少量粒子如B、D所示。在交叉路口如C所示，在所有可能的方向上采樣，這種情況下可以根據(jù)實(shí)際采集到的地磁強(qiáng)度與指紋數(shù)據(jù)庫(kù)中地磁強(qiáng)度的差值先進(jìn)行K近鄰(KNearest Neighbors,KNN)算法的聚類，濾除外部粒子，增強(qiáng)粒子在轉(zhuǎn)角處的跟蹤性能。對(duì)于突然轉(zhuǎn)向可能帶來(lái)的轉(zhuǎn)向計(jì)算偏差較大的情況，如A所示，由于墻體阻隔，粒子會(huì)很快湮滅，只留下正確行進(jìn)方向上的粒子。

圖3 地圖約束的自適應(yīng)粒子重采樣模型

3 RANSAC算法確定初始位置及WiFi融合

RANSAC算法要求內(nèi)群數(shù)據(jù)可以模型化，并以此為依據(jù)濾除離群數(shù)據(jù)，PDR的行走軌跡正好可以作為模型。為了簡(jiǎn)化計(jì)算，假設(shè)用戶剛開始走的是一段直線，從采樣點(diǎn)中隨機(jī)取最小的幾個(gè)點(diǎn)設(shè)定為內(nèi)群，則該部分?jǐn)?shù)據(jù)點(diǎn)滿足的數(shù)學(xué)模型為:

MPDR(x,y)=ax+b;x∈[xmin,xmax]

(5)

(6)

其中:Dis(d;MPDR(x,y))是采樣點(diǎn)到模型的距離，點(diǎn)A(xA,yA)到直線的距離計(jì)算為:

(7)

將式(7)等于0，得到線段上離A點(diǎn)最近的點(diǎn)(x0,y0)，

Dis(d;MPDR(x,y))=

通過(guò)式(8)可以得到所有內(nèi)部采樣點(diǎn)與擬合模型的錯(cuò)誤率，通過(guò)錯(cuò)誤率評(píng)估模型的正確與否。上述過(guò)程被重復(fù)執(zhí)行固定的次數(shù)，每次產(chǎn)生的模型或者因?yàn)榫謨?nèi)點(diǎn)太少而被舍棄，或者被更好的模型替代，所以可以得到最佳的匹配模型，即為初始路徑，同時(shí)確定初始位置，然后利用粒子濾波糾偏，實(shí)現(xiàn)定位和跟蹤。若存在WiFi時(shí)，可以根據(jù)WiFi先確定一個(gè)粗略的初始位置，再與RANSAC算法計(jì)算的初始位置根據(jù)兩者的可靠性進(jìn)行加權(quán)，就能得到更加精確的初始位置。研究發(fā)現(xiàn)，WiFi信號(hào)雖然信號(hào)波動(dòng)大，但可以有效識(shí)別出最近的WiFi接入點(diǎn)(Access Point, AP)，所以可以根據(jù)信號(hào)最強(qiáng)的AP位置校準(zhǔn)用戶位置。

4 實(shí)驗(yàn)結(jié)果

4.1 計(jì)步算法誤差分析

本文指紋庫(kù)數(shù)據(jù)采集和定位終端手機(jī)型號(hào)均為華為榮耀7，采樣頻率20 Hz，實(shí)驗(yàn)場(chǎng)景為桂林電子科技大學(xué)圖書館四樓，是一個(gè)總面積40 m×50 m，寬2.5 m的回形走廊，數(shù)據(jù)處理平臺(tái)為Matlab。計(jì)步是定位的基礎(chǔ)，首先對(duì)上述的計(jì)步算法進(jìn)行實(shí)驗(yàn)，評(píng)估其性能。根據(jù)前文對(duì)加速度的相加平滑窗處理，并對(duì)多組測(cè)量數(shù)據(jù)進(jìn)行分析，可以考慮先將波峰閾值設(shè)為20，波峰波谷之間差值暫定為40，峰谷差值比對(duì)的初始閾值設(shè)為13。每種運(yùn)動(dòng)狀態(tài)進(jìn)行了多次實(shí)驗(yàn)，與實(shí)際步數(shù)進(jìn)行比對(duì)，得到總體計(jì)步精度。表1展示了計(jì)步算法分別在慢走、正常行走、快走、跑步的不同行走狀態(tài)下的計(jì)步精度。實(shí)驗(yàn)證明，該算法在計(jì)步方面具有良好性能，能為后面的定位算法提供足夠精度。

表1 不同運(yùn)動(dòng)狀態(tài)下的計(jì)步算法正確率

4.2 初始位置的確定

定位初始時(shí)，用戶行走過(guò)程中記錄地磁數(shù)據(jù)，通過(guò)與地圖中數(shù)據(jù)比對(duì)后得到的匹配程度采樣分布如圖4(a)空心點(diǎn)所示，可以看出曲線的匹配結(jié)果大部分比較集中在x軸7.5～12.5 dm及y軸0～150 dm的范圍內(nèi)，少部分分散。利用RANSAC算法擬合的內(nèi)群數(shù)據(jù)如圖4(b)實(shí)心點(diǎn)所示，即為初始行走軌跡，再通過(guò)PDR得到的行走距離確定初始位置。

圖4 RANSAC采樣擬合

為了比較得到的初始位置的精度，本文在相同場(chǎng)景中分別使用單獨(dú)或聯(lián)合的定位算法進(jìn)行比較，得到的結(jié)果如圖5所示。從圖中可以看出，由于室內(nèi)障礙物多，信號(hào)強(qiáng)度多徑干擾嚴(yán)重，導(dǎo)致利用WiFi接收信號(hào)強(qiáng)度方法定位精度誤差較大，誤差集中在1.5～2.2 m，而WiFi指紋定位方法需要額外增加一張WiFi指紋地圖，資源占用較大，RANSAC可以僅憑地磁信息完成初始位置的確定，也可以與WiFi信號(hào)進(jìn)行聯(lián)合定位，定位平均誤差為0.372 m，最大誤差0.836 m，在誤差容忍范圍以內(nèi)。且實(shí)驗(yàn)中，完成定位只需十幾秒的時(shí)間，而粒子濾波收斂根據(jù)環(huán)境情況不穩(wěn)定，因此利用該算法進(jìn)行路徑匹配可以加快粒子濾波的收斂。

圖5 不同算法的定位初始位置誤差比較

4.3 自適應(yīng)粒子濾波算法性能分析

實(shí)驗(yàn)中，粒子濾波算法的粒子數(shù)N=100，KNN算法的K=50，在拐角處用傳統(tǒng)粒子濾波算法及改進(jìn)后粒子算法分別定位，并對(duì)粒子的狀態(tài)估計(jì)進(jìn)行分析，如圖6所示，發(fā)生突變處為轉(zhuǎn)向?？梢钥闯鲎赃m應(yīng)粒子濾波算法在拐角處較傳統(tǒng)粒子濾波能更快地跟上運(yùn)動(dòng)狀態(tài)的改變，同時(shí)在方向不變時(shí)，改進(jìn)后的粒子濾波對(duì)狀態(tài)的估計(jì)也要優(yōu)于傳統(tǒng)粒子濾波；并且，圖7將粒子濾波算法與常用的匹配融合算法卡爾曼濾波算法、加權(quán)KNN(K=4)，即WKNN算法進(jìn)行定位誤差的對(duì)比，從圖中可以看出基于地磁指紋和PDR的粒子濾波算法定位精度優(yōu)于其他兩種算法。

圖6 自適應(yīng)粒子濾波與傳統(tǒng)粒子濾波狀態(tài)估計(jì)對(duì)比

圖7 WKNN、卡爾曼濾波和粒子濾波定位精度對(duì)比

4.4 定位結(jié)果

為了消除設(shè)備之間讀數(shù)差異，本文將地磁值減去了各自的平均值后，再進(jìn)行匹配。以圖示坐標(biāo)為起點(diǎn)位置，沿著預(yù)定軌跡順時(shí)針行走3圈，得到的定位軌跡如圖8所示。可以看出PDR融合地磁的運(yùn)動(dòng)軌跡始終保持在真實(shí)軌跡上，并不會(huì)因?yàn)閼T性傳感器的累積誤差而出現(xiàn)偏離真實(shí)軌跡的情況。因此，三者融合可以有效克服 PDR的累積誤差。

圖8 地磁/WiFi/PDR融合定位移動(dòng)軌跡圖

對(duì)行走一圈的定位結(jié)果進(jìn)行定量分析，定位誤差如表2所示。在定位的初始階段，粒子濾波的粒子還未收斂，定位波動(dòng)大，誤差大，這個(gè)時(shí)期PDR的定位結(jié)果較為精確，隨著定位的進(jìn)行，粒子逐漸收斂，定位結(jié)果趨于穩(wěn)定，而PDR的累積誤差逐漸增大，所以融合后的結(jié)果優(yōu)于PDR的定位結(jié)果。由表2可知，融合地磁后的定位平均誤差為18.14 cm，最大誤差為41.08 cm，最小誤差為9.7 cm；而PDR單獨(dú)定位的平均誤差為115.44 cm，最大誤差高達(dá)307.78 cm，且隨著定位的進(jìn)行，誤差將愈來(lái)愈大。

表2 不同定位組合誤差統(tǒng)計(jì)結(jié)果 cm

5 結(jié)語(yǔ)

針對(duì)手機(jī)傳感器的累計(jì)誤差明顯而引起的定位精度問(wèn)題，提出了自適應(yīng)粒子濾波融合室內(nèi)地磁指紋和PDR定位結(jié)果，利用地圖約束的自適應(yīng)重采樣方法提高粒子對(duì)狀態(tài)的估計(jì)，并用WiFi作為地標(biāo)校準(zhǔn)。對(duì)于WiFi信號(hào)不穩(wěn)定，確定初始位置時(shí)誤差可能較大，定位初始階段采用WiFi和RANSAC算法進(jìn)行初始位置的確定，能有效提高定位精度。實(shí)驗(yàn)結(jié)果表明，自適應(yīng)粒子濾波在狀態(tài)估計(jì)、定位精度方面優(yōu)于傳統(tǒng)粒子濾波、卡爾曼濾波、KNN算法，但是利用地磁定位，精度受限于地磁指紋的區(qū)分度，在地磁信號(hào)變化平坦的環(huán)境下，定位可能存在困難，如何充分挖掘地磁指紋的特征，提高地磁指紋與指紋數(shù)據(jù)庫(kù)的匹配率，是一個(gè)值得研究的問(wèn)題。