一種基于UWB的CTK-IDS聯合定位算法

王文慶,朱 梁,吳益凡

(西安郵電大學 自動化學院,陜西 西安 710121)

目前,全球衛星定位系統(Global Position System,GPS)和北斗導航衛星系統具有應用范圍廣、定位精度高等特點,在戶外可以提供高精度的定位服務[1]。但是,室內環境復雜,衛星信號容易受到障礙物、建筑物的影響,導致衛星定位無法在室內實現精準定位。隨著相關室內活動的日益豐富,人們對于室內定位服務的需求也在日益增加[2]。超寬帶(Ultra Wide Band,UWB)定位技術作為一種近年來新型的技術,因其精度高、功耗低、時間分辨率高及抗噪聲能力強等優點受到廣泛關注[3]。

UWB常見的定位模式[4-9]主要包括基于信號到達時間差(Time Difference of Arrival,TDOA)、基于信號到達角度(Angle of Arrival,AOA)、基于信號到達時間(Time of Arrival,TOA)及基于到達信號強度(Received Signal Strength Indication,RSSI)。TDOA通過測量UWB信號從UWB標簽到基站之間傳播時間的差值[4],得到UWB標簽到基站之間的距離差,不要求基站與標簽遵循嚴格的時間同步。AOA定位一般是基于相位差的方式計算出角度,對硬件設備要求較高[5]。TOA基于空中傳播時間求出基站與標簽的距離,要求發送設備和接收設備時鐘始終同步[6]。RSSI是將接收信號強度與傳播距離的路徑損耗進行定位,環境的變化對定位精度影響極大[7]。為了準確獲得UWB標簽坐標值,需要準確獲取基站和標簽之間的距離值,飛行時間測距(Time-of-Flight,TOF)屬于點對點的雙向測距技術,其主要利用信號在兩個異步收發機之間的傳輸時間測量節點間的距離,測距結果相較于其他測距定位算法更加精準[8]。因此,選用TOF技術測量基站與標簽之間的距離值。

近年來,UWB定位技術主要應用于工廠倉庫、監獄、展廳及施工場地等室內場景,隨著UWB技術的發展,其在室內運動方面的應用逐漸引起重視。室內運動定位系統可以對運動員進行實時定位,能夠幫助教練更加全面、系統和具體地了解到運動員的訓練情況,從而制定科學、準確的訓練策略。文獻[9]研究了UWB系統用于籃球運動中的精度與運動員速度干擾問題,提出了一種評估位置數據精度與速度干擾間可靠性的方法。文獻[10]分析了UWB系統在曲線運動區間的向心力測量精度。Alejandro Bastida Castillo等[11]研究了UWB系統用于足球運動,對比了誤差精度與系統覆蓋距離的關系。文獻[12]比較了UWB系統和GPS系統在足球運動中的定位精度。文獻[13]運用UWB定位系統在室內場地進行步行、慢跑、加速跑和折返跑,測量了運動中的距離、速度及加速度的誤差和變化。但是,上述研究沒有提出一種具體的提高室內運動定位抗干擾性能的算法。

為了能夠滿足室內運動定位的高精度定位需求,擬提出一種改進的Chan-泰勒-卡爾曼雙邊雙向(Chan-Taylor-Kalman-Improved Double-Sided,CTK-IDS)聯合定位算法。用改進的雙邊雙向測距(Improved Double-sided Two-way Ranging,IDS-TWR)算法計算信號傳播時間,然后將Chan算法估計的位置坐標作為Taylor算法的初值點進行迭代,從而解算出運動員的位置坐標。最后,結合Kalman濾波進行坐標的二次估計,獲得標簽坐標值,以期實現提升測距精度,提高室內運動定位的準確度和系統抗干擾能力。

1 TOF測距算法

在UWB定位系統中常用的TOF測距方式有單邊雙向(Single-sided Two-way Ranging,SS-TWR)測距和雙邊雙向(Double-sided Two-way Ranging,DS-TWR)測距兩種。

1.1 SS-TWR和DS-TWR測距算法

SS-TWR是一種通過一次數據往返就能夠測出兩個節點間距離的方法[14]。DS-TWR是在SS-TWR的基礎上,再增加一次信息傳遞,其優點是響應時間不用嚴格相同,測距精度比SS-TWR更高。但是,DS-TWR計算相對復雜,而且時鐘偏移引起的誤差始終存在,沒有考慮環境因素帶來的誤差干擾[15]。SS-TWR測距流程與DS-TWR測距相比,只進行了一次信息傳遞,下面重點圍繞DS-TWR測距方法進行敘述。DS-TWR測距原理示意圖如圖1所示。

圖1 雙邊雙向測距原理

在圖1中,DS-TWR測距的具體流程是標簽A發送請求數據,記錄發送時間T1;基站B收到請求數據,記錄接收時間R1;基站延遲一段時間后在T2時刻發送響應數據,該時間為基站延遲時間;標簽A收到響應數據,記錄接收時間R2,標簽A短暫延遲后,在T3時刻發送結束數據,標簽A接收基站B的信號,等待一段時間發回至基站B,該段時間稱為標簽延遲時間;基站B接收結束數據,記錄接收時間R3。為便于下文公式的解釋,定義標簽A發送數據到標簽A接收數據的時間差為標簽發送數據和接收時間差。定義基站B發送信號至標簽A,到標簽A發回信號至基站B的時間差為基站發送和接收數據時間差。

基站延遲時間、標簽發送數據和接收數據時間差、標簽延遲時間及基站發送數據和接收數據時間差的表達式為分別為

Try,B=T2-R1Trd,A=R2-T1Try,A=T3-R2Trd,B=R3-T2

因為信號在收發過程中存在傳播時間,標簽發送數據和接收數據時間差、基站發送數據和接收數據時間差可以進一步表示為

Trd,A=2Ttof+Try,B

(1)

Trd,B=2Ttof+Try,A

(2)

聯立式(1)和式(2)兩個方程,信號傳播時間Ttof可以表示為

(3)

考慮時鐘偏移,假設標簽A和基站B的時鐘偏移分別是eA和eB,則式(3)可以進一步表示為

(4)

由式(3)和(4)可以得到DS-TWR測距誤差為

(5)

由式(5)可知,時鐘漂移的大小影響測距精度,為得到更為精確的測距值,需要對測距算法改進。

1.2 改進雙邊雙向測距算法

1.2.1 距離計算修正

通過對考慮時鐘偏移后的測距誤差分析可知,時鐘漂移對測距精度的影響比較大。因此,在DS-TWR的算法上,文獻[16]進一步改進時間計算距離的方法。

對式(1)和式(2)進行變換,可得

Trd,A·Trd,B=(2Ttof+Try,B)(2Ttof+Try,A)

(6)

則信號傳播時間Ttof可以表示為

(7)

代入時鐘漂移eA和eB,式(7)變換為

(8)

由式(7)和式(8)可以得到IDS-TWR測距誤差為

(9)

1.2.2 預測擬合修正

空曠的室內環境中信號的接發沒有障礙物阻擋,UWB信號的強度與傳輸時間能夠精確的得到[17]。但是,應用到實際室內運動場景中時,信號傳播過程中一定會受到觀眾、教練的阻擋,從而干擾信號傳輸,增大測距誤差。為減小環境因素造成的誤差,可以通過線性擬合的方式對測量距離進行預處理,通過測量值尋求距離真實值的最優估計值。

假設在一個UWB定位系統中,基站與標簽的位置不變,用DS-TWR的方法多次測量基站與標簽之間距離,得出距離平均值為L。多次改變基站與標簽的距離,用傳統的DS-TWR多次測量距離,求得的平均值分別為L1,L2,L3,…,Ln。對測量平均值需要用數學統計分析里面的線性擬合進行處理,利用線性擬合結果對測量數據進行數據標定。

為了對比算法的精度,需要用標準差和均方根誤差判斷誤差精度的優劣,標準差和均方根誤差值越小,算法的性能就越好。標準差和均方根誤差的表達式分別為

(10)

(11)

式中:Xi為測量值;X為測量期望值;N為測量次數;dN為參考值與測量值之間的偏差。

2 基于TDOA的室內定位算法

TDOA是基于信號到達時間差的定位模式。該模式是根據標簽到兩個基站的距離建立雙曲線方程[18],然后根據多基站建立雙曲線方程組求解獲得目標節點的位置。設移動標簽的坐標為(x,y),有N(N≥3)個基站,坐標為(xi,yi),i=1,2,…,N。選擇基站1為主基站,可以得到主基站到其他基站的距離差為

di,1=cti,1=di-d1,i=1,2,...,N

(12)

式中:di為基站i到標簽的距離;ti,1為移動標簽到基站i與1號基站的時間差值,c為信號傳播速度,約等于真空中的光速。又由二維空間的歐式距離公式可以進一步得到

(13)

由式(12)和式(13)可得聯合方程

(14)

其中,

式中:xi,1為基站i與基站1的橫向差值;yi,1為基站i與基站1的縱向差值。

式(14)是未知數為x,y和d1的線性方程,可以通過Chan算法和Taylor算法計算出移動標簽的坐標。

2.1 Chan算法

Chan算法可以只使用3個基站定位,也可以通過添加基站提高其定位精度。當基站數在4個及4個以上時,算法進行兩次加權最小二乘估計求解出標簽估計值。

1)當定位場景中只存在3個基站時。將式(14)組成矩陣形式方程為

(15)

將式(15)代入式(13)能得到關于d1的兩個解,能夠根據先驗知識和坐標范圍排除其中一個解,就可以得到d1,再由式(15)得到標簽的估計坐標。

2)當定位環境中存在4個及4個以上固定基站時。設Za=(x,y,d1)T,將其中的3個元素x,y,d1作為自變量,由式(14)可得到線性方程組

h-GaZa=0

(16)

其中,

在h,Ga矩陣可逆的情況下,由式(16)基于最小二乘法可獲得初始解

(17)

式中,ψ=c2BQB為誤差矢量,B=diag(d1,d2,…,dn) 為基站與標簽的距離距陣。Q是TDOA測量值誤差的協方差矩陣,可近似表示為

(18)

在實際測量中,先用式(18)計算出標簽初始位置,估計計算對應B矩陣和ψ的近似值。再用式(17)得到第一次加權最小二乘解的估計結果Za。估計結果與真實值的關系分別為

Za,1=x+e1Za,2=y+e2Za,3=d1+e3

Za是在假設Za中的3個元素相互獨立的情況下得到的,但在實際中Za的3個元素相關,而且Q近似代替ψ也會有誤差。因此,用Za的元素重新構造一個方程組,再進行第二次加權最小二乘估計可以減小誤差,其表達式為

(19)

其中,

再經過第二次加權最小二乘解估計出最終位置估計坐標信息為

(20)

2.2 Taylor算法

Taylor算法是一種需要初始估計位置的遞歸算法,優點在于可以應用于各種環境,并能在一定程度上抑制非視距誤差(Non-Line of Sight,NLOS)。

假設有一個初始估計位置坐標為(x0,y0),參考坐標為(x,y),Δx,Δy為坐標偏差。在(x0,y0)處進行泰勒展開,忽略二階及以上的高階分量,式(14)可以轉換為

ψ=h-Gaδ

(21)

其中,

不斷使用最小二乘算法迭代計算δ,直至初始值滿足|Δx+Δy|<ε,ε為設定的閾值。

2.3 CTK-IDS聯合定位算法

為克服Chan算法在NLOS環境下精度低,以及Taylor算法初始值選取困難的問題,引入Kalman濾波技術加以改進。先用IDS-TWR算法獲得TDOA值,然后利用Chan算法得到標簽的初始估計坐標,進而將該坐標作為Taylor算法迭代的初始點,得到更為精準的標簽估計坐標。最后利用Kalman濾波對估計坐標進行二次估計,去除環境因素導致的異常數據,使定位結果更加精確和穩定。

在獲得Chan-Taylor算法解算出的標簽坐標之后,運用卡爾曼濾波對坐標進行二次估計,使得定位結果更加精確和穩定。卡爾曼濾波算法是使用上一次的最優估計結果預測當前狀態的值,同時用當前狀態的觀測值修正,從而得到當前狀態的最優估計結果。

卡爾曼濾波的狀態方程和觀測方程的表達式分別為

xt=Fxt-1+But+WtZt=Hxt+vt

式中:xt為t時刻的狀態;ut為系統的控制量;Wt為過程噪聲;zt為觀測量;vt是觀測噪聲;F為狀態轉移矩陣;B為輸入控制矩陣;H為觀測矩陣。

運用卡爾曼濾波進行二次估計第一步是狀態預測,根據前一時刻的狀態和系統輸入量預測當前時刻的狀態,并計算出估計值的協方差矩陣。第二步是進行更新,引入卡爾曼增益,將當前時刻的測量值與估計值反饋給當前狀態的估計值,根據更新后的協方差矩陣和卡爾曼增益計算當前時刻的最優估計。

將Chan-Taylor融合算法估計的標簽坐標信息作為卡爾曼濾波的輸入值,進行標簽位置坐標的二次估計,最后得到標簽位置的最優估計值。

2.4 不同算法的仿真對比

為了更直觀地體現出CTK-IDS聯合定位算法的性能,通過仿真軟件將Fang算法,Chan算法,Taylor算法,Chan-Taylor算法與CTK-IDS算法進行對比。

實驗環境配置的仿真場地參照標準籃球場半場,大小為15 m×15 m,4個基站的位置信息分別為BS1(0,0),BS2(15,0),BS3(15,15),BS4(0,15)。

設定標簽參考坐標為(1300,1400)(單位:cm),不同算法運行各得到2 000組估計點。為了更直觀地對比出散點分布的差異,將不同算法的散點分布放在一起比較,結果如圖2所示,相較于其他算法,CTK-IDS估計位置的離散度最小,即定位精度最高。

圖2 不同算法散點分布比較

累計分布函數(Cumulative Distribution Function,CDF)是對概率密度函數進行積分得到,可以用于描述定位精度的好壞,X軸為定位誤差,Y軸為誤差累計概率,CDF值越大表示定位精度越高。對不同算法的定位結果進行CDF構建,比對情況如圖3所示。CTK-IDS算法定位的結果90%以上都在8 cm范圍內。表1為不同算法進行5×2 000次位置估計的平均誤差和均方根誤差,CTK-IDS算法的平均誤差和均方根誤差最小。因此,CTK-IDS算法相比于Fang算法、Chan算法、Taylor算法和Chan-Taylor算法精度有所提高。

表1 不同算法進行4×2 000次位置估計的平均誤差、均方根誤差/cm

圖3 不同算法的CDF對比結果

3 驗證結果及分析

為了進一步驗證CTK-IDS聯合定位算法的性能,將分別進行測距實驗和室內軌跡定位實驗,實驗需要4個基站和1個標簽。

3.1 測距實驗

在室內籃球場搭建實驗環境,將基站和移動標簽分別放置在同一高度,分別用DS-TWR和IDS-TWR測距方法進行測距實驗,基站A與標簽進行測距實驗,由3 m到21 m,每隔3 m測一次距離,每次測6 000組數據,采樣頻率為1 s測100組數據。

為了可以直觀表現出IDS-TWR的測距精度優于DS-TWR,分別進行了7組測距實驗,具體的測距結果分別如表2、表3和表4所示。

表2 DS-TWR測距結果 單位：cm

表3 未進行線性擬合的測距結果 單位：cm

表4 IDS-TWR測距結果 單位：cm

表2是DS-TWR測距方法進行實驗的數據結果,表3是未進行線性擬合的數據結果。由表2和表3可以看出,算法進行距離修正之后,DS-TWR算法測距精度提升了,但在測距實驗中存在一些環境因素降低了測距精度,導致測距誤差還是很大。因此,需要對測距值進行擬合修正,降低測距誤差。對測距值取平均,并對其線性擬合,線性擬合結果如圖4所示。

圖4 線性擬合結果

由表2和表4可以得到IDS-TWR相比較于DS-TWR,測距平均誤差降低,均方根偏差明顯減少。由表3和表4可以得到,數據標定后的測距結果平均誤差減少了,均方根誤差明顯降低。為了直觀對比出DS-TWR算法和IDS-TWR算法測距誤差,分別在測距誤差為3 m和18 m的情況下進行對比,具體的測距誤差分布對比情況如圖5所示。由圖5可以看出,在3 m測距誤差分布中,DS-TWR誤差分布在7～27 cm,IDS-TWR誤差分布在8 cm以內;在18 m測距誤差分布中,DS-TWR誤差分布在5～18 cm,IDS-TWR誤差分布在7 cm以內。因此,IDS-TWR測距誤差要比DS-TWR測距誤差偏差更小。

圖5 DS-TWR與IDS-TWR算法測距誤差對比結果

從實驗結果可以看出,DS-TWR測距誤差明顯大于IDS-TWR測距誤差,且誤差分散較大。但是,IDS-TWR算法測距誤差在3 cm以內,測距結果相對穩定,與DS-TWR算法相比誤差波動較小。

3.2 軌跡定位實驗

預設軌跡定位測試可以分成兩個移動軌跡測試,采用CTK-IDS算法對運動員進行定位。首先,以(350,350)為起點,經(800,350)、(800,850)、(350,850)作一個長為450 cm,寬為500 cm的矩形。其次,以(800,500)為圓心作半徑為300 cm的圓。圖6中線條為預設軌跡,運動員攜帶移動標簽按照預設路徑進行跑動,采集運動員位置信息,解算標簽位置,通過上位機實時顯示運動員移動軌跡,可以直觀看到室內運動定位系統的定位性能。運動員運動軌跡如圖6所示。在圖6中,可以看出運動員移動軌跡平滑,軌跡偏離的地方誤差最大也不超過13 cm,誤差波動小,能夠滿足室內運動定位的要求。

圖6 預設軌跡定位結果

4 結語

利用UWB實現運動員在室內環境中的實時精準定位,在IDS-TWR測距算法、Chan算法、Taylor算法與Kalman濾波的基礎上提出了CTK-IDS聯合定位算法。通過測距計算修正與預測擬合技術,采用IDS-TWR算法計算信號傳播時間。同時,將Chan算法估計的位置坐標作為Taylor算法的初值點進行迭代,結合Kalman濾波進行坐標的二次估計,從而實現運動員室內運動環境中的實時、精準定位。比對實驗結果表明:CTK-IDS算法在定位精度和抗干擾能力比其他算法要更好;IDS-TWR測距算法能夠將測距平均誤差控制在4 cm以內,測距誤差波動小;CTK-IDS聯合定位算法定位的最大定位誤差不超過13 cm,定位誤差波動小、數據較為穩定,定位精度高,能夠滿足室內運動定位的要求。