陳守剛，趙波，廖明明

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

0 引言

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

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

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

1.1 UWB 優(yōu)勢(shì)分析

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

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

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ā)時(shí)序。成本較低，基站之間無需光纖連接。該算法簡(jiǎn)單，可由DSP 車載終端完成位置計(jì)算，然后直接發(fā)送到車輛CAN 總線。

本文采用異步式UWB 定位技術(shù)，在車上安裝十分方便。異步式 UWB 定位系統(tǒng)三邊測(cè)量原理，圖2 中，分別以A、B、C 三點(diǎn)為圓心畫圓，并建立對(duì)應(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 定位中，使用信號(hào)時(shí)間戳計(jì)算基站從發(fā)射信號(hào)到接受信號(hào)所用時(shí)間，即飛行時(shí)間TOF（Time of Flight）法，根據(jù)位移時(shí)間公式計(jì)算得到信號(hào)經(jīng)過的位移距離，這個(gè)距離是測(cè)量距離的2 倍（信號(hào)發(fā)送與接收）。位移時(shí)間公式為：

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

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

2 車與車之間的相對(duì)位姿計(jì)算

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

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

圖4 車輛實(shí)際模型與測(cè)量原理Fig.4 The actual vehicle model and measurement principle

因?yàn)楸疚奶岢龅姆椒ㄋ肬WB 設(shè)備安裝在動(dòng)態(tài)運(yùn)動(dòng)的車輛上，所以基站位置不固定。本文方法不需要絕對(duì)的角度信息，只需要相對(duì)的距離信息。標(biāo)簽與基站之間的距離分別為OA、OB、OD、DA、DB，標(biāo)簽O 與基站D 和基站A 與基站B 的距離與車身寬度相等，即AB=OD 。OA、OB、DA、DB、OD 的距離可以通過UWB 測(cè)量得到。

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

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

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

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

本文提出的方法中，卡爾曼濾波系統(tǒng)[8]的輸入狀態(tài)量為x=（L，θ，β）T，即通過UWB 測(cè)量得到的原始數(shù)據(jù)值。首先建立數(shù)據(jù)的計(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控制輸入，因此可以得到更為簡(jiǎn)潔的表達(dá)形式：

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

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

卡爾曼濾波的增益矩陣為

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

3 實(shí)驗(yàn)結(jié)果對(duì)比

3.1 實(shí)驗(yàn)內(nèi)容

3.1.1 實(shí)驗(yàn)設(shè)備

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

圖5 實(shí)驗(yàn)設(shè)備Fig.5 Experimental equipment

3.1.2 測(cè)試方案

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

3.2 實(shí)驗(yàn)結(jié)果

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

圖6 直線距離測(cè)試Fig.6 Straight-line distance test

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

圖7 方位角測(cè)試Fig.7 Azimuth angle test

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

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

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

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

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

4 結(jié)語(yǔ)

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