融合粒子濾波與藍(lán)牙地標(biāo)矯正的定位算法

鄭晨輝，陳 璟 ，2，張雨婷，薛 偉

1.江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 無錫 214122

2.江南大學(xué) 物聯(lián)網(wǎng)技術(shù)應(yīng)用教育部工程研究中心，江蘇 無錫 214122

1 引言

隨著智能移動(dòng)設(shè)備的普及，人們對(duì)于定位精度的要求也越來越高。在室外，GPS 可以獲取精確的位置信息，但在室內(nèi)，由于信號(hào)的遮擋，GPS 無法進(jìn)行精確定位，所以在室內(nèi)獲取高精度定位結(jié)果具有很大的研究?jī)r(jià)值。

常見的室內(nèi)定位技術(shù)有很多，但各自有缺陷，如紅外技術(shù)[1]，此方法定位效果好，但設(shè)備昂貴，且容易受到環(huán)境的影響。射頻技術(shù)（RFID）[2]，需要在定位空間中布置大量設(shè)備。WiFi[3]是室內(nèi)定位中常用的方法，但是WiFi存在信號(hào)強(qiáng)度波動(dòng)性大和功率高等特點(diǎn)，因此定位精度不高且耗能大。近幾年，智能手機(jī)得到長(zhǎng)足發(fā)展，內(nèi)部傳感器種類也越來越豐富，移動(dòng)端的室內(nèi)定位技術(shù)越來越受關(guān)注，常見的有利用傳感器數(shù)據(jù)[4]結(jié)合PDR進(jìn)行位置估計(jì)。PDR需要得到行人的初始位置，進(jìn)而利用方向數(shù)據(jù)和步長(zhǎng)數(shù)據(jù)進(jìn)行迭代計(jì)算，得出用戶的運(yùn)動(dòng)軌跡。由此可知，這種定位方法受信號(hào)傳播的影響不大，對(duì)比之前的方法，PDR算法更具普適性，低成本，低復(fù)雜度等特點(diǎn)。

在PDR算法方面，文獻(xiàn)[5]的研究?jī)?nèi)容有計(jì)步算法、運(yùn)動(dòng)步長(zhǎng)估計(jì)[6]和運(yùn)動(dòng)方向估計(jì)[7]。文獻(xiàn)[8]中結(jié)合陀螺儀和磁場(chǎng)傳感器提出了基于智能手機(jī)的行人航位推算算法。由于它們均使用智能手機(jī)中的傳感器來實(shí)現(xiàn)定位，且手機(jī)內(nèi)置方向傳感器的數(shù)據(jù)容易產(chǎn)生方向偏差，加之PDR 算法容易產(chǎn)生漂移誤差等問題，一些研究團(tuán)隊(duì)將室內(nèi)特殊區(qū)域[9]（如拐角等地）設(shè)置為地標(biāo)，但在真實(shí)場(chǎng)景中的定位精度一般只是房間級(jí)（5 m 左右）。Li等[9]利用iBeacon 所在位置設(shè)定為地標(biāo)，通過iBeacon 地標(biāo)來矯正路徑，但常常出現(xiàn)過分矯正的情況。

基于以上PDR 容易產(chǎn)生方向誤差的問題，本文在粒子濾波算法[10]中為方向?qū)傩蕴砑臃较蚪瞧?。同時(shí)，利用iBeacon 作為地標(biāo)（landmark），對(duì)產(chǎn)生的漂移誤差進(jìn)行矯正，若地標(biāo)有反復(fù)矯正的現(xiàn)象產(chǎn)生，本文利用RSS（Received Signal Strength）設(shè)定進(jìn)出地標(biāo)矯正的閾值，使iBeacon地標(biāo)能有效的矯正漂移誤差。

2 總體框架

算法的整體框架如圖1 所示，在定位前，本文進(jìn)行數(shù)據(jù)采集與步長(zhǎng)步數(shù)的測(cè)量，然后進(jìn)入融合算法定位模塊，該模塊分為兩個(gè)部分：（1）基于粒子濾波的定位算法，該階段粒子濾波算法中的粒子加入方向角偏差彌補(bǔ)了PDR算法中容易產(chǎn)生方向誤差的問題。在粒子濾波定位算法中，經(jīng)歷粒子初始化與預(yù)測(cè)，方向誤差校正兩個(gè)過程，從而對(duì)行人的位置進(jìn)行估計(jì)。（2）條件地標(biāo)矯正算法，為了能夠控制行人航位推算中的漂移誤差，在該階段，本文利用iBeacon 作為地標(biāo)對(duì)行人位置進(jìn)一步矯正，并根據(jù)藍(lán)牙信號(hào)強(qiáng)度添加進(jìn)出地標(biāo)的條件，防止地標(biāo)過度矯正，最終配合融合算法持續(xù)估計(jì)行人位置。

圖1 算法的整體框架圖

3 定位預(yù)備工作

智能手機(jī)內(nèi)置傳感器精度不高，采集到的數(shù)據(jù)帶有大量噪聲，本文在采集數(shù)據(jù)后，對(duì)藍(lán)牙數(shù)據(jù)與加速度數(shù)據(jù)進(jìn)行卡爾曼濾波[11]處理，如圖2所示，經(jīng)過處理過后的數(shù)據(jù)有效的抑制了噪聲并且波形得到了平滑，既保留了原始的信號(hào)特征，又減少了信號(hào)的劇烈波動(dòng)。

圖2 經(jīng)過卡爾曼濾波后的加速度與藍(lán)牙數(shù)據(jù)

經(jīng)過處理后的加速度數(shù)據(jù)被用來估計(jì)步數(shù)與步長(zhǎng)，本文利用差分有限狀態(tài)機(jī)[12]（Finite-State Machine，F(xiàn)SM）估計(jì)步數(shù)，利用斯立卡特步長(zhǎng)估計(jì)法[13]估計(jì)步長(zhǎng)。

4 基于粒子濾波的方向校正

在本文的粒子濾波算法中，每個(gè)粒子將獲得方向?qū)傩圆⑻砑臃较蚪瞧?，獲取初始位置后進(jìn)行粒子初始化，利用PDR 算法模型對(duì)每個(gè)粒子進(jìn)行傳播估計(jì)。在方向矯正階段，首先結(jié)合室內(nèi)地圖數(shù)據(jù)進(jìn)行權(quán)重更新，根據(jù)粒子的數(shù)量進(jìn)行重采樣，在重采樣的過程中方向角偏差會(huì)被留存下來，對(duì)方向進(jìn)行矯正，最后根據(jù)粒子狀態(tài)和歸一化結(jié)果進(jìn)行位置估計(jì)，算法流程如圖3所示。

4.1 粒子的初始化與粒子預(yù)測(cè)

在初始化模塊中，首先在已知起始位置處產(chǎn)生粒子，并將粒子的權(quán)重設(shè)置為（N為粒子總數(shù)），同時(shí)獲取運(yùn)動(dòng)方向，其中i表示粒子的編號(hào)，t表示時(shí)刻。

圖3 粒子濾波算法流程圖

在粒子的狀態(tài)轉(zhuǎn)移階段，結(jié)合PDR 算法可以得到基本的狀態(tài)轉(zhuǎn)移方程如公式（1），其中L為行人步長(zhǎng)，若檢測(cè)到計(jì)步，每個(gè)粒子的狀態(tài)發(fā)生改變且的狀態(tài)都是根據(jù)該粒子的上一個(gè)狀態(tài)而改變的。

4.2 方向誤差矯正

方向矯正分為權(quán)重更新和重采樣兩個(gè)階段，在粒子權(quán)重更新階段，本文結(jié)合室內(nèi)地圖數(shù)據(jù)進(jìn)行粒子的權(quán)重更新。如果粒子運(yùn)動(dòng)狀態(tài)超出可活動(dòng)范圍（即粒子撞墻），粒子的權(quán)重將被置零。

置零代表著粒子已經(jīng)消亡，此時(shí)粒子數(shù)量會(huì)有一定的減少，所以在權(quán)重更新后要對(duì)粒子的權(quán)重進(jìn)行歸一化處理，以便計(jì)算出用戶位置。歸一化處理如公式（3）：

粒子撞墻消失會(huì)使粒子數(shù)量減少，影響定位精度和狀態(tài)估計(jì)，因此該算法進(jìn)入粒子重采樣階段。根據(jù)目前存在的粒子來生成新的有效粒子，所以當(dāng)?shù)闹敌∮诹Ｗ涌倲?shù)的三分之二[14]時(shí)需要進(jìn)行粒子重采樣，如公式（4）：

重采樣階段的主要目的是對(duì)粒子的方向?qū)傩赃M(jìn)行計(jì)算，方向?qū)傩缘谋磉_(dá)如公式（5）所示，其中為粒子的方向?qū)傩裕萾為傳感器數(shù)據(jù)，θbias為實(shí)際方向與傳感器方向之間的偏差，為服從零均值分布的高斯噪聲。在粒子初始化過程中θbias為零，在重采樣之后存留下來的粒子方向噪聲會(huì)積累下來更新θbias。

因此在每次重采樣的時(shí)候都會(huì)把存活粒子中的方向噪聲誤差均值，保存到重采樣后粒子的固定方向角偏差θbias上。其更新方程如公式（6）所示，其中，Ns表示存活的粒子數(shù)，ηθ是增益系數(shù)。一般情況下，因?yàn)榉较蛏系脑肼曆a(bǔ)償是服從零均值的高斯分布，所以θbias的值會(huì)接近于零。如果運(yùn)動(dòng)方向與傳感器方向產(chǎn)生一定角度誤差時(shí)，方向噪聲接近角度偏差的粒子會(huì)繼續(xù)留存下來，其他粒子相繼超出活動(dòng)范圍導(dǎo)致粒子消失，此時(shí)θbias的值將不會(huì)接近零。

經(jīng)過以上兩個(gè)階段后，結(jié)合所有粒子狀態(tài)和歸一化結(jié)果可以計(jì)算出當(dāng)前目標(biāo)的位置信息，如公式（7）：

從圖4中可以看出，在粒子初始化階段添加高斯噪聲粒子間會(huì)產(chǎn)生一定的差異性，并且隨著行人的不停運(yùn)動(dòng)，粒子發(fā)生了擴(kuò)散，所以結(jié)合室內(nèi)地圖數(shù)據(jù)可以得到比較準(zhǔn)確的方向。

圖4 粒子濾波方向矯正示意圖

由圖5 可以看出當(dāng)粒子數(shù)量大于300 時(shí)，定位誤差和定位成功率趨于平穩(wěn)，若粒子數(shù)量小于100時(shí)定位誤差大且定位成功率較低，因此本文結(jié)合算法運(yùn)行時(shí)間與定位誤差將粒子數(shù)量設(shè)置為300。

圖5 粒子數(shù)量對(duì)粒子濾波定位效果的影響

5 條件地標(biāo)矯正

雖然粒子濾波對(duì)用戶行走方向進(jìn)行了矯正，但是在位置估計(jì)的時(shí)候依然會(huì)產(chǎn)生漂移誤差，所以本文利用地標(biāo)對(duì)行人位置進(jìn)行矯正，地標(biāo)一般在室內(nèi)作為輔助定位的工具。矯正過程為（如圖6所示）：當(dāng)行人走到地標(biāo)區(qū)域時(shí)，該地標(biāo)將會(huì)糾正之前一段路的累積誤差。不過在實(shí)際情況中，藍(lán)牙信號(hào)的波動(dòng)導(dǎo)致對(duì)于識(shí)別范圍沒有一個(gè)清晰的界限，因此會(huì)出現(xiàn)還未到地標(biāo)中心點(diǎn)或者已經(jīng)離開地標(biāo)中心點(diǎn)，地標(biāo)將強(qiáng)制矯正的情況，從而產(chǎn)生一定的矯正誤差，因此本文進(jìn)一步利用iBeacon 的RSS 設(shè)定進(jìn)出地標(biāo)的條件，使iBeacon地標(biāo)進(jìn)行合理的矯正。

圖6 地標(biāo)矯正示意圖

條件地標(biāo)矯正的過程為：先對(duì)藍(lán)牙信號(hào)進(jìn)行卡爾曼濾波，通過多項(xiàng)式回歸模型擬合出信號(hào)強(qiáng)度與用戶之間的距離關(guān)系，從而設(shè)定出合理的進(jìn)出地標(biāo)的信號(hào)閾值，最后對(duì)行人位置進(jìn)行矯正，流程框圖如圖7所示。

圖7 條件地標(biāo)矯正流程圖

5.1 矯正條件生成

本文利用多項(xiàng)式回歸模型[15]（PRM）計(jì)算出矯正的恰當(dāng)條件。對(duì)于信號(hào)傳播模型而言，由于在室內(nèi)環(huán)境中，藍(lán)牙信號(hào)易受到遮擋，多徑效應(yīng)等的影響，因此信號(hào)傳播模型不適合來估計(jì)信號(hào)與距離的關(guān)系。PRM模型將RSS 和距離的關(guān)系假設(shè)為一個(gè)n階多項(xiàng)式，通過采集和訓(xùn)練藍(lán)牙數(shù)據(jù)擬合出多項(xiàng)式參數(shù)，PRM 模型如公式（8）：

其中ci是n階多項(xiàng)式的系數(shù)，RSSi是藍(lán)牙信號(hào)強(qiáng)度，dPRM表示對(duì)應(yīng)的估計(jì)距離。此時(shí)多項(xiàng)式模型擬合誤差平方和如公式（9）：

其中M表示采樣點(diǎn)個(gè)數(shù)，dj表示第j個(gè)采樣點(diǎn)與iBeacon設(shè)備之間的真實(shí)際距離，表示第j個(gè)采樣點(diǎn)處的RSS。本文利用最小二乘法求最優(yōu)解，即求E的最小值，所以對(duì)公式（9）求偏導(dǎo)，如公式（10）：

公式（10）可以簡(jiǎn)化成：

其中A矩陣內(nèi)的元素為：（i的取值為 1 到n，B、C矩陣同理），B矩陣內(nèi)的元素為：C矩陣內(nèi)的元素為：ci，利用公式（11）求得多項(xiàng)式系數(shù)矩陣C，經(jīng)過以上步驟可以得出多項(xiàng)式回歸模型公式（12），另外，本文使用的iBeacon 都是統(tǒng)一品牌統(tǒng)一參數(shù)，所以僅需要一個(gè)PRM模型。

如圖8 所示是使用多項(xiàng)式回歸模型擬合出了RSS與距離間的關(guān)系圖經(jīng)過PRM 模型計(jì)算，將RSS 值轉(zhuǎn)換成距離，得到iBeacon 的矯正條件：進(jìn)入矯正范圍的信號(hào)強(qiáng)度值?52 dBm，離開矯正范圍的信號(hào)強(qiáng)度值?64 dBm。所以當(dāng)用戶走到iBeacon 附近時(shí)，藍(lán)牙信號(hào)進(jìn)入矯正范圍之后，開始位置矯正，離開矯正范圍時(shí)停止矯正。

圖8 RSS與距離的關(guān)系圖

5.2 位置矯正過程

輸入數(shù)據(jù)為藍(lán)牙數(shù)據(jù)（bluetooth）和方向傳感器數(shù)據(jù)（orientation），輸出數(shù)據(jù)為矯正后的位置。首先，算法遍歷數(shù)據(jù)獲取用戶當(dāng)前在哪一區(qū)域，即查找狀態(tài)為IN的對(duì)應(yīng)地標(biāo)。若用戶接收到來自對(duì)應(yīng)iBeacon的RSS值小于?64 dBm，則表示用戶已經(jīng)離開矯正區(qū)域；若搜索到 iBeacon 的 RSS 值大于?52 dBm 且 iBeacon 的狀態(tài)為OUT，此時(shí)進(jìn)行位置矯正，修改對(duì)應(yīng)iBeacon 的狀態(tài)為IN，隨即計(jì)算行人位置。如果用戶此時(shí)繼續(xù)在地標(biāo)區(qū)域內(nèi)，并不會(huì)觸發(fā)該算法去矯正，而使用粒子濾波算法繼續(xù)為用戶提供位置估計(jì)，算法中進(jìn)入地標(biāo)區(qū)域的觸發(fā)條件是RSS 大于?52 dBm。判斷用戶離開當(dāng)前iBeacon的信號(hào)范圍要選一個(gè)較小的RSS 值（本文選取的是?64 dBm），若RSS值較大，則導(dǎo)致該算法會(huì)頻繁觸發(fā)地標(biāo)矯正，引起矯正誤差，該算法的偽代碼如下。

偽代碼：條件地標(biāo)矯正算法

6. iBeacon_in=which iBeacon state is IN

7. if state of iBeacon_in is IN and RSS of iBeacon

8. change the stateofiBeacon_in= OUT

9. iBeacon_max = which iBeacon has max RSS

10. if iBeacon_max is OUT and max RSS of iBeacon>?52：

11. distance=map max RSS of iBeacon to distance

12.X=iBeacon_max_postion_x-distance*sin（orientation）

13.Y=iBeacon_max_postion_y-distance*cos（orientation）

14. change the stateofiBeacon_max=IN

15. return positionX，Y

16. End Loop

由圖9（a）可以看出，利用普通的地標(biāo)矯正，會(huì)使位置結(jié)果聚集在地標(biāo)附近，由于重復(fù)矯正所致；而從圖9（b）可以看出，加入條件地標(biāo)后的位置結(jié)果則不存在重復(fù)矯正的問題。

圖9 普通地標(biāo)矯正與條件地標(biāo)矯正對(duì)比圖

6 實(shí)驗(yàn)分析

6.1 實(shí)驗(yàn)環(huán)境及結(jié)果

本文的實(shí)驗(yàn)環(huán)境是學(xué)院B座的走道內(nèi)，如圖10（a），走道寬2.4 m（走道加休息區(qū)寬6.4 m），長(zhǎng)30 m。綠色點(diǎn)為起點(diǎn)和終點(diǎn)，行走路線由西向東順時(shí)針行走，最后回到起點(diǎn)，墻壁上的藍(lán)色圓點(diǎn)是iBeacon設(shè)備的位置，為了不影響每個(gè)地標(biāo)點(diǎn)的矯正，本文每10 m 布置1 個(gè)iBeacon 設(shè)備，在休息區(qū)的iBeacon 則布置到窗臺(tái)中間，并將iBeacon 的發(fā)射頻率設(shè)置為100 ms，發(fā)射功率設(shè)置為0 dBm。采集數(shù)據(jù)的設(shè)備是Vivox9plus手機(jī)。

圖10 實(shí)驗(yàn)環(huán)境及結(jié)果

由圖10（a）可以看出，由于PDR算法在估計(jì)路徑的時(shí)候無法及時(shí)矯正自身的方向誤差，導(dǎo)致方向角誤差越來越大，最后與真實(shí)路徑相差甚遠(yuǎn)。因此，在粒子濾波算法中設(shè)置了方向角偏差，矯正了方向誤差，由圖10（a）中PDR+PF 路徑可知，經(jīng)過粒子濾波處理后的估計(jì)路徑有了很大的改善，方向誤差有了很好控制，但是在圖10（a）中紅色箭頭處卻有路徑漂移，本文通過引入條件地標(biāo)來進(jìn)一步矯正行人位置。加入條件地標(biāo)矯正后的估計(jì)路徑得到了進(jìn)一步的改善，如圖10（b）中PDR+PF+LandMark路徑所示，需要說明的是，條件地標(biāo)矯正類似于信號(hào)的“拖拽”，所以形成了一個(gè)“拖拽軌跡”，將路徑拉回到正常范圍內(nèi)，如圖10（b）紅色箭頭處所示。

6.2 誤差分析

由圖11 可以看出，條件地標(biāo)矯正可以在粒子濾波算法的基礎(chǔ)上更有效地控制誤差，粒子濾波與條件地標(biāo)的融合算法的平均誤差達(dá)到了1.05 m（如表1所示），且相對(duì)于PDR算法誤差收斂更快，效果更好。

圖11 三種算法的定位精度

表1 三種算法的定位誤差m

7 結(jié)論及展望

本文使用粒子濾波算法對(duì)PDR算法中方向誤差的矯正有較好的表現(xiàn)，同時(shí)，加入條件地標(biāo)后一定程度上消除了行人路徑的漂移誤差，并且使地標(biāo)矯正有了一定的合理性。通過實(shí)驗(yàn)分析，融合粒子濾波與條件地標(biāo)矯正的平均定位誤差達(dá)到1.05 m。

此外，本文使用的外部設(shè)備較少，iBeacon輕量方便價(jià)格低廉，智能手機(jī)普及率高。在未來的工作當(dāng)中，本文將普適性進(jìn)一步擴(kuò)大，并將重點(diǎn)轉(zhuǎn)移到多個(gè)房間穿梭，進(jìn)出門的狀態(tài)判斷，嘗試結(jié)合本文算法來估計(jì)上下樓的軌跡。