PDR+BLE融合的室內(nèi)實時定位方法

徐 浩,吳 雪,李春光,秘金鐘,陳 沖,李得海

(1.濟南市房產(chǎn)測繪研究院,山東 濟南 250001; 2.山東皓宇資訊有限公司,山東 濟南 250101;3.中國測繪科學(xué)研究院,北京 100830; 4.中國電子科技集團第五十四研究所,河北 石家莊 050081)

近年來,隨著智能手機的廣泛普及和無線電通信技術(shù)的快速發(fā)展,基于位置的服務(wù)(location based service,LBS)日益成為大眾用戶的迫切需求[1]。GNSS衛(wèi)星由于受到信號遮擋,難以獲得可用的室內(nèi)定位結(jié)果[2],于是逐漸出現(xiàn)了許多室內(nèi)定位技術(shù)。大致可分為:基于射頻信號的定位技術(shù)、基于傳感器的定位技術(shù)和多源融合定位技術(shù)[3]。基于射頻信號定位技術(shù)有藍牙、Wi-Fi、UWB、射頻識別和偽衛(wèi)星等。基于傳感器的定位技術(shù)有行人航跡推算(pedestrian dead reckoning, PDR)、地磁定位、視覺定位和音頻定位等。而多源融合定位技術(shù)就是將兩種或兩種以上不同的定位手段進行融合的定位技術(shù),目前常見的融合方法主要有卡爾曼濾波(Kalman filter, KF)[4]、擴展KF (EKF)、UKF和粒子濾波等。當(dāng)下,開發(fā)出一種穩(wěn)定、高精度的新型室內(nèi)定位技術(shù)已成為眾多學(xué)者與企業(yè)的共識。

針對PDR和低功耗藍牙(bluetooth low energy, BLE)定位各自存在的問題及融合過程中的難題,已有眾多專家學(xué)者提出了嘗試性的解決方案。文獻[5]融合加速度計、磁力計和陀螺儀,利用載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下的水平角速度分量建立一個優(yōu)化問題,求解航向角,但有時坐標(biāo)系轉(zhuǎn)換不準(zhǔn)確。文獻[6]提出了將慣性定位與磁場進行雙重濾波融合的策略,試驗結(jié)果精度大為提高,但是硬件成本和計算復(fù)雜度太高。文獻[7]使用粒子過濾器將Wi-Fi指紋、PDR和地圖匹配相融合,提高了定位的準(zhǔn)確性和穩(wěn)健性。文獻[8]將PDR和BLE定位結(jié)果進行粒子濾波融合,并使用地圖匹配進行輔助定位,提高了算法解算性能,使定位結(jié)果更具穩(wěn)健性。

以上解決方案雖然實現(xiàn)了基本的融合定位,但對于融合過程中射頻信號定位方式粗差大的問題未能得到有效的解決。因此本文針對PDR+BLE融合定位過程中PDR累積誤差難以消除,以及BLE定位粗差大的問題,提出BLE信號連續(xù)最大值臨近校正PDR算法和基于EKF的自適應(yīng)抗差PDR+BLE融合定位算法,以期達到限制PDR累積誤差和BLE定位粗差的效果,進而提高融合定位精度和穩(wěn)健性。

1 PDR與BLE定位方法

1.1 PDR定位

PDR算法就是行人在上一步的已知位置出發(fā),根據(jù)當(dāng)前的航向、步長推算出行人的當(dāng)前位置,然后再從當(dāng)前時刻的位置出發(fā)推算出行人下一步的位置[9]。假設(shè)行人從當(dāng)前位置(E0,N0)沿著航向角φ0行走了步長S0,到達的位置為(E1,N1),則這種位置推算可表示為

(1)

由式(1)可以看出,PDR算法研究的關(guān)鍵點在于確定行人步長、航向角及步頻。因此PDR研究的方向可分為以下3個步驟:步態(tài)探測、步長估計及航向角估計。

(1)步態(tài)探測算法有波峰檢測法、零點交叉法、自相關(guān)分析法、有限狀態(tài)機法等[10],其中有限狀態(tài)機算法實現(xiàn)簡單,抗誤差干擾能力強,為當(dāng)前進行步態(tài)檢測相對最合適的方法。

(2)步長估計算法是PDR算法中的核心步驟。目前常用的步長估計算法有常數(shù)步長估計模型、線性步長估計模型、非線性步長估計模型、人工智能步長估計模型[11]。本文采用一種非線性步長估算模型,非線性步長模型基于一個步態(tài)周期中加速度的最大值與最小值,步長模型公式為

(2)

式中,K1為待定參數(shù);Amax、Amin分別為步態(tài)周期中加速度的最大值與最小值。

(3)除了步態(tài)檢測和步長估計外,航向估計是PDR的另一個重要組成部分。電子羅盤[12]或陀螺儀通常作為估計航向角的手段,由于智能終端固有的傳感器噪聲,電子羅盤獲得的航向精度不高,但不會長時間漂移。相比之下,利用陀螺儀估計航向,在短時間內(nèi)精度較高,但會出現(xiàn)漂移問題。為了解決這種漂移問題,本文使用Mahony互補濾波器[13]進行航向角的估計,其基本原理就是用加速度和磁力計磁場對陀螺儀累積誤差進行補償。假設(shè)陀螺儀校正誤差為e=[exeyez]T,則補償公式可表示為

e=ea+em

(3)

式中,ea、em分別為加速度計和磁強計讀數(shù)的誤差修正項,可分別表示為ea=[eaxeayeaz]T、em=[emxemyemz]T,計算公式為

(4)

(5)

式中,Kp、Ki分別為比例控制參數(shù)和積分控制參數(shù)。將修正后的陀螺儀數(shù)據(jù)代入龍格-庫塔四元數(shù)微分方程中對四元數(shù)進行更新。得到更新后的四元數(shù)后,即可求得姿態(tài)角,繼而獲取航向角。

1.2 BLE定位

BLE定位使用的是一種指紋定位算法,通過BLE信號強度的特征來實現(xiàn)定位,只需通過每個接入點(access point,AP)信號強度與位置之間的相對關(guān)系即可實現(xiàn)定位。BLE指紋定位包括離線建庫階段和在線定位階段。離線建庫階段,就是通過離線采集數(shù)據(jù)建立位置指紋數(shù)據(jù)庫。本文使用靜態(tài)單點采集方法,在按照指紋點布設(shè)規(guī)律設(shè)置好參考點(reference point,RP)后,在每個RP上采集一定時間的指紋信息,將所有RP都采集完成指紋信息后完成指紋庫的構(gòu)建。假設(shè)在室內(nèi)場景中,有n個參考點(RPs)和m個接入點(APs),最終構(gòu)建的指紋庫可表示為

(6)

在線定位階段,即在實時定位過程中,在待測點位采集各個AP的RSSI值和MAC地址,再通過匹配算法將實時指紋與指紋庫數(shù)據(jù)進行匹配,找到與待測點位最為相似的參考點[14],最終以參考點位置坐標(biāo)估算出待測點位的位置。實時定位時,智能終端采集到的實時指紋可表示為

實時指紋=[〈rssi1,rssi2,…,rssim〉]

(7)

匹配算法采用了WKNN[15]算法。計算出待定點與參考點的RSSI歐式距離,取歐式距離最小的前k個參考點位置加權(quán)平均即可得到待定點位置坐標(biāo)。

除了基于指紋庫信號匹配的BLE指紋定位之外,還有一種BLE測距算法,隨著距離的增大,RSSI會隨之衰減,根據(jù)此對應(yīng)關(guān)系,實現(xiàn)基于RSSI的測距。對數(shù)距離路徑損耗模型[16]為最常用的BLE測距模型,具體表達式為

RSSI(d)=A-10nlgd

(8)

式中,RSSI(d)為BLE節(jié)點與信號采集設(shè)備距離為d時的RSSI值;A代表參考距離取1 m時的RSSI值;n為信號衰減因子。本文選取不同間隔距離作為BLE信標(biāo)與智能終端的距離對模型參數(shù)進行擬合,擬合后的對數(shù)路徑損耗模型公式為

(9)

2 PDR+BLE室內(nèi)實時融合定位算法

2.1 BLE信號連續(xù)最大值臨近校正PDR的融合定位

BLE臨近校正PDR是BLE與PDR融合的一種定位算法。PDR過程中,所持智能終端檢測到臨近BLE節(jié)點的RSSI滿足臨近條件時,將BLE節(jié)點位置坐標(biāo)賦予當(dāng)前PDR坐標(biāo),繼續(xù)進行PDR行走,起到消除PDR累積誤差的作用。傳統(tǒng)BLE臨近校正PDR算法流程如圖1所示。

圖1 傳統(tǒng)BLE臨近校正PDR算法流程

傳統(tǒng)BLE臨近校正PDR算法的閾值判別方法,僅僅設(shè)置一次RSSI閾值判斷,臨近判別極不準(zhǔn)確,本文提出了BLE信號連續(xù)最大值臨近校正PDR算法。BLE信號連續(xù)最大值臨近校正法的具體實施步驟如下。

(1)將待臨近BLE的MAC地址和臨近坐標(biāo)事先存到設(shè)備,為進行PDR行走過程的臨近做好準(zhǔn)備。

(2)在實驗場環(huán)境下進行數(shù)據(jù)采集,將終端設(shè)備放在離某一BLE臨近點1 m距離處采集1 min的RSSI,計算出RSSI的平均值A(chǔ)VG及標(biāo)準(zhǔn)差STD。

(3)將1 m處RSSI的 AVG和STD設(shè)為臨近判別RSSI的閾值;將1 m設(shè)置為臨近判別距離閾值。

(4)進行PDR行走定位。行走過程中隨時掃描存入設(shè)備中的待臨近BLE的MAC地址。

(5)若掃描到已存入手機中的待臨近BLE的MAC地址,則繼續(xù)獲取待臨近BLE的信號強度RSSI;反之,則繼續(xù)進行PDR定位。

(6)將掃描到的所有待臨近點BLE的RSSI進行信號強度閾值判別。若RSSI>AVG且AVG-RSSI

(7)對所有滿足信號強度閾值對比的待臨近點RSSI進行排序,并將RSSI最大的BLE 作為臨近BLE。

(8)對該臨近BLE連續(xù)獲取3次滿足信號閾值判別條件的RSSI最大值。

(9)根據(jù)前文選擇的三次多項式測距模型反算距離。若3次最大RSSI值反算出的距離均滿足距離小于1 m的臨近判別距離閾值,則將該臨近BLE坐標(biāo)作為當(dāng)前PDR定位坐標(biāo),校正PDR定位;否則,不進行臨近校正,繼續(xù)直接使用PDR定位。

BLE信號連續(xù)最大值臨近校正PDR的算法流程如圖2所示。

圖2 BLE信號連續(xù)最大值臨近校正PDR算法流程

2.2 基于EKF的自適應(yīng)抗差PDR+BLE融合定位

為使兩種定位方式實現(xiàn)優(yōu)勢互補,可將兩種定位方法基于KF組合成一個融合系統(tǒng)。由于PDR定位方法是一種非線性模型,因此用EKF代替KF方法進行融合。融合定位模型主要包括兩個模型:一是狀態(tài)轉(zhuǎn)移模型,二是觀測模型。

狀態(tài)轉(zhuǎn)移模型是由PDR的位置坐標(biāo)、步長和航向角組成的狀態(tài)向量。狀態(tài)向量可表示為

X=[NESψ]

(10)

狀態(tài)轉(zhuǎn)移模型在k時刻時可表示為

(11)

(12)

觀測模型是由BLE定位的位置坐標(biāo)組成的觀測向量。觀測向量及模型表示為

Zk=[N′E′]

(13)

其中

觀測矩陣Hk可表示為

(14)

通過PDR和BLE定位獲取狀態(tài)向量和觀測向量時,利用EKF估計隨時間更新狀態(tài)參數(shù)和觀測參數(shù),在EKF中,先驗估計的過程可表示為

(15)

增益矩陣表示為

(16)

然后根據(jù)觀測值更新狀態(tài)向量和協(xié)方差矩陣,更新過程可表示為

(17)

PDR+BLE濾波融合定位,可以通過EKF模型有效抑制PDR定位的漂移誤差,提高整體融合定位算法的精度和穩(wěn)定性,但對抑制BLE定位粗差的效果較差,因此需要對BLE定位進行抗差處理。為實現(xiàn)在EKF融合定位過程中對BLE定位的抗差處理,本文引入向量Dk,Dk可表示為

Dk=Zk-Hk·Xk

(18)

為判別在融合過程中是否需要對BLE定位進行抗差處理,可通過計算PDR定位與BLE定位坐標(biāo)結(jié)果的距離差進行判斷。若距離相差過大,說明此時BLE定位的結(jié)果出現(xiàn)了較大偏差,需要進行抗差處理。由于PDR定位與BLE定位的結(jié)果分別屬于兩種不同的分布,因此不可使用歐式距離,需使用Mahalanobis距離,即馬氏距離[17],PDR定位結(jié)果與BLE定位結(jié)果馬氏距離的協(xié)方差矩陣可表示為

(19)

那么,PDR定位結(jié)果與BLE定位結(jié)果之間的馬氏距離MDk可表示為

(Zk-Hk·Xk)

(20)

由上文可知觀測誤差協(xié)方差矩陣Rk表示為

(21)

式中,σN為BLE定位結(jié)果在北方向的誤差方差值;σE為BLE定位結(jié)果在東方向的誤差方差值。

假設(shè)PDR定位誤差平均值為Pavg,最大誤差值為Pmax,若Pavg

(22)

若MDk

(23)

本文判斷是否需要抗差的依據(jù)為:Rk越大,代表權(quán)重越小,反之代表權(quán)重越大。若計算出的馬氏距離小于Pavg,說明BLE定位結(jié)果接近PDR定位結(jié)果,只需將BLE定位結(jié)果的權(quán)重賦值小一些便可,從而實現(xiàn)對BLE定位的抗差更新。

3 試驗與分析

試驗場地設(shè)置在中國電子科技集團第五十四研究所衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室三層C7試驗場二樓,如圖3所示。二層試驗場長27.49 m,寬24.92 m,由一個矩形走廊和幾個房間組成。華為P40智能手機被選為測試設(shè)備。二層測試場地安裝了18個BLE信標(biāo)。此外,在二層試驗場按照地板磚的位置,共選定154個指紋RPs。五角星為行走路線起點,黑實線為參考路徑路線。指紋信息在行走前采集完成,每個RP采集指紋信息時長1 min,在154個RP上收集了9240個實時BLE指紋數(shù)據(jù),完成BLE指紋數(shù)據(jù)庫的構(gòu)建。行人從起點出發(fā),行走一圈勻速到達終點。

圖3 試驗場地及行進路線

3.1 BLE臨近校正PDR融合定位試驗

圖4為BLE信號連續(xù)最大值臨近校正PDR與傳統(tǒng)BLE臨近校正PDR定位對比效果。將行走軌跡中途經(jīng)已知點的定位點與已知點坐標(biāo)進行對比,定位結(jié)果誤差統(tǒng)計信息見表1。

表1 兩種臨近方法定位誤差對比m

圖4 BLE臨近校正PDR算法對比

由圖4可知,傳統(tǒng)BLE臨近校正PDR算法僅檢測并臨近上到臨近點1和6,而BLE信號連續(xù)最大值臨近校正PDR算法則將臨近點全部檢測到并臨近。在定位精度上,平均定位精度提高了0.295 m,約19%,定位精度和穩(wěn)定性有所提升。

3.2 基于EKF的自適應(yīng)抗差PDR+BLE融合定位試驗

基于EKF常規(guī)PDR+BLE融合定位算法與改進的基于EKF的自適應(yīng)抗差PDR+BLE融合定位算法定位效果對比如圖5所示。將行走軌跡中途經(jīng)已知點的定位點與已知點坐標(biāo)進行對比,定位結(jié)果誤差統(tǒng)計信息見表2。

表2 兩種融合定位算法定位誤差對比m

圖5 基于EKF的PDR+BLE融合定位算法對比

基于EKF的自適應(yīng)PDR+BLE融合定位平均定位精度為1.219 m,定位精度提高了21%,與基于EKF的常規(guī)PDR+BLE融合定位相比,平均定位精度提高了0.334 m。在定位精度和穩(wěn)定性上都顯著優(yōu)于常規(guī)EKF融合效果。

3.3 PDR+BLE融合定位算法對比試驗

為對比本文所述所有定位算法的定位效果,在規(guī)劃路徑上將所有定位方法行走一圈。本文所述所有定位算法的定位軌跡如圖6所示。對參考點已知坐標(biāo)進行對比,得到如圖7所示的定位誤差序列。定位誤差累積分布函數(shù)(cumulative distribution function,CDF)如圖8所示,定位誤差統(tǒng)計見表3。

表3 定位誤差統(tǒng)計對比m

圖6 定位軌跡對比

圖7 定位誤差對比

圖8 誤差累積分布對比

由圖6—圖8和表3可以看出,PDR+BLE+臨近的定位效果明顯好于單獨一種定位方法或某兩種定位方法的結(jié)合,并且從累積誤差分布可知,PDR+BLE+臨近的線處于最上方且是最先到達1的線,兼顧定位精度和穩(wěn)健性。綜上所述,在本文所介紹的定位方法中,PDR+BLE+臨近共同結(jié)合的PDR+BLE融合定位系統(tǒng)定位精度最高。

4 結(jié) 語

當(dāng)前,在室內(nèi)定位研究領(lǐng)域已有多種行之有效的技術(shù)手段和方法,多種定位方法的融合能有效彌補單一定位手段所展現(xiàn)出的不足之處,進而提高定位精度。而對于與信號相關(guān)的定位方法仍然存在著如信號衰減和多徑效應(yīng)等影響定位精度的因素,因此與其他定位方法融合降低此類因素的影響至關(guān)重要。本文提出的基于EKF的自適應(yīng)抗差PDR+BLE室內(nèi)融合定位方法,考慮了PDR定位的短期相對穩(wěn)定性,當(dāng)BLE定位超過限差時降低其在EKF中的權(quán)值,抑制BLE定位粗差,達到兩種定位方法融合并取長補短,進而提高融合定位精度的目的。試驗結(jié)果表明,基于EKF的自適應(yīng)抗差PDR+BLE融合定位方法,具有較好的定位精度和穩(wěn)定性。但是,本文并沒有詳細分析地形因素對定位結(jié)果的影響,在今后的研究工作中,會考慮加入地圖匹配等手段來輔助定位。