基于PDR 和藍牙加權定位的卡爾曼濾波算法

何夢園，劉小瑜，劉正熙，熊運余，袁鈺涵

（四川大學計算機學院，成都610065）

0 引言

隨著基位置服務（LBS）[1]的興起，對于室內定位的需求就越來越強烈，例如商場、醫院、停車場等。各學者對各種無線信號定位進行了各方面的研究，包括無線局域網（WLAN）和無線傳感器網絡。例如紅外線[2]、超寬帶（UWB）[3]、ZigBee[4]、藍牙[5]、Wi-Fi[6]等室內定位方法。

但是由于室內環境復雜，單一信號定位精度較低，所以就需要將多種定位技術結合使用。文獻[7]將GPS、RFID、Wi-Fi 和計步器 4 種定位融合，提高了定位精度與穩定性；文獻[8]利用PDR 與UWB 定位技術結合，采用約束濾波器，使得定位精度達到亞米級；文獻[9]采用PDR 與Wi-Fi 的UKF 算法，對室內車輛定位取得較好的效果；在以上方法中，往往需要額外的硬件部署，系統實現復雜。而如今的智能手機性能越來越好，其提供了一系列傳感器以及可用于接受信號的Wi-Fi 模塊、藍牙模塊等，可直接用于室內定位。文獻[10]利用手機中的藍牙、Wi-Fi 和慣性導航結果進行融合濾波，可得到較好的定位結果。基于移動端慣性傳感器的PDR 算法具有短時間內定位精度高，但存在累計誤差，而藍牙定位單點定位誤差大但沒有誤差積累，因此將這兩種定位手段結合起來，構成PDR-ibeacon組合定位系統，既能達到較好的定位精度又能提高定位結果的可靠性。

本文提出一種基于位置加權的PDR-ibeacon 卡爾曼融合濾波室內定位方法，并使用智能手機實現。

1 藍牙指紋定位

1.1 藍牙指紋采集

藍牙室內定位技術可分為兩種：第一種是基于信號強度的傳播模型測定法，通過接受信號強度與信號衰減模型來計算待定位點與參考點的距離，但是由于室內環境復雜，存在多徑效應，魯棒性較差；第二種是通過建立藍牙指紋庫來進行匹配，但該方法需要采集指紋數據，數據庫維護壓力較大。目前，室內藍牙定位為了較高精度的定位，主要基于位置指紋實現。

本文采用位置指紋的方式實現，但是為了減少施工成本，不用常用的單點指紋庫收集再進行處理的方式采集指紋。為減少采集指紋時間，使用時間-位置匹配的方式采集指紋。同時為減少只采集一次的指紋的隨機性的影響，設置置信點指紋，即當收到某個藍牙標簽的信號值大于某個值時，我們判斷其在藍牙標簽附近，置信點由在藍牙標簽下收到的該藍牙信號強度決定，經測試在藍牙標簽1 米范圍內的1 分鐘內收到的60 次藍牙信號值一般在-60db～-70db，根據其均值確定置信點指紋數據，例如P={( )x,y ,“rssi:-60db,device:1”}代表在藍牙標簽ID 為1 處的置信指紋。

圖1 1米范圍內藍牙信號強度分布圖

采集指紋時，采集示意圖如下圖所示，手持手機勻速行走，根據長度S 和時間T 計算出其速度V=S/T ，根據V 和起點估算出每次收到藍牙信號時所在位置經緯度，形成一條藍牙指紋l1={（x,y）,{“rssi:signal intensity,device:id”,…,“rssi:-mdb,device:id”}}，指紋庫由多條指紋組成L={l1,l2,…,ln},其中n 為采集的指紋個數，即收到的藍牙信號次數。

圖2 指紋采集示意圖

本文使用iPhone 智能手機進行數據采集，因為在Android 平臺的智能手機與iPhone 智能手機接受ibeacon 藍牙信號存在差異，在相同時間下，iPhone 在60 秒內收到的藍牙信號穩定在60 次左右，而Android 平臺手機的結果則較為不穩定，實驗結果如圖2 所示，對13個藍牙標簽分別進行實驗，Android 手機60 秒內最多接受到43 次藍牙信號，將會導致藍牙標簽分布不均。（其中iPhone 手機型號為iPhone 6s，Android 手機型號為一加3T。）

圖3 不同平臺下的藍牙接收頻率

1.2 藍牙指紋匹配算法

（1）WKNN 匹配算法

離線采集完指紋后，定位時需要將收到的藍牙信號與指紋庫進行匹配，才能得到定位結果。最常用的匹配算法為加權 K 最近鄰法（Weighted K-Nearest Neighbor，WKNN），定位時首先掃描藍牙信號，得到信號 待 定 位 點 信 號 強 度 向 量 Signal_V，Signal_V=(v1,v2,…,vj,…,vn)，其中 vj(1 ≤j ≤n)代表收到的第j 個藍牙標簽的信號值，n 為收到的藍牙標簽總個數。然后根據收到的藍牙信號強度與指紋庫中的藍牙信號強度進行匹配，計算其相似度SI，一般使用歐氏距離計算。在實際環境中，由于范圍較大，藍牙指紋點較多，為提高匹配效率，在計算歐氏距離之前，首先檢索本條指紋是否含有Signal_V 中的藍牙標簽信息，如果沒有，則不計算。按照歐氏距離計算的公式如下：

式中，k 表示第k 個指紋點，lki表示第k 個指紋點中的第i 個指紋。取前SI 值最小的前k 個點對應的坐標{P1，P2，…，Pi，…，Pk}，使用前 k 個 SI 進行加權，加權公式如下：

最終的加權定位結果計算方式如下所示：

（2）SVM（支持向量機）匹配算法

在實驗中發現WKNN 會有跳變加大的情況發生，且較為頻繁而且定位會有不在道路上的情況發生。所以為減少此種情況的發生，本文使用基于SVM 的匹配算法，其分為兩步：①模型訓練；②在線分類。我們可以將定位問題轉化為分類問題，即將指紋點所在坐標的二維空間作為類別標記，訓練時找出區分不同指紋點的超平面，在線定位時根據藍牙信號值進行分類得到一個作為類別的坐標點。基于指紋點存在線性不可分的情況，所以引入核函數，使之高維可分，即：

式中K( xi,xj)=xTy 為引入的核函數，C 為懲罰因子表示對錯誤分類的懲罰程度，然后根據Smo 算法求得最優解 a′=(a1′,a2′,a3′,…,an′)T,并計算：

最終，求解出決策函數：

2 基于智能手機的PDR（行人航位推算）

一般情況下認為人在很短的時間內，做直線運動，當得知人的行走方向、距離以及起始點時就可推算出下一時刻的位置，我們假設起點為（x1,y1），距離為S，方向角為θ，那么推算下一個點（x2,y2）的公式為：

原理如圖4 所示：

其后的每一步定位信息都可用式（3）推算得到，其中兩個關鍵參數S 和θ 需要從傳感器中獲取，S 代表步長，θ 代表方向角，其中方向角可由智能手機傳感器直接獲得一個指北方向角，而步長可由典型的步頻-步長模型解算，步頻可由手機傳感器的計步器接口給出，步長可根據以下公式計算：

圖4 PDR原理示意圖[11]

其中a 和b 為固定常量，本文使用文獻[12]中的值，f 為步頻，可由手機中計步器得到步頻。

3 多信號位置加權的卡爾曼濾波算法

卡爾曼濾波的主要思想為獲取測量值對估計值進行修正，而基于PDR 的融合濾波一般把步長推算作為估計值，測量值往往是藍牙定位以及其他信號源定位，而在線定位中，無法在每次有步進時都收到藍牙定位信息，所以在卡爾曼濾波中無法使用關于步長的狀態方程，本文使用速度模型，為了較少計算量，本文采用最簡單的線性卡爾曼濾波。多信號位置加權的卡爾曼濾波算法流程圖如圖5 所示，算法流程分為3 步：

Step1：獲取多信號加權測量值，根據每一時刻的信號源定位不同進行不同的加權處理。

Step2：根據設計的狀態方程的測量方程進行濾波融合。

Step3：為使輸出點跡更為平滑，加入平滑處理，得到更為流暢的軌跡。

在Step1 中，在獲取所需測量值時，由于有多種定位信號輸出，本文提出多信號位置的加權處理，根據實際效果，設定各加權系數如圖5 所示，因為在短時間內的PDR 具有更高的精度，所以其權重較藍牙定位和置信點都大。

圖5 多信號位置加權的卡爾曼濾波算法流程圖

Step2 中，建立相關卡爾曼濾波方程，k-1 時刻到k時刻的狀態方程和測量方程如下：

其中A 為狀態轉移矩陣，B 和uk分別為系統輸入變量，ωk為過程噪聲，H 為測量矩陣，vk為測量噪聲。卡爾曼濾波分為兩部分，預測與更新，預測階段使用上一時刻的值對當前時刻作出估計，而更新階段，利用測量值對預測值進行校正，以獲得一個更接近真實值的新估計值。

預測：

更新：

其中zk為測量值，K 為卡爾曼增益，每次濾波時隨著計算更新，R 為測量噪聲的協方差矩陣，xk為最終卡爾曼估計值，Pk為卡爾曼估計誤差的協方差矩陣。根據實際情況建模如下：

(xk,yk)為k 時刻的位置坐標，(vxk,vyk)為k 時刻x和y 方向的速度，T 為時間間隔。測量方程為：

(xk,yk)為通過多信號位置加權得到的定位位置。

本文設初始協方差矩陣P=0.1*I ，I 為單位矩陣；由于沒有系統輸入，所以忽略系統輸入。

過程噪聲的協方差矩陣：

測量噪聲的協方差矩陣：

Step3 中，因為濾波輸出的點會有較多鋸齒狀以及回拉的點，如圖6（a）所示，所以在濾波融合后進行平滑處理，平滑算法示意圖如圖6（b）所示，我們根據前2 個時刻的運動狀態來進行平滑，假設在3 個運動時刻里，運動狀態不變，那么根據k-2 時刻和k-1 時刻可推算出k 時刻的位置為(x′k,y′

k)，而k 時刻的卡爾曼濾波結果為，將其進行加權得到平滑結果，計算公式如下：

其中w1+w2=1，權重系數應根據實際情況選取，本文將其都設為0.5。

圖6 平滑處理示意圖

4 實驗結果分析

實驗測試環境位于成都市高新區理想中心14 樓，平面圖如圖7 所示，主要在過道進行測試，地圖面積約為1500m2，藍牙標簽部署如圖上藍點所示，總計部署13 個，指紋點共計 170 個。

圖7 定位區域平面圖

4.1 WWKKNNNN與SSVVMM定位結果對比分析

在靜止狀態下，隨機挑選13 個點作為測試點，分別使用基于距離的WKNN 與SVM 進行定位，得出定位結果，使用平均誤差、累計誤差分布和最大誤差分布作為評價指標，其平均誤差如表1 所示。

表1 不同匹配算法平均誤差

從平均誤差可以看出SVM 較WKNN 具有更高的定位精度，累計誤差分布結果和點位最大誤差統計圖8所示，可以看出SVM 收斂較WKNN 快，且最大誤差由20 米降低到14 米，較WKNN 具有更穩定的定位效果。

圖8 誤差累計分布圖

4.2 卡爾曼融合濾波定位結果

在測試環境中，濾波效果如圖9 所示，紅色的線為濾波融合后的效果，可以看出其與真實路線較為符合，擬合較好，減少了PDR 的累計誤差以及藍牙定位的誤差影響。

在此行走過程中，根據速度估計其真實位置，各種定位平均誤差表2 所示，SVM_PDR_KF 融合濾波方法將行走狀態下的平均誤差降低到1.8 米，且最大誤差僅3.6 米，較SVM 和PDR 分別提高了81.98%和86.11%。

5 結語

在室內環境中，通過采集的藍牙指紋信息與SVM算法輔助PDR 進行了定位測驗。比較了WKNN 與SVM 匹配算法定位效果，結果表明，SVM 具有更高的精度與更好的穩定性。使用卡爾曼濾波進行信號融合，基于多信號的位置加權得到測量值，該方法定位結果優于藍牙和PDR 單獨定位的效果。此方法計算量小，實現簡單，可實用于智能手機定位。