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

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

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

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

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

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

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

前言

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

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

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

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

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

圖2 障礙物點(diǎn)云柵格化

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

圖3 障礙物外包框定點(diǎn)確立

2 目標(biāo)跟蹤

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

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

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

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

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

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

M={MM…,M} （1）

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

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

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

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

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

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

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

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

2.2 狀態(tài)更新

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

預(yù)測：

更新：

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

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

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

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

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

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

3 試驗(yàn)結(jié)果

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

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

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

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

4 結(jié)論

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

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

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

[3] 田垅,劉宗田.最小二乘法分段直線擬合[J].計(jì)算機(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é)報(bào)(工學(xué)版), 2019,49(05):1420-1427.

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

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

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

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

[12] 鄒斌,劉康,王科未.基于三維激光雷達(dá)的動(dòng)態(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