基于TOF的UWB可移動基站快速自定位算法

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

(1.中北大學電子測試技術國家重點實驗室，太原 030051； 2.中北大學儀器科學與動態測試教育部重點實驗室，太原 030051； 3. 濟南金豐源電子科技有限公司，濟南 250101 )

0 引言

現如今，越來越多的高新技術被應用到各體育運動中,以更好地對運動員的運動狀況進行分析[1-2]。其中，運動場中運動員的定位精度要求為分米級，更新率大于10Hz，可實時準確顯示運動員的運動軌跡。目前常用的定位技術如超聲波定位技術[3]、藍牙定位技術[4]和WiFi定位技術[5]等，在覆蓋范圍及定位精度方面均不能滿足運動場高精度實時定位的要求；全球導航衛星系統(Global Navigation Satellite System， GNSS)[6-7]定位技術適合室外大范圍定位，單點定位精度低；實時動態定位(Real-Time Kinematic，RTK)技術雖可實現單點實時定位，但所需設備復雜，操作難度大。

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

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

1 定位算法

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

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

令4個時間差分別為

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

tround1為基站j發送輪詢信號到接收到基站i應答信號的時間；tround2為基站i發送應答信號到接收到基站j發送的測距信息的時間；treply1為基站i接收到輪詢信號到發送應答信號的時間；treply2為基站j接收到應答信號到發送測距信息的時間。

則進行一次測距所需時間t可表示為

(1)

根據式(1)可進一步得出距離d

d=ct+Δd

(2)

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

圖1 TOF測距原理圖Fig.1 TOF ranging schematic diagram

1.2 基站快速定位原理

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

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

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

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

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

(4)

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

由文獻[16]以及實驗數據可知，距離值的測量存在誤差且誤差服從正態分布。由數理統計知識：當誤差服從正態分布時，最小二乘是最優線性無偏估計。通過最小二乘求解，使得測距誤差的平方和為最小。下文將通過上述泰勒展開式求方程的最小二乘解。

記系數矩陣Hk為

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

其中

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

(5)

其中

(6)

令各個基站到地面的距離差為Δ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)

運用最小二乘法解算ΔXk，得

(9)

第一次迭代的X1為

X1=ΔX+X0

(10)

通過牛頓迭代法對ΔXk進行解算，當ΔXk小于誤差閾值時停止迭代，此時的Xk+1為

Xk+1=Xk+ΔXk

(11)

最終基站x、y軸坐標值為Xk+1，z軸坐標值為基站和地面之間的距離，實現了基于TOF測距的UWB基站快速定位方式。

在設定的局部坐標系內求得基站坐標后，在運動場自身坐標系x′o′y′以及建立的局部坐標系xoy內選取典型位置點，求得坐標轉換關系，將坐標進行平移旋轉即可得到基站在運動場內的坐標。

2 定位誤差傳播規律

基站坐標ΔXk的期望為

(12)

基站坐標ΔXk的方差為

(13)

(14)

每個基站坐標處的誤差系數可表示為

(15)

通過誤差系數及測距標準差，可進一步得到基站x坐標和y坐標的標準差

誤差系數反映了測距誤差和定位誤差的關系。根據文獻[17]，誤差系數與基站個數及基站布設方式有關。

3 仿真實驗

假設UWB可移動基站的測距誤差服從均值為0、標準差為0.1m的正態分布。通過上述定位算法解算基站坐標，并進行誤差系數及標準差計算，分析布設數目分別為4、6、8時，對UWB可移動基站自定位精度的影響。

表1 基站坐標仿真數據(4個基站)

表2 基站坐標仿真數據(6個基站)

表3 基站坐標仿真數據(8個基站)

3組仿真實驗結果表明，基站數目多的一組相較于基站數目少的一組，測量誤差及標準差均減小。在100m×50m區域內，布設6個UWB可移動基站時，誤差均值及標準差均在誤差可接受范圍內。布設8個UWB可移動基站時，相對于6個基站布設情況，誤差標準差有所減少，但是減小效果不明顯。在100m×50m區域內，布設6個基站即可達到理想定位精度，繼續增加基站數目，定位精度的提高效果并不明顯，同時提高了系統復雜度，使得定位速度變慢。因此，在長為100m、寬為50m的運動場內，布設6個UWB基站即可達到精確定位的目的。

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

圖3 仿真結果與實際位置對比效果Fig.3 Comparison between simulation results and actual position

4 運動場實驗

仿真結果表明，100m×50m的運動場內6個UWB基站及8個UWB基站均可達到精確定位的目的。考慮到運動場定位精度的要求，采用仿真實驗中6個UWB基站的布局方式驗證算法的可行性。在足球場布設基站，用全站儀進行測量，全站儀測量精度為0.001m，如圖4所示。基站1坐標為(0,0,1)，測得其余5個基站坐標情況如表4所示。6個UWB基站完成一次定位用時小于1s，可達到快速定位的目的。

圖4 基站布設環境Fig.4 Layout environment of base station

表4 運動場基站坐標數據

實驗結果表明，基于該算法的UWB移動基站自定位結果與全站儀的標定結果相比，坐標相對誤差最大為0.05m，因此基站的自定位結果準確。

5 結論

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

1)通過最小二乘法及牛頓迭代法對算法進行了誤差傳播規律的推導；通過室外運動場定位實驗進行算法的可行性驗證，定位結果表明，可移動基站的定位精度在0.05m以內。

2)相對于人工測量基站的位置，本文算法不需要借助高精度儀器輔助測量，大大節省了基站布設時間，解決了傳統的UWB可移動基站布設復雜、成本高等方面的問題。

3)本文算法利用所有可移動基站互相通信的方式，采用最小二乘法求解，可實現誤差補償。通過對誤差傳遞系數的分析，該算法在誤差傳遞過程中，可減小測距誤差對測量結果的影響，提高了定位結果的準確度。該算法可實現運動場內運動員的三維定位，適用于UWB定位等無線定位技術中局部定位系統的組建，具有很強的指導意義和使用價值。