李建國，張 睿，王 凱，曾 杰

基于匈牙利匹配和卡爾曼濾波的動態(tài)多目標(biāo)跟蹤

李建國，張 睿，王 凱，曾 杰

（陜西重型汽車有限公司汽車工程研究院，陜西 西安 710200）

為了得到無人駕駛汽車目標(biāo)檢測的準確信息，文章通過對標(biāo)定后的原始激光雷達點云進行分割和聚類，利用霍夫直線檢測提取障礙物的外包框，最后使用匈牙利算法對被跟蹤的障礙物和新檢測出的障礙物進行關(guān)聯(lián)匹配，并求出最優(yōu)解，最終使用卡爾曼濾波算法對其進行狀態(tài)優(yōu)化。通過實驗表明，本算法可以在激光雷達一幀0.1 s內(nèi)，快速、準確地跟蹤被檢測出的障礙物。

匈牙利匹配；霍夫直線檢測；卡爾曼濾波

前言

中國2035新能源汽車規(guī)劃研究及汽車相關(guān)“十四五”科技規(guī)劃工作的建議中指出“十四五”期間是我國實現(xiàn)制造強國的關(guān)鍵階段，汽車產(chǎn)業(yè)在國民經(jīng)濟和社會發(fā)展中占有舉足輕重的地位，汽車強國是制造強國的重要支撐。而智能網(wǎng)聯(lián)已經(jīng)成為汽車行業(yè)的公認的發(fā)展方向?？v觀智能網(wǎng)聯(lián)汽車的發(fā)展路徑，自動駕駛技術(shù)的產(chǎn)品研發(fā)是其中不可忽視的重要路徑之一。多目標(biāo)跟蹤技術(shù)是自動駕駛技術(shù)中重要的研究課題。近年來隨著自動駕駛技術(shù)熱度的上升也備受國內(nèi)外學(xué)者的關(guān)注。多目標(biāo)跟蹤可以依靠純視覺的方式實現(xiàn)，但是由于一些環(huán)境的局限性，不能達到較好的檢測和跟蹤效果。而激光雷達正好彌補了計算機視覺方面的不足。本文通過激光雷達感知技術(shù)，應(yīng)用霍夫直線檢測，匈牙利匹配，卡爾曼濾波實現(xiàn)了動態(tài)多目標(biāo)的檢測和跟蹤，相比計算機視覺的方式，獲得了障礙物更精確的位置和速度，結(jié)果具有更高的穩(wěn)定性和準確性。

1 障礙物邊框構(gòu)建

汽車激光雷達檢測時，可行駛區(qū)域的目標(biāo)跟蹤一般對象分為行人和車輛[1]。激光雷達掃描過程中，激光雷達最多只能掃描到前方車身的兩個面，從而導(dǎo)致聚類之后會形成“L”型邊框，影響識別效果。為了達到更有效的識別目的，本算法障礙物邊框構(gòu)建是對“L”型確立的外接長方體。確立障礙物外接長方體方法有很多，比如平行坐標(biāo)軸法、最小凸包法[2]和最小二乘法[3]等。前幾種方法的優(yōu)點雖然快捷、簡單，但是不準確的外包框會產(chǎn)生錯誤信息造成決策模塊做出錯誤的判斷。如圖1中的(a)所示。本文采用了霍夫直線檢測的方法來獲取障礙物的主方向，進而獲取準確的外邊框以及航向角等信息，可更加準確地完成障礙物的輪廓描述，為智能決策提供精準完整的環(huán)境信息。

圖1 不同算法外邊框投影圖

本文中運用霍夫直線檢測進行障礙物檢測的效果如圖1(b)所示。在立體空間中，激光雷達掃描到車身的點云在空間分布如圖2(a)所示。首先將三維空間點云投影到--平面，對其投影點云柵格化，用0、1代表柵格中是否存在點云，標(biāo)注為1的代表存儲了障礙物點云。投影后的柵格如圖2(b)所示。

圖2 障礙物點云柵格化

用點（0，0）表示圖2(b)中存儲了障礙物點的柵格，再運用霍夫直線檢測，得出在--平面經(jīng)過每一個點的直線簇方程為0=0+(是斜率，是截距)[3]。但由于在直角坐標(biāo)系中，直線在垂直于軸時斜率無法表示，所以將其對應(yīng)到極坐標(biāo)系下，表示為=cos+sin（為原點到直線的距離）[4]。對于極坐標(biāo)系下的每一條（，）曲線在--平面都有通過點（0，0）的一條直線與其對應(yīng)，其中>0，0<<2π。在-坐標(biāo)系下，選取所有曲線最多的交點，來代表在-平面經(jīng)過“L”型點云最長邊直線的主方向，得到與軸夾角，如圖3(a)所示得到夾角之后，將“L”型點云繞中心旋轉(zhuǎn)，使主方向的直線與坐標(biāo)軸平行。在軸方向遍歷，求出頂點1和2，邊框長12=，再在軸方向進行遍歷，得出頂點3和4，邊框?qū)?3=。最后在三維空間中，“L”型點云在軸方向進行遍歷，得出軸方向最大最小值，兩者差值為邊框高，然后再按照原始方向逆時針旋轉(zhuǎn)角。使障礙物返回原始位置，得出準確的外邊框以及航向角信息。

圖3 障礙物外包框定點確立

2 目標(biāo)跟蹤

三維激光雷達動態(tài)目標(biāo)跟蹤的研究重點主要是數(shù)據(jù)關(guān)聯(lián)和狀態(tài)估計兩大方面[5]。在目標(biāo)跟蹤之前，需要進行檢測，并建立好障礙物邊框，從而得知障礙物的相關(guān)信息，實現(xiàn)特征建模。

在動態(tài)場景下的多目標(biāo)跟蹤，可以認為是數(shù)據(jù)關(guān)聯(lián)和狀態(tài)更新互相作用的過程。在激光雷達點云每一幀序列中進行跨檢測結(jié)果的關(guān)聯(lián)，而為了解決數(shù)據(jù)關(guān)聯(lián)的問題，就需要對運動過程和運動目標(biāo)的外觀特征進行建模[6]。對于匹配上的障礙物，用關(guān)聯(lián)矩陣表示，采用匈牙利匹配的算法得到最優(yōu)解[7]。再對配對之后的障礙物，采用卡爾曼濾波算法進行狀態(tài)更新。整個目標(biāo)跟蹤流程如圖4所示。

圖4 目標(biāo)跟蹤流程圖

2.1 數(shù)據(jù)關(guān)聯(lián)

在實際道路場景中，可行駛區(qū)域道路動態(tài)目標(biāo)數(shù)量不定，位置隨機發(fā)生變化的復(fù)雜路況，通常會采用數(shù)據(jù)關(guān)聯(lián)方法。激光雷達相鄰幀之間被跟蹤障礙物與新檢測障礙物進行匈牙利匹配，得出障礙物的關(guān)聯(lián)矩陣。關(guān)聯(lián)矩陣用于計算每個對象和新建對象之間的關(guān)聯(lián)距離，使用匈牙利算法為每個對象和新建對象進行最優(yōu)分配[8]。

本研究主要考慮到車在行駛過程中，會面臨各種不確定性，相應(yīng)的車速也在不停發(fā)生變化，所以沒有利用速度進行位置預(yù)測。而是需要將第一幀和第二幀的目標(biāo)去建立一個關(guān)聯(lián)矩陣。假設(shè)激光雷達在第幀時，有個目標(biāo)模型特征集，即：

M={MM…,M} （1）

第+1幀，在可行駛區(qū)域環(huán)境中，有個障礙物特征模型，即：

N={1，2，，N} （2）

這時需要將第幀和+1幀的目標(biāo)建立一個關(guān)聯(lián)矩陣，實際是第幀個目標(biāo)與第+1幀個目標(biāo)一一計算關(guān)聯(lián)系數(shù)。關(guān)聯(lián)系數(shù)越小，說明兩者越有可能為同一障礙物，這樣可以建立×的矩陣。在實際工程應(yīng)用中，匈牙利算法主要是解決目標(biāo)分配問題而提出的有效算法[9]。這種算法首先要確認數(shù)據(jù)集的關(guān)聯(lián)程度，關(guān)聯(lián)程度的效果直接會影響到最終的匹配結(jié)果。本研究中同一障礙物相鄰兩幀數(shù)據(jù)位姿變化微小，關(guān)聯(lián)程度需考慮到兩點：第一，已有障礙物特征M與下一幀檢測出的障礙物特征N之間的位置差D，即：

第二，已有障礙物點云數(shù)目[]與下一幀檢測出的障礙物點云數(shù)目[]之間的點云數(shù)目差N[10]。

數(shù)據(jù)之間的關(guān)聯(lián)程序表示為，即

式中1和2分別是D和N的經(jīng)驗系數(shù)，其中1+2=1。在實際應(yīng)用中根據(jù)實際情況而定。基于匈牙利算法匹配數(shù)學(xué)模型可描述為：

在上式中，X為1時，代表兩幀之間的障礙物特征匹配，X為0時，表示不匹配。然而，利用匈牙利算法求取其最優(yōu)解，在矩陣Q中，選取一行或者一列中的最小元素，得到新矩陣Q，再以Q為系數(shù)矩陣獲得最優(yōu)解。求解過程中，最初的矩陣變換為含有多處為0元素的新矩陣，而最優(yōu)解不發(fā)生變化。在系數(shù)矩陣中，分析每行或每列的0元素。如果在系數(shù)矩陣Q中得到個0元素，并帶入到目標(biāo)函數(shù)中，并求和得到的值，一定是最小值，因此得到系數(shù)矩陣Q關(guān)聯(lián)的最優(yōu)解。

但是最優(yōu)解在實際測試中需要根據(jù)場景具體分析，針對有遮擋的情況，需要調(diào)整參數(shù)。

2.2 狀態(tài)更新

目標(biāo)跟蹤的最后一步中使用卡爾曼濾波進行狀態(tài)更新，卡爾曼濾波是針對某種運動系統(tǒng)的狀態(tài)而采用的一種遞推估計算法，整個過程在不斷地實現(xiàn)“預(yù)測—更新—預(yù)測—更新……”，并且在實現(xiàn)過程中可以把序列中的任意點作為起點來觀測[11]。其基本思想是：以最小均方誤差為最佳估計準則，采用信號與噪聲的狀態(tài)空間模型，利用前一時刻的估計值和當(dāng)前時刻的觀測值來更新對狀態(tài)變量的估計，求出當(dāng)前時刻的估計值，算法根據(jù)建立的系統(tǒng)方程和觀測方程對需要處理的信號作出滿足最小均方誤差的估計[12]。

預(yù)測：

更新：

其中，轉(zhuǎn)移矩陣：

u－1為1狀態(tài)的控制量，由于本模型是勻速直線運動，控制量為0，所以預(yù)測狀態(tài)方程為：

估計值和預(yù)測值的協(xié)方差矩陣為：

式中T為連續(xù)2幀時間間隔。

動態(tài)障礙物在圖5中有1、2、3、4四種情況，這四種情況下求得障礙物在車體坐標(biāo)系下航向角θ如下：

在所有實驗中都是以UTM坐標(biāo)作為基準，所以把車體坐標(biāo)下障礙物的信息轉(zhuǎn)換到UTM坐標(biāo)系。當(dāng)從GPS中獲得航向角大于等于0時，輸出的車輛航向角為；當(dāng)GPS中獲得航向角小于0時，輸出車輛的航向角為360+，保證車輛航向角范圍在[0，360]。圖5中顯示北東地坐標(biāo)，要使車身坐標(biāo)下障礙物的航向角轉(zhuǎn)換到北東地坐標(biāo)系下，即：當(dāng)≥0時，北東地坐標(biāo)系下障礙物的航向角為－360，否則為。而本次實驗均以UTM坐標(biāo)為輸出基準，因此UTM坐標(biāo)系下障礙物的航向角為90（），同理，范圍在[0，360]。

3 試驗結(jié)果

實驗平臺是課題組自主研發(fā)的無人駕駛港口車，主要傳感器包括2臺velodyne VLP-16激光雷達、1臺組合導(dǎo)航和一臺搭載了主頻為4.0 GHz的Intel i7處理器，內(nèi)存為8 G的工控機。

為了驗證實驗的有效性，利用無人駕駛港口車在某工業(yè)園區(qū)進行數(shù)據(jù)采集，本文中選取了具有代表性的復(fù)雜路段進行測試，采集實驗數(shù)據(jù)。通過仿真驗證所運用目標(biāo)跟蹤算法的有效性和可靠性。圖6為動態(tài)目標(biāo)跟蹤時和方向預(yù)測速度和更新速度誤差對比。從圖中可以看出，預(yù)測速度與更新速度誤差接近于0，而在600步時誤差比較大，是由于障礙物在靜止時，相鄰幀中心點坐標(biāo)會存在瞬間變化，當(dāng)物體運動時，誤差又開始趨于0，這也是傳統(tǒng)算法避免不了的誤差，同樣也是后續(xù)算法改進的方向。

是由于目標(biāo)障礙物的初始速度不為0，而測量值和預(yù)測值的初始速度為0。因此從0增加到某一個速度值時，有一個時間過程，從而導(dǎo)致初始誤差較大。

圖6 車身坐標(biāo)系下障礙物不同方向速度誤差對比圖

4 結(jié)論

本文提出了一種基于激光雷達檢測的3D多目標(biāo)跟蹤方法。利用匈牙利算法對前后兩幀檢測的障礙物進行匹配，利用卡爾曼濾波算法進行更新、預(yù)測和跟蹤。通過仿真驗證了卡爾曼濾波算法在預(yù)測和更新有著較高精度，在實際道路測試中，有著較好的實用性和穩(wěn)定性。

[1] 劉子熠,余思雨,鄭南寧.一種基于共點映射的無人車可行駛區(qū)域檢測方法[J].Engineering,2018,4(04):109-133.

[2] 劉玉珍,劉潤濤.簡單多邊形的最小外接矩形算法[J].哈爾濱理工大學(xué)學(xué)報, 2008(02) :10-13.

[3] 田垅,劉宗田.最小二乘法分段直線擬合[J].計算機科學(xué), 2012,39(S1):482-484.

[4] 田文利.基于霍夫直線檢測與二維透視變換的圖像校正恢復(fù)算法[J].電子測量技術(shù),2017,40(09):128-131.

[5] Matthew T. MacLean,Jerzy R. Lysikowski,Robert V. Rege, Dorothy M.Sendelbach,Angela P.Mihalic.Optimizing Medical Student Clerkship Schedules Using a Novel Application of the Hungarian Algorithm[J].Academic Medicine,2020,92 (11): 79-169.

[6] 谷穩(wěn).匈牙利算法的目標(biāo)分配問題研究及應(yīng)用[D].西安:西安電子科技大學(xué),2013.

[7] 王鵬宇,趙世杰,馬天飛,等.基于聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)的車用多傳感器目標(biāo)跟蹤融合算法[J].吉林大學(xué)學(xué)報(工學(xué)版), 2019,49(05):1420-1427.

[8] 李華楠,曹林,王東峰,等.結(jié)合匈牙利指派和改進粒子濾波的多目標(biāo)跟蹤算法[J].電訊技術(shù),2019,59(05):587-593.

[9] 羅凱鑫.基于最大熵魯棒自適應(yīng)濾波的水下組合導(dǎo)航方法研究[D].哈爾濱:哈爾濱工程大學(xué),2019.

[10] 黃毅,鄧志英.“管理運籌學(xué)”指派問題解法探析[J].湖南工程學(xué)院學(xué)報(社會科學(xué)版),2016,26(04):80-82.

[11] 王欣明.GPS/INS組合導(dǎo)航系統(tǒng)中卡爾曼濾波算法的研究與實現(xiàn)[D].北京:北京交通大學(xué),2012.

[12] 鄒斌,劉康,王科未.基于三維激光雷達的動態(tài)障礙物檢測和追蹤方法[J].汽車技術(shù),2017(08):19-25.

[13] 梁敏.基于粒子濾波的多目標(biāo)跟蹤算法研究[D].西安:西安電子科技大學(xué),2010.

Multi-Object Tracking Algorithm Based on Hungarian Matching and Kalman Filtering

LI Jianguo, ZHANG Rui, WANG Kai, ZENG Jie

( Shaanxi Heavy Duty Automobile Co., Ltd., Automotive Engineering Research Institute, Shaanxi Xi’an 710200 )

In order to obtain accurate information about the target detection of unmanned vehicles,by segmenting and clustering the calibrated original lidar point cloud, the Hough line is used to detect the outsourcing frame of the obstacle, and finally the Hungarian algorithm is used to correlate and match the tracked obstacle and the newly detected obstacle, and find the optimal solution, and finally use the Kalman filter algorithm to optimize its state. Experiments show that the algorithm can quickly and accurately track the detected obstacles within 0.1 s of a lidar frame.

Hungarian matching; Hough line detection; Kalman filtering

A

1671-7988(2022)01-45-06

TP301.6

A

1671-7988(2022)01-45-06

CLC NO.: TP301.6

李建國，就職于陜西重型汽車有限公司汽車工程研究院。

10.16638/j.cnki.1671-7988.2022.001.011