一種基于到達時間差的卡爾曼濾波定位算法

駱新偉 郭 英 劉寒爍 葉 瑾

(山東科技大學 測繪與空間信息學院,山東 青島 266590)

0 引言

隨著網(wǎng)絡(luò)化、信息化和智能化時代的到來,室內(nèi)定位技術(shù)近年來不斷發(fā)展。超寬帶技術(shù)因其較低的系統(tǒng)復雜性、良好的抗多徑效應以及較強的穿透性等優(yōu)勢,廣泛應用于室內(nèi)定位系統(tǒng)中[1]。

超寬帶(ultra-wideband,UWB)中常用的有4種定位算法[2]：①基于接收信號強度(received signal strength,RSS)的變化,但由于多徑效應,定位精度較差,難以滿足定位要求；②基于到達時間(time of arrive,TOA),但因其基站和標簽難以做到時鐘精確的同步,會導致其無法精確的獲得標簽的位置；③基于到達角度(angle of arrival,AOA),其所需硬件設(shè)備復雜,且需要視距傳輸,一般與要與其他定位方法相結(jié)合；④基于到達時間差 (time difference of arrival,TDOA)算法,與上文的TOA算法相比,它不僅降低了TOA對基站和標簽要同步的高要求,而且具有較低的設(shè)備復雜度[3]。

自最小二乘(least square,LS)算法、Chan氏算法、Taylor算法被用在室內(nèi)定位技術(shù)以來,國內(nèi)外很多學者針對無論是視距環(huán)境還是非視距環(huán)境都提出了自己的方法。其中,Chan氏算法利用兩步加權(quán)最小二乘法(weighted least squares,WLS)可以在視距( line of sight,LOS)環(huán)境中達到很好的定位效果[4]。但在非視距環(huán)境中,Chan算法定位精度下降較為明顯[5]。Taylor算法較Chan算法相比,精度有所提升,但Taylor算法需要一個精度較高的初始值,否則可能出現(xiàn)Taylor算法不收斂的現(xiàn)象[6]。西南交通大學移功通信研究所提出的Chan-Taylor聯(lián)合算法,相較于單獨的Chan和Taylor算法雖然提高了精度,但還存在一定數(shù)量受非視距誤差影響的觀測值沒有篩除,定位精度沒有達到理想的最優(yōu)狀態(tài)[7]。針對非視距環(huán)境中TDOA測量值建立的基站與標簽之間關(guān)系方程為非線性的情況,武大測繪遙感信息工程實驗室提出了一種擴展卡爾曼濾波器(extended Kalman filter,EKF)的定位方法,可以達到亞米級的實時動態(tài)定位精度。但是EKF必須對非線性方程的雅各比矩陣進行求解,計算量大且容易出錯[8]。河南工業(yè)大學學者提出了一種基于Chan和改進無跡卡爾曼濾波器(unscented kalman filter,UKF)的UWB室內(nèi)聯(lián)合定位算法,雖然簡化了UKF的步驟,但采用Cholesky分解在迭代中出現(xiàn)協(xié)方差矩陣為非正定時,算法將發(fā)生錯誤,不能保證算法的穩(wěn)定性[9]。

針對上述問題,為了盡可能降低室內(nèi)環(huán)境中非視距(non line of sight,NLOS)的影響和在保證算法穩(wěn)定性的前提下提高定位結(jié)果的精度,本文結(jié)合TDOA定位方法的特點和上述算法的優(yōu)點,分別從TDOA值的獲取、TDOA值的篩選和TDOA的算法3個方向入手,對Chan、Taylor、UKF算法進行聯(lián)合和改進。在對待受非視距誤差影響較大的觀測值時,在Chan、Taylor算法設(shè)置閾值以對誤差較大的測量值進行篩除。而在針對狀態(tài)方程為線性的實際情況時,對標準UKF進行精簡,只對測量方程進行無跡(UT)變換;針對平方根法(Cholesky)分解易產(chǎn)生不正定的問題,采用奇異值分解進行代替,最后將本文提出的方法與改進前定位算法進行對比分析。

1 TDOA的原理

1.1 TDOA簡介

TDOA是一種利用信號到達不同基站的時間差進行定位的方法,它又被稱為雙曲線定位[10]。根據(jù)雙曲線的基本原理,到已知兩點為常數(shù)的點一定處于這兩點為焦點的雙曲線上。那么有3個已知點就會有3條雙曲線,而3條雙曲線的焦點就是標簽的位置。如圖1所示為TDOA定位原理示意圖,A、B、C為基站位置,雙曲線的焦點T即為標簽位置。

圖1 TDOA原理圖

1.2 TDOA的獲取及其評估標準

1.2.1TDOA的獲取

TDOA可以通過兩種方法獲取,一種是互相關(guān)法,另一種是間接計算法[11]?；ハ嚓P(guān)法是通過兩個傳感器來獲得信號間的互相關(guān)最大化來獲得信號到達兩基站的時間延誤。而間接計算法是首先獲得信號到達不同基站的TOA值,然后再通過不同基站的TOA差值來得到TDOA的值。本文使用的是DWM1000定位模塊對標簽進行測量,它的數(shù)據(jù)傳輸率可高達6.8 MB/s,對于多路徑有更強的抗干擾能力,可以直接測量出標簽到各基站的距離,進而經(jīng)過計算獲得TDOA值。

1.2.2評估標準

均方根誤差(root mean square error,RMSE)表示預測值和觀測值之間差異的樣本標準差[12]。由于RMSE的評估方法更容易受到異常值的影響,在采用不同算法得到估計坐標后,可以更明顯地看出估計坐標值與參考值的差異性。所以本文主要根據(jù)定位值的RMSE對各種定位算法的定位性能進行評估。

2 TDOA的定位算法

2.1 Chan氏定位算法

Chan氏算法是基于雙曲線交點的定位方法。它是根據(jù)TDOA測量值,通過TDOA與光速的乘積獲得主基站到其余基站的距離差,繼而根據(jù)已知的基站坐標,列出基站坐標與距離差的雙曲線方程組,再基于最小二乘原理進行兩次迭代估算出未知點坐標。流程如下。

(1)列出TDOA雙曲線模型式,如式(1)所示。

(1)

(2)對初始非線性TDOA方程組的線性化,即對式(2)進行線性化,得到一系列線性方程組如式(3)所示,其中,i代表基站的個數(shù)。

(3)第一次WLS,估計結(jié)果如式(4)所示。

(4)

(4)第二次WLS估計結(jié)果如式(5)所示。

(5)

Chan氏算法在測量噪聲較小時,定位精度較高,可以達到厘米級,但如果存在非視距誤差,定位精度將會不同程度的下降[13]。

2.2 Taylor定位算法

Taylor是一種迭代算法,它的本質(zhì)是求解位置估計誤差的局部最小二乘解,通過不斷迭代來修正待定位標簽的位置的估計值[14]。令標簽初始估計值為(x0,y0)。則Taylor算法可以分為在(x0,y0)處的泰勒級數(shù)展開和計算偏差值的WLS估計兩個步驟。

Taylor算法在初始值與真實標簽位置接近時,精度相較于Chan氏算法會有所提高。但如果初始估計值偏差較大,Taylor算法可能將不會收斂,標簽坐標估計將出現(xiàn)錯誤,標簽位置將難以估計。

2.3 UKF定位算法

UKF濾波算法作為目前處理非線性系統(tǒng)的有效算法,與EKF相比較,它不需要對非線性方程求解雅可比矩陣,并且對高階項進行了保留,既降低了算法復雜度又提高了精度,在定位系統(tǒng)中得到了廣泛的應用[15]。本文僅列舉標準UKF的主要幾個步驟,詳細的描述可參見文獻[15]。

(1)建立線性狀態(tài)方程和非線性觀測方程，見式(6)和式(7)。

式中，F(xiàn)和H分別為狀態(tài)矩陣和觀測矩陣，Wk,Vk分別是狀態(tài)系統(tǒng)h和觀測系統(tǒng)的高斯白噪聲，其均值為0，且相互獨立。

(2)Cholesky協(xié)方差陣分解。

Cholesky把矩陣分解為一個下三角矩陣和它的轉(zhuǎn)置矩陣的乘積,為了保證這個分解一定存在且唯一,其分解的矩陣必須為實對稱正定矩陣,令要分解的矩陣分別為A和P*(n+λ),則原理式和在Matlab中的應用式分別如式(8)和式(9)所示。L和LT分別為上三角矩陣和其轉(zhuǎn)置的矩陣。

(3)測量更新方程，見式(10)～式(13)。

3 基于TDOA定位模型的算法改進

在基于TDOA的定位系統(tǒng)中,一般的定位算法常采用基于TOA的間接法來獲取TDOA值,且在實際應用環(huán)境中,我們采集的數(shù)據(jù)都不同程度地收到了NLOS誤差的影響,將這些獲取的TDOA值直接帶入上述算法中將會增大定位誤差,本文針對這幾種情況,對此做出改進，過程如下：

(1)本文采用基于TOF的DS-TWR測距方法來獲取TOA值,然后以A基站為主基站,將標簽到其他基站的距離與到A基站的距離相減得到TDOA值。

(2)將得到的TDOA值代入改進的Chan算法中,即在Chan算法進行兩次加權(quán)最小二乘之前加入篩選一步,以篩除與實際距離明顯差距較大的TDOA值,之后再進行Chan算法,在Matlab中代碼如下所示。

r=Z(j,:)'-Fenmu';

flag=[abs(r)>閾值];

if [flag(1) || flag(2) || flag(3) || flag(4)]

continue;

end

其中,Z為測量的標簽到4個基站的距離；Fenmu為標簽到基站真實的距離,代碼表示如果差值大于閾值,則將其篩除掉,如果小于,則循環(huán)繼續(xù)。

(14)

(4)進行NLOS判斷與篩選,設(shè)置門限值為d1,與殘差值進行比較,剔除包含較大NLOS誤差的TDOA測量值，見式(15)。

(15)

當門限值小于殘差值時,說明Chan氏算法計算獲得的估計坐標所對應的TDOA測量值存在較大偏差,將對應的一組TDOA值剔除。若殘差值在門限值范圍內(nèi),則表示所對應TDOA值合乎要求,可進行下一步程序。

(6)設(shè)置門限值為d2,與殘差值進行比較,進行第二次篩選,剔出包含較大NLOS誤差的TDOA 測量值，見式(16)。

(16)

(7)定位標簽位置坐標估計。

將最終篩選出的較小誤差的TDOA測量值代入改進的UKF濾波算法中,將標準UKF中UT變換的初始化改為式(17)。

(17)

(8)用SVD分解代替Cholesky分解,如式(18)和式(19)所示。

(9)其余步驟不變,運行程序得出結(jié)果。

4 數(shù)據(jù)采集與結(jié)果分析

4.1 UKF定位算法

本文實驗選取山東科技大學J6-534教室作為實驗環(huán)境,本次實驗采用的是四基站定位,選定教室靠墻的四周布設(shè)基站,中間放置幾個桌子作為遮擋物,并在基站與標簽的路徑上放置了板凳增加遮擋物,同時加上了人的走動來模擬實際室內(nèi)真實的環(huán)境,之后量測基站間距離,在軟件上設(shè)置基站坐標,設(shè)定5個真實參考標簽坐標作為估計值與真實點的比較。進行測量時,將標簽放置在真實坐標上進行觀測,每一個點觀測30 s,最后存貯觀測的位置坐標和距離信息。測量的數(shù)據(jù)為標簽到基站的距離。

4.2 數(shù)據(jù)處理與對比

分別利用LS算法、標準Chan算法、改進的Chan算法(即在Chan算法中加入閾值對NLOS誤差較為明顯的TDOA值進行篩除一個步驟)、Chan-Taylor聯(lián)合算法、改進的Chan-Taylor聯(lián)合算法及本文算法對測量數(shù)據(jù)進行解算,計算出標簽RMSE及標簽的定位誤差值。

4.2.1 在進行濾波前的Chan-Taylor聯(lián)合算法

基于Chan、Taylor算法的優(yōu)點,本文采用改進的Chan-Taylor算法來對含有NLOS誤差的TDOA值進行處理,如圖2所示為本文算法與單獨的CHAN、LS、初始的Chan-Taylor算法的RMSE對比,通過圖2可看出,改進的Chan-Taylor算法與其余幾種算法對比,在精度上進行了一定的提升。

圖2 均方根誤差對比圖

4.2.2對UKF的改進

由于基于TDOA方法的定位狀態(tài)方程為線性,且標準UKF濾波算法在UT變換采用Choleskey分解構(gòu)造sigma點時易產(chǎn)生不正定,本文對標準UKF算法進行簡化,只進行一次UT變換,并在進行sigma點構(gòu)造時用SVD代替Choleskey分解,最后將改進的Chan-Taylor聯(lián)合算法篩選的TDOA值代入改進的UKF中進行RMSE對比。如圖3為本文算法與改進的Chan-Taylor算法、Chan-Taylor-EKF、Chan-Taylor-為改進的UKF的RMSE對比。

圖3 均方根誤差對比圖

從圖3可看出,雖然在3號點處Chan-Taylor-EKF比較Chan-Taylor-UKF的精度更高,但是在進行UKF的改進后,將篩選過后的數(shù)據(jù)帶入,本文算法的精度更高,并且SVD分解以其不要求協(xié)方差陣正定則保證了程序的穩(wěn)定性。

接下來對比UKF與SVD-UKF-(只進行一次UT變換和采用SVD分解進行sigma點求解)的運行時間(表1),采用SVD分解和對UKF的簡化使得運行時間較標準UKF具有一定的優(yōu)勢。

表1 運行時間 單位：s

為了進一步觀察本文算法的優(yōu)勢，選取3、4號點處數(shù)據(jù)進行處理，之后對其處理后兩點的跟蹤誤差曲線進行對比，如圖4、圖5所示分別為3、4號點用本文算法解算得到的誤差曲線圖。

通過對3、4號點數(shù)據(jù)的誤差曲線進行繪制可看出，與Chan-Taylor-UKF算法相比，本文算法的誤差曲線與其近似一致，說明本文算法的在使用SVD分解并沒有對精度造成了損失，而速度與前者相比分別提高了55.6%、35.3%，本文算法在保證了精度的同時提高了運行速度。

圖4 3號點誤差曲線圖

圖5 4號點誤差曲線圖

5 結(jié)束語

本文針對復雜室內(nèi)環(huán)境下，測量數(shù)據(jù)易受到NLOS誤差的影響而導致定位精度下降的問題，提出了一種Chan-Taylor-SVDUKF聯(lián)合定位算法。經(jīng)代入實驗后，實驗結(jié)果表明：

(1)采用改進的Chan-Taylor算法的估計值作為NLOS誤差的篩選可以降低測量值受NLOS誤差的影響,為后續(xù)研究降低NLOS對測量數(shù)據(jù)的影響提供了參考。

(2)基于標準UKF濾波算法,去掉對狀態(tài)方程進行的UT變換,只對測量方程進行UT變換,可在保證算法的精度上提高了算法的運行速度。

(3)在UT變換中,用SVD分解代替Choleskey分解,然后再把Chan-Taylor聯(lián)合算法篩選出來的數(shù)據(jù)帶入改進的UKF得到的定位結(jié)果會不但具有較高的定位精度,也會保證算法的穩(wěn)定性。

(4)實驗可能存在一些局限性,例如本文的實驗數(shù)據(jù)采集僅是在一間教室進行的,另外的遮擋物也只是教室的桌椅,安排的人員走動也較少,并沒有在多個室內(nèi)環(huán)境中進行實驗,未來將針對更多的室內(nèi)環(huán)境做進一步研究。