一種抑制NLOS誤差的UWB定位方法*

鮑小雨,王 慶,陽 媛,程向紅

(東南大學(xué) 儀器科學(xué)與工程學(xué)院,江蘇 南京 210096)

0 引 言

基于超寬帶(ultra-wide band,UWB)的定位技術(shù)具有多徑分辨能力強、抗干擾性強、定位精度高等優(yōu)點,隨著設(shè)備成本的降低,UWB定位技術(shù)逐步成為性價較高的一種定位方法[1]。由于UWB信號時間分辨率可高達(dá)納秒(ns)級,因此,UWB定位系統(tǒng)常用基于時間的方法獲得基站與標(biāo)簽之間的距離,然后利用距離信息對標(biāo)簽進(jìn)行定位[2],但往往會受到多徑效應(yīng)、非視距(non-line of sight,NLOS)誤差的影響[3]。

在室內(nèi)定位的應(yīng)用場景中,測距精度對定位精度有很大的影響。為了提高定位精度,提高測距精度和研究定位算法至關(guān)重要。Garcia E等人[4]提出了基于估計的信道脈沖響應(yīng)的偏度的NLOS檢測算法并使用擴展卡爾曼濾波來減輕NLOS傳播引起的正偏差。賈俊超[5]提出一種卡爾曼濾波增益自適應(yīng)的濾波方法,并基于幾何關(guān)系推導(dǎo)出一種定位算法。文獻(xiàn)[6]提出了一種基于誤差先驗知識引導(dǎo)的NLOS移動節(jié)點定位算法,在降低定位誤差的同時降低了計算復(fù)雜度。王長強等人[7,8]提出用一種偏移卡爾曼濾波的方法結(jié)合測距殘差和卡爾曼濾波實現(xiàn)實時抑制NLOS誤差,提高NLOS環(huán)境下的定位精度。文獻(xiàn)[9]將誤差歸化為虛擬延時,得到關(guān)于虛擬延時因子、終端位置參數(shù)的欠定方程組,該方法在平面上可達(dá)到厘米(cm)級的定位精度。

本文設(shè)計了一種改進(jìn)的擴展卡爾曼濾波(improved extended Kalman filtering,IEKF)算法,首先對UWB設(shè)備進(jìn)行標(biāo)定實現(xiàn)對測距信息的修正,然后在普通擴展卡爾曼濾波(extended Kalman filtering,EKF)的基礎(chǔ)上進(jìn)行改進(jìn),以修正后的測距值作為濾波器的觀測值；并增加NLOS誤差判別環(huán)節(jié),設(shè)定閾值區(qū)分LOS數(shù)據(jù)與NLOS數(shù)據(jù),調(diào)整濾波增益,抑制NLOS誤差,提高定位精度。

1 UWB定位系統(tǒng)

圖1為UWB定位系統(tǒng)的標(biāo)準(zhǔn)架構(gòu),由4個錨(anchor)基站、1個標(biāo)簽(tag)、路由器和服務(wù)器組成[10]。Tag和Anchor之間進(jìn)行通信,測量兩者間的距離[11]。

圖1 UWB定位原理

實際環(huán)境中,測得的Tag和Anchor間的距離可表示為

di=ri+ei

(1)

式中di為Tag與Anchor距離測量值,ri為真實值,ei為標(biāo)準(zhǔn)偏差。

為了獲得精確的測距值,本文對UWB模塊進(jìn)行標(biāo)定,即對測距數(shù)據(jù)進(jìn)行一次線性擬合,并利用擬合優(yōu)度判定系數(shù)來判斷擬合優(yōu)劣

(2)

式中R2定義為擬合優(yōu)度的判定系數(shù),yj為實際數(shù)據(jù),Yj為擬合數(shù)據(jù)。R2分布在區(qū)間(0,1),R2越小,擬合得越差；R2越大,擬合得越好。

當(dāng)Tag與Anchor間存在障礙物干擾時,測得的距離會變大[12],從而使得定位精度明顯下降。因此非常有必要對NLOS誤差進(jìn)行判別與剔除,從而提高定位精度,得到穩(wěn)定可靠的定位服務(wù)。

2 IEKF算法

EKF對非線性系統(tǒng)局部線性化,適用于弱非線性系統(tǒng)[13,14]。本文所提出的IEKF定位算法,以標(biāo)定后的UWB測距數(shù)據(jù)作為IEKF的觀測數(shù)據(jù),設(shè)定閾值進(jìn)行NLOS判別并調(diào)整濾波增益,剔除受NLOS影響的數(shù)據(jù),抑制其對定位精度的不良影響。

2.1 狀態(tài)方程

算法的狀態(tài)向量為Tag的運動狀態(tài),即

Xk=[xkykkk]T

(3)

式中xk和yk為Tag的位置；k和k為矢量速度。

狀態(tài)方程為

Xk+1=FXk+ωk

(4)

(5)

式中F為狀態(tài)轉(zhuǎn)移矩陣；Tc為采樣時間；Q為過程噪聲協(xié)方差；q為過程噪聲的功率譜密度。

2.2 觀測方程

在二維平面內(nèi),UWB的Anchor與Tag之間的距離為

(6)

式中N為Anchor的個數(shù)；di為第i個Anchor與Tag的距離；(xi,yi)為第i個Anchor的坐標(biāo),是已知量；(x,y)為Tag的坐標(biāo),是待求量。

系統(tǒng)的觀測向量為Tag與各Anchor之間的距離,即

Zk=[d1d2…dN]T

(7)

由式(6)可知,觀測向量Zk與狀態(tài)向量Xk為非線性關(guān)系,將觀測方程局部線性化,可簡化為

Zk=HXk+υk

(8)

(9)

式中υk為第k個采樣的觀測噪聲,通常假設(shè)為加性的并且對角自相關(guān)的矩陣R。

2.3 算法流程

根據(jù)狀態(tài)方程和觀測方程,通過EKF進(jìn)行狀態(tài)更新及觀測更新,即

(10)

式(10)為狀態(tài)的一步預(yù)測與均方誤差的一步預(yù)測方程。

由于在室內(nèi)環(huán)境下,行走的人員、障礙物都會對UWB信號造成遮擋,帶來NLOS誤差,因此本文對EKF稍加改進(jìn),將實際觀測值與狀態(tài)向量的預(yù)測值進(jìn)行比較,再將二者的差值與閾值相比。若差值小于等于閾值,可認(rèn)為數(shù)據(jù)是LOS場景下的,濾波增益不作改變；若差值大于閾值,表明該組數(shù)據(jù)存在NLOS誤差,將增益設(shè)置為0,則有

(11)

(12)

式中thresh為事先選定的閾值。最后,對狀態(tài)和均方誤差進(jìn)行更新

(13)

2.4 誤差判定標(biāo)準(zhǔn)

為了對定位精度進(jìn)行定量分析、便于比較經(jīng)典EKF算法與IEKF算法的定位精度,對定位誤差定義為

(14)

3 實驗與實驗結(jié)果

實驗使用的UWB模塊為南京沃旭公司的WxRobot機器人定位產(chǎn)品,由UM208(Tag)和4個UK—100(Anchor)組成。

3.1 UWB測距標(biāo)定

實驗在李文正北樓一樓大廳內(nèi)進(jìn)行,可保證實驗過程中Tag和Anchor之間無障礙物遮擋、無人員走動,完全為LOS場景。首先用Anchor1(A1)進(jìn)行實驗:A1固定不動,Tag從0.5 m移動至6.5 m,步長為0.5 m,共計13個測距點；在每個點處獲取上百個測距數(shù)據(jù),先用3σ準(zhǔn)則剔除粗大誤差,再對剩余數(shù)據(jù)取平均值,用作該測距點的測量距離；最后與真實距離進(jìn)行一次線性擬合,獲得基站的標(biāo)準(zhǔn)偏差的誤差模型,完成基站的標(biāo)定。其余Anchor用同樣的方法進(jìn)行處理,以供后續(xù)定位實驗測距數(shù)據(jù)預(yù)處理使用。

表1為4個基站的擬合函數(shù)的相關(guān)參數(shù),并用式(2)計算擬合優(yōu)度。根據(jù)擬合優(yōu)度的定義,可以看出一次線性擬合提供了接近完美的擬合結(jié)果。

表1 基站標(biāo)定參數(shù)

3.2 UWB定位實驗

為了驗證本文提出的IEKF算法對NLOS誤差有抑制作用,分別進(jìn)行了靜態(tài)實驗和動態(tài)實驗,并將IEKF的結(jié)果與經(jīng)典EKF的定位結(jié)果進(jìn)行對比。實驗在李文正北樓一樓大廳內(nèi)進(jìn)行,場地大小為5 m×9 m；獲取實驗數(shù)據(jù)過程中,人員不停走動或短暫停留引入NLOS誤差。

3.2.1 UWB靜態(tài)實驗

UWB靜態(tài)定位實驗是指Tag靜止不動,對其進(jìn)行定位。實驗布置如圖2所示,五角星(A1～A4)代表Anchor,三角形(P1～P8)為靜態(tài)實驗測試點；先后將Tag置于P1～P8位置,獲取其與基站的測距數(shù)據(jù),分別用EKF算法以及IEKF算法進(jìn)行定位解算,利用式(14)計算定位誤差,比較定位精度。該實驗中,設(shè)定NLOS判定閾值為15 cm。

圖2 實驗布置

表2列出了靜態(tài)定位的兩種算法結(jié)果的誤差統(tǒng)計,以P5為例,觀測環(huán)境較好時,兩種算法的精度相當(dāng)；當(dāng)存在NLOS誤差時,EKF誤差為33 cm,IEKF誤差僅有8 cm。整體來看,本文IEKF算法能在NLOS環(huán)境下有效準(zhǔn)確地進(jìn)行定位,定位精度較EKF提高約7 %。

3.2.2 UWB動態(tài)實驗

實驗布置如圖2所示,動態(tài)實驗中行走路徑:從P2出發(fā),沿P1,P4,P5,P8,P7運動(5字型)。該實驗中,設(shè)定NLOS判定閾值為65 cm。

圖3為EKF和IEKF的定位結(jié)果。表3列出了兩種算法定位結(jié)果的誤差統(tǒng)計。

表2 測試點定位誤差比較 cm

圖3 二種算法定位結(jié)果

EKF最大值最小值平均值 IEKF最大值最小值平均值44.120.118.6616.150.306.88

圖3(a)中,利用經(jīng)典EKF對NLOS/LOS混合場景下移動的Tag進(jìn)行定位,結(jié)果表明,觀測環(huán)境良好時,EKF算法解算結(jié)果可以很好地貼近真實路徑；存在NLOS誤差時,定位誤差較大,高達(dá)44 cm,甚至定位軌跡出現(xiàn)斷點。在圖3(b)中,利用本文提出的IEKF算法進(jìn)行定位,定位結(jié)果明顯優(yōu)于圖3(a),最大誤差僅有16 cm且未出現(xiàn)定位斷點。整體來看,IEKF算法定位精度較EKF提高了約20 %,且定位連續(xù)性更佳。

4 結(jié)束語

UWB定位系統(tǒng)中亟待解決的問題是測距標(biāo)準(zhǔn)偏差和NLOS誤差對定位精度的不利影響,針對前者,本文進(jìn)行測距實驗,利用一次線性擬合求解測距數(shù)據(jù)中的標(biāo)準(zhǔn)偏差,進(jìn)而對設(shè)備進(jìn)行標(biāo)定；針對后者,本文提出了一種改進(jìn)的EKF算法,以修正后的測距值作為濾波器的觀測數(shù)據(jù),設(shè)定閾值判別LOS數(shù)據(jù)與NLOS數(shù)據(jù),調(diào)整濾波增益,剔除NLOS誤差。實驗結(jié)果表明:視距環(huán)境下,靜態(tài)定位和動態(tài)定位精度均可高達(dá)5 cm；非視距環(huán)境下,靜態(tài)定位精度為9 cm,動態(tài)定位精度為20 cm。本文所提出的方法能大幅提高定位精度,適用于LOS/NLOS混合環(huán)境的定位。