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

(西安工業(yè)大學計算機學院,陜西 西安 710000)

1 引言

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

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

2 TDOA定位算法描述

依第一個基站作為主基站構建雙曲線模型,TDOA可表示為

(1)

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

2.1 Chan氏定位算法

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

2.2 Taylor級數展開算法

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

2.3 Chan-Taylor協同定位算法

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

圖1 Chan-Taylor協同算法流程

3 NLOS誤差識別

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

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

(2)

(3)

其中,rm(ti)為基站與待測標簽在NLOS環(huán)境下ti時刻的距離測量值,dm(ti)表示為LOS( Line of Sight,LOS,視距)環(huán)境在在ti時刻基站與待測標簽的距離,nm(ti)為系統測量誤差,NLOSm(ti)為NLOS環(huán)境下ti時刻的測距誤差,平滑處理定位標簽到各個基站的測量距離值,即

(4)

(5)

rm(ti)的樣本標準差可以計算出來,則表示為

(6)

上式(6)中測距樣本數量用k表示。由式(2)可以看出由于系統測量值數據中既有NLOS信號傳播的誤差也有測量噪聲的誤差,因此,測量值的標準差要大于平滑值的標準差。

(7)

殘差值為

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

(8)

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

4 NLOS誤差的抑制與重構

4.1 卡爾曼濾波抑制算法

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

估計測量值觀測向量Yk為

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

(9)

Yk與估計值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)

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

4.2 NLOS誤差距離重構算法

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

(17)

(18)

(19)

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

5 仿真分析

選擇8mx8m的室內復雜NLOS場地,隨機設置三個個不均勻的坐標為B0、B1、B2,以0.15s頻率采集數據,每個坐標點采集約100個數據點左右的數據集,在靜態(tài)情況下分別對4個不同坐標點進行定位,動態(tài)情況下對移動機器人的真實軌跡進行定位,利用2種NLOS誤差抑制方案,分別對測量觀測數據進行定位結果分析,實驗測試分析的方案如圖2所示。

圖2 實驗方案

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

圖3 測試點T1(2.3,2.53)

圖4 測試點T2(2.42,2.6)

圖5 測試點T3(2.5,2.6)

圖6 測試點T4(2.22,2.5)

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

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

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

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

圖9 兩種方案定位目標與第一個基站距離

圖10 兩種方案目定位標與第二個基站距離

圖11 兩種方案定位目標與第三個基站距離

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

6 結束語

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