張 園,譚興龍，趙長(zhǎng)勝，李曉明

(江蘇師范大學(xué)，江蘇 徐州 221116)

由于信號(hào)遮蔽、精度制約等問題，GPS難以實(shí)現(xiàn)高精度室內(nèi)定位。而超寬帶UWB技術(shù)具有時(shí)間分辨率極高、阻礙穿透能力強(qiáng)等特點(diǎn)，在LOS環(huán)境下能獲得厘米甚至毫米級(jí)的測(cè)距和定位精度，適用于室內(nèi)環(huán)境下的高精度定位[1- 4]。

在UWB室內(nèi)定位中，濾波算法是關(guān)鍵環(huán)節(jié)。在高斯白噪聲情況下，卡爾曼濾波基本可以滿足定位需求。文獻(xiàn)[5]提出將有色噪聲一階AR模型的自適應(yīng)卡爾曼濾波應(yīng)用到精密單點(diǎn)定位中。文獻(xiàn)[6]提出為了減小室內(nèi)環(huán)境中障礙物對(duì)超寬帶(UWB)傳感器測(cè)距結(jié)果的影響,使用一種基于卡爾曼濾波(KF)的超寬帶室內(nèi)定位算法，在復(fù)雜環(huán)境下可達(dá)亞米級(jí)動(dòng)態(tài)實(shí)時(shí)定位精度。文獻(xiàn)[7]提出針對(duì)傳統(tǒng)的卡爾曼濾波算法在運(yùn)用于室內(nèi)UWB系統(tǒng)定位時(shí),由于初始參數(shù)不定以及在環(huán)境變化或系統(tǒng)運(yùn)行偏差后定位誤差增大等問題,提出一種自適應(yīng)卡爾曼濾波算法。傳統(tǒng)卡爾曼濾波要求觀測(cè)噪聲為高斯白噪聲序列，對(duì)有色噪聲而言卡爾曼濾波不能直接使用，而粒子濾波可以不受噪聲模型的約束[8]。文獻(xiàn)[9]提出采用多項(xiàng)式長(zhǎng)除法將有色觀測(cè)噪聲模型展開成無(wú)窮級(jí)數(shù),截?cái)嗳∑溆邢揄?xiàng)獲得有色噪聲的先驗(yàn)信息；然后利用粒子濾波能夠處理非高斯噪聲的特點(diǎn)對(duì)有色觀測(cè)噪聲進(jìn)行處理。文獻(xiàn)[10]針對(duì)含有色噪聲的語(yǔ)音,提出了一種基于Unscented粒子濾波的單通道語(yǔ)音增強(qiáng)方法。采用時(shí)變自回歸模型(TVAR)對(duì)干凈語(yǔ)音建模,通過Unscented粒子濾波器估計(jì)AR模型的參數(shù)并濾除有色噪聲。上述是學(xué)者們針對(duì)觀測(cè)噪聲為有色噪聲情況下，對(duì)粒子濾波的研究情況。從粒子濾波出發(fā)，文獻(xiàn)[11]提出了基于似然分布自適應(yīng)調(diào)整的粒子濾波算法，該算法選取先驗(yàn)密度函數(shù)作為重要性密度函數(shù)。文獻(xiàn)[12]針對(duì)粒子濾波算法的實(shí)時(shí)性較差,計(jì)算量隨著粒子數(shù)的增加成級(jí)數(shù)增加,提出一種基于似然分布的樣本數(shù)自適應(yīng)UPF算法，將似然分布與樣本數(shù)自適應(yīng)UPF結(jié)合。

本文在前人的工作基礎(chǔ)之上，針對(duì)高斯白噪聲和有色噪聲兩種情況，分別將拓展卡爾曼濾波與似然分布自適應(yīng)粒子濾波應(yīng)用到UWB定位中，研究這兩種算法的優(yōu)勢(shì)與不同。

1 基于距離的UWB定位原理

考慮一般的行人室內(nèi)定位問題，假定行人沿直線運(yùn)動(dòng)，觀測(cè)站坐標(biāo)為(x0,y0)，狀態(tài)方程式為

X(k)=ΦX(k-1)+Gω(k-1)

(1)

式中，Φ為狀態(tài)轉(zhuǎn)移矩陣；G為過程噪聲驅(qū)動(dòng)矩陣；ω(k)為過程噪聲。行人狀態(tài)矢量為

X(k)=[x1(k)y1(k)x2(k)y2(k)]T

(2)

式中，x1(k)為東方向位置；y1(k)為北方向位置；x2(k)為東方向速度；y2(k)為北方向速度。

行人觀測(cè)方程式為

Z(k)=h(X(k))+v(k-1)

(3)

函數(shù)h表示觀測(cè)站與行人狀態(tài)之間的非線性關(guān)系

(4)

式中，v(k)為觀測(cè)噪聲。

2 似然分布自適應(yīng)粒子濾波

采用似然分布自適應(yīng)調(diào)整粒子濾波方法，由精度因子α來自適應(yīng)調(diào)整似然分布狀態(tài)，增加先驗(yàn)和似然的重疊區(qū)。改變似然函數(shù)的分布即是改變對(duì)應(yīng)粒子權(quán)值的分布，因此只對(duì)粒子濾波權(quán)值更新作改進(jìn)[13]。似然分布自適應(yīng)調(diào)整粒子濾波(LDAPF)過程如下：

(2) 更新粒子。計(jì)算每個(gè)粒子的似然概率，同時(shí)更新權(quán)值。

(5)

(6)

式中，ε為閾值，根據(jù)經(jīng)驗(yàn)確定；K為比例系數(shù)，K/α>0。

(5) 歸一化權(quán)值。

3 試驗(yàn)仿真與結(jié)果分析

3.1 跟蹤系統(tǒng)建模

假設(shè)行人在室內(nèi)行走，軌跡為矩形，x方向?yàn)闁|方向，y方向?yàn)楸狈较颍跏嘉恢脼?0,0)，初始狀態(tài)為(0,0,1,0)，其水平速度x2=1 m/s，其余狀態(tài)參數(shù)均為0，PF中粒子數(shù)目為N=200，采樣次數(shù)為100。選取拓展卡爾曼濾波、粒子濾波和似然分布自適應(yīng)粒子濾波，研究它們?cè)诟咚拱自肼暫陀猩肼曄碌膬?yōu)勢(shì)與不同。

3.1.1 高斯噪聲情況

在高斯噪聲情況下，圖1為行人真實(shí)軌跡和預(yù)測(cè)軌跡，3種算法處理后的誤差及用時(shí)對(duì)比情況見表1。

圖1表明在高斯白噪聲情況下，EKF算法、PF算法和LDAPF算法均能較好地實(shí)現(xiàn)對(duì)行人的跟蹤。由表1均方根誤差(RMSE)對(duì)比情況可見，位移上EKF算法x方向定位精度為0.113 m，PF算法x方向定位精度為0.134 m，LDAPF算法x方向定位精度為0.078 m。由此可以看出x方向LDAPF算法精度優(yōu)于PF算法、EKF算法。y方向定位精度各算法差距不大。由速度均方根誤差(RMSE)可看出，3種算法精度均較高，EKF算法較其他兩種算法x方向均方根誤差稍大，但沒有明顯影響運(yùn)動(dòng)軌跡。

表1 3種算法處理后的誤差及用時(shí)對(duì)比

在用時(shí)上，PF算法和LDAPF算法較EKF算法用時(shí)較多，實(shí)時(shí)性上不如EKF算法。但整體上LDAPF算法在高斯白噪聲情況下的精度優(yōu)于其他兩種算法，可達(dá)到較高的定位精度。

3.1.2 有色噪聲情況

傳統(tǒng)UKF要求噪聲是高斯白噪聲，實(shí)際上，由于載體的機(jī)制等原因，不同系統(tǒng)的噪聲不一定表現(xiàn)出高斯特性，如GPS動(dòng)態(tài)定位中觀測(cè)誤差和動(dòng)力學(xué)模型是具有一定時(shí)空相關(guān)、異常特性的有色噪聲[14- 15]。

在有色噪聲情況下，圖2給出了預(yù)測(cè)軌跡，3種算法處理后的誤差及用時(shí)對(duì)比情況見表2。由圖2可見，EKF算法跟蹤效果較PF和LDAPF兩種算法差。

表2 3種算法處理后的誤差及用時(shí)對(duì)比情況

由表2可見，EKF算法x方向定位精度為0.569 m，y方向定位精度為0.672 m，x方向速度精度為0.104 m/s、y方向速度精度為0.045 m/s，導(dǎo)致x、y方向速度誤差變大，因此在圖2平面跟蹤軌跡圖中波動(dòng)較大。對(duì)于PF和LDAPF算法，在y方向位移上，LDAPF算法較PF算法精度略高出約16 cm，x方向上差別不大。

在有色噪聲情況下，3種算法實(shí)時(shí)性上與高斯白噪聲情況基本一致。從定位精度上來說，拓展卡爾曼濾波不能直接用于處理有色噪聲的情況，而粒子濾波性能表現(xiàn)良好。在量測(cè)精度較高時(shí)，LDAPF算法能在一定程度上改善濾波結(jié)果，提高定位精度，優(yōu)于PF算法。

3.2 實(shí)例驗(yàn)證

試驗(yàn)地點(diǎn)為室內(nèi)，試驗(yàn)場(chǎng)景為L(zhǎng)OS環(huán)境，采樣80次，由測(cè)距精度厘米級(jí)的通信和測(cè)距模塊采集獲得觀測(cè)站和UWB流動(dòng)站的觀測(cè)距離。UWB流動(dòng)站初始坐標(biāo)為(20.35,17.68),沿預(yù)定軌道緩慢移動(dòng)，采用拓展卡爾曼濾波EKF算法、粒子濾波PF算法和似然分布自適應(yīng)粒子濾波LDAPF算法分別進(jìn)行定位解算。

用3種方法進(jìn)行濾波解算，圖3給出了平面軌跡跟蹤圖。3種算法處理后的誤差及用時(shí)對(duì)比情況見表3。由圖3可看出，PF算法與真實(shí)軌跡比較接近，EKF算法軌跡偏差較大。結(jié)合表3，EKF算法x和y方向均方根誤差均大于PF算法的均方根誤差，而PF和LDAPF兩種算法跟蹤效果比較接近。

由表3分析PF和LDAPF算法的不同。x方向位移誤差，LDAPF比PF算法更接近真實(shí)值。y方向位移誤差，LDAPF算法均方根誤差小于PF算法。這說明在量測(cè)模型精度較高時(shí)，似然分布自適應(yīng)粒子濾波可以進(jìn)一步提高粒子濾波的精度和可靠性，從而得到精度更高、穩(wěn)定性更好的UWB跟蹤效果。在速度上PF算法、LDAPF算法均方根誤差相差不大，優(yōu)于EKF算法。在用時(shí)上PF和LDAPF算法時(shí)間相同，而EKF算法用時(shí)更短。

表3 3種算法處理后的誤差及用時(shí)對(duì)比情況

4 結(jié) 論

對(duì)于基于距離的UWB算法而言，重要的環(huán)節(jié)是選取合適的濾波算法。當(dāng)觀測(cè)噪聲為高斯白噪聲時(shí)，可以采取卡爾曼濾波算法計(jì)算獲得更好的預(yù)測(cè)結(jié)果。在實(shí)際情況中，觀測(cè)噪聲如果是有色噪聲，這時(shí)直接采用卡爾曼濾波算法受到限制，而粒子濾波算法可以很好地處理觀測(cè)噪聲為有色噪聲的情況。同時(shí)在量測(cè)精度要求較高時(shí)，采用似然分布自適應(yīng)粒子濾波能在一定程度上改善濾波結(jié)果，提高定位精度。