王文慶,朱 梁,吳益凡

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

目前,全球衛(wèi)星定位系統(tǒng)(Global Position System,GPS)和北斗導(dǎo)航衛(wèi)星系統(tǒng)具有應(yīng)用范圍廣、定位精度高等特點,在戶外可以提供高精度的定位服務(wù)[1]。但是,室內(nèi)環(huán)境復(fù)雜,衛(wèi)星信號容易受到障礙物、建筑物的影響,導(dǎo)致衛(wèi)星定位無法在室內(nèi)實現(xiàn)精準定位。隨著相關(guān)室內(nèi)活動的日益豐富,人們對于室內(nèi)定位服務(wù)的需求也在日益增加[2]。超寬帶(Ultra Wide Band,UWB)定位技術(shù)作為一種近年來新型的技術(shù),因其精度高、功耗低、時間分辨率高及抗噪聲能力強等優(yōu)點受到廣泛關(guān)注[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定位一般是基于相位差的方式計算出角度,對硬件設(shè)備要求較高[5]。TOA基于空中傳播時間求出基站與標簽的距離,要求發(fā)送設(shè)備和接收設(shè)備時鐘始終同步[6]。RSSI是將接收信號強度與傳播距離的路徑損耗進行定位,環(huán)境的變化對定位精度影響極大[7]。為了準確獲得UWB標簽坐標值,需要準確獲取基站和標簽之間的距離值,飛行時間測距(Time-of-Flight,TOF)屬于點對點的雙向測距技術(shù),其主要利用信號在兩個異步收發(fā)機之間的傳輸時間測量節(jié)點間的距離,測距結(jié)果相較于其他測距定位算法更加精準[8]。因此,選用TOF技術(shù)測量基站與標簽之間的距離值。

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

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

1 TOF測距算法

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

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

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

圖1 雙邊雙向測距原理

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

基站延遲時間、標簽發(fā)送數(shù)據(jù)和接收數(shù)據(jù)時間差、標簽延遲時間及基站發(fā)送數(shù)據(jù)和接收數(shù)據(jù)時間差的表達式為分別為

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

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

Trd,A=2Ttof+Try,B

(1)

Trd,B=2Ttof+Try,A

(2)

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

(3)

考慮時鐘偏移,假設(shè)標簽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 預(yù)測擬合修正

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

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

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

(10)

(11)

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

2 基于TDOA的室內(nèi)定位算法

TDOA是基于信號到達時間差的定位模式。該模式是根據(jù)標簽到兩個基站的距離建立雙曲線方程[18],然后根據(jù)多基站建立雙曲線方程組求解獲得目標節(jié)點的位置。設(shè)移動標簽的坐標為(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)可得聯(lián)合方程

(14)

其中,

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

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

2.1 Chan算法

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

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

(15)

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

2)當定位環(huán)境中存在4個及4個以上固定基站時。設(shè)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測量值誤差的協(xié)方差矩陣,可近似表示為

(18)

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

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

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

(19)

其中,

再經(jīng)過第二次加權(quán)最小二乘解估計出最終位置估計坐標信息為

(20)

2.2 Taylor算法

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

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

ψ=h-Gaδ

(21)

其中,

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

2.3 CTK-IDS聯(lián)合定位算法

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

在獲得Chan-Taylor算法解算出的標簽坐標之后,運用卡爾曼濾波對坐標進行二次估計,使得定位結(jié)果更加精確和穩(wěn)定?？柭鼮V波算法是使用上一次的最優(yōu)估計結(jié)果預(yù)測當前狀態(tài)的值,同時用當前狀態(tài)的觀測值修正,從而得到當前狀態(tài)的最優(yōu)估計結(jié)果。

卡爾曼濾波的狀態(tài)方程和觀測方程的表達式分別為

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

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

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

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

2.4 不同算法的仿真對比

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

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

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

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

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

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

圖3 不同算法的CDF對比結(jié)果

3 驗證結(jié)果及分析

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

3.1 測距實驗

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

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

表2 DS-TWR測距結(jié)果 單位：cm

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

表4 IDS-TWR測距結(jié)果 單位：cm

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

圖4 線性擬合結(jié)果

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

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

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

3.2 軌跡定位實驗

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

圖6 預(yù)設(shè)軌跡定位結(jié)果

4 結(jié)語

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