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

駱新偉 郭 英 劉寒爍 葉 瑾

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

0 引言

隨著網絡化、信息化和智能化時代的到來,室內定位技術近年來不斷發展。超寬帶技術因其較低的系統復雜性、良好的抗多徑效應以及較強的穿透性等優勢,廣泛應用于室內定位系統中[1]。

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

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

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

1 TDOA的原理

1.1 TDOA簡介

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

圖1 TDOA原理圖

1.2 TDOA的獲取及其評估標準

1.2.1TDOA的獲取

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

1.2.2評估標準

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

2 TDOA的定位算法

2.1 Chan氏定位算法

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

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

(1)

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

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

(4)

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

(5)

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

2.2 Taylor定位算法

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

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

2.3 UKF定位算法

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

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

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

(2)Cholesky協方差陣分解。

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

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

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

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

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

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

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

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

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

continue;

end

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

(14)

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

(15)

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

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

(16)

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

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

(17)

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

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

4 數據采集與結果分析

4.1 UKF定位算法

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

4.2 數據處理與對比

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

4.2.1 在進行濾波前的Chan-Taylor聯合算法

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

圖2 均方根誤差對比圖

4.2.2對UKF的改進

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

圖3 均方根誤差對比圖

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

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

表1 運行時間 單位：s

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

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

圖4 3號點誤差曲線圖

圖5 4號點誤差曲線圖

5 結束語

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

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

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

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

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