融合PDR/地磁指紋的室內(nèi)定位算法

吳 怡， 李希勝,2

(1．北京科技大學(xué) 自動(dòng)化學(xué)院，北京 100083; 2．北京市工業(yè)波譜成像工程技術(shù)研究中心，北京 100083)

0 引 言

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

本文改進(jìn)螢火蟲算法，設(shè)計(jì)粒子濾波過(guò)程，通過(guò)實(shí)驗(yàn)驗(yàn)證優(yōu)化粒子濾波的效果。

1 算法整體架構(gòu)

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

圖1 算法流程框圖

2 螢火蟲自適應(yīng)粒子濾波

2.1 自適應(yīng)權(quán)重

粒子濾波中，粒子表示對(duì)當(dāng)前狀態(tài)的假設(shè)，其權(quán)重表示發(fā)生的近似概率。本文提出自適應(yīng)權(quán)重求解方法，以地磁信息和PDR約束兩個(gè)方面計(jì)算權(quán)重，根據(jù)兩種定位方式的特點(diǎn)設(shè)計(jì)自適應(yīng)因子，得到最終的粒子權(quán)重系數(shù)。算法的整體計(jì)算流程如下：

1)計(jì)算地磁權(quán)重與PDR權(quán)重

粒子的權(quán)重是由量測(cè)值的后驗(yàn)概率模型P(sk|zk)計(jì)算得來(lái)的，其中zk表示量測(cè)信息，sk表示狀態(tài)信息即粒子信息，由于實(shí)際應(yīng)用中無(wú)法準(zhǔn)確獲得P(sk|zk)，一般認(rèn)為其服從高斯分布。計(jì)算地磁權(quán)重與PDR權(quán)重

(1)

(2)

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

(3)

式中l(wèi)k-1為第k-1步的步長(zhǎng)，ψk-1為第k-1步的航向，f(ψk-1)為正余弦函數(shù)。

2)計(jì)算自適應(yīng)因子

PDR定位過(guò)程中，航向發(fā)揮著至關(guān)重要的作用，可以根據(jù)航向變化情況設(shè)計(jì)PDR權(quán)重的自適應(yīng)因子，求解公式如式(4)，其中，εk表示前k步的轉(zhuǎn)向次數(shù)

(4)

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

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

(5)

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

(6)

Wj=ρpWp,j+ρmWm,j

(7)

2.2 螢火蟲策略重采樣

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

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

Ij=Wj

(8)

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

Bj=B0e-NWj

(9)

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

為加快尋優(yōu)進(jìn)程，選用亮度最高粒子作為最優(yōu)位置，得到位置更新公式

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

(10)

式中sbest為亮度最高粒子位置，α為步長(zhǎng)參數(shù)。

2.3 自適應(yīng)粒子分布

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

(11)

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

(12)

螢火蟲自適應(yīng)粒子濾波算法整體流程如下：

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

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

3)計(jì)算自適應(yīng)權(quán)重，并進(jìn)行歸一化;

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

6)當(dāng)χk

7)轉(zhuǎn)至步驟(2)，進(jìn)行下一步求解，直至結(jié)束。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 地磁圖構(gòu)建

為驗(yàn)證本文算法的實(shí)際應(yīng)用價(jià)值，以購(gòu)物商場(chǎng)作為實(shí)驗(yàn)環(huán)境。初始采樣點(diǎn)間隔為0.8 m，克里金插值[8]后間隔為0.2 m，繪制的地磁強(qiáng)度基準(zhǔn)圖如圖2所示。

圖2 地磁強(qiáng)度基準(zhǔn)圖

3.2 實(shí)驗(yàn)結(jié)果與分析

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

圖3 定位結(jié)果對(duì)比

由圖3可以看出，標(biāo)準(zhǔn)粒子濾波算法由于粒子貧化的原因，結(jié)果偏離真實(shí)路徑。螢火蟲優(yōu)化粒子濾波算法由于地磁擾動(dòng)的存在，定位結(jié)果單點(diǎn)波動(dòng)較大。而本文提出的算法，有效降低了累計(jì)誤差，緩解了單點(diǎn)漂移的情況。

如表1所示，本文提出的粒子濾波算法定位效果最佳，定位精度達(dá)到1.1 m，相比于現(xiàn)有的優(yōu)化粒子濾波算法，降低了時(shí)間成本的同時(shí)，提高了定位精度，驗(yàn)證了螢火蟲自適應(yīng)粒子濾波的有效性。

表1 算法定位結(jié)果參數(shù)

4 結(jié) 論

本文研究了一種基于粒子濾波的融合PDR/地磁的室內(nèi)定位算法，螢火蟲自適應(yīng)粒子濾波算法。根據(jù)PDR與地磁定位的特點(diǎn)，設(shè)計(jì)自適應(yīng)權(quán)重的求解方式，改進(jìn)螢火蟲算法的亮度和吸引度，設(shè)計(jì)重采樣過(guò)程，根據(jù)粒子聚集程度定義差異度參數(shù)，進(jìn)行自適應(yīng)粒子分布。實(shí)驗(yàn)結(jié)果表明：優(yōu)化算法能夠有效改善濾波定位效果，達(dá)到1.1 m室內(nèi)定位精度。