基于三維激光雷達的動態障礙物檢測和追蹤方法＊

鄒斌 劉康 王科未

（武漢理工大學，現代汽車零部件技術湖北省重點實驗室 汽車零部件技術湖北省協同創新中心，武漢 430070）

基于三維激光雷達的動態障礙物檢測和追蹤方法＊

鄒斌 劉康 王科未

（武漢理工大學，現代汽車零部件技術湖北省重點實驗室 汽車零部件技術湖北省協同創新中心，武漢 430070）

為解決無人駕駛車輛在城市路況下對多個動態障礙物同時檢測和跟蹤的關鍵問題，提出一種基于三維激光雷達的多目標實時檢測和跟蹤方法。通過對單幀激光雷達點云數據進行聚類，提取障礙物外接矩形輪廓特征；采用多假設跟蹤模型（MHT）算法對連續兩幀的障礙物信息進行數據關聯；利用卡爾曼濾波算法對動態障礙物進行連續地預測和跟蹤。試驗結果表明，該算法能夠在自行搭建的智能車平臺上以每幀100 ms的速度準確、穩定地檢測和跟蹤。

1 前言

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

本文以Velodyne 16線三維激光雷達作為主要傳感器，運用DBSCAN（Density-Based Spatial Clustering of Applications with Noise）方法，并結合可變閾值，對原始數據進行聚類，大大提高了聚類的準確性，簡化應用了模糊線段法準確提取動態障礙物的矩形輪廓，最后應用多目標假設跟蹤（Multiple Hypothesis Tracking，MHT）方法以及卡爾曼濾波器對目標進行跟蹤。

2 障礙物檢測

2.1 有效障礙點的篩選

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

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

圖1 三維激光雷達原始數據

圖2 障礙點篩選結果

2.2 特征提取

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

式中，rn-1為障礙點Pn-1的深度值；σr為激光雷達的測量誤差；Δ?為激光雷達的角度分辨率，本文取Δ?=0.2°；γ為閾值參數，它決定了最大距離閾值的大小，本文取γ=1°；NT為預聚類中mi中障礙點的個數；?為閾值因數，由經驗獲得，本文取?=5。

圖3為障礙點的預聚類結果。結果顯示不同障礙物誤識別為同一個障礙物。這是由于兩車體傾斜，障礙點投影到x軸后，其x坐標相差較小，障礙點投影到y軸后，其y坐標相差也較小。

圖3 預聚類結果

圖4為應用DBSCAN算法后的聚類結果，結果顯示，該算法可將預聚類誤識別同一障礙物的不同障礙物分開，同時可將長度過長，面積過小，點密度太稀疏的障礙物剔除，它們可能是墻壁、樹葉、樹干等靜態障礙物。

圖4 DBSCAN聚類結果

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

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

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

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

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

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

圖5 模糊線段提取

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

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

3 障礙物跟蹤

為了跟蹤障礙物目標并預測其運動軌跡，本文首先采用MHT方法對相鄰兩幀之間的障礙物進行數據關聯，由于目標的位置存在一定的噪聲（如同一個障礙物跟蹤點位置發生偏移），本文采用卡爾曼濾波器對目標障礙物的位置和速度進行濾波和預測，算法流程如圖8所示。

圖6 單個障礙物輪廓提取結果

圖7 單幀特征提取結果

圖8 障礙物跟蹤算法流程

3.1 數據關聯

表1列舉了常用的多目標數據關聯算法，并列出了各算法的優缺點。

顯然NN法無法適應自主駕駛車輛周圍的復雜環境，JPDA法不需要任何目標和雜波的先驗信息，較為適合解決障礙點密集情況下的數據關聯問題。自主駕駛車輛周圍障礙物間距離較遠，對雷達點云進行數據處理后存在大量的特征信息，從之前幀的匹配概率可得到大量雜波的先驗信息，因此MHT法是最佳選擇。

表1 數據關聯算法對比

建立每個障礙物的幾何特征列表。具體信息包括：障礙物矩形的頂點坐標Q(x,y)、寬度W、長度L、矩形邊界與x軸的夾角θ。這是利用MHT進行多障礙物數據關聯的前提，進而根據障礙物之間的幾何特征建立關聯關系。

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

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

圖9 目標匹配過程

根據圖9出現的情況，可以用矩陣表示兩幀之間關聯的概率：

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

式中，Vi1為其中一個矩形的頂點，Vi1、Vi2、Vi3、Vi4依次為逆時針排列的矩形頂點；Gjx為另一個矩形的某個頂點。

各概率的計算公式分別為：

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

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

下面對各種假設進行處理，選取最優假設，由于障礙物數量較少，可以依次計算各種假設Hk的出現概率。假設Hk出現的概率為：

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

3.2 濾波器預測與更新

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

預測的狀態方程為：

測量轉換矩陣為：

預測狀態量和預測最小均方差矩陣方程分別為：

圖10 判別結果

計算誤差增益、最優估計狀態量方程和均方差誤差更新矩陣分別為：

可以通過R、Q的大小調節觀測值的可信度。Q值越大，預測結果越接近預測值，R值越大，預測結果越接近觀測值。

4 試驗結果

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

圖11 無人自主車

圖12為攝像頭拍攝實景，圖13為文中算法實時連續跟蹤和檢測動態障礙物25幀的結果。工況為校園路況，圖中同時跟蹤車輛和行人，目標車輛正在遠離無人車，行人正在接近無人車，車輛表示為較大的矩形，行人表示為較小的矩形，圖中顯示障礙物在第2、12、22幀時刻的位置。“+”表示障礙物每幀跟蹤點的位置坐標，折線表示經過卡爾曼濾波器濾波后的運動軌跡，可以觀察出，未經濾波，跟蹤點變動較大，經過卡爾曼器濾波后軌跡曲線趨于平滑。

由圖13可知，雷達投射到車輛上的激光點分布不均勻，自適應閾值的DBSCAN算法可以對障礙點進行準確聚類，目標車輛在經過周圍停放車輛時，雖然相距較近，本文聚類算法也可以準確分辨目標車輛。隨著目標車輛的遠離，激光雷達投射到車上的激光點逐漸減少，最終導致目標車輛的輪廓逐漸變小，如圖中第22幀檢測到的目標車輛，但并不影響對目標車輛的跟蹤。

圖12 攝像頭拍攝實景

圖13 雷達跟蹤結果

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

本文通過所跟蹤車輛的航向角和速度來體現跟蹤效果的穩定性和準確性。將使用毫米波雷達的跟蹤效果與本文算法的跟蹤效果進行對比，速度跟蹤對比結果如圖14所示，圖中同時檢測了車輛和行人的速度。運動車輛行進約20 m，車輛在超車過程中速度變慢，之后速度持續加快，由曲線可以看出，本文算法對于障礙物速度檢測與毫米波雷達所檢測的速度最大相差0.432 m/s，驗證了該算法對速度跟蹤的準確性。

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

圖14 速度對比結果

圖15 航向角對比結果

5 結束語

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

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 周俊靜，段建民.基于柵格地圖的智能車輛運動目標檢測.系統工程與電子技術，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 楊飛，朱株，龔小謹，等.基于三維激光雷達的動態障礙實時檢測與跟蹤.浙江大學學報：工學版，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 劉健.基于三維激光雷達的無人駕駛車輛環境建模關鍵技術研究：[學位論文].合肥：中國科學技術大學，2016.

（責任編輯 斛 畔）

修改稿收到日期為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

三維激光雷達 DBSCAN MHT 卡爾曼濾波

TP181；U495；TP14 文獻標識碼：A 文章編號：1000-3703（2017）08-0019-07

湖北省科技廳資助項目（2016BEC116）。