以粒子濾波為基礎的多信息融合室內定位方法設計詳解

0 引言

目前，國內外已有不少對室內定位的研究。FOXLIN E 等人提出基于慣性測量單元（Inertial Measurement Unit，IMU）的傳統導航機制，通過對加速度進行兩次積分得到行走距離，對陀螺儀積分得到航向變化值，結果得到較為精確的定位結果，然而其高精度性能的基礎是價格昂貴的IMU，在行人定位中并不能普及；LEVI R W和 JUDD T 提出了行人航跡推算（Pedestrian Dead Reckoning，PDR）算法來實現行人定位，該算法利用加速度信號探測用戶行走時的跨步，采用有關模型估計步長，利用從陀螺儀獲得的方向信息推算出行人的速度、位置和距離等信息，其算法簡單，比傳統慣性導航機制的定位精度更高。然而，隨著時間的增加，傳感器的誤差不斷積累，使得到達一定距離后PDR的定位精度急劇下降；謝宏偉 等人設計了一款基于穩定性增強粒子濾波磁場匹配算法的手機室內定位系統，該系統實現了在未知初始方向和位置的情況下的定位，并達到1～2 m的定位精度。但該算法實現復雜，計算速度慢。

基于上述原因，本文提出一種基于粒子濾波的多信息融合室內定位方法，通過融合PDR和地磁信息，實時動態補償PDR的定位誤差，實現高精度、低成本、低復雜度的PDR/地磁融合室內定位。

1 融合定位算法

PDR算法利用加速度計、陀螺儀和磁力計等慣性器件得到行人行走時的原始數據，原始數據經擴展卡爾曼濾波融合得到行人的步頻、步長以及方向信息。由于慣性器件誤差的積累導致定位誤差不斷增大，所以本文選用地磁來輔助PDR實現室內定位，原理框圖如圖1所示。

地磁定位包含3個方面：一是導航區域地磁數據庫的建立；二是載體上磁力計的實時測量；三是地磁匹配。首先把載體經過區域中某些點的地磁場特征量繪制成基準圖保存下來，當載體穿越這些區域時，將地磁的相似程度作為基準進行匹配，本文選取粒子濾波算法來完成相似度的匹配。

2 PDR定位原理

PDR技術基本思想是通過載體攜帶的加速度傳感器及方向傳感器獲得單位時間內載體的位移及方向，從而可以根據上一時刻載體的位置和方向推算出載體在當前時刻的位置和方向。若已知初始位置坐標為（x0，y0），則第k步的位置坐標為：

2.1 步態檢測

跨步探測方法有零點探測、峰值探測、自相關、步相探測、零速修正等，具體采取哪種方法與加速度安裝位置有關 。本文以正常持手機方式采集數據，設計一種基于加速度信號的滑動窗口+峰值探測的步頻探測算法。

步頻探測算法的結果如圖2所示，行走過程中的步態基本都被標注出來，步態判別準確率接近100%。

2.2 步長估計

PDR算法中步長估計方法主要有兩種：一是基于慣性導航原理對加速度二次積分；二是利用人員行進特性對其特定動作下的行進距離進行建模 ?；诓介L模型的方法大多使用低成本慣性器件實現步長估計。

本文PDR算法用建模的方法來確定行人的步長。假設在行走過程中加速度的變化與步長成比例，在一個周期內，第k步的步長lk可以通過下式得到：

其中，app表示加速度峰值與谷值的差，β是比例因子，定位之前將得到的訓練數據經最小二乘法擬合得到。

為了檢測步長估計算法的性能，進行8次實驗來驗證該算法。每次測試者行走的總長度是20 m，表1是8組數據的解算距離及誤差，表明該步長估計算法的精度在短距離內可以達到1 m。

2.3 方向估計

對于行人行走時方向角測量，普遍采用低成本陀螺儀、加速度計和磁力計等實現。陀螺儀的零點存在溫度漂移，加速度計會受到載體震動的影響，磁力計會被外部磁場干擾。因此，為了得到可靠、準確的方向角，必須利用濾波器融合不同傳感器的數據、濾除不必要的干擾 。本文選取擴展卡爾曼濾波器實現融合算法。

圖3是由擴展卡爾曼濾波器得到的行人方向角，由于誤差的存在，由擴展卡爾曼濾波得到的方向角與參考方向還是存在一定的偏差，最大誤差接近30°。

3 地磁定位

地磁定位的關鍵是地磁匹配算法，地磁匹配是指利用地磁圖進行導航定位的過程。首先把載體經過區域中某些點的地磁場特征量繪制成地磁匹配參考圖或稱基準圖保存在載體計算機中，當載體穿越這些區域時，將地磁的相似程度作為基準進行匹配，認為最佳匹配點就是行人的位置 。

3.1 地磁基準數據庫的建立

克里金插值法是一種最優、線性和無偏的估計方法，是應用最廣泛的空間內插方法之一，能最大程度地利用所給的信息，具有很高的插值精度 。圖4是經克里金插值后的室內地磁基準圖。

3.2 粒子濾波

粒子濾波是一種使用有限數量粒子來表示狀態的后驗分布的非參數化濾波算法。在粒子濾波算法中，狀態的后驗分布采樣點被稱為粒子，每個粒子都表示對當前狀態的假設，通過粒子的權重來衡量該粒子所表示狀態的可能性大小，如果一個區域內的粒子非常密集，表示實際狀態在該區域的概率非常大 。

粒子濾波主要由狀態方程、觀測方程以及重采樣等部分組成。本文的狀態方程如式（3）所示：

式中，n表示觀測值z的維度，本文用磁場向量的模值作為觀測值，所以n=1；V是協方差；obss表示由地磁匹配得到的當前位置對應的地磁值；z表示當前位置地磁的觀測值。

PDR算法可以計算行人的步長，然而，由于慣性器件的累積誤差使得步長的誤差較大。本文通過設定步長初始值并由粒子權重不斷修正的方式來得到動態步長。正常人行走的步長范圍為0.5 m~0.7 m，假設步長的初始值l0=0.6 m，則步長計算如式（5）所示：

3.3 重采樣

在粒子濾波中存在粒子退化的問題，粒子權值的方差隨著時間增大，狀態空間中的有效粒子數較少，隨著無效采樣粒子數目的增加，使得大量的計算浪費在對估計后驗濾波概率分布幾乎不起作用的粒子上，使得估計性能下降，粒子群逐漸發散 。

重采樣方法可以有效解決粒子退化問題 。當有效粒子數小于某一閾值時，便對粒子進行重采樣。重采樣的思想就是舍棄權重小的粒子，將權重大的粒子復制幾次得到新粒子，用這些新粒子代替舍棄掉的粒子，至于每個粒子復制幾次完全由粒子的權重決定。

4 實驗結果及分析

為了驗證本文提出的基于粒子濾波的多信息融合室內定位算法的可行性，在室內環境進行了實驗。實驗場地為大學圖書館四樓413實驗室。為了提高磁場定位的精度，采用克里金插值算法對采集到的地磁數據進行0.5 m間隔的空間插值。設定初始位置已知，實驗者手持手機在實驗室沿指定路徑行走一周。在行走的過程中，手機以20 Hz的頻率采集并保存加速度計、陀螺儀以及磁力計的原始數據，最終由MATLAB軟件仿真出結果。

本文對兩種定位算法的結果進行對比：第一種是基于擴展卡爾曼濾波的純PDR定位算法，第二種是基于粒子濾波的多信息融合室內定位算法。圖5是由兩種算法得到的MATLAB仿真圖，由圖5看出基于粒子濾波的融合室內定位算法的性能明顯優于PDR算法。由于慣性器件的精度不高，隨著時間的增加由擴展卡爾曼濾波器得到的方向角誤差不斷增大，導致由PDR得到的行人軌跡嚴重偏離實際軌跡。由于粒子濾波算法能夠不斷地修正步長和方向角，這就解決了PDR中誤差不斷積累的問題。所以，隨著行走距離的增加，基于粒子濾波的融合室內定位算法的精度能夠保持穩定。

本文比較兩條仿真軌跡與參考軌跡的坐標數據，得到兩種算法的誤差，如表2所示。

由表2可知，本文提出的算法與PDR算法相比，平均誤差減少了70.5%，均方誤差減少了70.8%。實驗結果表明，本文提出的基于粒子濾波的多信息融合室內定位算法可以解決定位誤差累積的問題，有效地提高了定位精度。

5 結論

在室內定位技術中，針對傳統PDR短距離精度高、長距離累積誤差嚴重和地磁匹配容易受外界干擾磁場的影響等問題，本文提出一種基于粒子濾波的多信息融合室內定位算法，該算法通過動態補償步長和方向角誤差有效地抑制了定位結果的發散。實驗結果表明，該算法的精度能達到1 m~2 m，驗證了該算法的有效性和可行性。