基于TOF的UWB可移動(dòng)基站快速自定位算法

韓玉香，張曉亮，張曉明，2，耿煜琛

(1.中北大學(xué)電子測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，太原 030051； 2.中北大學(xué)儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，太原 030051； 3. 濟(jì)南金豐源電子科技有限公司，濟(jì)南 250101 )

0 引言

現(xiàn)如今，越來(lái)越多的高新技術(shù)被應(yīng)用到各體育運(yùn)動(dòng)中,以更好地對(duì)運(yùn)動(dòng)員的運(yùn)動(dòng)狀況進(jìn)行分析[1-2]。其中，運(yùn)動(dòng)場(chǎng)中運(yùn)動(dòng)員的定位精度要求為分米級(jí)，更新率大于10Hz，可實(shí)時(shí)準(zhǔn)確顯示運(yùn)動(dòng)員的運(yùn)動(dòng)軌跡。目前常用的定位技術(shù)如超聲波定位技術(shù)[3]、藍(lán)牙定位技術(shù)[4]和WiFi定位技術(shù)[5]等，在覆蓋范圍及定位精度方面均不能滿足運(yùn)動(dòng)場(chǎng)高精度實(shí)時(shí)定位的要求；全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System， GNSS)[6-7]定位技術(shù)適合室外大范圍定位，單點(diǎn)定位精度低；實(shí)時(shí)動(dòng)態(tài)定位(Real-Time Kinematic，RTK)技術(shù)雖可實(shí)現(xiàn)單點(diǎn)實(shí)時(shí)定位，但所需設(shè)備復(fù)雜，操作難度大。

超寬帶(Ultra Wide Band，UWB)定位技術(shù)由于功耗低、系統(tǒng)復(fù)雜度低、多徑分辨率高、系統(tǒng)安全性高、定位精度可達(dá)分米級(jí)[8-9]等優(yōu)點(diǎn)，在室內(nèi)外定位領(lǐng)域得到了廣泛應(yīng)用[10]。傳統(tǒng)的UWB可移動(dòng)基站定位方法借用外部設(shè)備(如高精度全站儀)進(jìn)行人工測(cè)量以獲取基站位置[11]，雖然人工測(cè)量基站位置較為精確，但使用不便，需利用全站儀等工具進(jìn)行輔助測(cè)量，不但增加了使用成本，還導(dǎo)致UWB定位系統(tǒng)的基站布設(shè)工作量增加，限制了定位系統(tǒng)的可移動(dòng)性[12]。除人工測(cè)量之外，還可以將移動(dòng)基站定位在標(biāo)準(zhǔn)運(yùn)動(dòng)場(chǎng)的場(chǎng)地邊緣，以運(yùn)動(dòng)場(chǎng)的標(biāo)準(zhǔn)尺寸確定基站坐標(biāo)[13]。多數(shù)運(yùn)動(dòng)場(chǎng)地在建設(shè)時(shí)不能達(dá)到標(biāo)準(zhǔn)尺寸要求，并且基站在安裝時(shí)可能與場(chǎng)地邊緣不完全重合，導(dǎo)致基站坐標(biāo)存在誤差。 因此，本文提出了一種可移動(dòng)基站快速自定位方法。

為滿足運(yùn)動(dòng)場(chǎng)中可移動(dòng)基站快速定位的需求，利用基于飛行時(shí)間(Time of Flight，TOF)[14]的UWB可移動(dòng)基站快速自定位方法，建立局部坐標(biāo)系以獲取基站坐標(biāo)。根據(jù)需要在運(yùn)動(dòng)場(chǎng)中布設(shè)基站，通過各基站間互相通信即可快速定位基站的相對(duì)位置。該方法可減少定位基站的布設(shè)時(shí)間和成本，降低UWB移動(dòng)定位基站的布設(shè)難度，提高系統(tǒng)的可移動(dòng)性，達(dá)到快速精確確定運(yùn)動(dòng)場(chǎng)可移動(dòng)基站坐標(biāo)的目的。

1 定位算法

1.1 TOF測(cè)距原理(雙邊雙向測(cè)距)

假設(shè)運(yùn)動(dòng)場(chǎng)內(nèi)有n個(gè)定位基站，任意2個(gè)基站之間均可進(jìn)行通信，為減小基站的時(shí)鐘誤差帶來(lái)的測(cè)距誤差，基站之間進(jìn)行雙向測(cè)距，如圖 1所示。雙邊雙向測(cè)距分為兩次測(cè)距：基站j主動(dòng)發(fā)起第一次測(cè)距消息并記錄發(fā)送時(shí)間戳t0，基站i記錄接收時(shí)間戳t1并產(chǎn)生應(yīng)答信息，同時(shí)記錄發(fā)送時(shí)間戳t2，當(dāng)基站j收到數(shù)據(jù)之后記錄時(shí)間戳t3；第二次為基站j返回帶有時(shí)間戳t0、t3、t4的數(shù)據(jù)，基站i接收并記錄時(shí)間戳t5。最終可以得到4個(gè)時(shí)間差，根據(jù)時(shí)間差計(jì)算基站i到基站j的距離[15]。

令4個(gè)時(shí)間差分別為

treply1=t2-t1treply2=t4-t3tround1=t3-t0tround2=t5-t2

tround1為基站j發(fā)送輪詢信號(hào)到接收到基站i應(yīng)答信號(hào)的時(shí)間；tround2為基站i發(fā)送應(yīng)答信號(hào)到接收到基站j發(fā)送的測(cè)距信息的時(shí)間；treply1為基站i接收到輪詢信號(hào)到發(fā)送應(yīng)答信號(hào)的時(shí)間；treply2為基站j接收到應(yīng)答信號(hào)到發(fā)送測(cè)距信息的時(shí)間。

則進(jìn)行一次測(cè)距所需時(shí)間t可表示為

(1)

根據(jù)式(1)可進(jìn)一步得出距離d

d=ct+Δd

(2)

其中，c為無(wú)線電傳播速度；Δd為TOF測(cè)距誤差。

圖1 TOF測(cè)距原理圖Fig.1 TOF ranging schematic diagram

1.2 基站快速定位原理

由于運(yùn)動(dòng)場(chǎng)地面平坦，將地面定義為三維坐標(biāo)系的xoy平面。運(yùn)動(dòng)場(chǎng)內(nèi)任意不同的2個(gè)基站作為基站1和基站2。基站1和基站2在地面投影點(diǎn)的連線定義為三維坐標(biāo)系的x軸，將經(jīng)過基站1的地面法線定義為三維坐標(biāo)系的z軸，根據(jù)右手定則，得到y(tǒng)軸方向。利用測(cè)量工具測(cè)得各基站到地面的距離作為z軸坐標(biāo)，測(cè)量誤差在0.001m內(nèi)。不失一般性，假設(shè)基站均在xoy平面的第一象限內(nèi)，如圖2所示?；?的坐標(biāo)為(0,0,z1)，基站2的坐標(biāo)為(x2,0,z2)，基站i的坐標(biāo)為(xi,yi,zi)，基站j的坐標(biāo)為(xj,yj,zj)，則每2個(gè)基站之間的距離表示為

dij2=(xi-xj)2+(yi-yj)2+(zi-zj)2i=1,2,3，…，n；j=1,2,3，…，n；i≠j

(3)

圖2 基站快速定位原理圖Fig.2 Schematic diagram of base station rapid positioning

根據(jù)式(3)可得關(guān)于距離及基站坐標(biāo)的非線性方程組，本文采用牛頓迭代法求解基站坐標(biāo)。假設(shè)Xk為第k次迭代的坐標(biāo)值，則

Xk=[x1k,x2k,y2k…xik,yik…xnk,ynk]T

對(duì)式(3)所得的非線性方程組進(jìn)行泰勒展開得

(4)

其中，Δxi=xi-xik,i=2,3,…,n；Δyi=yi-yik,i=3,4,…,n。

由文獻(xiàn)[16]以及實(shí)驗(yàn)數(shù)據(jù)可知，距離值的測(cè)量存在誤差且誤差服從正態(tài)分布。由數(shù)理統(tǒng)計(jì)知識(shí)：當(dāng)誤差服從正態(tài)分布時(shí)，最小二乘是最優(yōu)線性無(wú)偏估計(jì)。通過最小二乘求解，使得測(cè)距誤差的平方和為最小。下文將通過上述泰勒展開式求方程的最小二乘解。

記系數(shù)矩陣Hk為

Hk=[Ak(m×n1)Bk(m×n2)]

其中

Xk迭代初始值由非線性方程組解得

(5)

其中

(6)

令各個(gè)基站到地面的距離差為Δz，則Δz可表示為

Δz={Δzij|1≤i≤n,1≤j≤n,i≠j}

令

ΔXk=[Δx2k,Δxik…Δxnk,Δy2k,Δyik…Δynk]T

令

ΔRk=f-fk

(7)

其中

f=[f12,f1i…fij]TXk=[x1,x2,…xi…xn,y2…yi…yn]Tfk=[f12|Xk,d1i|Xk…fij|Xk]T

則

ΔRk=HkΔXk

(8)

運(yùn)用最小二乘法解算ΔXk，得

(9)

第一次迭代的X1為

X1=ΔX+X0

(10)

通過牛頓迭代法對(duì)ΔXk進(jìn)行解算，當(dāng)ΔXk小于誤差閾值時(shí)停止迭代，此時(shí)的Xk+1為

Xk+1=Xk+ΔXk

(11)

最終基站x、y軸坐標(biāo)值為Xk+1，z軸坐標(biāo)值為基站和地面之間的距離，實(shí)現(xiàn)了基于TOF測(cè)距的UWB基站快速定位方式。

在設(shè)定的局部坐標(biāo)系內(nèi)求得基站坐標(biāo)后，在運(yùn)動(dòng)場(chǎng)自身坐標(biāo)系x′o′y′以及建立的局部坐標(biāo)系xoy內(nèi)選取典型位置點(diǎn)，求得坐標(biāo)轉(zhuǎn)換關(guān)系，將坐標(biāo)進(jìn)行平移旋轉(zhuǎn)即可得到基站在運(yùn)動(dòng)場(chǎng)內(nèi)的坐標(biāo)。

2 定位誤差傳播規(guī)律

基站坐標(biāo)ΔXk的期望為

(12)

基站坐標(biāo)ΔXk的方差為

(13)

(14)

每個(gè)基站坐標(biāo)處的誤差系數(shù)可表示為

(15)

通過誤差系數(shù)及測(cè)距標(biāo)準(zhǔn)差，可進(jìn)一步得到基站x坐標(biāo)和y坐標(biāo)的標(biāo)準(zhǔn)差

誤差系數(shù)反映了測(cè)距誤差和定位誤差的關(guān)系。根據(jù)文獻(xiàn)[17]，誤差系數(shù)與基站個(gè)數(shù)及基站布設(shè)方式有關(guān)。

3 仿真實(shí)驗(yàn)

假設(shè)UWB可移動(dòng)基站的測(cè)距誤差服從均值為0、標(biāo)準(zhǔn)差為0.1m的正態(tài)分布。通過上述定位算法解算基站坐標(biāo)，并進(jìn)行誤差系數(shù)及標(biāo)準(zhǔn)差計(jì)算，分析布設(shè)數(shù)目分別為4、6、8時(shí)，對(duì)UWB可移動(dòng)基站自定位精度的影響。

表1 基站坐標(biāo)仿真數(shù)據(jù)(4個(gè)基站)

表2 基站坐標(biāo)仿真數(shù)據(jù)(6個(gè)基站)

表3 基站坐標(biāo)仿真數(shù)據(jù)(8個(gè)基站)

3組仿真實(shí)驗(yàn)結(jié)果表明，基站數(shù)目多的一組相較于基站數(shù)目少的一組，測(cè)量誤差及標(biāo)準(zhǔn)差均減小。在100m×50m區(qū)域內(nèi)，布設(shè)6個(gè)UWB可移動(dòng)基站時(shí)，誤差均值及標(biāo)準(zhǔn)差均在誤差可接受范圍內(nèi)。布設(shè)8個(gè)UWB可移動(dòng)基站時(shí)，相對(duì)于6個(gè)基站布設(shè)情況，誤差標(biāo)準(zhǔn)差有所減少，但是減小效果不明顯。在100m×50m區(qū)域內(nèi)，布設(shè)6個(gè)基站即可達(dá)到理想定位精度，繼續(xù)增加基站數(shù)目，定位精度的提高效果并不明顯，同時(shí)提高了系統(tǒng)復(fù)雜度，使得定位速度變慢。因此，在長(zhǎng)為100m、寬為50m的運(yùn)動(dòng)場(chǎng)內(nèi)，布設(shè)6個(gè)UWB基站即可達(dá)到精確定位的目的。

以6個(gè)基站布局仿真圖中(50,0)為例，測(cè)量坐標(biāo)的誤差情況如圖3所示。

圖3 仿真結(jié)果與實(shí)際位置對(duì)比效果Fig.3 Comparison between simulation results and actual position

4 運(yùn)動(dòng)場(chǎng)實(shí)驗(yàn)

仿真結(jié)果表明，100m×50m的運(yùn)動(dòng)場(chǎng)內(nèi)6個(gè)UWB基站及8個(gè)UWB基站均可達(dá)到精確定位的目的?？紤]到運(yùn)動(dòng)場(chǎng)定位精度的要求，采用仿真實(shí)驗(yàn)中6個(gè)UWB基站的布局方式驗(yàn)證算法的可行性。在足球場(chǎng)布設(shè)基站，用全站儀進(jìn)行測(cè)量，全站儀測(cè)量精度為0.001m，如圖4所示?；?坐標(biāo)為(0,0,1)，測(cè)得其余5個(gè)基站坐標(biāo)情況如表4所示。6個(gè)UWB基站完成一次定位用時(shí)小于1s，可達(dá)到快速定位的目的。

圖4 基站布設(shè)環(huán)境Fig.4 Layout environment of base station

表4 運(yùn)動(dòng)場(chǎng)基站坐標(biāo)數(shù)據(jù)

實(shí)驗(yàn)結(jié)果表明，基于該算法的UWB移動(dòng)基站自定位結(jié)果與全站儀的標(biāo)定結(jié)果相比，坐標(biāo)相對(duì)誤差最大為0.05m，因此基站的自定位結(jié)果準(zhǔn)確。

5 結(jié)論

本文提出了一種基于TOF的UWB可移動(dòng)基站快速自定位方法。

1)通過最小二乘法及牛頓迭代法對(duì)算法進(jìn)行了誤差傳播規(guī)律的推導(dǎo)；通過室外運(yùn)動(dòng)場(chǎng)定位實(shí)驗(yàn)進(jìn)行算法的可行性驗(yàn)證，定位結(jié)果表明，可移動(dòng)基站的定位精度在0.05m以內(nèi)。

2)相對(duì)于人工測(cè)量基站的位置，本文算法不需要借助高精度儀器輔助測(cè)量，大大節(jié)省了基站布設(shè)時(shí)間，解決了傳統(tǒng)的UWB可移動(dòng)基站布設(shè)復(fù)雜、成本高等方面的問題。

3)本文算法利用所有可移動(dòng)基站互相通信的方式，采用最小二乘法求解，可實(shí)現(xiàn)誤差補(bǔ)償。通過對(duì)誤差傳遞系數(shù)的分析，該算法在誤差傳遞過程中，可減小測(cè)距誤差對(duì)測(cè)量結(jié)果的影響，提高了定位結(jié)果的準(zhǔn)確度。該算法可實(shí)現(xiàn)運(yùn)動(dòng)場(chǎng)內(nèi)運(yùn)動(dòng)員的三維定位，適用于UWB定位等無(wú)線定位技術(shù)中局部定位系統(tǒng)的組建，具有很強(qiáng)的指導(dǎo)意義和使用價(jià)值。