鄒斌 劉康 王科未

（武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室 汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070）

基于三維激光雷達(dá)的動(dòng)態(tài)障礙物檢測(cè)和追蹤方法＊

鄒斌 劉康 王科未

（武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室 汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070）

為解決無人駕駛車輛在城市路況下對(duì)多個(gè)動(dòng)態(tài)障礙物同時(shí)檢測(cè)和跟蹤的關(guān)鍵問題，提出一種基于三維激光雷達(dá)的多目標(biāo)實(shí)時(shí)檢測(cè)和跟蹤方法。通過對(duì)單幀激光雷達(dá)點(diǎn)云數(shù)據(jù)進(jìn)行聚類，提取障礙物外接矩形輪廓特征；采用多假設(shè)跟蹤模型（MHT）算法對(duì)連續(xù)兩幀的障礙物信息進(jìn)行數(shù)據(jù)關(guān)聯(lián)；利用卡爾曼濾波算法對(duì)動(dòng)態(tài)障礙物進(jìn)行連續(xù)地預(yù)測(cè)和跟蹤。試驗(yàn)結(jié)果表明，該算法能夠在自行搭建的智能車平臺(tái)上以每幀100 ms的速度準(zhǔn)確、穩(wěn)定地檢測(cè)和跟蹤。

1 前言

利用激光雷達(dá)對(duì)動(dòng)態(tài)障礙物進(jìn)行檢測(cè)和軌跡預(yù)測(cè)是無人駕駛課題的研究重點(diǎn)之一[1]。近年來，國(guó)內(nèi)外學(xué)者在此課題中主要采用兩種方法。一種方法是柵格地圖法。周俊靜等[2]建立柵格地圖，利用貝葉斯推理推測(cè)由于傳感器誤差造成柵格狀態(tài)的不確定性，通過比較當(dāng)前時(shí)刻和隨時(shí)間累積形成的柵格狀態(tài)的不一致性檢測(cè)運(yùn)動(dòng)目標(biāo)。Trung-Dung Vu等[3]也利用該方法對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行檢測(cè)跟蹤，但是相比于文獻(xiàn)[2]，沒有對(duì)更新后的后驗(yàn)概率進(jìn)行修正。文獻(xiàn)[4]和文獻(xiàn)[5]均采用了類似方法。以上方法可以較為準(zhǔn)確地檢測(cè)到運(yùn)動(dòng)的柵格，但仍存在缺陷，如果運(yùn)動(dòng)目標(biāo)體積較大，幾個(gè)周期內(nèi)柵格狀態(tài)一直保持“占據(jù)”,容易誤判為靜態(tài)障礙物。另一種方法是對(duì)原始數(shù)據(jù)進(jìn)行分割、聚類，提取障礙物形態(tài)特征，對(duì)每幀的障礙物進(jìn)行數(shù)據(jù)關(guān)聯(lián)，從而對(duì)其進(jìn)行跟蹤。斯坦福大學(xué)Anna Petrovskaya等[6]提取被跟蹤車輛的幾何、動(dòng)態(tài)特性，建立相應(yīng)特征模型，利用貝葉斯濾波器對(duì)跟蹤目標(biāo)進(jìn)行更新，但是被遮擋的車輛仍無法檢測(cè)。文獻(xiàn)[7]通過建立二維虛擬掃描模型較好地解決了這一問題。然而，兩種方法都無法識(shí)別行人、自行車等運(yùn)動(dòng)障礙物。楊飛等[8]利用模板匹配法對(duì)障礙物的外輪廓形狀特征進(jìn)行匹配，但模型類別較少，障礙物匹配的適應(yīng)性較差[9]。

本文以Velodyne 16線三維激光雷達(dá)作為主要傳感器，運(yùn)用DBSCAN（Density-Based Spatial Clustering of Applications with Noise）方法，并結(jié)合可變閾值，對(duì)原始數(shù)據(jù)進(jìn)行聚類，大大提高了聚類的準(zhǔn)確性，簡(jiǎn)化應(yīng)用了模糊線段法準(zhǔn)確提取動(dòng)態(tài)障礙物的矩形輪廓，最后應(yīng)用多目標(biāo)假設(shè)跟蹤（Multiple Hypothesis Tracking，MHT）方法以及卡爾曼濾波器對(duì)目標(biāo)進(jìn)行跟蹤。

2 障礙物檢測(cè)

2.1 有效障礙點(diǎn)的篩選

由于激光雷達(dá)每次采集障礙點(diǎn)時(shí)視角不同，采集的部分障礙點(diǎn)坐標(biāo)變化較大，而且很多障礙點(diǎn)與障礙物的跟蹤不相關(guān)，如路面、樹葉、墻體、高樓等。障礙點(diǎn)過多會(huì)影響外圍矩形輪廓的提取，因此有必要對(duì)原始數(shù)據(jù)進(jìn)行篩選。

首先建立感興趣空間區(qū)域，將此空間以外的點(diǎn)剔除。圖1為三維激光雷達(dá)原始數(shù)據(jù)，坐標(biāo)（0,0）為激光雷達(dá)所在位置，本文選取的高度范圍為0.5～2 m，x軸方向范圍為0～25 m，y軸方向范圍為-10～10 m。篩選結(jié)果如圖2所示，經(jīng)篩選，剔除了樹枝、地面等不在感興趣區(qū)域內(nèi)的障礙點(diǎn)，只保留了障礙物大致的輪廓，有利于提取車輛外圍矩形。

圖1 三維激光雷達(dá)原始數(shù)據(jù)

圖2 障礙點(diǎn)篩選結(jié)果

2.2 特征提取

為了提取障礙物的外圍矩形輪廓，有必要對(duì)障礙物點(diǎn)進(jìn)行聚類，本文采用DBSCAN算法，該算法利用密度進(jìn)行聚類，即要求聚類空間中一定區(qū)域內(nèi)所包含對(duì)象（點(diǎn)或其他空間對(duì)象）的數(shù)目不小于給定閾值，計(jì)算速度快且能夠有效處理噪聲點(diǎn)和發(fā)現(xiàn)任意形狀的空間聚類。為減少DBSCAN聚類算法的運(yùn)算量、提高實(shí)時(shí)性，先對(duì)所有的障礙點(diǎn)進(jìn)行預(yù)聚類。具體做法為：在x軸方向依據(jù)點(diǎn)與點(diǎn)的距離差值聚類，在此基礎(chǔ)上再在y軸方向根據(jù)點(diǎn)與點(diǎn)的距離差值聚類。最后輸出障礙點(diǎn)數(shù)聚類集合m={m1,m2,m3,…,mn}。然后對(duì)輸出數(shù)據(jù)集中的每個(gè)障礙點(diǎn)集合分別應(yīng)用DBSCAN聚類算法，對(duì)障礙點(diǎn)進(jìn)行進(jìn)一步分類。DBSCAN聚類算法需要事先輸入過濾噪聲的核心點(diǎn)領(lǐng)域距離閾值Eps以及核心點(diǎn)鄰域范圍內(nèi)最少點(diǎn)個(gè)數(shù)的閾值MinPts。這兩個(gè)值如果過大，多個(gè)障礙物易被誤分為同一個(gè)障礙物；相反，則同一障礙物有可能被誤分為若干障礙物。由于激光雷達(dá)采集的障礙點(diǎn)的密度隨激光點(diǎn)深度的變大而變小，所以Eps、MinPts應(yīng)隨激光點(diǎn)深度而變化。本文采用自適應(yīng)閾值的方法：

式中，rn-1為障礙點(diǎn)Pn-1的深度值；σr為激光雷達(dá)的測(cè)量誤差；Δ?為激光雷達(dá)的角度分辨率，本文取Δ?=0.2°；γ為閾值參數(shù)，它決定了最大距離閾值的大小，本文取γ=1°；NT為預(yù)聚類中mi中障礙點(diǎn)的個(gè)數(shù)；?為閾值因數(shù)，由經(jīng)驗(yàn)獲得，本文取?=5。

圖3為障礙點(diǎn)的預(yù)聚類結(jié)果。結(jié)果顯示不同障礙物誤識(shí)別為同一個(gè)障礙物。這是由于兩車體傾斜，障礙點(diǎn)投影到x軸后，其x坐標(biāo)相差較小，障礙點(diǎn)投影到y(tǒng)軸后，其y坐標(biāo)相差也較小。

圖3 預(yù)聚類結(jié)果

圖4為應(yīng)用DBSCAN算法后的聚類結(jié)果，結(jié)果顯示，該算法可將預(yù)聚類誤識(shí)別同一障礙物的不同障礙物分開，同時(shí)可將長(zhǎng)度過長(zhǎng)，面積過小，點(diǎn)密度太稀疏的障礙物剔除，它們可能是墻壁、樹葉、樹干等靜態(tài)障礙物。

圖4 DBSCAN聚類結(jié)果

本文中提取的障礙物主要特征有跟蹤點(diǎn)位置Q（x,y）、運(yùn)動(dòng)方向與x軸的偏離角度θ、長(zhǎng)度L、寬度W。提取包羅障礙物的最小外接矩形框上距原點(diǎn)最近的頂點(diǎn)作為Q(x,y)，因?yàn)榇它c(diǎn)隨雷達(dá)視角的變化一般較小，矩形框的最長(zhǎng)邊線與x軸的夾角作為障礙物角度信息θ。本文選用最小凸包法結(jié)合模糊線段的方法提取障礙物的包羅矩形框。首先，利用Graham掃描提取凸包點(diǎn)，然后利用簡(jiǎn)化的模糊線段法[10]提取障礙物的矩形邊框。具體步驟如下（見圖5）：

a.選取距起點(diǎn)（如圖中P0）的距離大于一定閾值的最近點(diǎn)（如圖中P2）。

b.計(jì)算起點(diǎn)P0、P2兩點(diǎn)確定的直線的斜率Kg1，且P0和P2之間的點(diǎn)全部屬于G1（圖5中只存在點(diǎn)P1）。

c.計(jì)算下一個(gè)點(diǎn)（如圖中P3）與P0確定的直線的斜率Kg2，令θ=arctan(Kg1-Kg2)，如果|θ|大于一定的閾值，則G1組關(guān)閉，以G1中最后一個(gè)點(diǎn)為起點(diǎn)建立G2。

d.在|θ|小于一定的閾值的情況下，向量P0P2、P0P3相加得到PP′，過PP′兩側(cè)距離最遠(yuǎn)的點(diǎn)P3、P2，分別作PP′的平行線，計(jì)算兩平行線的距離η，如果η大于設(shè)定的閾值ηtr，則P3∈G1，否則G1關(guān)閉，以G1中最后一個(gè)點(diǎn)為起點(diǎn)建立G2。

e.若步驟c、d中有新起點(diǎn)建立，則循環(huán)從步驟a開始，否則循環(huán)從步驟c開始，直到計(jì)算完成最后一個(gè)點(diǎn)，輸出G1、G2、……、Gx。然后對(duì)每組內(nèi)的點(diǎn)運(yùn)用最小二乘求得最佳斜率，作出每組點(diǎn)的直線，選取距離最長(zhǎng)、鄰域超過一定數(shù)量點(diǎn)的直線作邊線，作出障礙點(diǎn)的邊框。

圖5 模糊線段提取

在圖5中，很顯然P4與P0～P3不屬于同一組，P4為G2的起點(diǎn)。P0～P4為最小凸包點(diǎn)，經(jīng)過判定，P0～P3為同一線段上的點(diǎn)，P4將被剔除，不作為線段上的點(diǎn)。

求得的障礙物外接邊框如圖6所示。本文可以提取任何角度的矩形框，對(duì)于行人等目標(biāo)較小且激光點(diǎn)較為密集的障礙物集合，矩形框與x軸的角度設(shè)為0°。單幀圖像的特征提取結(jié)果如圖7所示。

3 障礙物跟蹤

為了跟蹤障礙物目標(biāo)并預(yù)測(cè)其運(yùn)動(dòng)軌跡，本文首先采用MHT方法對(duì)相鄰兩幀之間的障礙物進(jìn)行數(shù)據(jù)關(guān)聯(lián)，由于目標(biāo)的位置存在一定的噪聲（如同一個(gè)障礙物跟蹤點(diǎn)位置發(fā)生偏移），本文采用卡爾曼濾波器對(duì)目標(biāo)障礙物的位置和速度進(jìn)行濾波和預(yù)測(cè)，算法流程如圖8所示。

圖6 單個(gè)障礙物輪廓提取結(jié)果

圖7 單幀特征提取結(jié)果

圖8 障礙物跟蹤算法流程

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

表1列舉了常用的多目標(biāo)數(shù)據(jù)關(guān)聯(lián)算法，并列出了各算法的優(yōu)缺點(diǎn)。

顯然NN法無法適應(yīng)自主駕駛車輛周圍的復(fù)雜環(huán)境，JPDA法不需要任何目標(biāo)和雜波的先驗(yàn)信息，較為適合解決障礙點(diǎn)密集情況下的數(shù)據(jù)關(guān)聯(lián)問題。自主駕駛車輛周圍障礙物間距離較遠(yuǎn)，對(duì)雷達(dá)點(diǎn)云進(jìn)行數(shù)據(jù)處理后存在大量的特征信息，從之前幀的匹配概率可得到大量雜波的先驗(yàn)信息，因此MHT法是最佳選擇。

表1 數(shù)據(jù)關(guān)聯(lián)算法對(duì)比

建立每個(gè)障礙物的幾何特征列表。具體信息包括：障礙物矩形的頂點(diǎn)坐標(biāo)Q(x,y)、寬度W、長(zhǎng)度L、矩形邊界與x軸的夾角θ。這是利用MHT進(jìn)行多障礙物數(shù)據(jù)關(guān)聯(lián)的前提，進(jìn)而根據(jù)障礙物之間的幾何特征建立關(guān)聯(lián)關(guān)系。

首先介紹本文的數(shù)據(jù)關(guān)聯(lián)算法。{Tj}（j=1,…,M）表示前一幀跟蹤的障礙物，{Zi}（i=1,…,N）為當(dāng)前幀檢測(cè)的障礙物。根據(jù)實(shí)際狀況，可以分為3種情況（見圖9）：已經(jīng)跟蹤的障礙物逐漸或突然消失，當(dāng)前幀沒有與之關(guān)聯(lián)的任何障礙物，可以表示為{TjN}；新障礙物突然或者逐漸進(jìn)入雷達(dá)搜索區(qū)域，之前幀沒有障礙物與之關(guān)聯(lián)，可以表示為{ZiN}；當(dāng)前幀某個(gè)障礙物與之前跟蹤的障礙物形成關(guān)聯(lián)，表示為{Y(Tj,Zi)}。圖9中圓形區(qū)域表示障礙物可能出現(xiàn)的位置，圓形半徑為SR，本文取SR=3 m。圖中障礙物T1、T2、T3表示經(jīng)過卡爾曼濾波器預(yù)測(cè)后的障礙物位置，并繼承之前幀的其他幾何特征，Z1、Z2、Z3為當(dāng)前幀出現(xiàn)的障礙物，那么根據(jù)上述3種情況，可以形成一系列假設(shè)：

假設(shè)在Hk中，P(Y(Tj,Zi))表示成功關(guān)聯(lián)的概率，P(TjN)表示之前幀障礙物無關(guān)聯(lián)的概率，P(ZiN)表示當(dāng)前幀障礙物無關(guān)聯(lián)的概率。P(Y(Tj,Zi))是以下4個(gè)關(guān)聯(lián)特征匹配概率的聯(lián)合概率：兩障礙物存在重疊面積的概率Pcov(Tj,Zi)；中心點(diǎn)的匹配概率Pcenter(Tj,Zi)；障礙物長(zhǎng)、寬的匹配概率PWL(Tj,Zi)；障礙物傾斜角度的匹配概率Pratio(Tj,Zi)。

圖9 目標(biāo)匹配過程

根據(jù)圖9出現(xiàn)的情況，可以用矩陣表示兩幀之間關(guān)聯(lián)的概率：

圓形區(qū)域內(nèi)障礙物由于存在一定的面積，如果兩個(gè)障礙物的面積有重疊部分，那么其為相鄰兩幀同一個(gè)障礙物的概率較大，本文判別是否存在重疊面積的條件為：

式中，Vi1為其中一個(gè)矩形的頂點(diǎn)，Vi1、Vi2、Vi3、Vi4依次為逆時(shí)針排列的矩形頂點(diǎn)；Gjx為另一個(gè)矩形的某個(gè)頂點(diǎn)。

各概率的計(jì)算公式分別為：

式中，Sij為可能匹配的兩障礙物跟蹤點(diǎn)之間的距離；WiO、LiO分別為濾波器預(yù)測(cè)障礙物的寬度和長(zhǎng)度；Wi、Li分別為當(dāng)前幀中可能與濾波器預(yù)測(cè)障礙物匹配的障礙物的寬度和長(zhǎng)度；KiO為濾波器預(yù)測(cè)障礙物矩形邊界與x方向的夾角；Ki為當(dāng)前幀中可能與之匹配的障礙物矩形邊界與x方向的夾角。

障礙物Tj、Zi成功匹配的概率表示為：

下面對(duì)各種假設(shè)進(jìn)行處理，選取最優(yōu)假設(shè)，由于障礙物數(shù)量較少，可以依次計(jì)算各種假設(shè)Hk的出現(xiàn)概率。假設(shè)Hk出現(xiàn)的概率為：

式中通過函數(shù)argmax選取概率最大的假設(shè)。以上算法在實(shí)車中運(yùn)行得到的連續(xù)3幀的判別結(jié)果如圖10所示。判別結(jié)果顯示，公路上1號(hào)和6號(hào)障礙物一直被跟蹤，3號(hào)障礙物突然消失，在第35幀和第36幀，8號(hào)、10號(hào)障礙物分別突然出現(xiàn)，跟蹤效果明顯。此算法能夠同時(shí)檢測(cè)障礙物的突然出現(xiàn)和消失。

3.2 濾波器預(yù)測(cè)與更新

受測(cè)量噪聲和自主駕駛車輛車體自身運(yùn)動(dòng)的影響，存在很大的測(cè)量誤差。為了解決目標(biāo)跟蹤中測(cè)量噪聲引起的目標(biāo)運(yùn)動(dòng)軌跡的大幅變動(dòng)對(duì)預(yù)測(cè)目標(biāo)運(yùn)動(dòng)軌跡造成的影響，以及在障礙物關(guān)聯(lián)中對(duì)于預(yù)測(cè)目標(biāo)下一幀位置的需要，有必要采用卡爾曼濾波器[11]對(duì)目標(biāo)的位置進(jìn)行濾波和預(yù)測(cè)。為了預(yù)測(cè)障礙物下一幀的位置，給出狀態(tài)向量（被估計(jì)的物理量）：X=[x,y,vx,vy]，vx、vy分別為障礙物在x、y方向上的速度分量。針對(duì)此問題，狀態(tài)轉(zhuǎn)換矩陣為：

預(yù)測(cè)的狀態(tài)方程為：

測(cè)量轉(zhuǎn)換矩陣為：

預(yù)測(cè)狀態(tài)量和預(yù)測(cè)最小均方差矩陣方程分別為：

圖10 判別結(jié)果

計(jì)算誤差增益、最優(yōu)估計(jì)狀態(tài)量方程和均方差誤差更新矩陣分別為：

可以通過R、Q的大小調(diào)節(jié)觀測(cè)值的可信度。Q值越大，預(yù)測(cè)結(jié)果越接近預(yù)測(cè)值，R值越大，預(yù)測(cè)結(jié)果越接近觀測(cè)值。

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

首先分析單幀雷達(dá)數(shù)據(jù)，利用DBSCAN、模糊線段等算法，準(zhǔn)確提取了障礙物的幾何特征，與文獻(xiàn)[1]、文獻(xiàn)[3]和文獻(xiàn)[8]中的算法相比，改進(jìn)的自適應(yīng)閾值DB?SCAN算法解決了由于雷達(dá)數(shù)據(jù)分布不均造成的聚類不準(zhǔn)確等問題，經(jīng)過大量的試驗(yàn)測(cè)試，正確率達(dá)到98.2%；模糊線段與最小凸包相結(jié)合的方法，解決了由于模板匹配造成的障礙物角度匹配精度不高的問題。基于兩幀提取的障礙物特征，采用多假設(shè)跟蹤算法進(jìn)行數(shù)據(jù)關(guān)聯(lián)，關(guān)聯(lián)正確率達(dá)到99%。將本文提出的算法應(yīng)用在武漢理工大學(xué)智能車團(tuán)隊(duì)搭建的實(shí)時(shí)運(yùn)算平臺(tái)上，試驗(yàn)結(jié)果表明，該算法能夠以每幀100 ms的速度實(shí)現(xiàn)準(zhǔn)確、穩(wěn)定的檢測(cè)和跟蹤多個(gè)動(dòng)態(tài)障礙物。該平臺(tái)硬件配置為INTEL 8核3.4 GHz處理器，內(nèi)存16 GB，軟件采用Ubuntu下的ROS機(jī)器人實(shí)時(shí)操作系統(tǒng)。圖11為自行搭建的無人車平臺(tái)。

圖11 無人自主車

圖12為攝像頭拍攝實(shí)景，圖13為文中算法實(shí)時(shí)連續(xù)跟蹤和檢測(cè)動(dòng)態(tài)障礙物25幀的結(jié)果。工況為校園路況，圖中同時(shí)跟蹤車輛和行人，目標(biāo)車輛正在遠(yuǎn)離無人車，行人正在接近無人車，車輛表示為較大的矩形，行人表示為較小的矩形，圖中顯示障礙物在第2、12、22幀時(shí)刻的位置?！?”表示障礙物每幀跟蹤點(diǎn)的位置坐標(biāo)，折線表示經(jīng)過卡爾曼濾波器濾波后的運(yùn)動(dòng)軌跡，可以觀察出，未經(jīng)濾波，跟蹤點(diǎn)變動(dòng)較大，經(jīng)過卡爾曼器濾波后軌跡曲線趨于平滑。

由圖13可知，雷達(dá)投射到車輛上的激光點(diǎn)分布不均勻，自適應(yīng)閾值的DBSCAN算法可以對(duì)障礙點(diǎn)進(jìn)行準(zhǔn)確聚類，目標(biāo)車輛在經(jīng)過周圍停放車輛時(shí)，雖然相距較近，本文聚類算法也可以準(zhǔn)確分辨目標(biāo)車輛。隨著目標(biāo)車輛的遠(yuǎn)離，激光雷達(dá)投射到車上的激光點(diǎn)逐漸減少，最終導(dǎo)致目標(biāo)車輛的輪廓逐漸變小，如圖中第22幀檢測(cè)到的目標(biāo)車輛，但并不影響對(duì)目標(biāo)車輛的跟蹤。

圖12 攝像頭拍攝實(shí)景

圖13 雷達(dá)跟蹤結(jié)果

從圖13可知，目標(biāo)車輛與x軸方向并不是平行的，本文所采用的模糊線段與最小凸包相結(jié)合的算法可以精確地提取到這一特征，并且車輛外接矩形框的方向也與目標(biāo)車輛運(yùn)動(dòng)方向相符。圖中可以觀察出較長(zhǎng)一段行人運(yùn)動(dòng)軌跡附近沒有出現(xiàn)“+”，由于測(cè)量誤差以及樹葉的遮擋，對(duì)目標(biāo)行人的檢測(cè)突然“消失”，本文中MHT算法繼續(xù)保持障礙物丟失前的特征信息，并利用卡爾曼濾波器繼續(xù)跟蹤，結(jié)果顯示，恢復(fù)了對(duì)目標(biāo)行人的跟蹤狀態(tài)。

本文通過所跟蹤車輛的航向角和速度來體現(xiàn)跟蹤效果的穩(wěn)定性和準(zhǔn)確性。將使用毫米波雷達(dá)的跟蹤效果與本文算法的跟蹤效果進(jìn)行對(duì)比，速度跟蹤對(duì)比結(jié)果如圖14所示，圖中同時(shí)檢測(cè)了車輛和行人的速度。運(yùn)動(dòng)車輛行進(jìn)約20 m，車輛在超車過程中速度變慢，之后速度持續(xù)加快，由曲線可以看出，本文算法對(duì)于障礙物速度檢測(cè)與毫米波雷達(dá)所檢測(cè)的速度最大相差0.432 m/s，驗(yàn)證了該算法對(duì)速度跟蹤的準(zhǔn)確性。

圖15為運(yùn)動(dòng)車輛航向角跟蹤對(duì)比結(jié)果，將從障礙點(diǎn)特征提取的航向角與跟蹤算法計(jì)算的航向角融合，使用卡爾曼濾波器進(jìn)行濾波，并與圖中毫米波雷達(dá)檢測(cè)到的航向角進(jìn)行對(duì)比，由于傳感器誤差的存在，單純從分析障礙點(diǎn)特征獲取的航向角變化較為劇烈，但經(jīng)過卡爾曼濾波器后，輸出的航向角度與毫米波雷達(dá)檢測(cè)的角度基本一致。兩傳感器測(cè)量值最大相差0.32°，驗(yàn)證了本文算法對(duì)航向角跟蹤的準(zhǔn)確性。

圖14 速度對(duì)比結(jié)果

圖15 航向角對(duì)比結(jié)果

5 結(jié)束語

本文基于單幀激光雷達(dá)點(diǎn)云數(shù)據(jù)提取障礙物的幾何特征，采用多假設(shè)跟蹤模型（MHT）、卡爾曼濾波算法對(duì)動(dòng)態(tài)障礙物進(jìn)行預(yù)測(cè)跟蹤，通過試驗(yàn)驗(yàn)證了本算法的實(shí)用性與穩(wěn)定性。但是本算法尚存在一些不足：在障礙物距離自主駕駛車輛較遠(yuǎn)時(shí)，由于障礙點(diǎn)數(shù)量不足，不僅會(huì)造成障礙物的分裂，而且提取的障礙物面積尺寸變化較為明顯，帶來了數(shù)據(jù)關(guān)聯(lián)上的誤差；無人車在行駛過程中，由于地面不平，激光雷達(dá)的抖動(dòng)會(huì)影響聚類效果，進(jìn)而影響幾何特征的提??；當(dāng)行人距靜態(tài)障礙物較近時(shí)，容易將行人判別為靜態(tài)障礙物。因此，本算法還尚需完善。

1 Martinez L,Paulik M,Krishnan M,et al.Map-based lane identification and prediction for autonomous vehicles.IEEE International Conference on Electro/information Technology，Milwaukee，2014.

2 周俊靜，段建民.基于柵格地圖的智能車輛運(yùn)動(dòng)目標(biāo)檢測(cè).系統(tǒng)工程與電子技術(shù)，2015（2）：436～442.

3 Baig Q,Vu T D,Aycard O.Online Localization and Mapping with Moving Object Tracking in Dynamic Outdoor Environments.IEEE IntelligentVehicles Symposium，Xi’an，2009.

4 Bouzouraa M E,Hofmann U.Fusion of occupancy grid mapping and model based object tracking for driver assistance systems using laser and radar sensors.IEEE Intelligent Vehicles Symposium，La Jolla，2010.

5 Konrad M,Szczot M,Schüle F,et al.Generic grid mapping for road course estimation.IEEE Intelligent Vehicles Symposium，Baden-Baden，2011.

6 Petrovskaya A,Thrun S.Model based vehicle detection and tracking for autonomous urban driving.Kluwer Academic Publishers,2009.

7 Chen T,Dai B,Liu D,et al.Likelihood-Field-Model-Based Dynamic Vehicle Detection with Velodyne.IEEE International Conference on Intelligent Human-Machine Systems and Cybernetics，Hangzhou，2015.

8 楊飛，朱株，龔小謹(jǐn)，等.基于三維激光雷達(dá)的動(dòng)態(tài)障礙實(shí)時(shí)檢測(cè)與跟蹤.浙江大學(xué)學(xué)報(bào)：工學(xué)版，2012，46（9）：1565～1571.

9 Broggi A,Cerri P,Ghidoni S,et al.A New Approach to Urban Pedestrian Detection for Automatic Braking.IEEE Transactions on Intelligent Transportation Systems，2009，10（4）：594～605.

10 Debled-Rennesson I,Feschet F,Rouyer-Degli J.Optimal blurred segments decomposition of noisy shapes in linear time.Computers&Graphics，2006，30（1）：30～36.

11 劉健.基于三維激光雷達(dá)的無人駕駛車輛環(huán)境建模關(guān)鍵技術(shù)研究：[學(xué)位論文].合肥：中國(guó)科學(xué)技術(shù)大學(xué)，2016.

（責(zé)任編輯 斛 畔）

修改稿收到日期為2017年6月22日。

Dynamic Obstacle Detection and Tracking Method Based on 3D LiDAR

Zou Bin,Liu Kang,Wang Kewei
（Hubei Key Laboratory of Advanced Technology for Automotive Components,Hubei Collaborative Innovation Center for Automotive Components Technology,Wuhan University of Technology,Wuhan 430070）

In order to solve the key problem of simultaneous detection and tracking of unmanned vehicles in urban traffic conditions,this paper presented a method based on 3D LiDAR for simultaneous detection and tracking of multitargets.By clustering the single-frame LiDAR point cloud data,external contour features of obstacles were extracted.Then,the Multi-Hypothetical Tracking model（MHT）algorithm was used to correlate the obstacle information of two consecutive frames.Finally,the Kalman filter algorithm was used to continuously predict and track the dynamic obstacles.Test results show that this algorithm can detect and track multi-target at 100 ms/frame speed on the self-built intelligent vehicle platform.

3D LiDAR,DBSCAN,MHT,Kalman filter

三維激光雷達(dá) DBSCAN MHT 卡爾曼濾波

TP181；U495；TP14 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1000-3703（2017）08-0019-07

湖北省科技廳資助項(xiàng)目（2016BEC116）。