鮑小雨,王 慶,陽(yáng) 媛,程向紅

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

0 引 言

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

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

本文設(shè)計(jì)了一種改進(jìn)的擴(kuò)展卡爾曼濾波(improved extended Kalman filtering,IEKF)算法,首先對(duì)UWB設(shè)備進(jìn)行標(biāo)定實(shí)現(xiàn)對(duì)測(cè)距信息的修正,然后在普通擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)的基礎(chǔ)上進(jìn)行改進(jìn),以修正后的測(cè)距值作為濾波器的觀測(cè)值；并增加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個(gè)錨(anchor)基站、1個(gè)標(biāo)簽(tag)、路由器和服務(wù)器組成[10]。Tag和Anchor之間進(jìn)行通信,測(cè)量?jī)烧唛g的距離[11]。

圖1 UWB定位原理

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

di=ri+ei

(1)

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

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

(2)

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

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

2 IEKF算法

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

2.1 狀態(tài)方程

算法的狀態(tài)向量為Tag的運(yùn)動(dòng)狀態(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為采樣時(shí)間；Q為過(guò)程噪聲協(xié)方差；q為過(guò)程噪聲的功率譜密度。

2.2 觀測(cè)方程

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

(6)

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

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

Zk=[d1d2…dN]T

(7)

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

Zk=HXk+υk

(8)

(9)

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

2.3 算法流程

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

(10)

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

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

(11)

(12)

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

(13)

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

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

(14)

3 實(shí)驗(yàn)與實(shí)驗(yàn)結(jié)果

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

3.1 UWB測(cè)距標(biāo)定

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

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

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

3.2 UWB定位實(shí)驗(yàn)

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

3.2.1 UWB靜態(tài)實(shí)驗(yàn)

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

圖2 實(shí)驗(yàn)布置

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

3.2.2 UWB動(dòng)態(tài)實(shí)驗(yàn)

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

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

表2 測(cè)試點(diǎn)定位誤差比較 cm

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

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

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

4 結(jié)束語(yǔ)

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