基于自適應(yīng)抗差卡爾曼濾波的UWB室內(nèi)定位*

劉 韜,徐愛功,隋 心

(遼寧工程技術(shù)大學(xué)測(cè)繪與地理科學(xué)學(xué)院,遼寧 阜新 123000)

近年來,隨著無線通信和無線傳感器網(wǎng)絡(luò)的不斷發(fā)展,無線定位技術(shù)越來越受到人們的關(guān)注,廣泛應(yīng)用于災(zāi)害急救、目標(biāo)跟蹤、車載導(dǎo)航、行人導(dǎo)航等領(lǐng)域[1]。以全球衛(wèi)星導(dǎo)航系統(tǒng)為基礎(chǔ)的室外定位技術(shù)已趨于成熟;在室內(nèi)定位中,超寬帶UWB(Ultra-Wide Band)技術(shù)利用功率譜密度極低、脈沖寬度極窄的脈沖信號(hào)傳輸數(shù)據(jù),具有時(shí)間分辨率極高、障礙穿透能力強(qiáng)等特點(diǎn),在視距LOS(Line-Of-Sight)環(huán)境下能獲得厘米甚至毫米級(jí)的測(cè)距和定位精度[2-3],適用于室內(nèi)環(huán)境下的高精度定位。

由于墻體和障礙物的存在,UWB信號(hào)通過折射和反射在移動(dòng)站和基準(zhǔn)站之間進(jìn)行傳播,從而形成非視距NLOS(Non-Line-Of-Sight)誤差,極大降低其測(cè)距精度,使定位結(jié)果產(chǎn)生較大的誤差[4-8]。文獻(xiàn)[4-5]對(duì)大量LOS和NLOS環(huán)境下的UWB信號(hào)波形特征進(jìn)行分析,利用支持向量機(jī)進(jìn)行NLOS狀態(tài)的鑒別和誤差削弱,能有效消除NLOS誤差并提高測(cè)距精度,但該方法建立在大量的數(shù)據(jù)統(tǒng)計(jì)上,工作量較大;文獻(xiàn)[6]針對(duì)LOS/NLOS環(huán)境下的定位,利用粒子濾波和無跡卡爾曼濾波方法進(jìn)行定位和測(cè)速,能有效抑制NLOS誤差的影響,但其算法復(fù)雜,實(shí)現(xiàn)困難;文獻(xiàn)[7]利用基于往返時(shí)間RTT(Round Trip Time)和基于到達(dá)角度AOA(Angle Of Arrival)的方法進(jìn)行混合測(cè)距,并提出了基于網(wǎng)格的聚類定位算法,其不需要室內(nèi)環(huán)境的先驗(yàn)信息,仿真效果明顯,但AOA方法需要天線陣列,會(huì)增加硬件成本;文獻(xiàn)[8]利用慣性導(dǎo)航INS(Inertial Navigation System)輔助UWB進(jìn)行定位,通過先驗(yàn)閾值進(jìn)行NLOS判斷,利用INS的短期高精度結(jié)果鑒別NLOS誤差并消除其影響,但該方法需要增加硬件成本,且算法也更復(fù)雜。

UWB測(cè)距不僅會(huì)受到NLOS測(cè)距誤差的影響,還會(huì)受到由實(shí)驗(yàn)環(huán)境、儀器硬件和人為干擾等因素產(chǎn)生的測(cè)距異常值的影響,這兩個(gè)測(cè)距誤差均表現(xiàn)為量測(cè)信息異常,從而導(dǎo)致距離濾波和定位解算出現(xiàn)異常。針對(duì)量測(cè)信息異常的問題,有學(xué)者提出抗差估計(jì)理論抵制量測(cè)信息異常誤差的影響[9]。文獻(xiàn)[10]結(jié)合神經(jīng)網(wǎng)絡(luò)和卡爾曼濾波構(gòu)建自適應(yīng)因子,自適應(yīng)調(diào)節(jié)狀態(tài)向量和量測(cè)噪聲的協(xié)方差矩陣,從而抑制異常信息對(duì)濾波結(jié)果的影響,但神經(jīng)網(wǎng)絡(luò)需要大量理想的樣本進(jìn)行訓(xùn)練;文獻(xiàn)[11]針對(duì)GNSS/INS組合系統(tǒng),利用新息向量構(gòu)建抗差因子并作用于量測(cè)噪聲的協(xié)方差矩陣,從而修正濾波增益矩陣,提高系統(tǒng)的精度和抗差性。

在UWB定位中,很難用一個(gè)準(zhǔn)確的統(tǒng)計(jì)特征來描述系統(tǒng)噪聲。針對(duì)上述問題,本文提出了一種抗差自適應(yīng)濾波算法并應(yīng)用于UWB測(cè)距和定位。該算法首先在UWB距離模型的基礎(chǔ)上,利用新息向量和LOS環(huán)境下的閾值鑒別量測(cè)信息是否出現(xiàn)異常,進(jìn)一步構(gòu)建量測(cè)噪聲的抗差協(xié)方差矩陣,在濾波過程中利用改進(jìn)的Sage-Husa濾波的次優(yōu)無偏極大后驗(yàn)估計(jì)特性對(duì)系統(tǒng)噪聲的協(xié)方差矩陣進(jìn)行實(shí)時(shí)估計(jì)和修正,削弱并消除NLOS測(cè)距誤差和測(cè)距異常值的影響,得到更準(zhǔn)確穩(wěn)定的測(cè)距信息;然后推導(dǎo)并構(gòu)建UWB定位的線性模型,利用卡爾曼濾波得到載體的位置和速度信息。

1 距離濾波方法

1.1 距離模型

Xd,k=Fd,k-1Xd,k-1+wd,k

(1)

以RTT方法量測(cè)的距離值作為量測(cè)信息,則距離模型的量測(cè)方程為:

Zd,k=Hd,kXd,k+Vd,k

(2)

1.2 自適應(yīng)抗差卡爾曼濾波

由于式(1)和式(2)組成的距離模型為線性模型,其標(biāo)準(zhǔn)卡爾曼濾波[8,14]KF(Kalman Filter)算法為:

(3)

(4)

式中:c為閾值,可通過UWB基準(zhǔn)站和流動(dòng)站之間為LOS環(huán)境下多次測(cè)量確定。采用Huber函數(shù)[9-11]方法構(gòu)建量測(cè)噪聲的抗差協(xié)方差矩陣,即:

(5)

在UWB測(cè)距過程中,很難用一個(gè)準(zhǔn)確的統(tǒng)計(jì)特征來描述系統(tǒng)噪聲,而Sage-Husa濾波能對(duì)系統(tǒng)噪聲統(tǒng)計(jì)特性進(jìn)行實(shí)時(shí)估計(jì)和修正,從而進(jìn)一步提高濾波的精度,且具有一定的抗干擾能力[15-17]。結(jié)合文獻(xiàn)[16],利用改進(jìn)的Sage-Husa濾波對(duì)系統(tǒng)噪聲的協(xié)方差矩陣進(jìn)行實(shí)時(shí)估計(jì):

(6)

在計(jì)算預(yù)測(cè)狀態(tài)協(xié)方差矩陣時(shí),利用式(6)對(duì)系統(tǒng)噪聲的協(xié)方差矩陣不斷進(jìn)行實(shí)時(shí)估計(jì)和修正,進(jìn)而減小狀態(tài)估計(jì)誤差,并結(jié)合式(5),實(shí)現(xiàn)UWB距離模型的自適應(yīng)抗差卡爾曼濾波ARKF(Adaptive Robust Kalman Filter)過程,在抑制量測(cè)信息異常誤差的同時(shí)進(jìn)一步提高濾波解算的精度,得到更準(zhǔn)確可靠的UWB測(cè)距信息。

針對(duì)自適應(yīng)抗差卡爾曼的距離濾波方法,利用一組靜止的UWB測(cè)距信息進(jìn)行分析驗(yàn)證。靜止數(shù)據(jù)由Time Domain的PulsON400通信與測(cè)距模塊采集,采樣頻率為2 Hz,共采集2 000 s,數(shù)據(jù)采集過程中有人員在UWB基準(zhǔn)站和流動(dòng)站之間隨機(jī)走動(dòng),同時(shí)對(duì)UWB天線進(jìn)行隨機(jī)遮擋。原始的RTT測(cè)距信息如圖1所示。

圖1 RTT測(cè)距信息

由圖1可以看出測(cè)距中存在大量的NLOS誤差和測(cè)距異常值。在對(duì)標(biāo)準(zhǔn)時(shí)間偏差誤差進(jìn)行改正之后,利用卡爾曼濾波、Sage-Husa自適應(yīng)濾波和文中的自適應(yīng)抗差卡爾曼濾波方法進(jìn)行濾波計(jì)算,利用全站儀觀測(cè)計(jì)算的距離作為參考真值進(jìn)行比較分析。

圖2 3種濾波方法的測(cè)距誤差

圖2給出了3種方法的測(cè)距誤差,表1給出3種方法的誤差統(tǒng)計(jì)信息。結(jié)合圖2和表1,相較于卡爾曼濾波和Sage-Husa自適應(yīng)濾波,自適應(yīng)抗差卡爾曼濾波的最大誤差能從3 dm降低至4 cm,其測(cè)距精度更高、穩(wěn)定性更強(qiáng)。可以看出:自適應(yīng)抗差卡爾曼濾波方法利用抗差估計(jì)理論能有效抑制UWB測(cè)距中的NLOS測(cè)距誤差和測(cè)距異常值,同時(shí)結(jié)合Sage-Husa自適應(yīng)濾波對(duì)系統(tǒng)噪聲進(jìn)行實(shí)時(shí)的估計(jì)和修正,進(jìn)而提高UWB測(cè)距的精度和穩(wěn)定性。

表1 3種濾波方法的測(cè)距誤差統(tǒng)計(jì)

2 UWB定位方法

在UWB平面定位中,利用3個(gè)及3個(gè)以上置于固定位置的UWB基準(zhǔn)站和1個(gè)流動(dòng)站進(jìn)行定位[8,18]。以流動(dòng)站的位置和速度信息作為狀態(tài)參數(shù)Xp,k=[xkykvx,kvy,k]T,則UWB定位的狀態(tài)方程為:

Xp,k=Fp,k-1Xp,k-1+wp,k

(7)

式中:xk和yk分別為第k時(shí)刻UWB流動(dòng)站在x和y方向的位置,vx,k和vy,k分別為第k時(shí)刻UWB流動(dòng)站在x和y方向的速度,Fp,k為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,wp,k為系統(tǒng)噪聲,wp,k為零均值的高斯白噪聲,其對(duì)應(yīng)的協(xié)方差矩陣為Qp,k,且有:

(8)

(9)

式中:ΔTk為UWB數(shù)據(jù)采樣間隔,q為系統(tǒng)噪聲的功率譜密度。

通過距離濾波方法對(duì)原始RTT測(cè)距信息進(jìn)行處理,削弱并消除測(cè)距中的NLOS測(cè)距誤差和測(cè)距異常值的影響,得到更準(zhǔn)確可靠的測(cè)距信息。那么,第k時(shí)刻第i個(gè)UWB基準(zhǔn)站和流動(dòng)站之間的距離di,k可表示為:

(10)

對(duì)式(10)進(jìn)行變形后,可以得到:

(11)

Zp,k=Hp,kXp,k+Vp,k

(12)

式中:Hp,k為量測(cè)矩陣,Vp,k為k時(shí)刻的量測(cè)噪聲,Vp,k為零均值的高斯白噪聲,其對(duì)應(yīng)的協(xié)方差矩陣為Rp,k,且有:

(13)

(14)

在UWB定位的狀態(tài)方程和量測(cè)方程中,其系統(tǒng)噪聲和量測(cè)噪聲互不相關(guān)。由于式(7)和式(11)組成的UWB定位模型為線性模型,在假設(shè)系統(tǒng)噪聲和量測(cè)噪聲為高斯白噪聲的前提下,利用式(3)的卡爾曼濾波方法能最優(yōu)估計(jì)UWB流動(dòng)站的位置和速度信息。

圖3 室內(nèi)實(shí)驗(yàn)場(chǎng)景

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

實(shí)驗(yàn)數(shù)據(jù)由Time Domain的PulsON400通信與測(cè)距模塊采集,其帶寬為3.1 GHz～5.3 GHz,數(shù)據(jù)采樣頻率為2 Hz,利用RTT方式測(cè)得UWB流動(dòng)站與基準(zhǔn)站間的距離,在LOS環(huán)境下測(cè)距精度為厘米級(jí)。為了驗(yàn)證本文算法的有效性,分別利用卡爾曼濾波(KF)、Sage-Husa自適應(yīng)濾波(Sage-Husa)和文中的自適應(yīng)抗差卡爾曼濾波(ARKF)進(jìn)行距離濾波,然后再進(jìn)行定位解算。在如圖3所示的室內(nèi)環(huán)境下分別進(jìn)行靜態(tài)定位和動(dòng)態(tài)定位實(shí)驗(yàn)。

靜態(tài)實(shí)驗(yàn)中,將3個(gè)UWB基準(zhǔn)站置于固定位置,利用全站儀確定其坐標(biāo),并假設(shè)其沒有誤差,UWB流動(dòng)站靜止不動(dòng),以全站儀觀測(cè)的UWB流動(dòng)站坐標(biāo)作為參考真值,實(shí)驗(yàn)時(shí)長約為6 min,實(shí)驗(yàn)過程中有多人隨機(jī)在實(shí)驗(yàn)場(chǎng)中走動(dòng)。圖4給出3個(gè)UWB原始的RTT測(cè)距信息,可以看出其中存在部分的NLOS測(cè)距誤差和測(cè)距異常值,圖5和圖6給出了3種濾波方法在x和y方向上的定位誤差,表2給出了3種濾波方法的誤差統(tǒng)計(jì)。

圖4 靜態(tài)實(shí)驗(yàn)中的UWB測(cè)距信息

圖5 3種濾波方法在x方向的位置誤差

圖6 3種濾波方法在y方向的位置誤差

表2 靜態(tài)實(shí)驗(yàn)中3種濾波方法的誤差統(tǒng)計(jì)

在動(dòng)態(tài)實(shí)驗(yàn)中,將3個(gè)UWB基準(zhǔn)站置于固定位置,利用全站儀確定其坐標(biāo),并假設(shè)其沒有誤差,將UWB流動(dòng)站置于實(shí)驗(yàn)小車上,人員推著實(shí)驗(yàn)小車移動(dòng),利用全站儀確定的UWB流動(dòng)站的位置作為參考軌跡,實(shí)驗(yàn)時(shí)長500 s。圖7給出了3個(gè)UWB原始的RTT測(cè)距信息,其中藍(lán)色、紅色和綠色線分別為測(cè)得的第1個(gè)、第2個(gè)和第3個(gè)UWB基準(zhǔn)站與流動(dòng)站之間的距離,由于遮擋導(dǎo)致測(cè)距中存在NLOS測(cè)距誤差和測(cè)距異常值。

圖7 動(dòng)態(tài)實(shí)驗(yàn)中UWB測(cè)距信息

圖8 動(dòng)態(tài)實(shí)驗(yàn)中3種濾波方法解算的動(dòng)態(tài)軌跡

利用3種方法進(jìn)行距濾波,然后進(jìn)行距離解算,圖8給出了3種方法的平面軌跡,其中紅色實(shí)線為參考軌跡。圖9和圖10分別給出了3種濾波方法在x和y方向上的定位誤差,其中綠色、藍(lán)色和紅色線分別為KF、Sage-Husa和ARKF方法的誤差,表3給出了3種濾波方法的誤差統(tǒng)計(jì)。

圖9 動(dòng)態(tài)實(shí)驗(yàn)中3種濾波方法在x方向的位置誤差

圖10 動(dòng)態(tài)實(shí)驗(yàn)中3種濾波方法在y方向的位置誤差

方向KFSage-HusaARKFx方向/mRMSMAX0.4357.4901.5126.6940.0480.531y方向/mRMSMAX0.2513.6450.9113.4030.0430.367

分析以上實(shí)驗(yàn)結(jié)果,可以得出:

①當(dāng)UWB的RTT測(cè)距中出現(xiàn)NLOS測(cè)距誤差和測(cè)距異常值時(shí),利用卡爾曼濾波方法進(jìn)行距離濾波時(shí),其完全不具有抗差性,濾波結(jié)果誤差較大,再進(jìn)行定位解算時(shí),UWB定位的精度和穩(wěn)定性難以得到保障;

②利用Sage-Husa自適應(yīng)濾波進(jìn)行距離濾波時(shí),靜態(tài)情況下具有一定的抗差性,能提高測(cè)距的精度和穩(wěn)定性,從而提高靜態(tài)定位的精度和可靠性;然而在動(dòng)態(tài)環(huán)境下,由于實(shí)驗(yàn)小車運(yùn)動(dòng)的不規(guī)律性,同時(shí)受到實(shí)驗(yàn)環(huán)境等因素的影響,很難同時(shí)精確地估計(jì)系統(tǒng)噪聲和量測(cè)噪聲,UWB定位結(jié)果的精度和穩(wěn)定性難以提高,一旦濾波發(fā)散,錯(cuò)誤的濾波值會(huì)污染測(cè)距良好情況下的定位結(jié)果,反而導(dǎo)致定位精度降低;

③利用文中提出的自適應(yīng)抗差卡爾曼濾波進(jìn)行距離濾波時(shí),其依據(jù)抗差估計(jì)理論對(duì)NLOS測(cè)距誤差和測(cè)距異常值進(jìn)行抗差處理,能提高UWB測(cè)距的精度和穩(wěn)定性,同時(shí)利用Sage-Husa自適應(yīng)濾波對(duì)系統(tǒng)噪聲進(jìn)行估計(jì)和修正,能綜合二者的優(yōu)勢(shì),進(jìn)一步提高距離濾波的精度和可靠性,進(jìn)而得到精度更高、穩(wěn)定性更好的UWB定位結(jié)果。

4 結(jié)論

在UWB室內(nèi)定位中,測(cè)距過程中受到NLOS測(cè)距誤差和測(cè)距異常值的影響,UWB定位精度會(huì)大幅下降,從而降低其穩(wěn)定性和可靠性。本文將抗差估計(jì)理論和Sage-Husa自適應(yīng)濾波用于UWB測(cè)距中的距離估計(jì),并提出了針對(duì)UWB距離濾波的自適應(yīng)抗差濾波方法,在此基礎(chǔ)上推導(dǎo)了線性化的UWB定位模型,通過卡爾曼濾波計(jì)算載體的位置和速度信息。在室內(nèi)環(huán)境下設(shè)計(jì)了UWB測(cè)距、靜態(tài)定位和動(dòng)態(tài)定位實(shí)驗(yàn),將文中提出的方法與卡爾曼濾波和Sage-Husa自適應(yīng)濾波進(jìn)行了對(duì)比和精度分析。實(shí)驗(yàn)結(jié)果表明,文中提出的自適應(yīng)抗差卡爾曼濾波方法能有效地消除并改正UWB測(cè)距中的NLOS誤差和異常測(cè)距值,提高UWB測(cè)距的精度和穩(wěn)定性,在此基礎(chǔ)上利用卡爾曼濾波進(jìn)行定位,能提升UWB定位的精度和可靠性。

參考文獻(xiàn):

[1] Liu H,Darabi H,Banerjee P,et al. Survey of Wireless Indoor Positioning Techniques and Systems[J]. IEEE Transactions on Systems,Man,and Cybernetics,Part C:Applications and Reviews,2007,37(6):1067-1080.

[2] de Angelis G,Moschitta A,Carbone P. Positioning Techniques in Indoor Environments Based on Stochastic Modeling of UWB Round-Trip-Time Measurements[J]. IEEE Transactions on Intelligent Transportation Systems,2016,17(8):2272-2281.

[3] Alarifi A,Al-Salman A M,Alsaleh M,et al. Ultra Wideband Indoor Positioning Technologies Analysis and Recent Advances[J]. Sensors,2016,16(5):1-36.

[4] Stefano M,Wesley M,Henk W,et al. NLOS Identification and Mitigation for Localization Based on UWB Experimental Data[J]. IEEE Journal on Selected Areas in Communications,2010,28(7):1026-1035.

[5] Wymeersch H,Maranò S,Gifford W M,et al. A Machine Learning Approach to Ranging Error Mitigation for UWB Localization[J]. IEEE Transactions on Communications,2012,60(6):1719-1728.

[6] Huerta J M,Vidal J,Giremus A,et al. Joint Particle Filter and UKF Position Tracking in Severe Non-Line of Sight Situations[J]. IEEE Journal of Selected Topics in Signal Processing,2009,3(5):874-888.

[7] 毛科技,鄔錦彬,金洪波,等. 面向非視距環(huán)境的室內(nèi)定位算法[J]. 電子學(xué)報(bào),2016,44(5):1174-1179.

[8] 劉韜,徐愛功,隋心. 超寬帶與慣性導(dǎo)航組合的室內(nèi)導(dǎo)航定位[J]. 測(cè)繪科學(xué),2016,41(12):162-166.

[9] Yany Y X,He H,Xu G. Adaptively Robust Filtering for Kinematic Geodetic Positioning[J]. Journal of Geodesy,2001,75(2):109-116.

[10] 高為廣,陳谷倉. 結(jié)合自適應(yīng)濾波和神經(jīng)網(wǎng)絡(luò)的GNSS/INS抗差組合導(dǎo)航算法[J]. 武漢大學(xué)學(xué)報(bào)(信息科學(xué)版),2014,39(11):1323-1328.

[11] 苗岳旺,周巍,田亮,等. 基于新息x2檢測(cè)的擴(kuò)展抗差卡爾曼濾波及其應(yīng)用[J]. 武漢大學(xué)學(xué)報(bào)(信息科學(xué)版),2016,41(2):269-273.

[12] 徐愛功,劉韜,隋心,等. UWB/INS緊組合的室內(nèi)定位定姿方法[J]. 導(dǎo)航定位學(xué)報(bào),2017,5(2):14-19.

[13] de Angelis A,Nilsson J,Skog I,et al. Indoor Positioning by Ultrawide Band Radio Aided Inertial Navigation[J]. Metrology and Measurement Systems,2010,17(3):447-460.

[14] 王瑞榮,鄭書萬,陳浩龍,等. 一種基于Taylor和Kalman的室內(nèi)協(xié)同定位方法[J]. 傳感技術(shù)學(xué)報(bào),2014,27(11):1557-1561.

[15] Sage A P,Husa G W. Adaptive Filtering with Unknown Prior Statistics[C]//Proceedings of Joint Automatic Control Conference,1969:760-769.

[16] 魏偉,秦永元,張曉東,等. 對(duì)Sage-Husa算法的改進(jìn)[J]. 中國慣性技術(shù)學(xué)報(bào),2012,20(6):678-686.

[17] Peng S,Chen C,Shi H,et al. State of Charge Estimation of Battery Energy Storage Systems Based on Adaptive Unscented Kalman Filter with a Noise Statistics Estimator[J]. IEEE Access,2017,5:13202-13212.

[18] Kok M,Hol J D,Sch?n T B. Indoor Positioning Using Ultrawideband and Inertial Measurements[J]. IEEE Transactions on Vehicular Technology,2015,64(4):1293-1303.