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

0 引言

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

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

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

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

圖1 基站布置示意圖

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

2 定位算法原理

2.1 算法基本思想

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

2.2 定位算法整體流程

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

圖2 定位算法流程圖

2.3 最小二乘法

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

(1)

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

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

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

(2)

其最小二乘解為：

X=(ATA)-1ATb

(3)

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

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

(4)

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

y=f(x)

(5)

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

Q-R=h-Gδ

(6)

R為Taylor展開余項(xiàng)向量，

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

δ=(GTG)-1GTh

(7)

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

2.5 曲線積分近似法

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

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

(8)

Cotes積分公式[9]為：

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

(9)

2.6 卡爾曼濾波算法

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

(10)

(11)

(12)

(13)

(14)

(15)

3 實(shí)驗(yàn)及分析

3.1 UWB模塊測(cè)距實(shí)驗(yàn)與性能分析

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

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

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

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

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

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

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

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

3.2 計(jì)算分析

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

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

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

3.2.1 對(duì)最小二乘法的效果分析

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

圖7 測(cè)距誤差為9.632時(shí)最小二乘法定位誤差曲線

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

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

圖8 測(cè)距誤差為9.632時(shí)Taylor迭代后定位誤差曲線

圖9 測(cè)距誤差為0.262時(shí)Taylor迭代后定位誤差曲線

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

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

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

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

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

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

圖11 測(cè)距誤差為9.632時(shí)Kalman濾波后定位誤差曲線

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

圖12 測(cè)距誤差為9.632時(shí)Kalman濾波后路程誤差曲線

4 結(jié)論

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