改進(jìn)無(wú)跡粒子濾波的室內(nèi)移動(dòng)目標(biāo)定位算法*

武 斌, 田 清

(天津城建大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，天津 300384)

0 引 言

科技的飛速發(fā)展使得位置信息服務(wù)(location-based services，LBS)[1]的需求越來(lái)越大，傳統(tǒng)位置信息服務(wù)主要針對(duì)室外環(huán)境進(jìn)行研究，例如：全球定位系統(tǒng)、北斗衛(wèi)星導(dǎo)航系統(tǒng)等；而對(duì)于室內(nèi)環(huán)境來(lái)說(shuō)，定位的精度和抗差性要求相對(duì)較高，當(dāng)前室內(nèi)定位技術(shù)主要包括射頻識(shí)別(radio frequency identification，RFID)[2]、藍(lán)牙、超聲波、超寬帶(ultra wide band，UWB)[3]、無(wú)線局域網(wǎng)(wireless fidelity，WiFi)[4]等技術(shù)。

粒子濾波(particle filtering，PF)算法[5]在處理非線性和非高斯噪聲時(shí)變系統(tǒng)的參數(shù)估計(jì)和狀態(tài)濾波問(wèn)題方面具有獨(dú)特的優(yōu)勢(shì)和良好的應(yīng)用前景，但該算法仍然存在粒子退化與貧化、實(shí)時(shí)性差、計(jì)算量大等問(wèn)題。為了解決粒子濾波存在的問(wèn)題，一些學(xué)者將無(wú)跡卡爾曼濾波(unscented Kalman filtering，UKF)算法于粒子濾波結(jié)合，提出了無(wú)跡粒子濾波(unscented particle filtering，UPF)算法，利用UKF得到PF的重要性采樣密度函數(shù)，對(duì)粒子進(jìn)行更新，提高濾波性能和采樣質(zhì)量，狀態(tài)估計(jì)性能優(yōu)于PF算法，適用于難以非線性化和高斯近似處理的情況。然而在實(shí)際情況中，很難用一個(gè)確定的模型描述某個(gè)系統(tǒng)，并且系統(tǒng)噪聲和觀測(cè)噪聲不確定性引起的誤差偶然性太大，對(duì)于算法的抗差性帶來(lái)很大影響。因此在提高濾波精度的前提下，如何提高算法的抗差性成為了當(dāng)前研究的熱門(mén)。文獻(xiàn)[6]利用粒子濾波器融合了雷達(dá)的掃描數(shù)據(jù)，實(shí)現(xiàn)了對(duì)目標(biāo)的位姿跟蹤，有效地減小了定位誤差；文獻(xiàn)[7]將接收信號(hào)強(qiáng)度指示(received signal strength indication，RSSI)直接作為觀測(cè)量建立非線性狀態(tài)空間模型，并利用馬爾科夫鏈蒙特卡洛移動(dòng)步驟改進(jìn)系統(tǒng)重采樣算法，增加粒子多樣性，提高了濾波性能和跟蹤精度；文獻(xiàn)[8]利用PF融合多傳感器數(shù)據(jù)，利用激光聚類(lèi)后的數(shù)據(jù)估算運(yùn)動(dòng)目標(biāo)的速度，與RFID相位差估算出的運(yùn)動(dòng)目標(biāo)速度進(jìn)行匹配，該方法的定位精度得到了明顯提高;文獻(xiàn)[9]提出粒子剔除策略和依據(jù)粒子方位賦予粒子權(quán)值策略，提高系統(tǒng)的定位精度和執(zhí)行效率，算法具有很好的魯棒性。上述文獻(xiàn)中對(duì)于室內(nèi)定位的研究融合了多傳感器數(shù)據(jù)，再結(jié)合PF算法進(jìn)行定位，定位結(jié)果有了一定程度的提高，但是在整體定位誤差控制上仍然存在不足，異常噪聲仍然會(huì)給算法的抗差性帶來(lái)影響，從而使得算法的控制能力減弱。提出了一種基于M估計(jì)的魯棒自適應(yīng)UPF室內(nèi)定位(M-estimated robust adaptive UPF，MRA-UPF)算法。為了能夠有效地抑制異常噪聲，本文將抗差估計(jì)與PF結(jié)合，根據(jù)目標(biāo)的RSSI的分布特性，進(jìn)行多項(xiàng)式擬合，修正觀測(cè)數(shù)據(jù)；將抗差估計(jì)思想引入U(xiǎn)PF算法，利用馬氏距離構(gòu)造等價(jià)權(quán)函數(shù)和自適應(yīng)權(quán)因子，得到基于抗差估計(jì)的MRA-UPF算法。通過(guò)相關(guān)實(shí)驗(yàn)驗(yàn)證，提出的算法定位精度明顯優(yōu)于PF算法和UPF算法，算法的抗差性有了明顯增強(qiáng)。

1 UPF算法

UPF算法的重要思想是利用UKF算法得到粒子濾波的重要性采樣密度函數(shù)，提高濾波性能。下面給出基于UPF的室內(nèi)移動(dòng)目標(biāo)定位的算法過(guò)程。

1.1 室內(nèi)移動(dòng)目標(biāo)運(yùn)動(dòng)模型

考慮一般移動(dòng)目標(biāo)運(yùn)動(dòng)模型，其運(yùn)動(dòng)系統(tǒng)的狀態(tài)方程和量測(cè)方程可由如下非線性狀態(tài)方程和量測(cè)方程表示

xk=f(xk-1,vk-1),yk=h(xk,nk)

(1)

式中xk為k時(shí)刻運(yùn)動(dòng)模型的狀態(tài)向量，xk=[xak,yak,vk]T，xak和yak分別為k時(shí)刻移動(dòng)目標(biāo)在x軸和y軸上的坐標(biāo)值，vk為k時(shí)刻移動(dòng)目標(biāo)的速度；yk為量測(cè)輸出，vk為系統(tǒng)過(guò)程噪聲，nk為量測(cè)噪聲，f(·)和h(·)分別為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)，過(guò)程噪聲和量測(cè)噪聲都是服從均值為零的獨(dú)立過(guò)程，對(duì)應(yīng)的協(xié)方差矩陣分別為Qk和Rk。

(2)

1.2 UPF算法步驟

2 改進(jìn)的UPF算法

本文在UPF的基礎(chǔ)上，首先利用多項(xiàng)式擬合修正觀測(cè)數(shù)據(jù)，然后對(duì)UPF構(gòu)造魯棒M估計(jì)等價(jià)權(quán)矩陣和自適應(yīng)因子，有效地對(duì)觀測(cè)值進(jìn)行抗差控制，該算法很好地結(jié)合了抗差估計(jì)和UPF算法的優(yōu)點(diǎn)，通過(guò)抗差估計(jì)有效地抑制了異常噪聲對(duì)定位結(jié)果的影響。

2.1 多項(xiàng)式擬合“RSSI—距離”函數(shù)

本文采用的信號(hào)傳播模型為“對(duì)數(shù)正態(tài)陰影”模型[11]，為了方便觀測(cè)數(shù)據(jù)后續(xù)處理，對(duì)RSSI和信號(hào)接收基站到移動(dòng)目標(biāo)的距離D進(jìn)行多項(xiàng)式函數(shù)擬合，更加接近RSSI的真實(shí)值，下面給出多項(xiàng)式函數(shù)擬合推導(dǎo)過(guò)程：

對(duì)于給定的樣本數(shù)據(jù)(xi,yi)，i=1,2,n,對(duì)應(yīng)的擬合多項(xiàng)式函數(shù)φ(x)為

(3)

式中w為多項(xiàng)式參數(shù)，m為多項(xiàng)式的階，且m

利用最小二乘方法確定參數(shù)W=[w0,w1,w2,…，wm]T，則損失函數(shù)Qw表示為

(4)

為了得到Qw最小的參數(shù)向量W=[w0,w1,w2,…，wm]T，令損失函數(shù)Qw對(duì)wk的偏導(dǎo)數(shù)等于0，得到矩陣計(jì)算如下

(5)

即XW=U，擬合參數(shù)向量W=[w0,w1,w2,…，wm]T可通過(guò)W=X-1U計(jì)算得到。

2.2 抗差自適應(yīng)UPF算法

利用IGGⅢ方案[12,13]構(gòu)造的權(quán)函數(shù)能夠有效的改善粒子濾波的魯棒性，但是IGGⅢ方案權(quán)函數(shù)根據(jù)標(biāo)準(zhǔn)化殘差設(shè)置判決門(mén)限，需要一定的先驗(yàn)知識(shí)。本文利用馬氏距離作為構(gòu)造新的M估計(jì)權(quán)函數(shù)的判決門(mén)限，有效抑制異常噪聲對(duì)濾波算法的影響。

k時(shí)刻系統(tǒng)的觀測(cè)殘差和對(duì)應(yīng)的協(xié)方差矩陣為

(6)

(7)

(8)

式中c0∈(1,1.5)，c1∈(3,8)。新的等價(jià)權(quán)函數(shù)中：當(dāng)|ck|≤c0時(shí)，無(wú)異常噪聲影響定位點(diǎn)；當(dāng)c0≤|ck|

(9)

用式(9)替代量測(cè)更新公式，可完成抗差自適應(yīng)UPF算法的全部過(guò)程，MRA-UPF算法的流程圖如圖1所示。

圖1 MRA-UPF算法流程圖

3 實(shí)驗(yàn)驗(yàn)證與分析

3.1 仿真實(shí)驗(yàn)環(huán)境

為了測(cè)試本文提出的定位算法的性能，仿真實(shí)驗(yàn)環(huán)境采用MATLAB2011b，計(jì)算機(jī)的硬件配置是Intel(R) Core(TM) i5—4170 CPU 3.70 GHz，RAM 8 GB。仿真區(qū)域大小為100 m×100 m的平面，4個(gè)角落布置4個(gè)信號(hào)接收基站，4個(gè)基站的位置坐標(biāo)為：A(0,0)，B(100,0)，C(0,100)，D(100,100)。移動(dòng)目標(biāo)作線速度為v=0.25 πm/s，半徑r=25 m的勻速圓周運(yùn)動(dòng)，運(yùn)動(dòng)過(guò)程中添加均值為0,標(biāo)準(zhǔn)差為1的高斯噪聲，仿真環(huán)境布置如圖2所示。

圖2 仿真環(huán)境布置圖

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

3.2.1 多項(xiàng)式擬合RSSI和距離D

多項(xiàng)式擬合信號(hào)接收強(qiáng)度RSSI和距離D的函數(shù)，仿真實(shí)驗(yàn)中采集了信號(hào)接收基站20 m之內(nèi)的RSSI值，由多項(xiàng)式擬合公式和RSSI的分布特性，取擬合冪次為2。得到最終的擬合函數(shù)為φ(x)=-49.959 5-2.867 9D+0.080 6D2，其中φ(x)表示接收信號(hào)強(qiáng)度RSSI，D表示距離，擬合得到的結(jié)果如圖3所示。

圖3 多項(xiàng)式擬合RSSI和距離D

3.2.2 不同算法定位結(jié)果分析

為了驗(yàn)證本文提出的定位算法的性能，采用傳統(tǒng)的PF算法和UPF算法進(jìn)行對(duì)比，三種不同算法的定位結(jié)果如圖4所示，由圖4可知，本文提出的MRA-UPF算法的定位效果遠(yuǎn)遠(yuǎn)優(yōu)于傳統(tǒng)PF算法和UPF算法，定位軌跡最接近于移動(dòng)目標(biāo)的真實(shí)運(yùn)動(dòng)軌跡。

圖4 不同算法定位結(jié)果

在目標(biāo)運(yùn)動(dòng)軌跡上均勻采集200個(gè)定位點(diǎn)的數(shù)據(jù)，并計(jì)算出不同算法的均方根誤差(RMSE)，得到三種不同算法的定位誤差數(shù)據(jù)如圖5和表1所示。

圖5 不同算法的定位誤差

表1 不同算法的定位誤差數(shù)據(jù)表

由圖5可知，本文提出的MRA-UPF算法的定位誤差整體基本在0.01～1 m之間，傳統(tǒng)的PF算法PF整體誤差在0.1～3 m之間，UPF算法UPF的整體誤差在0.06～2.5 m之間。通過(guò)表1的數(shù)據(jù)可知，本文提出的MRA-UPF算法均方根誤差為0.411 5 m，與PF算法和UPF算法相比，平均誤差分別減小了62.19 %和56.79 %。

圖6為不同算法的誤差累積概率分布圖，由圖6可知，當(dāng)誤差為1 m時(shí)，PF算法的精度為51.5 %，UPF算法的定位精度為56 %，本文MRA-UPF算法的精度基本達(dá)到了100 %；當(dāng)誤差為2 m時(shí)，PF算法的精度為91 %，UPF算法的定位精度為95.5 %。由此可知，本文提出的算法定位精度明顯優(yōu)于PF算法和UPF算法。

圖6 不同算法的定位誤差累積概率分布

從圖4～圖6和表1相關(guān)數(shù)據(jù)可知，本文提出的MRA-UPF算法不僅具有良好的定位精度，并且顯示了良好的抗差性能，很好地抑制了定位過(guò)程中的異常噪聲，達(dá)到了預(yù)期的抗差效果。

3.2.3 不同噪聲定位結(jié)果分析

為了進(jìn)一步測(cè)試本文算法的性能，數(shù)據(jù)觀測(cè)過(guò)程中分別添加均值為0,標(biāo)準(zhǔn)差為2和均值為0,標(biāo)準(zhǔn)差為3的高斯噪聲，并統(tǒng)計(jì)了誤差累計(jì)概率分布和前100個(gè)定位點(diǎn)的誤差，如圖7和圖8所示。

圖7 μ=0,σ=2的噪聲下不同算法的CDF和誤差

圖8 μ=0,σ=3的噪聲下不同算法的CDF和誤差

由圖7和圖8所示可知，當(dāng)噪聲增大時(shí)，PF算法和UPF算法誤差明顯出現(xiàn)大幅度波動(dòng)，算法基本接近崩潰，而本文提出的MRA-UPF算法由于引入了抗差估計(jì)，且利用等價(jià)權(quán)很好地控制了噪聲對(duì)定位結(jié)果的影響，依然能夠保持良好的定位精度，體現(xiàn)出了良好的抗差性。

3.2.4 算法時(shí)間復(fù)雜度分析

為了測(cè)試本文算法的時(shí)間復(fù)雜度，通過(guò)對(duì)移動(dòng)目標(biāo)進(jìn)行200定位，得到了不同算法每次定位的平均運(yùn)行時(shí)間如表2所示。

表2 不同算法的定位運(yùn)行時(shí)間

從表2可知，PF算法得到一次定位結(jié)果的時(shí)間最快，MRA-UPF運(yùn)行時(shí)間最長(zhǎng)，分析可知，雖然本文提出的算法在獲取定位結(jié)果上時(shí)間復(fù)雜度有所增大，但是整體定位效果遠(yuǎn)遠(yuǎn)優(yōu)于PF和UPF兩種算法。

4 結(jié) 論

通過(guò)實(shí)驗(yàn)結(jié)果分析可知：本文提出的MRA-UPF算法不僅具有強(qiáng)穩(wěn)健性，而且能夠有效地抑制異常噪聲對(duì)于定位精度的影響，具有良好的推廣性，能夠有效克服異常噪聲對(duì)定位模型帶來(lái)的不良影響。