基于UWB 的自動駕駛編隊車輛相對位姿計算

陳守剛，趙波，廖明明

（201600 上海市 上海工程技術(shù)大學(xué) 機械與汽車工程學(xué)院）

0 引言

近年來，自動駕駛物流車編隊行駛成為智能交通中的重要環(huán)節(jié)，智能網(wǎng)聯(lián)車輛將成為國內(nèi)汽車產(chǎn)業(yè)發(fā)展新的突破點[1]。自動駕駛物流車編隊需要安裝各種各樣的傳感器用以獲取車輛之間與周圍環(huán)境的信息，是編隊行駛做出適當(dāng)規(guī)劃決策的基礎(chǔ)[2]。隨著科學(xué)與技術(shù)的飛速進(jìn)步，車輛定位技術(shù)使用的傳感器種類更加豐富多樣，如激光雷達(dá)、GNSS系統(tǒng)、慣導(dǎo)系統(tǒng)等。激光雷達(dá)成本高，計算量大，在編隊行駛車隊中成本更高；GNSS 系統(tǒng)在不開闊的場景下容易失效；慣導(dǎo)系統(tǒng)的數(shù)據(jù)易產(chǎn)生漂移[3]。

針對上述問題，本文提出基于UWB 的相對位姿估計，計算量小、數(shù)據(jù)不易漂移，而且在使用場景沒有限制，數(shù)據(jù)更新頻率快。將UWB 技術(shù)應(yīng)用于自動駕駛編隊中，提出了基于UWB 的位姿計算方法，以及基于卡爾曼濾波的數(shù)據(jù)去噪方法。

1 UWB 技術(shù)特性分析

1.1 UWB 優(yōu)勢分析

超寬帶（Ultra Wide Band，UWB）技術(shù)是無線載波通信技術(shù)的重要組成部分[4]。UWB 技術(shù)沒有使用正弦載波作為傳輸載體，而是使用非正弦波窄脈沖信號進(jìn)行數(shù)據(jù)傳輸，所以UWB 技術(shù)所能使用的頻譜范圍極其寬泛，使得UWB 系統(tǒng)結(jié)構(gòu)的實現(xiàn)比較簡單，并且可以實現(xiàn)高速數(shù)據(jù)傳播。UWB 系統(tǒng)使用持續(xù)時間很短的脈沖信號進(jìn)行數(shù)據(jù)發(fā)送，占空比很低，系統(tǒng)耗電很低，在能耗方面比傳統(tǒng)無線通信設(shè)備有非常大的優(yōu)勢。由于UWB 信號在極寬的頻帶范圍之內(nèi)，對于傳統(tǒng)的通訊設(shè)備而言，UWB 信號與白噪聲信號相似，UWB 信號的功率譜密度低于自然的電子噪聲的功率譜密度，因此UWB 技術(shù)安全性很高。傳統(tǒng)的無線通信設(shè)備的射頻信號大多為連續(xù)信號，或者延續(xù)時間大于多徑傳播的時間，多徑傳播效應(yīng)明顯，但是UWB 技術(shù)發(fā)射的是持續(xù)時間很短、占空比很小的單周期脈沖信號。UWB 多徑信號在時間序列上可進(jìn)行分離，因此多徑分辨能力強。沖激脈沖信號具有很高的定位精度，UWB 技術(shù)很容易將定位需求與通信需求合二為一，但是傳統(tǒng)無線電通信設(shè)備無法做到這一點。UWB 可以滿足室內(nèi)和地下精確定位的需求，而GPS 只能在定位衛(wèi)星的可視范圍之內(nèi)使用。UWB技術(shù)可以進(jìn)行相對定位，并且定位精度可以達(dá)到厘米級別。在實際工程中，UWB 技術(shù)比傳統(tǒng)無線通信設(shè)備要簡單得多，可通過全數(shù)字化實現(xiàn)[5]。

綜上所述，UWB 相對于傳統(tǒng)的定位技術(shù)，具有系統(tǒng)構(gòu)造復(fù)雜度低、高速的數(shù)據(jù)傳播、能耗低、對信道衰落魯棒、發(fā)射信號功率譜密度低、被截獲的概率極低、可以提供厘米級別定位精度、工程造價低等一系列優(yōu)點。

1.2 UWB 定位原理

UWB 高精度定位技術(shù)主要分為同步式 UWB定位系統(tǒng)和異步式 UWB 定位系統(tǒng)[6]。

（1）同步式 UWB 定位系統(tǒng)

原理如圖1 所示。基于 TDOA 算法，定位精度可以達(dá)到 10 cm CEP，刷新頻率＞50 Hz。但成本很高，基站之間需光纖連接，施工比較復(fù)雜，算法復(fù)雜。需要定位服務(wù)器，定位信息需從服務(wù)器無線發(fā)送至車載終端，延遲較高。

圖1 同步式 UWB 定位系統(tǒng)Fig.1 Synchronous UWB positioning system

（2）異步式 UWB 定位系統(tǒng)

原理如圖2 所示。基于 TOF 算法，目前定位精度為 20 cm CEP，需要控制收發(fā)時序。成本較低，基站之間無需光纖連接。該算法簡單，可由DSP 車載終端完成位置計算，然后直接發(fā)送到車輛CAN 總線。

本文采用異步式UWB 定位技術(shù)，在車上安裝十分方便。異步式 UWB 定位系統(tǒng)三邊測量原理，圖2 中，分別以A、B、C 三點為圓心畫圓，并建立對應(yīng)的坐標(biāo)系，A、B、C 的坐標(biāo)分別是（Xa，Ya）、（Xb，Yb）、（Xc，Yc），D為標(biāo)簽，以A、B、C為圓心的圓相交于標(biāo)簽D。

圖2 異步式 UWB 定位系統(tǒng)Fig.2 Asynchronous UWB positioning system

圓A、B、C 的半徑分別是da、db、dc，假設(shè)D的坐標(biāo)為（X，Y），根據(jù)最小二乘法可以列出公式：

可以得到標(biāo)簽D 的坐標(biāo)為

所以，只需要知道基站A、B、C 的坐標(biāo)及到標(biāo)簽的距離，就可以得到D 的坐標(biāo)，從而定位到D 的位置。該方法適用于室內(nèi)的定位，在基站位置確定的情況下，使用該方法可以快速進(jìn)行標(biāo)簽的決定位置定位。

1.3 基于TOF 算法的UWB 技術(shù)

在UWB 定位中，使用信號時間戳計算基站從發(fā)射信號到接受信號所用時間，即飛行時間TOF（Time of Flight）法，根據(jù)位移時間公式計算得到信號經(jīng)過的位移距離，這個距離是測量距離的2 倍（信號發(fā)送與接收）。位移時間公式為：

評價TOF 測距方法的指標(biāo)就是精度。單邊雙向測距方法是常用方法之一，但是由于該方法精度不高，誤差與所測量的距離成指數(shù)關(guān)系，特別在本文的動態(tài)測量定位案例中，會導(dǎo)致誤差更大，不符合實際情況。為了減小誤差，本文使用雙邊雙向測距[7]。使用基站與標(biāo)簽正反兩個方向的測量方法，可以有效減少時鐘偏移所導(dǎo)致的測距誤差。基站首先標(biāo)簽發(fā)送測量信息，標(biāo)簽接收測量信息之后進(jìn)行回復(fù)，基站接受信號后向標(biāo)簽回復(fù)信息。這樣，通過求取飛行時間的平均值，可以減少基站與標(biāo)簽兩者之間的時鐘偏移，從而改善測量精度。原理如圖3 所示。

圖3 雙邊雙向測距原理Fig.3 Principle of bilateral two-way ranging

2 車與車之間的相對位姿計算

2.1 車車相對位姿數(shù)據(jù)獲取

在自動駕駛物流車輛直行行駛編隊中，因為路徑規(guī)劃和底盤線控的需求，需要獲取跟隨車相對于牽引車的距離、方位角與姿態(tài)角，使得編隊駕駛更穩(wěn)定協(xié)調(diào)。圖4（a）所示為實際車輛模型，圖4（b）所示為數(shù)據(jù)測量模型。圖4（b）中：兩車之間的距離L 為牽引車尾部的中心點與跟隨車頭部的中心點之間的距離；方位角θ為跟隨車與L 所在方向之間的夾角；姿態(tài)角β為跟隨車軸線與牽引車軸線的夾角。圖4 中定義的進(jìn)行方向，順時針為正，逆時針為負(fù)，因此θ為正方向，β為負(fù)方向。

圖4 車輛實際模型與測量原理Fig.4 The actual vehicle model and measurement principle

因為本文提出的方法所用UWB 設(shè)備安裝在動態(tài)運動的車輛上，所以基站位置不固定。本文方法不需要絕對的角度信息，只需要相對的距離信息。標(biāo)簽與基站之間的距離分別為OA、OB、OD、DA、DB，標(biāo)簽O 與基站D 和基站A 與基站B 的距離與車身寬度相等，即AB=OD 。OA、OB、DA、DB、OD 的距離可以通過UWB 測量得到。

需要建立相應(yīng)的坐標(biāo)系確定基站與標(biāo)簽之間的相對關(guān)系。以標(biāo)簽O 為坐標(biāo)原點，OA 方向為x軸，經(jīng)過原點且垂直于x 的直線為y 軸。則各坐標(biāo)點為O（0，0），A（XA，YA），B（XB，YB），D（XD，YD），并且可以得到∠AOD 與∠BOD 的角度值，設(shè)∠AOD=α，∠BOD=λ。

則A 點的坐標(biāo)為（OA·sinα，OA·cosα），B 點的坐標(biāo)為（OB·sinλ，OB·cosλ），D 點的坐標(biāo)為（OD，0）。將數(shù)據(jù)代入可得A 點為（c·tOA·sinα，c·tOA·cosα），B 點為（c·tOB·sinλ，c·tOB·cosλ），D 點為（c·tOD，0）。因此可以計算得L、θ、β：

根據(jù)式（5）—式（7），可以得到相關(guān)的數(shù)據(jù)。但是實際測距時，由于環(huán)境復(fù)雜多變，加之車輛自身抖動，測得的數(shù)據(jù)中有噪聲，即得到的L、θ、β數(shù)據(jù)不平滑，無法應(yīng)用于高速運動的實際場景中。為了提高測試數(shù)據(jù)的穩(wěn)定性和精度，需要對原始數(shù)據(jù)進(jìn)行平滑處理，因此引入卡爾曼濾波。卡爾曼濾波能夠從一系列包含噪聲的測量中估計動態(tài)系統(tǒng)的狀態(tài)。

2.2 基于卡爾曼濾波的數(shù)據(jù)平滑

本文提出的方法中，卡爾曼濾波系統(tǒng)[8]的輸入狀態(tài)量為x=（L，θ，β）T，即通過UWB 測量得到的原始數(shù)據(jù)值。首先建立數(shù)據(jù)的計算模型：

式中：k，k-1——當(dāng)前狀態(tài)與前一狀態(tài)；A∈Rn×n——系統(tǒng)的轉(zhuǎn)移矩陣；u∈Rl——可控制的輸入；B∈Rn×l——控制輸入的轉(zhuǎn)移矩陣；w——過程噪聲。由于本文提出的方法不需要使用u控制輸入，因此可以得到更為簡潔的表達(dá)形式：

卡爾曼濾波系統(tǒng)的數(shù)據(jù)觀測模型為

式中：x∈Rn——要估計的狀態(tài)；z∈Rm——實際測量值；H∈Rm×n——當(dāng)前狀態(tài)實際測量值的轉(zhuǎn)移矩陣；v——觀測噪聲。

卡爾曼濾波的增益矩陣為

式中：Pk|k-1——預(yù)測協(xié)方差矩陣；Pk——觀測被觀測的噪聲；H∈Rm×n——當(dāng)前狀態(tài)實際測量值的轉(zhuǎn)移矩陣。當(dāng)Pk|k-1增大，卡爾曼濾波的增益Kk相應(yīng)增大，噪聲Rk增大，卡爾曼濾波的增益Kk相應(yīng)減小。

3 實驗結(jié)果對比

3.1 實驗內(nèi)容

3.1.1 實驗設(shè)備

為了檢驗本文所提出方法的有效性與合理性，對該理論進(jìn)行實驗驗證。實驗設(shè)備為2 輛自動擋解放車，使用該車型的目的就是為了檢測UWB 在大型車輛上的可行性與穩(wěn)定性。實驗設(shè)備如圖5 所示。

圖5 實驗設(shè)備Fig.5 Experimental equipment

3.1.2 測試方案

2 輛解放車進(jìn)行直行跟隨行駛，勻速跟隨，速度為30 km/h，跟隨距離2～ 60 m。然后跟隨車靜止，前車沿直線向前行駛至60 m 處，目的是為了模擬車間距從小距離變化到大距離的情況。

3.2 實驗結(jié)果

直線跟隨距離精度測試以及經(jīng)卡爾曼濾波平滑之后的對比如圖6 所示。圖6 中直線表示跟車距離從2～60 m 的真實值，圖6（a）中折線表示跟車距離的測量值，可見波動明顯；圖6（b）中折線表示經(jīng)過卡爾曼濾波之后的跟車距離。

圖6 直線距離測試Fig.6 Straight-line distance test

直線跟隨方位角精度測試以及經(jīng)過卡爾曼濾波平滑之后的對比如圖7 所示。

圖7 方位角測試Fig.7 Azimuth angle test

圖7 中的直線表示跟車距離2～60 m 過程中的方位角，圖7（a）中折線表示根據(jù)方位角的測量值，可見波動明顯，波峰可達(dá)15°；圖7（b）中折線表示經(jīng)過卡爾曼濾波之后的跟車方位角，可以看出波動范圍明顯減小，濾波后的波峰小于6°。

直線跟隨姿態(tài)角精度的測試以及經(jīng)過卡爾曼濾波平滑之后的對比如圖8 所示。圖8 中的直線表示跟車距離從2～60 m 過程中的姿態(tài)角，圖8（a）中折線表示跟車姿態(tài)角的測量值，可見波動明顯，波峰可達(dá)10°；圖8（b）中折線表示經(jīng)卡爾曼濾波后的跟車姿態(tài)角，波動明顯減小，波峰小于5°。

圖8 姿態(tài)角測試Fig.8 Attitude angle test

表1 是UWB 測量數(shù)據(jù)濾波前后的誤差統(tǒng)計。從表1 可以看出，經(jīng)過濾波之后，直線距離的平均誤差從16.3%降到5.2%；方位角的平均誤差從19.7%降為9.4%；姿態(tài)角的平均誤差從17.4%降為8.9%。說明卡爾曼濾波很有效。距離的精度最高，說明UWB 完全適用。但是角度誤差較大，可能是因為基站A 和基站B 與標(biāo)簽O 和基站D 之間的距離與車輛等寬，在車距變遠(yuǎn)時，對角度的獲取有限制。但是經(jīng)過平滑之后的數(shù)據(jù)是滿足車輛編隊行駛需求的，說明UWB 技術(shù)應(yīng)用在自動駕駛編隊中有效且便利。

表1 UWB 數(shù)據(jù)誤差前后對比Tab.1 Comparison of UWB data error before and after filtering

4 結(jié)語

本文利用UWB 定位精度高的特性，將UWB應(yīng)用到自動駕駛車輛中。利用UWB 計算自動駕駛編隊直行時車輛間的相對位姿，包括距離、方位角與姿態(tài)角，然后使用卡爾曼濾波對原始測量得到的數(shù)據(jù)進(jìn)行平滑濾波，結(jié)果提高了距離、方位角、姿態(tài)角在直行時的精度，驗證了低成本傳感器UWB應(yīng)用在自動駕駛行業(yè)的可能性是極大的，為相對位姿的獲取提供了新的思路和方法。下一步可以將慣導(dǎo)器件與UWB 進(jìn)行多傳感器融合，改進(jìn)編隊車輛在左轉(zhuǎn)和右轉(zhuǎn)時的精度。