融合PDR/地磁指紋的室內定位算法

吳 怡， 李希勝,2

(1．北京科技大學 自動化學院，北京 100083; 2．北京市工業波譜成像工程技術研究中心，北京 100083)

0 引 言

行人航跡推算(pedestrian dead reckoning,PDR)技術具有定位自主性和連續性的優勢，但由于存在累計誤差，需要與其他定位技術配合使用。地磁定位技術利用天然磁信息，實現自主定位[1]。融合PDR與地磁指紋進行室內定位算法設計，一方面，地磁能夠有效修正PDR累計誤差；另一方面，可以通過PDR減少地磁的定位漂移。粒子濾波在PDR/地磁融合定位中應用普遍[2]，但存在權重退化和粒子貧化的問題。

本文改進螢火蟲算法，設計粒子濾波過程，通過實驗驗證優化粒子濾波的效果。

1 算法整體架構

融合PDR/地磁指紋的室內定位主要分為三部分工作：PDR結果估計，地磁指紋建立以及粒子濾波融合。流程框圖如圖1所示。PDR根據人體運動生理特征，利用慣性信息進行步態檢測，步長估計以及航向更新，根據當前的位置更新下一時刻行人位置[3～5]。磁圖信息采集選用路徑連續采集方式，勻速沿規劃通道行走，每跨一步標記一個地磁點，跨步之間的數據作為該地磁點指紋信息[6]，粒子濾波融合階段，PDR作為預測信息，地磁作為量測信息[7]，輸出定位結果。

圖1 算法流程框圖

2 螢火蟲自適應粒子濾波

2.1 自適應權重

粒子濾波中，粒子表示對當前狀態的假設，其權重表示發生的近似概率。本文提出自適應權重求解方法，以地磁信息和PDR約束兩個方面計算權重，根據兩種定位方式的特點設計自適應因子，得到最終的粒子權重系數。算法的整體計算流程如下：

1)計算地磁權重與PDR權重

粒子的權重是由量測值的后驗概率模型P(sk|zk)計算得來的，其中zk表示量測信息，sk表示狀態信息即粒子信息，由于實際應用中無法準確獲得P(sk|zk)，一般認為其服從高斯分布。計算地磁權重與PDR權重

(1)

(2)

psk=psk-1+lk-1f(ψk-1)

(3)

式中lk-1為第k-1步的步長，ψk-1為第k-1步的航向，f(ψk-1)為正余弦函數。

2)計算自適應因子

PDR定位過程中，航向發揮著至關重要的作用，可以根據航向變化情況設計PDR權重的自適應因子，求解公式如式(4)，其中，εk表示前k步的轉向次數

(4)

式中ρp,0為初始的PDR自適應因子，取值在0～1之間，初始航向具有較高的信任度，本文定義ρp,0=0.9，εmax為最終轉向次數，由具體實驗確定。

地磁定位中，可以由磁干擾強度決定地磁自適應因子的取值。地磁變化越大，磁干擾越大，地磁自適應因子取值應越小，具體計算如式(5)所示

(5)

得到自適應因子后，將地磁權重與PDR權重進行歸一化，公式如式(6)。歸一化后的權重乘以相應的自適應因子，得到粒子的最終權重，如式(7)

(6)

Wj=ρpWp,j+ρmWm,j

(7)

2.2 螢火蟲策略重采樣

螢火蟲算法是一種啟發式群智能尋優算法，目前已有的群智能算法優化粒子濾波大多存在時效性差的問題，為此，本文改進螢火蟲算法的亮度以及吸引度，平衡搜索能力和尋優速度。

螢火蟲的亮度表征粒子所處位置的優劣，粒子濾波中，粒子優劣由權重確定，因此，定義亮度為粒子的權重，如式(8)所示

Ij=Wj

(8)

螢火蟲的吸引度可視為粒子個體的尋優搜索范圍。邊緣粒子距離種群目標較遠，難以獲取有效亮度信息，易成為失效粒子。然而粒子濾波中，邊緣粒子是重采樣的主要修正對象，為此，本文對吸引度公式進行如下改進

Bj=B0e-NWj

(9)

式中B0為最大吸引度，N為粒子的樣本數目。權重越小，粒子搜索范圍越寬，越容易獲取距離較遠粒子的有效亮度信息。

為加快尋優進程，選用亮度最高粒子作為最優位置，得到位置更新公式

sj=sj+Bj(sbest-sj)+α×(rand-0.5)

(10)

式中sbest為亮度最高粒子位置，α為步長參數。

2.3 自適應粒子分布

螢火蟲算法策略的重采樣過程有效緩解了粒子貧化，然而隨著粒子濾波次數的增加，粒子逐漸聚集，量測約束失效，影響定位精度。為此，本文提出自適應粒子分布，定義差異度參數，判斷粒子聚集程度。粒子的聚集程度可以由粒子間距方差來表示，因此差異度參數定義為

(11)

sj=Sk-1+4lk×(rand-0.5)

(12)

螢火蟲自適應粒子濾波算法整體流程如下：

1)粒子初始化：采樣N個粒子，得到初始粒子集{(s0,j),j=1,2,3,…,N};

2)更新粒子狀態：sk,j=sk-1,j+lk-1f(ψk-1);

3)計算自適應權重，并進行歸一化;

4)計算有效粒子數Neff，設定有效粒子閾值為2N/3，判斷是否重采樣;

6)當χk

7)轉至步驟(2)，進行下一步求解，直至結束。

3 實驗結果與分析

3.1 地磁圖構建

為驗證本文算法的實際應用價值，以購物商場作為實驗環境。初始采樣點間隔為0.8 m，克里金插值[8]后間隔為0.2 m，繪制的地磁強度基準圖如圖2所示。

圖2 地磁強度基準圖

3.2 實驗結果與分析

為驗證算法的性能，除采用標準粒子濾波算法對比外，另外加入現有的群智能優化算法：螢火蟲優化粒子濾波算法作為參照，將螢火蟲算法整體嵌入粒子濾波中。三種算法定位結果如圖3(a)所示，累計誤差分布概率如圖3(b)所示。

圖3 定位結果對比

由圖3可以看出，標準粒子濾波算法由于粒子貧化的原因，結果偏離真實路徑。螢火蟲優化粒子濾波算法由于地磁擾動的存在，定位結果單點波動較大。而本文提出的算法，有效降低了累計誤差，緩解了單點漂移的情況。

如表1所示，本文提出的粒子濾波算法定位效果最佳，定位精度達到1.1 m，相比于現有的優化粒子濾波算法，降低了時間成本的同時，提高了定位精度，驗證了螢火蟲自適應粒子濾波的有效性。

表1 算法定位結果參數

4 結 論

本文研究了一種基于粒子濾波的融合PDR/地磁的室內定位算法，螢火蟲自適應粒子濾波算法。根據PDR與地磁定位的特點，設計自適應權重的求解方式，改進螢火蟲算法的亮度和吸引度，設計重采樣過程，根據粒子聚集程度定義差異度參數，進行自適應粒子分布。實驗結果表明：優化算法能夠有效改善濾波定位效果，達到1.1 m室內定位精度。