融合地圖信息與WiFi地標(biāo)的室內(nèi)粒子濾波定位算法

張 斌李志遠(yuǎn)魏 瑋趙建立

(1.山東科技大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院，山東 青島 266590；2.青島海爾(膠州)空調(diào)器有限公司，山東 青島 266300；3.青島海爾工業(yè)智能研究院有限公司，山東 青島 266000)

隨著數(shù)字信息時(shí)代的到來(lái)，基于位置的服務(wù)(location based service，LBS)變得日益重要，在火災(zāi)救援、醫(yī)療服務(wù)、停車(chē)定位等方面都擁有廣闊的應(yīng)用前景。全球定位系統(tǒng)(global positioning systems,GPS)基本可以滿足行人在室外的定位，但是基于衛(wèi)星進(jìn)行導(dǎo)航定位的GPS系統(tǒng)難以覆蓋復(fù)雜的室內(nèi)區(qū)域，因此室內(nèi)定位成為了新的研究熱點(diǎn)。目前典型的室內(nèi)定位技術(shù)主要包括基于無(wú)線網(wǎng)絡(luò)的指紋定位[1-2]，基于慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)的行人航位推算(pedestrain dead rechoning, PDR)算法[3]、三角三邊定位[4]，基于紅外的定位[5]以及基于射頻(radio frequency,RF)[6]的定位等。

目前，智能手機(jī)傳感器功能的完善與普及使得利用INS解決室內(nèi)定位問(wèn)題成為可能。利用INS定位的PDR系統(tǒng)由于傳感器數(shù)據(jù)的不準(zhǔn)確，會(huì)導(dǎo)致最終定位誤差的累積。因此，如何抑制傳感器數(shù)據(jù)誤差累積是目前急需解決的問(wèn)題。Li等[7]提出一種自適應(yīng)噪聲的擴(kuò)展卡爾曼濾波算法，將WiFi定位結(jié)果與PDR結(jié)果進(jìn)行融合以提高定位精度；Shang等[8]提出一種基于距離約束的聚類算法，通過(guò)識(shí)別環(huán)境中存在的Landmark和回溯粒子對(duì)行人位置進(jìn)行校正；Qian等[9]則提出一種基于網(wǎng)格矢量圖輔助粒子加權(quán)的粒子濾波算法來(lái)修正步長(zhǎng)和航向估計(jì)的偏差；Widyawan等[10]將一種新型的回溯粒子濾波器(backtracking particle filter,BPF)與不同層次的建筑平面圖細(xì)節(jié)相結(jié)合，以改善PDR性能；Yu等[11]利用無(wú)跡卡爾曼濾波算法(unscented Kalman filter,UKF)，將WiFi定位與行人航跡推算相結(jié)合，獲得了更高的定位精度;Lee 等[12]提出了一種虛擬鏈路(virtual link,VL)算法構(gòu)建地圖數(shù)據(jù)庫(kù)，利用擴(kuò)展卡爾曼濾波器(extended Kalman filter，EKF)估計(jì)行人位置和慣性測(cè)量單元(IMU)傳感器的誤差。Xi等[13]則提出利用WiFi信號(hào)峰值檢測(cè)地標(biāo)以更新PDR定位坐標(biāo)從而糾正行人位置的Lap算法(landmark-aided PDR,Lap)。以上方案仍然存在設(shè)施部署成本昂貴、過(guò)于依賴WiFi定位結(jié)果和大量無(wú)線訪問(wèn)節(jié)點(diǎn)(access point,AP)、計(jì)算復(fù)雜度過(guò)高等問(wèn)題，在實(shí)際的定位過(guò)程中應(yīng)當(dāng)考慮到缺少現(xiàn)有指紋庫(kù)或者AP的情況。

針對(duì)以上問(wèn)題，本研究提出融合地圖信息與WiFi地標(biāo)的粒子濾波定位算法WL+PF(WiFi landmark+particle filter)算法，該算法無(wú)需專人進(jìn)行細(xì)粒度的指紋采集，利用地圖信息作為先驗(yàn)知識(shí)，將無(wú)效粒子權(quán)重賦值為0，并通過(guò)房間內(nèi)WiFi信息峰值檢測(cè)[13]地標(biāo)信息替代PDR的定位坐標(biāo)，實(shí)時(shí)消除PDR累積誤差。同時(shí)，利用地圖中所表達(dá)出的拐點(diǎn)信息、WiFi峰值檢測(cè)出的地標(biāo)信息更新粒子權(quán)重，輸出粒子位置，增強(qiáng)粒子使用效率，以提高最終的定位精度。相比較其他常規(guī)濾波，本研究提出的算法沒(méi)有直接將WiFi定位結(jié)果與PDR定位結(jié)果相融合，因此在一定程度上規(guī)避了因WiFi信號(hào)受干擾而導(dǎo)致的定位跳躍性帶來(lái)的融合結(jié)果誤差；在PDR定位結(jié)果加入有限的地標(biāo)信息的基礎(chǔ)之上，粒子濾波可以通過(guò)大量的隨機(jī)樣本獲得最優(yōu)的狀態(tài)結(jié)果，同時(shí)避免由于有限地標(biāo)的加入而出現(xiàn)PDR定位結(jié)果跳躍的情況，進(jìn)一步提升了最終的定位精度。實(shí)驗(yàn)結(jié)果證明，基于地圖信息的粒子濾波與使用WiFi Landmark相結(jié)合的算法，不需要耗費(fèi)額外人力資源，也可以定期糾正傳感器信息漂移，有效改善PDR算法的誤差累積問(wèn)題。

1 行人航跡推算

(1)

1.1 步行檢測(cè)

行人在行走的過(guò)程中，加速度信息主要由水平、垂直、側(cè)向三個(gè)分量組成，其分別對(duì)應(yīng)手機(jī)坐標(biāo)系的y、z、x軸。檢測(cè)步行的常用方法是波峰檢測(cè)算法，由于垂直方向的加速度信息會(huì)有周期性的變化，利用加速度的差異，檢測(cè)一步之內(nèi)滿足閾值限制的峰值和谷值，從而獲得行人的步數(shù)。

1.2 步長(zhǎng)估計(jì)

步長(zhǎng)估計(jì)方法一般分為兩種：一種是根據(jù)行人的身高體重等狀態(tài)設(shè)置一個(gè)常數(shù)值作為行人的步長(zhǎng)，另一種是根據(jù)行走特征建立自適應(yīng)步長(zhǎng)計(jì)算公式。后者雖然比較復(fù)雜，但相對(duì)于前者，精度會(huì)有一定的提升。本研究采用文獻(xiàn)[15]的算法，利用線性回歸模型來(lái)獲得步長(zhǎng)計(jì)算公式中自適應(yīng)K值：

(2)

其中，vstep為每一步中三維軸vstepx、vstepy、vstepz上平均速度的大小,即：

(3)

最終獲得步長(zhǎng)

(4)

其中，Amax與Amin分別表示一步之內(nèi)的最大、最小加速度值。

1.3 方向角估計(jì)

方向角的準(zhǔn)確測(cè)量對(duì)最終的定位結(jié)果有著較大的影響。由于本研究采用電子羅盤(pán)對(duì)方向數(shù)據(jù)進(jìn)行收集，用戶的移動(dòng)方式和環(huán)境磁場(chǎng)變化都會(huì)在一定程度上導(dǎo)致電子羅盤(pán)采集的航向數(shù)據(jù)產(chǎn)生噪聲，因此，需對(duì)方向角加以限定，以在一定程度上抑制噪聲，這對(duì)于提高最終的定位精度至關(guān)重要。本研究首先通過(guò)閾值檢測(cè)行人的運(yùn)動(dòng)狀態(tài)，當(dāng)檢測(cè)到行人轉(zhuǎn)彎時(shí)，若下一時(shí)刻狀態(tài)為直行狀態(tài)，則使用數(shù)據(jù)庫(kù)中存儲(chǔ)的走廊方向信息對(duì)此時(shí)的電子羅盤(pán)采集的方向角進(jìn)行匹配從而校準(zhǔn)航向信息，并在后續(xù)的直行過(guò)程中一直使用，直到檢測(cè)到下一個(gè)轉(zhuǎn)彎狀態(tài)再重新匹配新的走廊方向信息。

具體的，通過(guò)下式檢測(cè)行人運(yùn)動(dòng)狀態(tài)：

||(θmi)|-|(θmi-1)||>Th1∪||θmi|-|θmi+1||

(5)

其中，θm表示方向角的測(cè)量值。用下面兩個(gè)條件對(duì)方向角進(jìn)行限定，確定行人轉(zhuǎn)彎行為：①當(dāng)前時(shí)刻設(shè)為i時(shí)刻的方向角θmi與上一時(shí)刻的方向角θmi-1求差值并大于給定的閾值Th1；②i時(shí)刻的θmi與i+1時(shí)刻的方向角θmi+1的差值小于閾值Th2。

在直行狀態(tài)下，可利用走廊方向?qū)π腥说男凶叻较蜻M(jìn)行修正，具體公式見(jiàn)參考文獻(xiàn)[16]。之后繼續(xù)利用式(5)檢測(cè)行人運(yùn)動(dòng)狀態(tài)，直到出現(xiàn)下一次直行狀態(tài)，并對(duì)方向角進(jìn)行修正。

2 AP有限情況下的Lap算法存在問(wèn)題分析

PDR定位算法在行人行走的過(guò)程中會(huì)產(chǎn)生累積誤差，因此需要加入地標(biāo)來(lái)定期矯正行人的位置。文獻(xiàn)[13]使用檢測(cè)到的WiFi信號(hào)峰值點(diǎn)來(lái)確定直行狀態(tài)下的地標(biāo)點(diǎn)，將當(dāng)前時(shí)刻PDR算法得到的定位點(diǎn)重置為真實(shí)的坐標(biāo)后，繼續(xù)計(jì)算后續(xù)的行人位置，但未考慮AP有限情況下算法所存在的局限性。

PDR定位結(jié)果如圖1所示，由于傳感器收集數(shù)據(jù)存在噪聲等其他因素的影響，導(dǎo)致PDR算法最終定位結(jié)果嚴(yán)重偏移真實(shí)軌跡；圖2中分別顯示了加入較少AP的前提下使用地標(biāo)獲得的定位結(jié)果，以及在此基礎(chǔ)上添加方向角修正的定位結(jié)果。在AP有限的情況下，由于檢測(cè)到的地標(biāo)較少，后續(xù)的定位結(jié)果依然存在漂移現(xiàn)象，且由于步長(zhǎng)估計(jì)產(chǎn)生的誤差等因素，會(huì)出現(xiàn)當(dāng)前時(shí)刻更新到地標(biāo)點(diǎn)的定位點(diǎn)與上一時(shí)刻利用PDR算法計(jì)算出的定位結(jié)果之間距離過(guò)長(zhǎng)、定位點(diǎn)跳躍的情況。對(duì)方向角進(jìn)行修正之后，行人軌跡的漂移情況雖然得到了一定的緩解，但是依舊存在定位點(diǎn)跳躍、拐點(diǎn)處軌跡不符合實(shí)際路徑的情況。因此，本研究將文獻(xiàn)[17]引入到Lap算法中，得到第3節(jié)的融合地圖信息與WiFi地標(biāo)的粒子濾波定位算法。

3 融合地圖信息與WiFi地標(biāo)的粒子濾波定位算法

單純地在PDR算法中加入地標(biāo)可以一定地提升定位精度，但是由于地標(biāo)數(shù)量的限制，加入較少的地標(biāo)依然無(wú)法解決后續(xù)定位過(guò)程中誤差累積的問(wèn)題。因此，在文獻(xiàn)[13]的基礎(chǔ)之上，本研究融合一種基于地圖信息的PF算法，該算法基于已知的地圖信息，使用WiFi峰值點(diǎn)識(shí)別的地標(biāo)點(diǎn)，在對(duì)觀測(cè)值修正的同時(shí)重新輸出粒子位置、更新粒子權(quán)重[17]。使用此算法可以避免在AP較少的情況下所獲得的地標(biāo)數(shù)目不足導(dǎo)致的定位點(diǎn)跳躍等問(wèn)題，進(jìn)一步提升最終定位精度。圖3為融合地圖信息與WiFi地標(biāo)的粒子濾波定位算法流程圖：

圖1 PDR定位結(jié)果Fig.1 PDR positioning results

圖2 使用地標(biāo)校準(zhǔn)后PDR結(jié)果Fig.2 PDR results after calibration using landmarks

圖3 融合地圖信息與WiFi地標(biāo)的粒子濾波定位算法流程圖Fig.3 Flow chart of particle filter positioning algorithm combining map information and WiFi landmarks

在很多場(chǎng)景中都需要利用一系列的觀測(cè)值對(duì)系統(tǒng)狀態(tài)向量進(jìn)行估計(jì)，常用的有卡爾曼濾波(Kalman filtering,KF)[18]、粒子濾波(particle filter,PF)[19]等算法，其中卡爾曼濾波是一種通過(guò)系統(tǒng)觀測(cè)值估計(jì)系統(tǒng)最優(yōu)狀態(tài)量的算法，但并不適用非線性系統(tǒng)。PF利用蒙特卡羅方法解決貝葉斯系統(tǒng)狀態(tài)估計(jì)問(wèn)題，通過(guò)離散的隨機(jī)樣本來(lái)逼近實(shí)際的概率分布p(x0:k|z1:k)的積分值，通過(guò)大量的樣本估計(jì)一個(gè)系統(tǒng)的最優(yōu)狀態(tài)值。PF可應(yīng)用于非高斯、非線性系統(tǒng)，應(yīng)用范圍較卡爾曼濾波算法而言相對(duì)廣泛。其公式為：

(6)

其中，p(x0)為該動(dòng)態(tài)系統(tǒng)的先驗(yàn)概率分布，k時(shí)刻系統(tǒng)狀態(tài)xk的后驗(yàn)概率分布為p(x0:k|z1:k)，N代表粒子數(shù)量，本研究設(shè)置為100。x0:k表示系統(tǒng)從0到k時(shí)刻的系統(tǒng)狀態(tài)，z1:k代表系統(tǒng)的觀測(cè)值，在應(yīng)用中表示與PDR結(jié)果的距離。粒子的權(quán)重需要滿足歸一化條件

(7)

權(quán)重更新如下式：

(8)

(9)

下面給出改進(jìn)算法的偽代碼。

算法1 基于地圖匹配的改進(jìn)粒子濾波算法輸入:地標(biāo)點(diǎn)LandMark,拐點(diǎn)數(shù)據(jù)coner,粒子數(shù)目Nx,PDR定位值Sensor,地標(biāo)點(diǎn)時(shí)間戳Landt,傳感器時(shí)間戳sensort,可行走區(qū)域Map /?其中地標(biāo)點(diǎn)包括WiFi峰值檢測(cè)地標(biāo)點(diǎn)與拐點(diǎn)信息?/輸出:行人位置position1:初始化粒子 P(xi0),P(yi0)2:i∈[1,N],Nx=1003:for k=1;k

圖4 實(shí)驗(yàn)環(huán)境平面圖Fig.4 Floor plan of the indoor environment

4 仿真實(shí)驗(yàn)

4.1 實(shí)驗(yàn)設(shè)置

本研究選擇的實(shí)驗(yàn)場(chǎng)地為山東科技大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院3樓。使用MATLAB2016b進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)定位終端采用三星Galaxys IV型號(hào)手機(jī)，對(duì)加速度計(jì)、電子羅盤(pán)、WiFi信號(hào)信息以及樣本時(shí)間戳進(jìn)行了采集。實(shí)驗(yàn)區(qū)域?yàn)?4 m×27.6 m，在該范圍內(nèi)選擇了4個(gè)AP作為錨點(diǎn)的檢測(cè)點(diǎn)，分別為T(mén)P_WR842N、TP_linkA5210E、JWY和anran_company。具體建筑信息圖和實(shí)驗(yàn)理想路線如圖4所示。圖中的圓圈表示行人的起點(diǎn)，三角代表行人終點(diǎn)。圖4中的黑色軌跡是實(shí)驗(yàn)設(shè)置的行人軌跡。實(shí)驗(yàn)者從初始點(diǎn)(0,0)出發(fā)，將智能手機(jī)水平手持于胸前，步伐保持勻速平穩(wěn)，按照路線圖中標(biāo)識(shí)的方向沿走廊行走一圈。

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

圖5顯示了傳統(tǒng)PF算法的定位結(jié)果，可以看出，應(yīng)用此算法的PDR定位精度有一定的提高，但是由于粒子的軌跡沒(méi)有受到任何地圖信息的約束，導(dǎo)致依然存在行人位置穿墻的問(wèn)題，且粒子輸出的位置波動(dòng)比較大；行人定位結(jié)果依舊存在漂移現(xiàn)象。

本研究提出的算法校正后的行人軌跡如圖6所示，此時(shí)的粒子位置受到地圖信息的約束并且重新校準(zhǔn)到地標(biāo)位置，能更準(zhǔn)確地獲得行人軌跡。且行人軌跡更符合真實(shí)軌跡，粒子輸出位置的波動(dòng)也更加平穩(wěn)，有效改善了單純加入地標(biāo)時(shí)出現(xiàn)的定位點(diǎn)跳躍過(guò)大的問(wèn)題。圖7為7種定位方式與實(shí)際路徑在真實(shí)地圖中的對(duì)比定位結(jié)果，可以看出，相比較[13]中單獨(dú)加入地標(biāo)的算法，以及傳統(tǒng)的PDR算法和PF算法，改進(jìn)后的算法行人軌跡比較平穩(wěn)，有效地改善了PDR算法誤差累計(jì)的問(wèn)題。

圖5 粒子濾波定位結(jié)果Fig.5 Particle filter positioning results

圖6 WL+PF定位結(jié)果Fig.6 WL+PF positioning results

圖7給出了WiFi定位結(jié)果，由圖可知，此時(shí)收集到的WiFi信號(hào)值波動(dòng)較大，出現(xiàn)了WiFi定位點(diǎn)跳躍嚴(yán)重、大部分定位結(jié)果在走廊以外的情況。而文獻(xiàn)[11]提出的UKF融合算法、文獻(xiàn)[7]提出的ADEKF(adaptive extended kalman filter,自適應(yīng)擴(kuò)展卡爾曼濾波)融合算法，都是將WiFi定位值與PDR定位信息相融合以提升定位精度，但是由于WiFi定位結(jié)果跳躍過(guò)于嚴(yán)重，此時(shí)使用濾波算法反而會(huì)導(dǎo)致定位結(jié)果精度下降。圖8誤差累計(jì)分布圖表明，本研究所提出的算法誤差分布基本在2 m左右，明顯優(yōu)于其他幾種定位算法。圖9和圖10分別給出了本算法與其他5種算法的定位誤差分布圖。在圖9中可以清晰的看出，由于WiFi信號(hào)受到外界因素干擾存在較大的波動(dòng)性，導(dǎo)致WiFi定位結(jié)果跳躍嚴(yán)重，因此采用EKF或者UKF算法將PDR定位結(jié)果與WiFi定位結(jié)果進(jìn)行融合，反而導(dǎo)致定位誤差增加、定位結(jié)果波動(dòng)性較大；而傳統(tǒng)的PF算法由于沒(méi)有受到地圖信息的約束，定位精度沒(méi)有太大的提升。由圖10的定位誤差分布圖可以看到，PDR定位軌跡伴隨時(shí)間增長(zhǎng)存在累計(jì)誤差，且定位誤差波動(dòng)較大；本研究提出的算法在一定程度上緩解了PDR的誤差累計(jì)問(wèn)題。相比較其他5種定位算法，本研究提出的定位算法誤差分布均勻、變化平穩(wěn)，誤差分布在2 m以內(nèi)，有著較好的定位精度提升效果。

圖7 本研究算法與6種定位算法定位結(jié)果對(duì)比Fig.7 Comparison of the results of this research algorithm with six kinds of positioning algorithms

圖8 本研究算法與6種定位算法誤差累積分布對(duì)比Fig.8 Comparison of the error cumulative distribution of this research algorithm with six kinds of positioning algorithms

為了更進(jìn)一步分析算法的定位精度，表1對(duì)比了其中6種定位算法的平均誤差與最大定位誤差。與單純的PDR算法相比，Lap、WiFi+PF、PF的定位結(jié)果精度分別提升了39%、79%、8.2%。其中PF算法由于沒(méi)有受到地圖信息的約束，定位精度提升比較??；基于地圖信息與地標(biāo)點(diǎn)的WiFi+PF算法相對(duì)于PF算法精度提升了78%，且此時(shí)的行人軌跡比較平穩(wěn)，最大定位誤差比PF定位算法降低了70%；Lap算法由于定位結(jié)果跳躍，最大定位誤差比PF、WiFi+PF、PDR三種算法分別增長(zhǎng)了9.7%、74%、18%。

圖9 6種定位算法誤差分布對(duì)比Fig.9 Comparison of error distribution of the six kinds of positioning algorithms

圖10 PDR與WL+IMPF的定位誤差分布對(duì)比Fig.10 Comparison of positioning error distribution between PDR and WL+IMPF

表1 定位誤差

從表1中可以看出，基于WiFi定位結(jié)果的ADEKF與UKF融合算法，由于采集的WiFi信號(hào)值波動(dòng)較大，導(dǎo)致最終的融合定位精度比原始的PDR算法分別下降了63%和55%，比本研究的WiFi+PF算法分別下降了77%和73%。

綜上所述，本研究提出的WiFi+PF算法能夠有效提升最終定位精度，在WiFi信號(hào)值波動(dòng)較大的情況下優(yōu)于常規(guī)的濾波融合算法，且在定位過(guò)程中有較好的穩(wěn)定性。

5 結(jié)論

研究了PDR室內(nèi)行人定位算法，提出一種融合室內(nèi)地圖信息與WiFi地標(biāo)的粒子濾波算法。該算法利用地圖信息以及地標(biāo)點(diǎn)對(duì)觀測(cè)值進(jìn)行修正的同時(shí)，對(duì)粒子的位置和權(quán)重進(jìn)行了更新，最后輸出行人的最終位置。實(shí)驗(yàn)結(jié)果表明，本研究提出的室內(nèi)行人定位算法能有效地抑制傳感器的誤差累積，增加粒子軌跡輸出的穩(wěn)定性，提升定位精度。