NLOS環(huán)境下基于UWB的定位算法研究

徐淑萍,郭 宇,王 雙,張玉西

(西安工業(yè)大學(xué)計(jì)算機(jī)學(xué)院,陜西 西安 710000)

1 引言

隨著移動(dòng)機(jī)器人的發(fā)展,室內(nèi)定位技術(shù)的需求也在不斷增加,但由于室內(nèi)環(huán)境存在復(fù)雜性、多變性,不可避免的存在遮擋物等非視距(Non Line of Sight,NLOS)環(huán)境,造成定位精度下降,而單一的定位算法又無(wú)法適用[1]。UWB 技術(shù)有穿透性能強(qiáng)、功耗低、定位精高的優(yōu)勢(shì)[2]。TDOA 技術(shù)復(fù)雜度低等特點(diǎn)成為常用的估計(jì)方法[3]。Chan算法在視距(Line of Sight,LOS)環(huán)境下定位精度較高,但在NLOS環(huán)境下定位效果很差[4]。Taylor算法[5]需要設(shè)置合理的初始值,否則會(huì)導(dǎo)致定位算法不收斂,Chan 和Taylor 的協(xié)同方法[6],將Chan 方法的定位結(jié)果作為Taylor算法的初始位置,可滿足其收斂性。文獻(xiàn)[7]基于Chan-Taylor協(xié)同定位,但沒考慮NLOS誤差。文獻(xiàn)[8]是拋棄利用定位結(jié)果殘差鑒別受到NLOS 污染嚴(yán)重的測(cè)距數(shù)據(jù),定位效果較好,但一定程度上消去了表征定位目標(biāo)的有效信息。文獻(xiàn)[9]利用信噪比區(qū)分視距和非視距環(huán)境,需要判斷測(cè)距閾值范圍。

本文在在TDOA技術(shù)的基礎(chǔ)上,考慮到Chan算法與Taylor算法的優(yōu)劣勢(shì),采用Chan-Taylor協(xié)同算法,識(shí)別NLOS誤差并分別結(jié)合卡爾曼濾波和距離重構(gòu),設(shè)計(jì)出兩個(gè)定位方案,并進(jìn)行靜動(dòng)態(tài)實(shí)驗(yàn)分析。

2 TDOA定位算法描述

依第一個(gè)基站作為主基站構(gòu)建雙曲線模型,TDOA可表示為

(1)

其中,ri,1為第i個(gè)基站與主基站距離與主基站到標(biāo)簽距離的差,(x,y)表示目標(biāo)標(biāo)簽的實(shí)際坐標(biāo),(xi,yi)表示第i個(gè)基站的坐標(biāo)。

2.1 Chan氏定位算法

Chan 算法是一種求解雙曲線方法的非遞歸算法[10]。在其求解中需要兩次利用最小二乘法加權(quán)估計(jì)結(jié)果。系統(tǒng)在服從高斯分布噪聲情況下,誤差相對(duì)較小,但在NLOS環(huán)境下定位性能將受到嚴(yán)重影響。

2.2 Taylor級(jí)數(shù)展開算法

Taylor算法是一種需要初始值的遞歸算法[11]。先前求解的結(jié)果作為當(dāng)前的初始值,利用最小二乘法不斷的迭代修正,直到求解的誤差小于設(shè)定的閾值,該算法定位精度較高,但易受初始值的影響。

2.3 Chan-Taylor協(xié)同定位算法

將Chan算法定位估計(jì)值作為Taylor算法的初始估計(jì)坐標(biāo),不斷修正迭代,直到滿足最小二乘法求解的誤差值小于先前設(shè)定閾值,則迭代停止,最終輸出一個(gè)更加精確定位估計(jì)值。

圖1 Chan-Taylor協(xié)同算法流程

3 NLOS誤差識(shí)別

在非視距環(huán)境下,標(biāo)簽的信號(hào)到達(dá)基站會(huì)存在一個(gè)較大延時(shí)引起的正向誤差值,ti時(shí)刻定位標(biāo)簽到基站之間距離的誤差觀測(cè)模型表示為[12]

rm(ti)=dm(ti)+nm(ti)+NLOSm(ti),i=1,2…k

(2)

(3)

其中,rm(ti)為基站與待測(cè)標(biāo)簽在NLOS環(huán)境下ti時(shí)刻的距離測(cè)量值,dm(ti)表示為L(zhǎng)OS( Line of Sight,LOS,視距)環(huán)境在在ti時(shí)刻基站與待測(cè)標(biāo)簽的距離,nm(ti)為系統(tǒng)測(cè)量誤差,NLOSm(ti)為NLOS環(huán)境下ti時(shí)刻的測(cè)距誤差,平滑處理定位標(biāo)簽到各個(gè)基站的測(cè)量距離值,即

(4)

(5)

rm(ti)的樣本標(biāo)準(zhǔn)差可以計(jì)算出來,則表示為

(6)

上式(6)中測(cè)距樣本數(shù)量用k表示。由式(2)可以看出由于系統(tǒng)測(cè)量值數(shù)據(jù)中既有NLOS信號(hào)傳播的誤差也有測(cè)量噪聲的誤差,因此,測(cè)量值的標(biāo)準(zhǔn)差要大于平滑值的標(biāo)準(zhǔn)差。

(7)

殘差值為

em(ti)=rm(ti)-dm(ti)

(8)

NLOS環(huán)境下的測(cè)量誤差確定后,結(jié)合卡爾曼濾波進(jìn)行優(yōu)化,之后采用定位算法確定目標(biāo)最優(yōu)估計(jì)位置。

4 NLOS誤差的抑制與重構(gòu)

4.1 卡爾曼濾波抑制算法

卡爾曼濾波能夠有效對(duì)數(shù)據(jù)進(jìn)行平滑處理,使之降低異常值的影響,當(dāng)存在NLOS誤差時(shí),后一時(shí)刻的卡爾曼濾波計(jì)算時(shí)可加入測(cè)距殘差值,由式(8)可令測(cè)距殘差表示為C=em(ti),從而考慮到非零殘差值C的卡爾曼濾波推到公式表示為:

估計(jì)測(cè)量值觀測(cè)向量Yk為

Yk/k-1=Hk/k-1+C

(9)

Yk與估計(jì)值Yk/k-1之間的差為

ek/k-1=Yk-Yk/k-1=Yk-Hkk/k-1-C

(10)

k/k-1=k/k-1+Kk(Yk-Hkk/k-1-C)

(11)

NLOS環(huán)境下一種新的卡爾曼濾波器表示為

k/k-1=AXk-1/k-1

(12)

(13)

(14)

(15)

Pk/k=[I-KkHk]Pk/k-1

(16)

利用新的卡爾曼濾波器對(duì)NLOS環(huán)境下的測(cè)量數(shù)據(jù)進(jìn)行LOS優(yōu)化重構(gòu),之后帶入到Chan-Taylor協(xié)同算法中再次對(duì)LOS優(yōu)化重構(gòu)數(shù)據(jù)進(jìn)行定位處理,最終得到最優(yōu)的目標(biāo)位置。

4.2 NLOS誤差距離重構(gòu)算法

由移動(dòng)標(biāo)簽運(yùn)動(dòng)是一個(gè)不斷連續(xù)的過程,因此當(dāng)前時(shí)刻的狀態(tài)可使用先前時(shí)刻的狀態(tài)進(jìn)行表示[14]。基于距離重構(gòu)的算法種NLOS的距離重構(gòu)是利用先前時(shí)刻的LOS環(huán)境下的距離值和距離變化速率。k時(shí)刻的距離使用k-1時(shí)刻的距離進(jìn)行表示,表示為

(17)

(18)

(19)

綜上,要想獲得當(dāng)前時(shí)刻的距離的信息,必須知道上一時(shí)刻的距離信息和移動(dòng)標(biāo)簽的狀態(tài)信息,最后完成的NLOS環(huán)境下的距離重構(gòu),之后帶入到Chan-Taylor協(xié)同算法中,獲取最終估計(jì)值。

5 仿真分析

選擇8mx8m的室內(nèi)復(fù)雜NLOS場(chǎng)地,隨機(jī)設(shè)置三個(gè)個(gè)不均勻的坐標(biāo)為B0、B1、B2,以0.15s頻率采集數(shù)據(jù),每個(gè)坐標(biāo)點(diǎn)采集約100個(gè)數(shù)據(jù)點(diǎn)左右的數(shù)據(jù)集,在靜態(tài)情況下分別對(duì)4個(gè)不同坐標(biāo)點(diǎn)進(jìn)行定位,動(dòng)態(tài)情況下對(duì)移動(dòng)機(jī)器人的真實(shí)軌跡進(jìn)行定位,利用2種NLOS誤差抑制方案,分別對(duì)測(cè)量觀測(cè)數(shù)據(jù)進(jìn)行定位結(jié)果分析,實(shí)驗(yàn)測(cè)試分析的方案如圖2所示。

圖2 實(shí)驗(yàn)方案

兩種方案靜態(tài)定位結(jié)果如下圖3、4、5、6所示。

圖3 測(cè)試點(diǎn)T1(2.3,2.53)

圖4 測(cè)試點(diǎn)T2(2.42,2.6)

圖5 測(cè)試點(diǎn)T3(2.5,2.6)

圖6 測(cè)試點(diǎn)T4(2.22,2.5)

由圖3、4、5、6可看出方案1的四個(gè)測(cè)試點(diǎn)定位精度相比方案2明顯提高,且從表1可以看出方案1均方根誤差與方案2相比明顯較小,定位效果更好。

從圖7、8可以看出方案1的定位軌跡更加吻合真實(shí)軌跡,且在誤差為20cm時(shí)方案1的累積分布函數(shù)比方案2更高,定位精度更好;圖9、10、11可以看出方案1距離各基站的誤差更小。

圖7 兩種方案定位動(dòng)態(tài)軌跡

圖8 兩種方案定位軌跡的CDF

圖9 兩種方案定位目標(biāo)與第一個(gè)基站距離

圖10 兩種方案目定位標(biāo)與第二個(gè)基站距離

圖11 兩種方案定位目標(biāo)與第三個(gè)基站距離

綜上所述,方案1和方案2均能消除定位方案在NLOS環(huán)境下的誤差,可以滿足日常的定位需求,但方案1定位精度更高,可適應(yīng)定位精度要求高的場(chǎng)景。

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

在室內(nèi)復(fù)雜多變的環(huán)境下,TDOA測(cè)得的原始觀測(cè)數(shù)會(huì)存在NLOS誤差和高頻噪聲誤差,這將會(huì)嚴(yán)重影響室內(nèi)移動(dòng)機(jī)器人的定位精度。本文基于Chan-Taylor協(xié)同定位算法,NLOS誤差識(shí)別,并分別用卡爾曼濾波與特征統(tǒng)計(jì)的距離重構(gòu)兩種方案,對(duì)觀測(cè)數(shù)據(jù)進(jìn)行異常值處理,消弱NLOS誤差,以提高定位精度,兩種方案在一定程度上都可以抑制NLOS誤差,但方案1的定位精度更加明顯,對(duì)室內(nèi)移動(dòng)機(jī)器人高精度定位有一定的參考價(jià)值。