(1.比亞迪通信信號有限公司，廣東 深圳 518000； 2.北京師范大學(xué) 核科學(xué)與技術(shù)學(xué)院，北京 100875)

0 引言

超寬帶無線電定位技術(shù)具有大帶寬、抗干擾能力強、多徑分辨力高、測距響應(yīng)速度快、精度高(厘米級)、功率譜密度低、系統(tǒng)簡單、成本低等特點[1]。UWB模塊通訊距離較短，適用于軌道交通車輛進站定位、局部區(qū)域的車輛調(diào)度和無人駕駛。安全高效運行始終是軌道交通系統(tǒng)的重要主題和發(fā)展方向，而系統(tǒng)的安全高效運行要求定位技術(shù)響應(yīng)速度快、精度高、結(jié)構(gòu)簡單。對于軌道交通的車站環(huán)境和隧道環(huán)境，GPS定位技術(shù)因為受到遮擋難以有效實現(xiàn)高精度定位。軌道交通目前廣泛應(yīng)用基于應(yīng)答器加里程計的定位方式，其在定位精度和閉塞效率方面仍有一定的局限性，UWB高精度連續(xù)定位可作為輔助定位技術(shù)，可進一步提高安全性和運營效率。

典型的無線技術(shù)定位算法包括質(zhì)心定位法、解析法、最小二乘法、Taylor法等[2-5]。在現(xiàn)有的研究中，質(zhì)心定位法計算精度低，算法靈活度低；有測距誤差條件下，兩定位圓之間無交點的情況，無法應(yīng)用解析法；已有的研究討論的都是幾個地面參考基站所包圍區(qū)域的定位問題，在包圍區(qū)域邊界附近定位精度變得較差；對于包圍區(qū)域外部的定位問題，很少見到相關(guān)研究。本文根據(jù)軌道交通的特點和UWB模塊特性，將軌道方程約束條件和連續(xù)性約束條件加入最小二乘法、Taylor遞歸法、Kalman濾波當(dāng)中，應(yīng)用五個步驟計算定位結(jié)果。先用最小二乘法粗略計算出位置，并將其作為迭代初值代入有軌道方程約束的Taylor遞歸法，計算出迭代后的坐標(biāo)，最后應(yīng)用具有速度約束性的Kalman濾波法，將迭代后的坐標(biāo)進行數(shù)據(jù)濾波修正，得到比較精確的坐標(biāo)位置，最后用曲線近似積分法計算出待測物體的行駛路程。另外，應(yīng)用待測物位置連續(xù)性約束條件，第二次及以后定位計算時，Taylor遞歸法的迭代初值也可取上個位置的坐標(biāo)。軌道方程事先由擬合得到。本論文算法在地面參考基站包圍區(qū)域內(nèi)、外均達到了一致的定位精度；對于非視距情況及多徑效應(yīng)產(chǎn)生的較大測距誤差，也能消除其對定位精度的影響。

1 定位系統(tǒng)的構(gòu)成

UWB定位方法主要有到達角度定位(angle of arrival, AOA)、信號強度分析法(received signal strength indication, RSSI)、到達時間定位(time of arrival, TOA)、到達時間差定位(time difference of arrival, TDOA)[6-7]。前兩種定位方法設(shè)備復(fù)雜度比較高，TDOA法需要在地面服務(wù)器端計算定位結(jié)果。如圖1所示，本文采用TOA法，移動站布置在待測物體(列車)上，地面參考站布置在軌旁，參考站兩兩之間在水平和豎直兩個方向上均間隔一定距離(不小于2 m)。靈活的布點方式有利于降低工程實施難度，特別是對于空間有限的高架軌道區(qū)間、站臺區(qū)域、隧道區(qū)段等。

圖1 基站布置示意圖

待測物與其它地面參考站的距離由信號到達時間乘以電磁波傳播速度得到，從而計算自身坐標(biāo)位置。對于靜態(tài)物體的定位，增加參考站的數(shù)量可提高定位精度[8]，但參考站數(shù)量增多會增加測距次數(shù)，增加時延，對于以較快速度運動的物體，會導(dǎo)致較大時延誤差。對于二維場景，至少需要3個參考站，對于三維場景，至少需要4個參考站，為簡化問題，本文采用3個基站。本文就一段軌道區(qū)間進行研究，對于全線定位的實現(xiàn)可依此類推，沿途間隔布置參考站。

2 定位算法原理

2.1 算法基本思想

待測物的位置可由以下幾個條件確定：①基本條件：各參考站的位置及其與待測物的測距距離；②軌道曲線方程約束：列車在軌道上行駛；③移動位置連續(xù)性約束：列車行駛過程不具有跳躍性；④速度連續(xù)性約束：在數(shù)十ms的間隔時間內(nèi)，列車運行速度不具有很大的跳躍性。由基本測距條件和約束條件，聯(lián)合使用最小二乘法、Talor迭代法，以及Kalman濾波，可計算出待測物坐標(biāo)，或者待測物(列車)距車站的路程，或者列車距離閉塞區(qū)間端點的路程(列車控制系統(tǒng)通常以列車距軌道上某一點的路程做定位參考)。

2.2 定位算法整體流程

有約束條件的定位融合算法整體流程如圖2所示，共有五個步驟。事先用樣條函數(shù)[9]對軌道曲線進行擬合，用最小二乘法粗略計算出位置坐標(biāo)，并將其作為迭代初值(第二次及以后定位時也可用上次的位置坐標(biāo)作為迭代初值)代入有軌道方程約束的Taylor遞歸法，計算出迭代后的位置坐標(biāo)，最后應(yīng)用具有速度約束性的Kalman濾波法，進一步進行數(shù)據(jù)濾波修正，得到比較精確的定位坐標(biāo)，最后用辛普森積分法或Cotes積分法計算出本次定位坐標(biāo)與上個位置的坐標(biāo)之間的路程，這樣每定位一次積分出一小段路程，累積的結(jié)果即待測物體的總行駛路程。分小段積分的優(yōu)勢在于減少計算量。

圖2 定位算法流程圖

2.3 最小二乘法

已知各參考站位置(xi,yi)及各參考站到待測物的水平距離ri(可由測距距離di、參考站與移動站的高度差hi構(gòu)造直角三角形求得)，當(dāng)測距無誤差時，待測物位置(x,y)滿足以下關(guān)系：

(1)

以各地面參考基站為圓心，以待測物與各地面參考基站測距距離為半徑畫圓，如圖3所示，圖中黑點表示待測物實際所在位置，因為實際測距存在誤差，三個圓并不交于一點。方程組(1)無解析解。兩兩圓的位置關(guān)系可能是相交、相切、相離、包含關(guān)系中的一種。

圖3 有測距誤差存在時定位圓關(guān)系示意圖

當(dāng)待測物位于參考基站所包圍區(qū)域時，定位圓的關(guān)系如圖3(a)所示；當(dāng)待測物位于包圍區(qū)域外部時(移動站在參考站有效測距范圍內(nèi))，如圖3(b)所示，幾個定位圓重疊度比較高，無法應(yīng)用幾何法計算，采用單一的方法計算誤差也比較大。(1)式為超定方程組，可找一個解使方程組誤差平方和最小。將其轉(zhuǎn)化為矩陣形式：

(2)

其最小二乘解為：

X=(ATA)-1ATb

(3)

2.4 有軌道方程約束的Taylor迭代法

對于定位圓重疊度比較高的情況，需要更精確的求解。定位點可進一步由Taylor遞歸算法[10]求得。由待測物(x,y)到地面參考站(xi,yi)的距離與水平距離ri的差可構(gòu)造方程：

(4)

工程上，軌道曲線方程常用樣條函數(shù)擬合，保證方程連續(xù)可導(dǎo)。設(shè)擬合的軌道方程為：

y=f(x)

(5)

由方程(4)、(5)求一點(x,y)使得Q取得最小值。將(5)代入(4)，令x=x0+Δx，并在迭代初始點x0進行一階泰勒展開得：

Q-R=h-Gδ

(6)

R為Taylor展開余項向量，

δ=[Δx]，式(6)的最小二乘解為：

δ=(GTG)-1GTh

(7)

第二次遞歸時，令x0=x0+Δx，重復(fù)以上過程，迭代直到Δx小于一個設(shè)定的閾值，此時的(x0,f(x0))即為定位點的坐標(biāo)。

2.5 曲線積分近似法

可采用累積積分的方法計算待測物的行駛路程。用直接積分法可能無法對實際的軌道曲線函數(shù)進行曲線積分，可采用辛普森積分法或Cotes積分法。

辛普森積分法具有三階代數(shù)精度，Cotes積分法具有五階代數(shù)精度，對于更高階數(shù)的方程，這兩種積分方法能以較少的計算次數(shù)獲得很高的計算精度。辛普森求積公式[9]為：

(8)

Cotes積分公式[9]為：

32f(a+3h)+7f(b)]

(9)

2.6 卡爾曼濾波算法

由于電子設(shè)備的本底噪聲、電磁波傳播過程中具有多徑效應(yīng)、非視距情況，UWB測距模塊表現(xiàn)出以較小概率出現(xiàn)大的測距誤差。需要對定位結(jié)果進行校正。Kalman濾波[11]為某時刻的狀態(tài)值與當(dāng)前時刻以及之前各時刻的測量值建立關(guān)系，是對高斯過程非常有效的一種實時濾波方法?？柭鼮V波分為預(yù)估和校正兩個過程。預(yù)測過程為：

(10)

(11)

(12)

(13)

(14)

(15)

3 實驗及分析

3.1 UWB模塊測距實驗與性能分析

本研究選用的UWB模塊為Time Domain公司的PulsOn 440，它是一種工作波段在3.1 G到4.8 G之間的超寬帶無線收發(fā)器，采用雙向飛行時間(TW-TOF)方式進行測距。系統(tǒng)時鐘同步精度達1 μs，宣稱的測距精度為2～5 cm，在自由空間的測距距離為240 m，測距速率最高為125 Hz，發(fā)射功率為-13 dB。

在一條車輛較少的公路上，對P440模塊進行了測試。公路兩旁有樹木、建筑物、空調(diào)外機，公路上有少量汽車通行，測試的同時，手機在模塊旁通話。使兩模塊在不同距離下測距次數(shù)超過1000次，采集數(shù)據(jù)并對數(shù)據(jù)進行統(tǒng)計分析，如表1所示，表中誤差為測距距離減去均值，測距距離的單位為mm。

表1 測距性能指標(biāo)統(tǒng)計表

由表1中數(shù)據(jù)知，測距距離越短，誤差的標(biāo)準差越小，該模塊測距精度與穩(wěn)定性越好。

圖4、圖5分別為測距距離50 m、15 m情況下誤差的分布圖，圖中曲線為其對應(yīng)方差的正態(tài)分布曲線。

圖4 測距距離為50 m的情況下誤差分布圖

圖5 測距距離為15 m的情況下誤差分布圖

50 m的距離下，測距1540次，誤差絕大部分分布在-40 mm到50 mm的區(qū)間范圍內(nèi)，小于-400 mm的情況只出現(xiàn)12次，占比0.78%。出現(xiàn)較大誤差是由電磁波傳播的多徑效應(yīng)與非視距情況導(dǎo)致。15 m的距離下測距1185次，測距誤差均在-1～1 cm范圍內(nèi)。統(tǒng)計結(jié)果表明測距誤差較大的概率比較小。基于P440模塊以上特點，本文采用高斯誤差近似代替實際誤差進行分析計算。

3.2 計算分析

為了驗證算法效果，分別在50 m、15 m對應(yīng)的兩種測距誤差(即N(0，9.632)、N(0，0.262)分布的高斯誤差)情況下進行了相關(guān)計算分析，為方便表述，后面直接用方差來代表高斯測距誤差。如圖6所示為參考基站位置及軌道曲線示意圖。

圖6 參考基站位置及軌道曲線示意圖

軌道曲線方程為y=0.00001x2+10，三個參考基站的坐標(biāo)為(4500,13)，(5000,460)，(5500,113)，待測物運動的起始坐標(biāo)為(0，10)，終止坐標(biāo)為(10000,1010)(單位：cm)，并且令待測物沿軌道變速運動。

3.2.1 對最小二乘法的效果分析

由最小二乘法計算待測物的坐標(biāo)，并比較計算坐標(biāo)與實際坐標(biāo)的距離差DISE。如圖7為測距誤差N(0，9.632)分布的最小二乘法定位誤差曲線，在基站附近區(qū)域，定位誤差很小，基本在25 cm以下。兩側(cè)呈發(fā)散趨勢，這是由于遠離三個參考基站的區(qū)域，線性漂移比較嚴重，如圖3(b)所示情況。即在參考基站包圍區(qū)域外部，簡單的算法無法保證定位精度。

圖7 測距誤差為9.632時最小二乘法定位誤差曲線

3.2.2 經(jīng)過迭代前的步驟與Taylor迭代法計算的效果分析

圖8、圖9是分別在兩種高斯測距誤差情況下，經(jīng)過最小二乘或位置連續(xù)性條件給定迭代初值，再由Taylor迭代后定位誤差的曲線。圖8相較圖7，經(jīng)過軌道方程約束的Taylor法迭代計算后使定位誤差得到了很大改善，距離誤差DISE都在20 cm以下，而且絕大部分定位點誤差在10 cm以下。在參考基站包圍區(qū)域內(nèi)外，達到了一致的定位精度。

圖8 測距誤差為9.632時Taylor迭代后定位誤差曲線

圖9 測距誤差為0.262時Taylor迭代后定位誤差曲線

圖9的定位誤差普遍在0.8 cm以下，測距誤差的方差越小，定位精度明顯提高，因此P440模塊近距離定位精度極高。因此，提高模塊測距精度與測距穩(wěn)定性，會顯著改善定位效果。

圖10為迭代次數(shù)統(tǒng)計曲線，Δx的閾值設(shè)置為0.1，如繼續(xù)進行迭代，Δx會繼續(xù)減小，但定位誤差并無明顯改善。迭代次數(shù)普遍在3次以下，迭代收斂很快，計算量比較少。

圖10 測距誤差為9.632時Taylor迭代次數(shù)統(tǒng)計曲線

3.2.3 經(jīng)過Kalman濾波前的步驟與Kalman濾波計算

對于小概率出現(xiàn)的遠大于誤差均值的測距誤差，需要對Taylor迭代后的數(shù)據(jù)進一步進行濾波改善。濾波初始狀態(tài)向量取為：

如圖11所示，對比圖8，定位誤差進一步得到顯著改善，距離誤差降到10 cm以下，且絕大部分定位點距離誤差小于5 cm。與前面測距實驗統(tǒng)計結(jié)果對比可知，Kalman濾波有效改善了小概率出現(xiàn)的大的測量誤差。

圖11 測距誤差為9.632時Kalman濾波后定位誤差曲線

圖12為濾波后的路程誤差PE曲線，路程誤差為實際路程與計算路程之差的絕對值。本文的定位方法可以很好地適用于基于路程定位的軌道交通信號控制系統(tǒng)。

圖12 測距誤差為9.632時Kalman濾波后路程誤差曲線

4 結(jié)論

本論文所研究的定位方法復(fù)雜度低，UWB模塊響應(yīng)速度快，參考點布置靈活，且算法自動化程度高、計算量少、精度高，比較適合于基于路程定位的軌道交通系統(tǒng)，對局部區(qū)域內(nèi)運動物體的實時無人駕駛定位也具有參考價值。濾波算法中涉及到一些先驗參數(shù)，需配合其它傳感器如慣性導(dǎo)航或者速度傳感器使用，利用加速度和速度等信息，將提高定位精度和穩(wěn)定度。