車路協同環境下的雙雷達車輛檢測和跟蹤優化方法

李 旭，秦嗣牧，王建春

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

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

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

1 雷達數據預處理

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

圖1 2種雷達安裝位置及檢測區域示意圖

圖2 雷達數據融合估計算法流程框圖

1.1 目標識別分類

采用基于密度的自適應DBSCAN聚類算法，該算法可以識別任意形狀簇，且利用數學統計分析的方法定義聚類所需的參數Eps與MinPts，避免識別過程中的人工干預。此處采用橫/縱向距離、速度3種屬性來進行聚類，以提高車輛與車輛、車輛與靜態物體的聚類準確性，并將聚類出的簇作為車輛的坐標點。

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

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

(1)

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

(2)

式中：Pi為第i個對象的Eps鄰域對象數量；n為數據集D中的對象總數。如果MinPts不變，Eps取值過大，會導致1個簇內包含范圍過大；Eps過小，會導致簇分裂為多個小簇；如果Eps不變，MinPts的值取得過大，會導致聚類成簇的條件過于嚴苛；MinPts過小，會導致核心點分裂。隨著Eps增加，MinPts參數值呈現平穩狀態，因此選擇進入平穩狀態的初始Eps值和MinPts值，聚類效果最優。

S集合是已知有7個簇類、共包含800個對象的數據集，分布如圖3所示，以該數據集來驗證自適應算法的合理性。

圖3 S數據散點圖

采用K-平均近鄰法和數學期望法生成Eps候選值和對應的MinPts候選值如圖4所示。

圖4 Eps和MinPts候選值

圖5展示了K-最近鄰距離與簇數的對應關系，可以看出：當K=12時，簇數開始進入穩定階段，到K=18時結束；此時根據計算對應的Eps=1.6，MinPts=32，聚類結果如圖6所示。該數據集利用該組參數可以準確聚類出7個簇，以此證明本文自適應算法的有效性。

圖5 K-最近鄰距離對應的簇類數量折線圖

圖6 S數據集聚類結果示意圖

1.2 坐標系轉換

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

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

圖7為護欄與其他車輛(2號車為自車)分別在自車雷達和路側雷達檢測下的相對位置和方向，o點為2個坐標系的原點，θ為2個護欄數據擬合出的直線夾角，前車對于自車的相對距離分別為：

圖7 路側雷達數據轉換示意圖

(3)

(4)

式中： ΔLA和ΔLB分別為2個雷達下的相對距離；v′為根據θ合成的前車車輛行駛方向和側向速度。

2 目標數據估計算法

路側雷達數據經過相對距離轉換后，與車載雷達處于同一坐標系中。采用無跡卡爾曼濾波算法，通過無跡變換使非線性系統方程適用于線性假設下的標準卡爾曼濾波體系[12]，具體流程如下：

一個n維非線性離散動態系統可以表示為[13]：

xk=f(xk-1)+wk

(5)

zk=h(xk)+vk

(6)

無跡卡爾曼濾波標準程序如下：

對于k≥1：

1) Sigma點取樣。生成1組具有相關權值的確定性樣本點[15]：

(7)

(8)

2) 時間更新。通過非線性函數變換樣本點，計算預測的均值和協方差。

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 實驗與分析

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

圖8 實驗場地圖

圖9 路側和車載雷達安置圖

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

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

圖11 行駛工況示意圖

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

圖12 EKF和本文算法的實驗結果

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

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

在算法效率方面，采用通過修正估計值達到穩定線的時間來作為驗證指標。圖13為3種不同算法和數據集的修正時間以及準確率折線圖，其中虛線為經過一定步長后，預測誤差達到的穩定線，點劃線為達到穩定線所使用的時間點。由圖12、13可得，將無跡卡爾曼濾波運用在車路協同下的雙雷達融合中可以有效提高車輛識別的精確性和效率。

圖13 3種預測值的誤差穩定圖

4 結論

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