融合粒子濾波與藍牙地標矯正的定位算法

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

1.江南大學 物聯網工程學院，江蘇 無錫 214122

2.江南大學 物聯網技術應用教育部工程研究中心，江蘇 無錫 214122

1 引言

隨著智能移動設備的普及，人們對于定位精度的要求也越來越高。在室外，GPS 可以獲取精確的位置信息，但在室內，由于信號的遮擋，GPS 無法進行精確定位，所以在室內獲取高精度定位結果具有很大的研究價值。

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

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

基于以上PDR 容易產生方向誤差的問題，本文在粒子濾波算法[10]中為方向屬性添加方向角偏差。同時，利用iBeacon 作為地標（landmark），對產生的漂移誤差進行矯正，若地標有反復矯正的現象產生，本文利用RSS（Received Signal Strength）設定進出地標矯正的閾值，使iBeacon地標能有效的矯正漂移誤差。

2 總體框架

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

圖1 算法的整體框架圖

3 定位預備工作

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

圖2 經過卡爾曼濾波后的加速度與藍牙數據

經過處理后的加速度數據被用來估計步數與步長，本文利用差分有限狀態機[12]（Finite-State Machine，FSM）估計步數，利用斯立卡特步長估計法[13]估計步長。

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

在本文的粒子濾波算法中，每個粒子將獲得方向屬性并添加方向角偏差，獲取初始位置后進行粒子初始化，利用PDR 算法模型對每個粒子進行傳播估計。在方向矯正階段，首先結合室內地圖數據進行權重更新，根據粒子的數量進行重采樣，在重采樣的過程中方向角偏差會被留存下來，對方向進行矯正，最后根據粒子狀態和歸一化結果進行位置估計，算法流程如圖3所示。

4.1 粒子的初始化與粒子預測

在初始化模塊中，首先在已知起始位置處產生粒子，并將粒子的權重設置為（N為粒子總數），同時獲取運動方向，其中i表示粒子的編號，t表示時刻。

圖3 粒子濾波算法流程圖

在粒子的狀態轉移階段，結合PDR 算法可以得到基本的狀態轉移方程如公式（1），其中L為行人步長，若檢測到計步，每個粒子的狀態發生改變且的狀態都是根據該粒子的上一個狀態而改變的。

4.2 方向誤差矯正

方向矯正分為權重更新和重采樣兩個階段，在粒子權重更新階段，本文結合室內地圖數據進行粒子的權重更新。如果粒子運動狀態超出可活動范圍（即粒子撞墻），粒子的權重將被置零。

置零代表著粒子已經消亡，此時粒子數量會有一定的減少，所以在權重更新后要對粒子的權重進行歸一化處理，以便計算出用戶位置。歸一化處理如公式（3）：

粒子撞墻消失會使粒子數量減少，影響定位精度和狀態估計，因此該算法進入粒子重采樣階段。根據目前存在的粒子來生成新的有效粒子，所以當的值小于粒子總數的三分之二[14]時需要進行粒子重采樣，如公式（4）：

重采樣階段的主要目的是對粒子的方向屬性進行計算，方向屬性的表達如公式（5）所示，其中為粒子的方向屬性，θt為傳感器數據，θbias為實際方向與傳感器方向之間的偏差，為服從零均值分布的高斯噪聲。在粒子初始化過程中θbias為零，在重采樣之后存留下來的粒子方向噪聲會積累下來更新θbias。

因此在每次重采樣的時候都會把存活粒子中的方向噪聲誤差均值，保存到重采樣后粒子的固定方向角偏差θbias上。其更新方程如公式（6）所示，其中，Ns表示存活的粒子數，ηθ是增益系數。一般情況下，因為方向上的噪聲補償是服從零均值的高斯分布，所以θbias的值會接近于零。如果運動方向與傳感器方向產生一定角度誤差時，方向噪聲接近角度偏差的粒子會繼續留存下來，其他粒子相繼超出活動范圍導致粒子消失，此時θbias的值將不會接近零。

經過以上兩個階段后，結合所有粒子狀態和歸一化結果可以計算出當前目標的位置信息，如公式（7）：

從圖4中可以看出，在粒子初始化階段添加高斯噪聲粒子間會產生一定的差異性，并且隨著行人的不停運動，粒子發生了擴散，所以結合室內地圖數據可以得到比較準確的方向。

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

由圖5 可以看出當粒子數量大于300 時，定位誤差和定位成功率趨于平穩，若粒子數量小于100時定位誤差大且定位成功率較低，因此本文結合算法運行時間與定位誤差將粒子數量設置為300。

圖5 粒子數量對粒子濾波定位效果的影響

5 條件地標矯正

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

圖6 地標矯正示意圖

條件地標矯正的過程為：先對藍牙信號進行卡爾曼濾波，通過多項式回歸模型擬合出信號強度與用戶之間的距離關系，從而設定出合理的進出地標的信號閾值，最后對行人位置進行矯正，流程框圖如圖7所示。

圖7 條件地標矯正流程圖

5.1 矯正條件生成

本文利用多項式回歸模型[15]（PRM）計算出矯正的恰當條件。對于信號傳播模型而言，由于在室內環境中，藍牙信號易受到遮擋，多徑效應等的影響，因此信號傳播模型不適合來估計信號與距離的關系。PRM模型將RSS 和距離的關系假設為一個n階多項式，通過采集和訓練藍牙數據擬合出多項式參數，PRM 模型如公式（8）：

其中ci是n階多項式的系數，RSSi是藍牙信號強度，dPRM表示對應的估計距離。此時多項式模型擬合誤差平方和如公式（9）：

其中M表示采樣點個數，dj表示第j個采樣點與iBeacon設備之間的真實際距離，表示第j個采樣點處的RSS。本文利用最小二乘法求最優解，即求E的最小值，所以對公式（9）求偏導，如公式（10）：

公式（10）可以簡化成：

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

如圖8 所示是使用多項式回歸模型擬合出了RSS與距離間的關系圖經過PRM 模型計算，將RSS 值轉換成距離，得到iBeacon 的矯正條件：進入矯正范圍的信號強度值?52 dBm，離開矯正范圍的信號強度值?64 dBm。所以當用戶走到iBeacon 附近時，藍牙信號進入矯正范圍之后，開始位置矯正，離開矯正范圍時停止矯正。

圖8 RSS與距離的關系圖

5.2 位置矯正過程

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

偽代碼：條件地標矯正算法

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）可以看出，利用普通的地標矯正，會使位置結果聚集在地標附近，由于重復矯正所致；而從圖9（b）可以看出，加入條件地標后的位置結果則不存在重復矯正的問題。

圖9 普通地標矯正與條件地標矯正對比圖

6 實驗分析

6.1 實驗環境及結果

本文的實驗環境是學院B座的走道內，如圖10（a），走道寬2.4 m（走道加休息區寬6.4 m），長30 m。綠色點為起點和終點，行走路線由西向東順時針行走，最后回到起點，墻壁上的藍色圓點是iBeacon設備的位置，為了不影響每個地標點的矯正，本文每10 m 布置1 個iBeacon 設備，在休息區的iBeacon 則布置到窗臺中間，并將iBeacon 的發射頻率設置為100 ms，發射功率設置為0 dBm。采集數據的設備是Vivox9plus手機。

圖10 實驗環境及結果

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

6.2 誤差分析

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

圖11 三種算法的定位精度

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

7 結論及展望

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

此外，本文使用的外部設備較少，iBeacon輕量方便價格低廉，智能手機普及率高。在未來的工作當中，本文將普適性進一步擴大，并將重點轉移到多個房間穿梭，進出門的狀態判斷，嘗試結合本文算法來估計上下樓的軌跡。