李 旭，秦嗣牧，王建春

(山東科技大學(xué) 交通學(xué)院， 山東 青島 266590)

車路協(xié)同基于傳感探測(cè)、無線通訊等技術(shù)感知車路信息，并通過車路、車車通信設(shè)備進(jìn)行信息共享和交互，實(shí)現(xiàn)車輛和基礎(chǔ)設(shè)施之間智能協(xié)同與配合，達(dá)到優(yōu)化利用系統(tǒng)資源、改善道路交通安全、緩解交通擁擠的目標(biāo)。多傳感器數(shù)據(jù)融合是車路協(xié)同的關(guān)鍵技術(shù)之一。眾多學(xué)者針對(duì)多傳感器環(huán)境感知領(lǐng)域進(jìn)行了相關(guān)研究。向易等[1]通過車載雙雷達(dá)的形式估計(jì)前方目標(biāo)位置狀態(tài)；王戰(zhàn)古等[2]提出了一種基于車載毫米波雷達(dá)和攝像頭信息融合的檢測(cè)方法，通過毫米波雷達(dá)獲取圖像中的感興趣區(qū)域，再利用圖像處理的方法識(shí)別車輛；同樣地，劉志強(qiáng)等[3]提出了基于GNN-DS來進(jìn)行毫米波雷達(dá)和攝像頭的信息融合算法，以保證檢測(cè)精度；薛培林等[4]考慮激光雷達(dá)與相機(jī)融合的車輛識(shí)別方法，利用改進(jìn)yolov3-tiny算法提高目標(biāo)檢測(cè)準(zhǔn)確率。在考慮路側(cè)和車載傳感器融合方面，李澤等[5]和謝伯元等[6]都利用卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，但未考慮目標(biāo)在非線性運(yùn)動(dòng)狀態(tài)下的預(yù)測(cè)精確度。

針對(duì)上述研究，提出了一種基于無跡卡爾曼濾波(unscented Kalman filter，UKF)算法，利用統(tǒng)一坐標(biāo)系的路側(cè)毫米波雷達(dá)和車載毫米波雷達(dá)數(shù)據(jù)進(jìn)行車輛行駛軌跡和速度的預(yù)測(cè)和更新。該算法考慮車輛非線性行駛工況，在無人駕駛多傳感器數(shù)據(jù)融合領(lǐng)域具有應(yīng)用前景。

1 雷達(dá)數(shù)據(jù)預(yù)處理

選用2個(gè)同規(guī)格AER毫米波雷達(dá)，其更新頻率為50 ms，以雷達(dá)自身為坐標(biāo)系原點(diǎn)，可以檢測(cè)目標(biāo)相對(duì)雷達(dá)的距離、速度等信息，檢測(cè)距離為遠(yuǎn)距170 m，近距70 m，距離精度為±0.20 m(遠(yuǎn)距)，±0.10 m(近距)，速度精度為±0.1 km/h。車載雷達(dá)和路側(cè)雷達(dá)安裝位置及檢測(cè)區(qū)域如圖1所示，由圖可知，如果與前車距離過近，車載雷達(dá)會(huì)受到前車遮擋而看不到對(duì)向車輛，因此需要與路側(cè)毫米波雷達(dá)數(shù)據(jù)融合才可得到完整的車路數(shù)據(jù)。雷達(dá)數(shù)據(jù)融合估計(jì)算法流程見圖2。

圖1 2種雷達(dá)安裝位置及檢測(cè)區(qū)域示意圖

圖2 雷達(dá)數(shù)據(jù)融合估計(jì)算法流程框圖

1.1 目標(biāo)識(shí)別分類

采用基于密度的自適應(yīng)DBSCAN聚類算法，該算法可以識(shí)別任意形狀簇，且利用數(shù)學(xué)統(tǒng)計(jì)分析的方法定義聚類所需的參數(shù)Eps與MinPts，避免識(shí)別過程中的人工干預(yù)。此處采用橫/縱向距離、速度3種屬性來進(jìn)行聚類，以提高車輛與車輛、車輛與靜態(tài)物體的聚類準(zhǔn)確性，并將聚類出的簇作為車輛的坐標(biāo)點(diǎn)。

通過給定數(shù)據(jù)集D={d(i)，i=0，1，2，3，…，n}，計(jì)算數(shù)據(jù)集D的距離分布矩陣[7]：

Dn×n={Dist(i，j)|1≤i≤n，1≤j≤n}

(1)

式中：Dn×n為n×n的實(shí)對(duì)稱矩陣；n為數(shù)據(jù)集D所包含的對(duì)象數(shù)量；Dist(i，j)為數(shù)據(jù)集D中第i個(gè)對(duì)象到第j個(gè)對(duì)象的距離。將矩陣D每行以升序排列，則第1列元素所組成的距離向量D0表示對(duì)象到自身的距離，全為0。第K列元素構(gòu)成所有數(shù)據(jù)點(diǎn)的K-最近鄰距離[8-9]向量DK，對(duì)Dk列進(jìn)行求平均值，作為Eps參數(shù)列表，采用數(shù)學(xué)期望法生成MinPts參數(shù)列表。對(duì)于給定的Eps參數(shù)列表，依次求出每個(gè)Eps參數(shù)對(duì)應(yīng)的Eps鄰域?qū)ο髷?shù)量，并計(jì)算所有對(duì)象的Eps鄰域?qū)ο髷?shù)量的數(shù)學(xué)期望值，作為數(shù)據(jù)集D的鄰域密度閾值MinPts參數(shù)[10]，表示為：

(2)

式中：Pi為第i個(gè)對(duì)象的Eps鄰域?qū)ο髷?shù)量；n為數(shù)據(jù)集D中的對(duì)象總數(shù)。如果MinPts不變，Eps取值過大，會(huì)導(dǎo)致1個(gè)簇內(nèi)包含范圍過大；Eps過小，會(huì)導(dǎo)致簇分裂為多個(gè)小簇；如果Eps不變，MinPts的值取得過大，會(huì)導(dǎo)致聚類成簇的條件過于嚴(yán)苛；MinPts過小，會(huì)導(dǎo)致核心點(diǎn)分裂。隨著Eps增加，MinPts參數(shù)值呈現(xiàn)平穩(wěn)狀態(tài)，因此選擇進(jìn)入平穩(wěn)狀態(tài)的初始Eps值和MinPts值，聚類效果最優(yōu)。

S集合是已知有7個(gè)簇類、共包含800個(gè)對(duì)象的數(shù)據(jù)集，分布如圖3所示，以該數(shù)據(jù)集來驗(yàn)證自適應(yīng)算法的合理性。

圖3 S數(shù)據(jù)散點(diǎn)圖

采用K-平均近鄰法和數(shù)學(xué)期望法生成Eps候選值和對(duì)應(yīng)的MinPts候選值如圖4所示。

圖4 Eps和MinPts候選值

圖5展示了K-最近鄰距離與簇?cái)?shù)的對(duì)應(yīng)關(guān)系，可以看出：當(dāng)K=12時(shí)，簇?cái)?shù)開始進(jìn)入穩(wěn)定階段，到K=18時(shí)結(jié)束；此時(shí)根據(jù)計(jì)算對(duì)應(yīng)的Eps=1.6，MinPts=32，聚類結(jié)果如圖6所示。該數(shù)據(jù)集利用該組參數(shù)可以準(zhǔn)確聚類出7個(gè)簇，以此證明本文自適應(yīng)算法的有效性。

圖5 K-最近鄰距離對(duì)應(yīng)的簇類數(shù)量折線圖

圖6 S數(shù)據(jù)集聚類結(jié)果示意圖

1.2 坐標(biāo)系轉(zhuǎn)換

為了更好地對(duì)同一物體在不同檢測(cè)設(shè)備下的數(shù)值做出比較，采用特征點(diǎn)匹配的方式，先利用毫米波雷達(dá)檢測(cè)易識(shí)別、特征值明顯的靜態(tài)障礙物，如道路中間的護(hù)欄，并通過最小二乘法擬合護(hù)欄的數(shù)據(jù)點(diǎn)，最后將路側(cè)雷達(dá)點(diǎn)云根據(jù)擬合結(jié)果進(jìn)行位置姿態(tài)矯正，具體步驟如下：

首先，利用密度聚類的方法分別建立2種雷達(dá)中護(hù)欄的數(shù)據(jù)集合，記為PA={p1，p2，…，pn}和PB={p1，p2，…，pn}；根據(jù)護(hù)欄的形狀特征，采用直線模型進(jìn)行擬合，對(duì)于集合P，隨機(jī)抽樣選取5個(gè)初始點(diǎn)擬合直線，計(jì)算所有點(diǎn)到該直線的距離，在預(yù)設(shè)閾值范圍內(nèi)點(diǎn)的數(shù)量記為最優(yōu)點(diǎn)數(shù)[11]；再重新隨機(jī)選取初始點(diǎn)進(jìn)行擬合并計(jì)算，重復(fù)上述步驟500次；最后，比較單次最優(yōu)點(diǎn)數(shù)最多的直線為擬合結(jié)果。由于時(shí)空連續(xù)特性，連續(xù)幀中護(hù)欄的位置偏差不會(huì)太大，因此，為了提高檢測(cè)結(jié)果的連續(xù)性，將相鄰幀的擬合結(jié)果進(jìn)行疊加。

圖7為護(hù)欄與其他車輛(2號(hào)車為自車)分別在自車?yán)走_(dá)和路側(cè)雷達(dá)檢測(cè)下的相對(duì)位置和方向，o點(diǎn)為2個(gè)坐標(biāo)系的原點(diǎn)，θ為2個(gè)護(hù)欄數(shù)據(jù)擬合出的直線夾角，前車對(duì)于自車的相對(duì)距離分別為：

圖7 路側(cè)雷達(dá)數(shù)據(jù)轉(zhuǎn)換示意圖

(3)

(4)

式中： ΔLA和ΔLB分別為2個(gè)雷達(dá)下的相對(duì)距離；v′為根據(jù)θ合成的前車車輛行駛方向和側(cè)向速度。

2 目標(biāo)數(shù)據(jù)估計(jì)算法

路側(cè)雷達(dá)數(shù)據(jù)經(jīng)過相對(duì)距離轉(zhuǎn)換后，與車載雷達(dá)處于同一坐標(biāo)系中。采用無跡卡爾曼濾波算法，通過無跡變換使非線性系統(tǒng)方程適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系[12]，具體流程如下：

一個(gè)n維非線性離散動(dòng)態(tài)系統(tǒng)可以表示為[13]：

xk=f(xk-1)+wk

(5)

zk=h(xk)+vk

(6)

無跡卡爾曼濾波標(biāo)準(zhǔn)程序如下：

對(duì)于k≥1：

1) Sigma點(diǎn)取樣。生成1組具有相關(guān)權(quán)值的確定性樣本點(diǎn)[15]：

(7)

(8)

2) 時(shí)間更新。通過非線性函數(shù)變換樣本點(diǎn)，計(jì)算預(yù)測(cè)的均值和協(xié)方差。

xi，k∣k-1=f(xi，k-1)，i=0，1，…，2n

(9)

(10)

(11)

zi，k∣k-1=h(xi，k∣k-1)，i=0，1，…，2n

(12)

(13)

(14)

(15)

(16)

(17)

其中Kk為增益矩陣。

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

實(shí)驗(yàn)場(chǎng)地為青島市某處雙向四車道，在天橋一側(cè)和車輛前方分別安裝毫米波雷達(dá)。圖8、9分別是實(shí)驗(yàn)場(chǎng)地和雷達(dá)安裝位置。實(shí)驗(yàn)主要從準(zhǔn)確度和算法效率2個(gè)方面來對(duì)提出的算法進(jìn)行可行性驗(yàn)證。為了驗(yàn)證數(shù)據(jù)配準(zhǔn)和濾波算法的準(zhǔn)確性，需要將車輛估計(jì)位置和速度與車輛真實(shí)坐標(biāo)和行駛速度進(jìn)行對(duì)比分析。通過在車輛上放置高精度GPS定位設(shè)備和OBD讀取設(shè)備來采集這兩類真實(shí)值。圖10是高精度定位和OBD讀取設(shè)備安裝位置圖。

圖8 實(shí)驗(yàn)場(chǎng)地圖

圖9 路側(cè)和車載雷達(dá)安置圖

圖10 OBD讀取設(shè)備和高精度定位安裝示意圖

考慮到占用對(duì)向車道超車的實(shí)驗(yàn)工況危險(xiǎn)性和特殊性，利用同向車道前方車輛換道行為進(jìn)行相似性實(shí)驗(yàn)分析，具體工況如下：自車在右側(cè)道路以5 m/s的速度勻速行駛，左側(cè)車道車輛以同樣速度勻速行駛，前車以初始速度為5 m/s在自車正前方勻加速行駛，并在橫向距離超過左側(cè)車輛時(shí)加速換道。具體行駛工況見圖11。在該測(cè)試場(chǎng)景中，自車的雷達(dá)檢測(cè)會(huì)在前車換道后形成視野受限。

圖11 行駛工況示意圖

在常用的目標(biāo)跟蹤算法中，普通卡爾曼濾波不適用于非線性系統(tǒng)，而擴(kuò)展卡爾曼濾波(EKF)是目前應(yīng)用于非線性狀態(tài)估計(jì)系統(tǒng)中最廣泛和最先進(jìn)的算法。因此，在同一實(shí)驗(yàn)工況下，選取EKF算法與本文算法進(jìn)行對(duì)比實(shí)驗(yàn)。通過GPS和OBD采集設(shè)備得到車輛的真實(shí)數(shù)據(jù)作為結(jié)果驗(yàn)證。圖12是將本文算法分別運(yùn)用在車路協(xié)同下雙傳感器融合后的數(shù)據(jù)，以及單自車車載雷達(dá)后，經(jīng)過多次重復(fù)實(shí)驗(yàn)，分別在橫向距離、縱向距離和速度3個(gè)方面的均值估計(jì)和誤差。

圖12 EKF和本文算法的實(shí)驗(yàn)結(jié)果

根據(jù)數(shù)理統(tǒng)計(jì)知識(shí)，均方誤差(mean-square error，MSE)是反映預(yù)測(cè)值與被預(yù)測(cè)值之間差異程度的一種度量，可以有效地反映預(yù)測(cè)模型是否具有良好的精確度，其數(shù)值越小表示預(yù)測(cè)精確度越高。表1展示了EKF算法和單雷達(dá)與雙雷達(dá)利用本文算法在多個(gè)方面的均方誤差。由于車輛換到鄰近車道，橫向行駛距離相比縱向行駛距離過小，均方誤差差距不大；縱向距離和速度上，在利用雙雷達(dá)融合后數(shù)據(jù)基礎(chǔ)上，本文算法相比EKF算法的均方誤差降低了65%以上，驗(yàn)證了所提出車輛跟蹤算法的精確性，且雙雷達(dá)數(shù)據(jù)相比單一雷達(dá)數(shù)據(jù)在精確性方面提高了70%左右。

表1 2種算法的均方誤差 ‰

在算法效率方面，采用通過修正估計(jì)值達(dá)到穩(wěn)定線的時(shí)間來作為驗(yàn)證指標(biāo)。圖13為3種不同算法和數(shù)據(jù)集的修正時(shí)間以及準(zhǔn)確率折線圖，其中虛線為經(jīng)過一定步長(zhǎng)后，預(yù)測(cè)誤差達(dá)到的穩(wěn)定線，點(diǎn)劃線為達(dá)到穩(wěn)定線所使用的時(shí)間點(diǎn)。由圖12、13可得，將無跡卡爾曼濾波運(yùn)用在車路協(xié)同下的雙雷達(dá)融合中可以有效提高車輛識(shí)別的精確性和效率。

圖13 3種預(yù)測(cè)值的誤差穩(wěn)定圖

4 結(jié)論

提出了車載和路側(cè)雙毫米波雷達(dá)數(shù)據(jù)融合的無跡卡爾曼濾波算法，用于對(duì)前方車輛的行駛距離和速度進(jìn)行檢測(cè)和跟蹤，并通過實(shí)車實(shí)驗(yàn)對(duì)算法進(jìn)行對(duì)比驗(yàn)證，證明車路協(xié)同下的雙雷達(dá)無跡卡爾曼濾波算法在道路視野受限的情況下可有效提高自車對(duì)于周圍路況的掌握，便于由此展開相關(guān)決策研究。但在雷達(dá)數(shù)據(jù)處理方面，如何提高聚類效果并更有效地提取車輛坐標(biāo)點(diǎn)有待進(jìn)一步探究。