基于粒子濾波的PDR定位算法*

鐘亞洲， 吳 飛， 任師濤

(上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620)

0 引 言

室內(nèi)復(fù)雜多變環(huán)境下，利用信號(hào)強(qiáng)度進(jìn)行定位時(shí)，易致電磁波信號(hào)產(chǎn)生衰減、多徑和干擾[1]?；谥讣y數(shù)據(jù)庫(kù)的無線室內(nèi)定位方法需要在目標(biāo)環(huán)境中選取若干參考點(diǎn)進(jìn)行采樣并通過指紋數(shù)據(jù)庫(kù)作為目標(biāo)環(huán)境特征進(jìn)行位置估計(jì)。無線接入點(diǎn)的性能和分布決定了定位的精度，如果想要在大區(qū)域級(jí)進(jìn)行人員定位，大規(guī)?；A(chǔ)設(shè)施建設(shè)導(dǎo)致基于指紋數(shù)據(jù)庫(kù)的無線室內(nèi)定位方案優(yōu)勢(shì)并不明顯。而智能手機(jī)感知環(huán)境的能力已經(jīng)非常強(qiáng)大。因此，基于行人航跡推算(pedestrian dead reckoning,PDR)的定位和導(dǎo)航方式將帶來新的機(jī)遇[2～4]。文獻(xiàn)[3]提出了利用加速度和角速度建立行人運(yùn)動(dòng)模型，采用足綁式(Foot-mounted)方法和擴(kuò)展卡爾曼濾波對(duì)導(dǎo)航參數(shù)誤差進(jìn)行估計(jì)和補(bǔ)償；文獻(xiàn)[4]為了提高人員動(dòng)作分類的準(zhǔn)確率，提出了利用慣性數(shù)據(jù)特征，通過支持向量機(jī)理論構(gòu)建行進(jìn)動(dòng)作分類器；文獻(xiàn)[5]針對(duì)人體三維坐標(biāo)精準(zhǔn)定位，提出了三維復(fù)雜運(yùn)動(dòng)模式航跡推算算法，通過有效跨步求解人體位置的三維坐標(biāo)。但以上文獻(xiàn)均未利用環(huán)境信息來提高定位精度。

本文設(shè)計(jì)了粒子濾波(particle filtering,PF)器并且融合室內(nèi)平面地圖和PDR信息，提出一種基于PF的PDR定位算法，提高了室內(nèi)環(huán)境下用戶定位精度。

1 基于PF的室內(nèi)定位方法

系統(tǒng)整體框架如圖1所示，首先使用手機(jī)內(nèi)置的加速度計(jì)進(jìn)行步伐檢測(cè)和計(jì)步，陀螺儀和磁力計(jì)進(jìn)行方向估計(jì);然后結(jié)合地圖匹配信息，使用PF算法對(duì)得到的定位數(shù)據(jù)進(jìn)行濾波融合。

圖1 基于地圖匹配的室內(nèi)定位流程

行人行走一步時(shí)，PDR算法需進(jìn)行方向和步長(zhǎng)估計(jì)。根據(jù)行人初始位置，可以推算出之后時(shí)刻行人的位置[6]。設(shè)坐標(biāo)系為東北坐標(biāo)系，行人位置更新為

(1)

式中xk為第k步時(shí)行人的東西坐標(biāo)；yk為第k步時(shí)行人的南北坐標(biāo)；SLk為第k步時(shí)的步長(zhǎng)；θk為第k步時(shí)相對(duì)于北向的夾角。

2 PDR算法

2.1 步伐檢測(cè)算法

行人實(shí)際的室內(nèi)運(yùn)動(dòng)中一般包含著多種運(yùn)動(dòng)模式，通過PDR，可以將行人的行走運(yùn)動(dòng)模式建立為一個(gè)三維運(yùn)動(dòng)模型[7]。PDR需要步伐和頻率檢測(cè)，如果步伐檢測(cè)出現(xiàn)錯(cuò)誤或者丟失步伐數(shù)值，將導(dǎo)致步行距離與實(shí)際行走的長(zhǎng)度偏差較大。本文選用零點(diǎn)交叉法來檢測(cè)步伐。

使用加速度傳感器進(jìn)行步伐檢測(cè)和計(jì)步時(shí)，會(huì)產(chǎn)生一些測(cè)量噪聲[8],將會(huì)導(dǎo)致步伐檢測(cè)和計(jì)步出現(xiàn)錯(cuò)誤，影響步長(zhǎng)估計(jì)的精度。用高通濾波器濾除重力加速度，即

aavg=a×0.1+aavg×0.9,ah=a-aavg

(2)

式中a,aavg,ah分別為加速度測(cè)量值、均值和經(jīng)高通濾波后的值。

輸出的過零點(diǎn)加速度計(jì)檢測(cè)使用此信號(hào)。步伐檢測(cè)算法的結(jié)果如圖2所示。

圖2 加速度值零點(diǎn)交叉法

2.2 步長(zhǎng)估計(jì)算法

由于步長(zhǎng)受線性模式的影響[9]，使用式(3)～式(5)確定行走頻率與加速度方差

SL=α·WF+β·AV+γ

(3)

WF=1/(tk-tk-1)

(4)

(5)

式中α，β和γ參數(shù)根據(jù)環(huán)境進(jìn)行設(shè)定；WF為行走頻率；AV為加速度方差。通過式(6)計(jì)算步行距離

(6)

3 PF融合

3.1 初始化

取粒子數(shù)為N=100，對(duì)用戶的行走方向進(jìn)行濾波。設(shè)定權(quán)值初始值為1/N，根據(jù)系統(tǒng)設(shè)定可知粒子的權(quán)值之和為1，每個(gè)粒子代表用戶的一個(gè)可能的運(yùn)動(dòng)狀態(tài)，即用戶的一個(gè)可能的位置。

3.2 粒子狀態(tài)轉(zhuǎn)移

通過粒子狀態(tài)不斷轉(zhuǎn)移，用戶的位置隨著時(shí)間的推移不斷更新。PF算法需要考慮到行人運(yùn)動(dòng)過程中的誤差控制， PDR算法中通過矢量疊加，不同的粒子權(quán)重在運(yùn)動(dòng)中將產(chǎn)生不同的路徑[10]。隨著時(shí)間和位置距離的累加，會(huì)導(dǎo)致位置信息的累計(jì)偏差增大。因此，將粒子權(quán)重w作為路徑航向平滑因子

(7)

式中θt為t時(shí)刻行人位置信息中方向信息,為引入平滑因子后第i個(gè)粒子代表的行走方向。圖3為粒子平滑示意。

圖3 粒子平滑示意

3.3 粒子更新

PF算法通過不斷更新粒子的位置和權(quán)重值來逼近系統(tǒng)的真實(shí)后驗(yàn)概率分布。權(quán)重值越大，系統(tǒng)在該狀態(tài)的可能性越大。通過運(yùn)動(dòng)模型，粒子不斷更新位置，同時(shí)，通過量測(cè)模型粒子更新權(quán)重值。PDR算法需要考慮定位實(shí)時(shí)性問題，所以PF算法的計(jì)算量需要滿足硬件的需求，在智能手機(jī)硬件的基礎(chǔ)上盡可能提高PF算法的實(shí)時(shí)性[11]。為降低PF算法的計(jì)算量，選擇地圖匹配的PF算法。主要思路是充分利用環(huán)境信息，將地圖信息導(dǎo)入PDR系統(tǒng)中，利用地圖中的障礙物來更新粒子權(quán)重。設(shè)定當(dāng)粒子遇到墻壁或者家居等障礙物時(shí)，粒子滅亡，粒子的權(quán)重變?yōu)榱?否則,粒子存活并更新粒子的位置和權(quán)重。

3.4 重采樣

重采樣能夠抑制退化現(xiàn)象[12],減少權(quán)值較小的粒子數(shù)目，對(duì)具有較大權(quán)值得粒子增加粒子數(shù)目，但會(huì)導(dǎo)致粒子多樣性降低，同時(shí)，增加粒子數(shù)目會(huì)導(dǎo)致PF算法的計(jì)算復(fù)雜度增加，從而消耗更多的計(jì)算時(shí)間。本文選擇權(quán)值選擇的PF算法[13]，通過選出權(quán)值較大的粒子來估計(jì)行人的運(yùn)動(dòng)位置和狀態(tài)。結(jié)合地圖匹配的PF算法，可以提高PDR系統(tǒng)的重采樣效率，同時(shí)也解決了粒子退化問題。

4 系統(tǒng)仿真測(cè)試與對(duì)比分析

4.1 系統(tǒng)仿真環(huán)境

實(shí)驗(yàn)場(chǎng)地是一個(gè)70 m×50 m 的長(zhǎng)方形區(qū)域，平面圖如圖4所示，圖中主要有走廊AB和走廊BC，走廊兩側(cè)的墻壁和門視為障礙物。本次仿真實(shí)驗(yàn)按照正常速度行走進(jìn)行測(cè)試。要求測(cè)試人員以A點(diǎn)為起點(diǎn)，沿著走廊AB從正西方向朝著正東方向行走，到達(dá)走廊BC之后立刻右轉(zhuǎn)彎，從B點(diǎn)沿著走廊BC從正北方向朝著正南方向行走，C點(diǎn)為單次實(shí)驗(yàn)終點(diǎn)。

圖4 實(shí)驗(yàn)場(chǎng)地平面

(8)

式中σv為高斯噪聲。設(shè)置高斯噪聲為零均值，標(biāo)準(zhǔn)差為0.08 m/s。

4.2 系統(tǒng)仿真結(jié)果對(duì)比分析

通過在室內(nèi)環(huán)境下進(jìn)行仿真實(shí)驗(yàn)，比較了使用PF的PDR算法前后的定位精度誤差。仿真實(shí)驗(yàn)要求測(cè)試人員以空心圓為起點(diǎn)，空心矩形為終點(diǎn)。使用的PDR算法記為PDR+PF_Method，使用PF的PDR算法記為PDR_Method。對(duì)于2種方法分別進(jìn)行10次行走測(cè)試。通過對(duì)行人的行走過程進(jìn)行軌跡分析和標(biāo)定，將行人最終定位軌跡繪制在實(shí)驗(yàn)平面圖中，為定位誤差分析提供了參考。圖5為仿真實(shí)驗(yàn)平面示意。

圖5 仿真實(shí)驗(yàn)平面

將2種方法計(jì)算的總距離與實(shí)際行走的距離進(jìn)行比較，得到圖6所示的實(shí)驗(yàn)誤差比較。

圖6 實(shí)驗(yàn)誤差比較

由圖6可知，測(cè)試10次之后，PDR_Method算法計(jì)算出的總距離和實(shí)際軌跡距離誤差較大，誤差范圍大于等于5 %,占比70 %以上，而PDR+PF_Method測(cè)試10次的誤差范圍均在5 %以內(nèi)。仿真實(shí)驗(yàn)結(jié)果表明：本文基于PF的PDR定位算法提高了行人室內(nèi)定位的精度。

5 結(jié) 論

本文針對(duì)目前PDR定位精度不高的缺點(diǎn)，提出一種基于PF的PDR定位算法。仿真實(shí)驗(yàn)結(jié)果表明，使用PF算法輔助PDR之后，提高了定位精度。