基于UWB技術(shù)的車(chē)輛定位算法研究

張碧仙

(福州理工學(xué)院，福建 福州 350500)

近年來(lái)，精確實(shí)時(shí)的車(chē)輛位置測(cè)量技術(shù)在城市智能交通、車(chē)輛自主導(dǎo)航、智能網(wǎng)聯(lián)汽車(chē)等領(lǐng)域的應(yīng)用越來(lái)越廣泛。最常用的定位方法如GPS(Global Positioning System)定位，雖然能在多數(shù)室外環(huán)境下提供位置信息，但其定位精度較低，且在復(fù)雜環(huán)境下存在信號(hào)盲點(diǎn)[1]。伴隨著計(jì)算機(jī)視覺(jué)的發(fā)展，基于視頻圖像的車(chē)輛檢測(cè)算法和技術(shù)被提出并應(yīng)用在道路車(chē)輛實(shí)時(shí)檢測(cè)中，如YOLO(You Only Look Once)目標(biāo)檢測(cè)算法，YOLO算法具有較好的實(shí)時(shí)性，但準(zhǔn)確率與召回率均不高，尤其對(duì)于小目標(biāo)車(chē)輛的漏檢率較高[2]。更復(fù)雜的基于視覺(jué)同時(shí)定位與制圖技術(shù)[3](Simultaneous Localization and Mapping，SLAM)的車(chē)輛定位技術(shù)也受到許多研究者關(guān)注，該方法雖然精度較高，但SLAM算法復(fù)雜，穩(wěn)定性差，且需要大量?jī)?yōu)化計(jì)算[4]。此外，常用的定位技術(shù)還有RFID射頻識(shí)別技術(shù)、ZigBee技術(shù)，以及WLAN定位技術(shù)等，然而這些技術(shù)存在傳輸距離短、抗多徑效應(yīng)差，定位精度低等問(wèn)題。在復(fù)雜環(huán)境的應(yīng)用場(chǎng)景中，定位系統(tǒng)往往容易受到多徑效應(yīng)、非視距(Non-line of Sight，NLOS)誤差等因素的干擾。

超寬帶(Ultra Wide Band，UWB)技術(shù)[5]是一種無(wú)線載波通信技術(shù)，它不采用正弦載波，而是利用納秒級(jí)的非正弦波窄脈沖傳輸數(shù)據(jù)，因此其所占的頻譜范圍很寬。具有系統(tǒng)復(fù)雜度低，發(fā)射信號(hào)功率譜密度低，對(duì)信道衰落不敏感，截獲能力低，抗多徑效應(yīng)，精度高等優(yōu)點(diǎn)。因此，UWB技術(shù)適用于在密集城市道路環(huán)境或地貌復(fù)雜的高速公路場(chǎng)景中進(jìn)行車(chē)輛精確定位，精度可達(dá)厘米級(jí)。

高精度的UWB定位一般是基于時(shí)間測(cè)量來(lái)計(jì)算的，從實(shí)現(xiàn)方式上，一般分為基于到達(dá)時(shí)間(Time of Arival，TOA)與基于到達(dá)時(shí)間差(Time Difference oF Arival，TDOA)兩種[6-7]。其中，基于TOA的定位方法需要移動(dòng)臺(tái)和參考節(jié)點(diǎn)之間的嚴(yán)格時(shí)間同步，并存在因時(shí)鐘漂移而引入的飛行時(shí)間誤差。仰勝，胡志剛[8]等人應(yīng)用雙邊雙向測(cè)距(double-sided two-way ranging，DS-TWR)抑制時(shí)鐘漂移引入的測(cè)距誤差，但需要移動(dòng)臺(tái)與參考節(jié)點(diǎn)間的嚴(yán)格時(shí)間同步，應(yīng)用受限，并且采用DS-TWR需要占用較多的空口時(shí)間(移動(dòng)臺(tái)與每個(gè)參考節(jié)點(diǎn)之間需要多次通信)，對(duì)于低功耗設(shè)備而言，意味著電池壽命的降低，此外空口時(shí)間的占用也帶來(lái)承載用戶數(shù)的降低?；赥DOA的定位方法，則只需要考慮定位基站即參考節(jié)點(diǎn)之間保持時(shí)鐘同步，不需要參考移動(dòng)臺(tái)于參考節(jié)點(diǎn)之間的時(shí)鐘同步，實(shí)現(xiàn)較為容易，設(shè)備成本低。朱代先，孫小婷[9]等人提出采用TDOA/RSS聯(lián)合定位方法，以TDOA觀測(cè)數(shù)據(jù)為主，RSS觀測(cè)數(shù)據(jù)為輔，通過(guò)粒子濾波進(jìn)行定位，定位精度有所提升，但是定位算法復(fù)雜，適用于煤礦井下環(huán)境惡劣，非高斯噪聲突出環(huán)境下的定位。

針對(duì)上述問(wèn)題，考慮實(shí)際部署規(guī)模及低功耗、低成本的應(yīng)用需求，本文基于UWB定位技術(shù)，提出基于TDOA的定位方法，僅需要移動(dòng)臺(tái)向參考節(jié)點(diǎn)發(fā)送一個(gè)報(bào)文，采用Chan算法來(lái)實(shí)現(xiàn)高精度定位解算，并且基于標(biāo)簽坐標(biāo)與距離關(guān)系進(jìn)行了二次加權(quán)計(jì)算，充分利用冗余的數(shù)據(jù)，獲得最優(yōu)估計(jì)位置，通過(guò)中位值平均濾波法對(duì)車(chē)輛位置進(jìn)一步優(yōu)化，降低NLOS誤差對(duì)定位精度的影響，實(shí)現(xiàn)對(duì)車(chē)輛位置的快速、便捷、精確定位。

1 UWB/TDOA定位基本原理

1.1 UWB車(chē)輛定位系統(tǒng)

典型的UWB定位系統(tǒng)由定位基站、移動(dòng)標(biāo)簽、定位服務(wù)器、數(shù)據(jù)存儲(chǔ)分析系統(tǒng)等幾部分組成。通?？煞譃槿齻€(gè)層次：感知層、網(wǎng)絡(luò)層、服務(wù)層。其中，感知層主要包括定位基站及標(biāo)簽，通過(guò)定位基站與標(biāo)簽的UWB定位信道實(shí)現(xiàn)對(duì)標(biāo)簽的測(cè)距；網(wǎng)絡(luò)層實(shí)現(xiàn)基站測(cè)距數(shù)據(jù)包的回傳并提供局域網(wǎng)服務(wù)；服務(wù)層主要包括定位引擎及數(shù)據(jù)存儲(chǔ)分析服務(wù)器，定位引擎根據(jù)測(cè)距結(jié)果進(jìn)行定位解算，將計(jì)算出的定位結(jié)果發(fā)送給數(shù)據(jù)存儲(chǔ)分析服務(wù)器中分析，最終在業(yè)務(wù)平臺(tái)上進(jìn)行呈現(xiàn)，定位系統(tǒng)示意圖如圖1所示。

圖1 UWB定位系統(tǒng)示意圖

1.2 TDOA同步方案

由于TDOA需要定位基站保持時(shí)鐘同步，或者獲知定位基站間的定位時(shí)間差，本文采用基于無(wú)線網(wǎng)絡(luò)的同步方式，同步原理如圖2所示。首先，由任意一個(gè)定位基站廣播發(fā)送一個(gè)測(cè)距報(bào)文，其他定位基站接收到測(cè)距報(bào)文，統(tǒng)計(jì)出時(shí)間差T；由于基站的相對(duì)位置D已知，從而計(jì)算出實(shí)際的時(shí)間差T'，得到各個(gè)基站的時(shí)間差為：

圖2 TDOA無(wú)線網(wǎng)絡(luò)同步原理

ΔT=T-T'

(1)

由于存在時(shí)鐘的漂移，因此，上述時(shí)間同步需要在定位過(guò)程中持續(xù)進(jìn)行。

2 定位算法設(shè)計(jì)

2.1 Chan定位算法

TDOA定位方法通過(guò)獲取被測(cè)移動(dòng)臺(tái)與多個(gè)已知位置的參考節(jié)點(diǎn)的報(bào)文飛行時(shí)間的時(shí)間差，計(jì)算移動(dòng)臺(tái)與參考節(jié)點(diǎn)間的距離差，利用雙曲線原理，采用Chan算法來(lái)實(shí)現(xiàn)高精度定位解算。當(dāng)定位基站超過(guò)3個(gè)時(shí)，采用Chan算法通過(guò)兩次加權(quán)最小二乘法(WLS)，在噪聲服從零均值的高斯分布的環(huán)境下，能夠充分利用冗余數(shù)據(jù)獲得較好的計(jì)算結(jié)果，得到較高的定位精度[10]，定位示意圖如圖3所示。

圖3 Chan定位示意圖

設(shè)BS1等基站的坐標(biāo)已知，坐標(biāo)為Xi(xi,yi),i=1,2,…,n，移動(dòng)臺(tái)MS的坐標(biāo)為M(x,y)。則M與Xi之間的距離為：

(2)

以X1為基準(zhǔn)，M到Xi(i≠1)與到X1的距離差定位為：

ri,1=cti,1=ri-r1,i=1,2,…,n

(3)

其中，c為電波傳播速度，ti,1為M到Xi(i≠1)的飛行時(shí)間差，由式(2)(3)可得：

(4)

當(dāng)定位基站的數(shù)量大于3時(shí)，TDOA值得到的非線性方程組個(gè)數(shù)要多于未知變量個(gè)數(shù)。采用加權(quán)最小二乘法(WLS)，充分利用冗余數(shù)據(jù)來(lái)獲得移動(dòng)臺(tái)最優(yōu)位置估計(jì)值。先將初始非線性TDOA方程組轉(zhuǎn)換為線性方程組，然后采用WLS得到初始解，再利用第一次得到的估計(jì)坐標(biāo)及附加變量等已知約束條件進(jìn)行第二次WLS估計(jì)，從而得到改進(jìn)的估計(jì)坐標(biāo)。

將式(4)中x,y,r1看作自變量，將式(4)化為線性方程組：

GaZa=h

(5)

則(5)的誤差矢量為：

(6)

假設(shè)e近似服從高斯分布，則式(4)的最小二乘解相當(dāng)于求解正規(guī)方程：

(7)

當(dāng)為對(duì)每組數(shù)據(jù)的誤差加權(quán)后，成為加權(quán)最小二乘問(wèn)題，得到第一次估計(jì)值為：

(8)

其中，Q為服從高斯分布的噪聲矢量協(xié)方差矩陣。利用式(8)得到的初始值重新計(jì)算協(xié)方差矩陣中的B，便可以得到Za的結(jié)果。

利用第一次估計(jì)值，重新構(gòu)造一組誤差方程組進(jìn)行第二次估計(jì)，可得到M的估計(jì)結(jié)果為：

(9)

其中，Za1表示Za的第1個(gè)分量。

2.2 基于中位值平均濾波法的位置優(yōu)化算法

在城市道路或高速公路車(chē)輛定位環(huán)境中，NLOS誤差主要來(lái)自過(guò)往快速車(chē)輛對(duì)UWB信號(hào)影響而產(chǎn)生的脈沖性干擾。中位值平均濾波法[11]融合了“中位值濾波法”與“算術(shù)平均濾波法”的優(yōu)點(diǎn)，對(duì)于偶然出現(xiàn)的脈沖性干擾，可有效消除由于脈沖干擾所引起的定位目標(biāo)位置估計(jì)值偏差，假設(shè)通過(guò)公式(9)估計(jì)得到一組長(zhǎng)度為N的定位坐標(biāo)為：

(10)

去除其中最大值和最小值，計(jì)算N-2個(gè)數(shù)據(jù)的算術(shù)平均值，得到目標(biāo)的最優(yōu)坐標(biāo)估計(jì)，如式(11)所示。

(11)

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

3.1 實(shí)驗(yàn)場(chǎng)景及傳感器模塊選擇

本次實(shí)驗(yàn)分別模擬在高速路直道場(chǎng)景和高速路彎道場(chǎng)景下，對(duì)本文所述定位算法在上述場(chǎng)景下進(jìn)行車(chē)輛位置估計(jì)，同時(shí)為了滿足高速路應(yīng)急車(chē)道統(tǒng)計(jì)的場(chǎng)景需求，需要將計(jì)算出的定位結(jié)果通過(guò)算法轉(zhuǎn)化成車(chē)道情況，并對(duì)車(chē)道判斷的準(zhǔn)確性進(jìn)行驗(yàn)證與分析。

高速路直道場(chǎng)景實(shí)驗(yàn)仿真平面圖如圖4所示，在高速直道路段500米內(nèi)部署四個(gè)基站，車(chē)道寬度按照標(biāo)準(zhǔn)六車(chē)道設(shè)置，即普通車(chē)道3.75米，應(yīng)急車(chē)道3.5米，在基站附近以及直道上每相隔一段的各個(gè)車(chē)道中選取測(cè)試點(diǎn)進(jìn)行測(cè)距。

圖4 高速路直道場(chǎng)景

高速路彎道場(chǎng)景實(shí)驗(yàn)仿真平面圖如圖5所示，車(chē)道寬度同樣按照標(biāo)準(zhǔn)六車(chē)道設(shè)置，即普通車(chē)道3.75米，應(yīng)急車(chē)道3.5米。由最大半徑為173m，最小半徑為147米的兩個(gè)扇形拼接成的彎道，車(chē)道內(nèi)各個(gè)點(diǎn)到基站的位置要求在500米以內(nèi)，保證測(cè)距能正常完成。在高速路彎道路段內(nèi)部署四個(gè)基站，在基站附近以及彎道之間每相隔一段的各個(gè)車(chē)道中選取測(cè)試點(diǎn)進(jìn)行測(cè)距。

圖5 高速路彎道場(chǎng)景

實(shí)驗(yàn)采用Decawave推出的DW1000UWB模塊，按照IEEE 802.15.4協(xié)議規(guī)定，物理層報(bào)文中同步報(bào)頭(Synchronization Header，SHR)用于進(jìn)行報(bào)文發(fā)現(xiàn)，規(guī)定其中幀起始分隔符(Start-of-Frame Delimiter，SFD)用于代表報(bào)文的收發(fā)時(shí)間點(diǎn)。UWB可以在發(fā)送報(bào)文時(shí)，填入報(bào)文的發(fā)射時(shí)間戳T1(Transmission Timestamp)，接收端在接收?qǐng)?bào)文時(shí)，按照SFD所在的位置，根據(jù)接收設(shè)備的時(shí)鐘，給接收?qǐng)?bào)文附加一個(gè)接收時(shí)間戳T2(Receive Timestamp)，接收端則根據(jù)時(shí)間戳差值T2-T1，計(jì)算出飛行時(shí)間(Time of Flight，TOF)，則測(cè)距方程為：

R=c×TOF=c×(T2-T1)

(12)

3.2 車(chē)輛位置的直線距離精度評(píng)估

實(shí)驗(yàn)分別選取高速路測(cè)試場(chǎng)景內(nèi)基站附近以及各個(gè)車(chē)道中每相隔一段距離的點(diǎn)為定位測(cè)試點(diǎn)。其中，直道場(chǎng)景中分別選取y=11、100、200、250、400、509，6組定位測(cè)試點(diǎn)進(jìn)行車(chē)輛位置估計(jì)，彎道場(chǎng)景中分別選取車(chē)道起始點(diǎn)x=180以及y=180，260，340，420，500，6組定位測(cè)試點(diǎn)進(jìn)行車(chē)輛位置估計(jì)?；驹诿總€(gè)測(cè)試點(diǎn)測(cè)距500次，剔除個(gè)別誤差嚴(yán)重的測(cè)距值，取平均值作為該距離下的估計(jì)測(cè)量值，并計(jì)算每個(gè)測(cè)試點(diǎn)的直線距離誤差，得到各車(chē)道上測(cè)試點(diǎn)的直線距離誤差統(tǒng)計(jì)結(jié)果如表1、表2所示。測(cè)試結(jié)果表明，2個(gè)場(chǎng)景中各車(chē)道直線距離誤差平均值最小為15.45cm，最大為27.39cm。其中直道場(chǎng)景中各車(chē)道直線距離誤差的平均值小于彎道場(chǎng)景，表明實(shí)驗(yàn)環(huán)境越復(fù)雜，誤差平均值會(huì)有所提高。

表1 直道場(chǎng)景直線距離誤差統(tǒng)計(jì)表

表2 彎道場(chǎng)景直線距離誤差統(tǒng)計(jì)表

3.3 車(chē)輛位置的X-Y軸坐標(biāo)估計(jì)值精度評(píng)估

選取直道場(chǎng)景下6個(gè)車(chē)道測(cè)量數(shù)據(jù)，利用融合了Chan算法和中位值平均濾波法的定位算法對(duì)X-Y軸坐標(biāo)的估計(jì)值進(jìn)行計(jì)算，并對(duì)其精度統(tǒng)計(jì)結(jié)果進(jìn)行分析，如表3所示。直線場(chǎng)景中6個(gè)車(chē)道車(chē)輛位置估計(jì)值X軸方向的平均偏差為10.32cm，平均均方根誤差為17cm；Y軸方向的平均偏差為12.43cm，平均均方根誤差為21.16cm，誤差之間沒(méi)有明顯相關(guān)性。分析車(chē)輛在行駛過(guò)程中一般位于車(chē)道中心線上，且以上誤差遠(yuǎn)小于車(chē)道寬度的一半，完全可以滿足車(chē)輛定位需求。

表3 直道場(chǎng)景X-Y軸坐標(biāo)估計(jì)值精度統(tǒng)計(jì)表

3.4 車(chē)道判斷準(zhǔn)確度評(píng)估

為進(jìn)一步分析測(cè)距誤差對(duì)最終車(chē)道判斷準(zhǔn)確度的影響，基于仿真實(shí)驗(yàn)的多組實(shí)際測(cè)試數(shù)據(jù)，疊加服從正態(tài)分布的測(cè)距誤差值構(gòu)成模擬測(cè)試數(shù)據(jù)，將模擬的測(cè)試數(shù)據(jù)使用以上算法計(jì)算出定位結(jié)果再轉(zhuǎn)換為所在車(chē)道，統(tǒng)計(jì)各個(gè)車(chē)道判斷準(zhǔn)確率，即車(chē)道1和車(chē)道6上的點(diǎn)是否被判斷為應(yīng)急車(chē)道，車(chē)道2，3，4，5上的點(diǎn)是否被判斷為正常車(chē)道。

3.4.1 直道場(chǎng)景車(chē)道判斷

各組車(chē)道判斷準(zhǔn)確度統(tǒng)計(jì)結(jié)果如圖6所示。其中，X軸表示測(cè)距誤差，服從正態(tài)分布(0，x)的隨機(jī)值，結(jié)果表明，當(dāng)測(cè)距誤差小于等于0.4m時(shí)，各組測(cè)試點(diǎn)的車(chē)道判斷準(zhǔn)確率均達(dá)到100%；測(cè)距誤差介于0.4～0.7m時(shí)，車(chē)道起始點(diǎn)及終點(diǎn)附近的測(cè)試點(diǎn)車(chē)道判斷準(zhǔn)確率仍介于95%～100%；當(dāng)測(cè)距誤差大于0.7m時(shí)，車(chē)道半程點(diǎn)及附近測(cè)試點(diǎn)的車(chē)道判斷準(zhǔn)確率急劇下降。

圖6 直道場(chǎng)景車(chē)道判斷準(zhǔn)確度統(tǒng)計(jì)圖

3.4.2 彎道場(chǎng)景車(chē)道判斷

各組車(chē)道判斷準(zhǔn)確度統(tǒng)計(jì)結(jié)果如圖7所示。其中，X軸表示測(cè)距誤差，服從正態(tài)分布(0，x)的隨機(jī)值，結(jié)果表明，當(dāng)測(cè)距誤差小于等于0.35m時(shí)，各組測(cè)試點(diǎn)的車(chē)道判斷準(zhǔn)確率達(dá)到100%；當(dāng)測(cè)距誤差大于0.4m時(shí)，y=180，260附近部分測(cè)試點(diǎn)車(chē)道判斷準(zhǔn)確率快速下降，主要原因在于彎道場(chǎng)景下，y=180，260附近的測(cè)試點(diǎn)與定位基站間的距離較遠(yuǎn)，測(cè)距誤差對(duì)定位精度影響較明顯。

圖7 彎道場(chǎng)景車(chē)道判斷準(zhǔn)確度統(tǒng)計(jì)圖

根據(jù)上述直線距離誤差統(tǒng)計(jì)及車(chē)道判斷準(zhǔn)確度統(tǒng)計(jì)結(jié)果可見(jiàn)，在直道場(chǎng)景及彎道場(chǎng)景下，測(cè)距平均誤差均小于0.3m，各車(chē)道的判斷準(zhǔn)確率均達(dá)到100%，滿足高速路應(yīng)急車(chē)道統(tǒng)計(jì)的需求。

3.5 車(chē)輛特性分析

該場(chǎng)景下由于車(chē)輛處于移動(dòng)狀態(tài)，且應(yīng)急車(chē)道較窄，單次判斷應(yīng)急車(chē)道的準(zhǔn)確率對(duì)測(cè)距精度要求較高，可進(jìn)一步采用分階段多次判斷算法來(lái)提高應(yīng)急車(chē)道的判斷準(zhǔn)確率。

(1)根據(jù)當(dāng)前定位結(jié)果判斷是否在應(yīng)急車(chē)道內(nèi)，保存當(dāng)次判斷結(jié)果；

(2)累計(jì)K(K可調(diào))次判斷結(jié)果，若超過(guò)50%以上判斷結(jié)果為應(yīng)急車(chē)道則初步判斷結(jié)果判定為在應(yīng)急車(chē)道，否則判定為在正常車(chē)道，保存初步判斷結(jié)果；

(3)累計(jì)三次的初步判斷結(jié)果中，超過(guò)兩次判定為應(yīng)急車(chē)道，則最終判定為應(yīng)急車(chē)道，否則為正常車(chē)道。

此外，根據(jù)汽車(chē)行業(yè)配件安裝慣例，如ETC系統(tǒng)的車(chē)載單元(On board Unit，OBU)，UWB定位標(biāo)簽一般安裝在車(chē)輛車(chē)頂靠右位置，因此工程上還需要考慮進(jìn)行位置校正，以提高車(chē)輛的車(chē)道判斷準(zhǔn)確性，參考校正值為20cm～30cm。

4 結(jié)論

設(shè)計(jì)的基于UWB傳感模塊的車(chē)輛位置定位系統(tǒng)實(shí)現(xiàn)了車(chē)輛位置的快速、便捷測(cè)量，有效降低非視距(NLOS)傳播對(duì)定位精度的影響。提出采用基于TDOA的Chan算法進(jìn)行車(chē)輛位置解算，將服從正態(tài)分布的測(cè)量值誤差考慮進(jìn)來(lái)，可以降低個(gè)別較大的隨機(jī)測(cè)量誤差的影響，并且基于終端坐標(biāo)與距離關(guān)系進(jìn)行了二次加權(quán)計(jì)算，充分利用了冗余的數(shù)據(jù)，最后通過(guò)中位值平均濾波法進(jìn)行車(chē)輛位置優(yōu)化，進(jìn)一步抑制脈沖性干擾所產(chǎn)生的定位誤差。實(shí)驗(yàn)分別選取高速路直道和彎道場(chǎng)景進(jìn)行仿真驗(yàn)證，結(jié)果表明，測(cè)距平均誤差最小達(dá)到15.45cm，最大為27.39cm；X軸方向的平均偏差為10.32cm，Y軸方向的平均偏差為12.43cm；在該精度下車(chē)輛所在車(chē)道判斷準(zhǔn)確率為100%。系統(tǒng)具有低功耗、低成本、算法簡(jiǎn)單、精度高等優(yōu)點(diǎn)。