基於三維激光雷達的動態障礙物檢測和追蹤方法
厚勢按：為解決無人駕駛車輛在城市路況下對多個動態障礙物同時檢測和跟蹤的關鍵問題，提出一種基於三維激光雷達的多目標實時檢測和跟蹤方法。
通過對單幀激光雷達點雲數據進行聚類，提取障礙物外接矩形輪廓特徵；採用多假設跟蹤模型（MHT）演算法對連續兩幀的障礙物信息進行數據關聯；利用卡爾曼濾波演算法對動態障礙物進行連續地預測和跟蹤。
試驗結果表明，該演算法能夠在自行搭建的智能車平台上以每幀 100 ms 的速度準確、穩定地檢測和跟蹤。
本文來自 2017 年 8 月 24 日出版的《汽車技術》，作者是武漢理工大學現代汽車零部件技術湖北省重點實驗室的鄒斌、劉康和王科未。
1. 前言
利用激光雷達對動態障礙物進行檢測和軌跡預測是無人駕駛課題的研究重點之一 [1]。近年來，國內外學者在此課題中主要採用兩種方法。
一種方法是柵格地圖法。周俊靜等 [2] 建立柵格地圖，利用貝葉斯推理推測由於感測器誤差造成柵格狀態的不確定性，通過比較當前時刻和隨時間累積形成的柵格狀態的不一致性檢測運動目標。Trung-Dung Vu等 [3] 也利用該方法對運動目標進行檢測跟蹤，但是相比於文獻 [2]，沒有對更新後的後驗概率進行修正。文獻 [4] 和文獻 [5] 均採用了類似方法。以上方法可以較為準確地檢測到運動的柵格，但仍存在缺陷，如果運動目標體積較大，幾個周期內柵格狀態一直保持「佔據」,容易誤判為靜態障礙物。
另一種方法是對原始數據進行分割、聚類，提取障礙物形態特徵，對每幀的障礙物進行數據關聯，從而對其進行跟蹤。斯坦福大學 Anna Petrovskaya 等 [6] 提取被跟蹤車輛的幾何、動態特性，建立相應特徵模型，利用貝葉斯濾波器對跟蹤目標進行更新，但是被遮擋的車輛仍無法檢測。文獻 [7] 通過建立二維虛擬掃描模型較好地解決了這一問題。
然而，兩種方法都無法識別行人、自行車等運動障礙物。楊飛等 [8] 利用模板匹配法對障礙物的外輪廓形狀特徵進行匹配，但模型類別較少，障礙物匹配的適應性較差[9]。
本文以 Velodyne 16 線三維激光雷達作為主要感測器，運用 DBSCAN（Density-Based Spatial Clustering ofApplications with Noise）方法，並結合可變閾值，對原始數據進行聚類，大大提高了聚類的準確性，簡化應用了模糊線段法準確提取動態障礙物的矩形輪廓，最後應用多目標假設跟蹤（Multiple Hypothesis Tracking，MHT）方法以及卡爾曼濾波器對目標進行跟蹤。
2. 障礙物檢測
2.1 有效障礙點的篩選
由於激光雷達每次採集障礙點時視角不同，採集的部分障礙點坐標變化較大，而且很多障礙點與障礙物的跟蹤不相關，如路面、樹葉、牆體、高樓等。障礙點過多會影響外圍矩形輪廓的提取，因此有必要對原始數據進行篩選。
圖 1 三維激光雷達原始數據
圖 2 障礙點篩選結果
首先建立感興趣空間區域，將此空間以外的點剔除。圖 1 為三維激光雷達原始數據，坐標（0,0）為激光雷達所在位置，本文選取的高度範圍為 0.5 ~ 2 m，X 軸方向範圍為 0~25 m，Y 軸方向範圍為 -10~10 m。篩選結果如圖 2 所示，經篩選，剔除了樹枝、地面等不在感興趣區域內的障礙點，只保留了障礙物大致的輪廓，有利於提取車輛外圍矩形。
2.2 特徵提取
為了提取障礙物的外圍矩形輪廓，有必要對障礙物點進行聚類，本文採用 DBSCAN 演算法，該演算法利用密度進行聚類，即要求聚類空間中一定區域內所包含對象（點或其他空間對象）的數目不小於給定閾值，計算速度快且能夠有效處理雜訊點和發現任意形狀的空間聚類。為減少 DBSCAN 聚類演算法的運算量、提高實時性，先對所有的障礙點進行預聚類。
具體做法為：在 x 軸方向依據點與點的距離差值聚類，在此基礎上再在 y 軸方向根據點與點的距離差值聚類。最後輸出障礙點數聚類集合 m = 。然後對輸出數據集中的每個障礙點集合分別應用 DBSCAN 聚類演算法，對障礙點進行進一步分類。
DBSCAN 聚類演算法需要事先輸入過濾雜訊的核心點領域距離閾值 Eps 以及核心點鄰域範圍內最少點個數的閾值 MinPts。這兩個值如果過大，多個障礙物易被誤分為同一個障礙物；相反，則同一障礙物有可能被誤分為若干障礙物。由於激光雷達採集的障礙點的密度隨激光點深度的變大而變小，所以 Eps、MinPts 應隨激光點深度而變化。本文採用自適應閾值的方法：
圖 3 為障礙點的預聚類結果。結果顯示不同障礙物誤識別為同一個障礙物。這是由於兩車體傾斜，障礙點投影到 x 軸後，其 x 坐標相差較小，障礙點投影到 y 軸後，其 y坐標相差也較小。
圖 3 預聚類結果
圖 4 為應用 DBSCAN 演算法後的聚類結果，結果顯示，該演算法可將預聚類誤識別同一障礙物的不同障礙物分開，同時可將長度過長，面積過小，點密度太稀疏的障礙物剔除，它們可能是牆壁、樹葉、樹榦等靜態障礙物。
圖 4 DBSCAN聚類結果
本文中提取的障礙物主要特徵有跟蹤點位置 Q(x, y)、運動方向與 x 軸的偏離角度 θ、長度 L、寬度 W。提取包羅障礙物的最小外接矩形框上距原點最近的頂點作為 Q(x,y)，因為此點隨雷達視角的變化一般較小，矩形框的最長邊線與 x 軸的夾角作為障礙物角度信息 θ。本文選用最小凸包法結合模糊線段的方法提取障礙物的包羅矩形框。
圖 5 模糊線段提取
首先，利用 Graham 掃描提取凸包點，然後利用簡化的模糊線段法 [10] 提取障礙物的矩形邊框。具體步驟如下（見圖 5）：
選取距起點（如圖中 P0）的距離大於一定閾值的最近點（如圖中 P2）；
計算起點 P0、P2 兩點確定的直線的斜率 K_g1，且 P0 和 P2 之間的點全部屬於 G_1（圖 5 中只存在點 P1）；
計算下一個點（如圖中 P3）與 P0 確定的直線的斜率 K_g2，令 θ=arctan(K_g1-K_g2)，如果 |θ| 大於一定的閾值，則 G1 組關閉，以 G1 中最後一個點為起點建立 G2；
在 |θ| 小於一定的閾值的情況下，向量 P0P2、P0P3 相加得到 PP′，過 PP′ 兩側距離最遠的點 P3、P2，分別作 PP′ 的平行線，計算兩平行線的距離 η，如果 η 大於設定的閾值 ηtr，則 P3∈G1，否則 G1 關閉，以 G1 中最後一個點為起點建立 G2；
若步驟 c、d 中有新起點建立，則循環從步驟 a 開始，否則循環從步驟 c 開始，直到計算完成最後一個點，輸出 G1、G2、……、Gx。然後對每組內的點運用最小二乘求得最佳斜率，作出每組點的直線，選取距離最長、鄰域超過一定數量點的直線作邊線，作出障礙點的邊框。
在圖 5 中，很顯然 P4 與 P0~P3 不屬於同一組，P4 為 G2的起點。P0~P4 為最小凸包點，經過判定，P0~P3 為同一線段上的點，P4 將被剔除，不作為線段上的點。
圖 6 單個障礙物輪廓提取結果
求得的障礙物外接邊框如圖 6 所示。本文可以提取任何角度的矩形框，對於行人等目標較小且激光點較為密集的障礙物集合，矩形框與 x 軸的角度設為 0°。單幀圖像的特徵提取結果如圖 7 所示。
圖 7 單幀特徵提取結果
3. 障礙物跟蹤
為了跟蹤障礙物目標並預測其運動軌跡，本文首先採用 MHT 方法對相鄰兩幀之間的障礙物進行數據關聯，由於目標的位置存在一定的雜訊（如同一個障礙物跟蹤點位置發生偏移），本文採用卡爾曼濾波器對目標障礙物的位置和速度進行濾波和預測，演算法流程如圖 8 所示。
圖 8 障礙物跟蹤演算法流程
3.1 數據關聯
表 1 列舉了常用的多目標數據關聯演算法，並列出了各演算法的優缺點。
表 1 數據關聯演算法對比
顯然 NN 法無法適應自主駕駛車輛周圍的複雜環境，JPDA 法不需要任何目標和雜波的先驗信息，較為適合解決障礙點密集情況下的數據關聯問題。自主駕駛車輛周圍障礙物間距離較遠，對雷達點雲進行數據處理後存在大量的特徵信息，從之前幀的匹配概率可得到大量雜波的先驗信息，因此 MHT 法是最佳選擇。
建立每個障礙物的幾何特徵列表。具體信息包括：障礙物矩形的頂點坐標Q(x,y)、寬度W、長度L、矩形邊界與 x 軸的夾角θ。這是利用 MHT 進行多障礙物數據關聯的前提，進而根據障礙物之間的幾何特徵建立關聯關係。
首先介紹本文的數據關聯演算法。{T_j}（j=1,…,M）表示前一幀跟蹤的障礙物，{Z_i}（i=1,…,N）為當前幀檢測的障礙物。根據實際狀況，可以分為 3 種情況（見圖 9）：
已經跟蹤的障礙物逐漸或突然消失，當前幀沒有與之關聯的任何障礙物，可以表示為{T_jN}；
新障礙物突然或者逐漸進入雷達搜索區域，之前幀沒有障礙物與之關聯，可以表示為{Z_iN}；
當前幀某個障礙物與之前跟蹤的障礙物形成關聯，表示為{Y(T_j, Z_i)}。
圖 9 目標匹配過程
圖 9 中圓形區域表示障礙物可能出現的位置，圓形半徑為 SR，本文取 SR = 3 m。圖中障礙物T_1、T_2、T_3表示經過卡爾曼濾波器預測後的障礙物位置，並繼承之前幀的其他幾何特徵，Z_1、Z_2、Z_3為當前幀出現的障礙物，那麼根據上述 3 種情況，可以形成一系列假設：
假設在
H_k中，P(Y(T_j, Z_i))表示成功關聯的概率，P(T_jN)表示之前幀障礙物無關聯的概率，P(Z_iN)表示當前幀障礙物無關聯的概率。P(Y(T_j, Z_i))是以下 4 個關聯特徵匹配概率的聯合概率：兩障礙物存在重疊面積的概率P_cov(Tj,Zi)；中心點的匹配概率P_center(T_j, Z_i)；障礙物長、寬的匹配概率P_WL(Tj, Zi)；障礙物傾斜角度的匹配概率P_ratio(Tj,Zi)。
根據圖 9 出現的情況，可以用矩陣表示兩幀之間關聯的概率：
圓形區域內障礙物由於存在一定的面積，如果兩個障礙物的面積有重疊部分，那麼其為相鄰兩幀同一個障礙物的概率較大，本文判別是否存在重疊面積的條件為：
式中，V_i1為其中一個矩形的頂點，V_i1、V_i2、V_i3、V_i4依次為逆時針排列的矩形頂點；G_jx為另一個矩形的某個頂點。
各概率的計算公式分別為：
式中，S_ij為可能匹配的兩障礙物跟蹤點之間的距離；W_iO、L_iO分別為濾波器預測障礙物的寬度和長度；W_i、L_i分別為當前幀中可能與濾波器預測障礙物匹配的障礙物的寬度和長度；K_iO為濾波器預測障礙物矩形邊界與 x 方向的夾角；K_i為當前幀中可能與之匹配的障礙物矩形邊界與 x 方向的夾角。
障礙物T_j、Z_i成功匹配的概率表示為：
下面對各種假設進行處理，選取最優假設，由於障礙物數量較少，可以依次計算各種假設 Hk 的出現概率。假設 Hk 出現的概率為：
選取最優假設 ? ：
式中通過函數argmax選取概率最大的假設。以上演算法在實車中運行得到的連續 3 幀的判別結果如圖 10 所示。判別結果顯示，公路上 1 號和 6 號障礙物一直被跟蹤，3 號障礙物突然消失，在第 35 幀和第 36 幀，8 號、10 號障礙物分別突然出現，跟蹤效果明顯。此演算法能夠同時檢測障礙物的突然出現和消失。
圖 10 判別結果
3.2 濾波器預測與更新
受測量雜訊和自主駕駛車輛車體自身運動的影響，存在很大的測量誤差。為了解決目標跟蹤中測量雜訊引起的目標運動軌跡的大幅變動對預測目標運動軌跡造成的影響，以及在障礙物關聯中對於預測目標下一幀位置的需要，有必要採用卡爾曼濾波器[11]對目標的位置進行濾波和預測。為了預測障礙物下一幀的位置，給出狀態向量（被估計的物理量）：X=[x,y,vx,vy]，v_x、v_y分別為障礙物在 x、y 方向上的速度分量。針對此問題，狀態轉換矩陣為：
預測的狀態方程為：
測量轉換矩陣為：
式中，vt 為觀測雜訊，滿足 vt~N(0，R) 的高斯分布，本文取 R=0.01E2， E2 為 2 階單位矩陣；wt 為狀態雜訊，滿足 wt~N(0，Q) 的高斯分布，本文取 Q=0.001E4；E4 為 4 階單位矩陣；T 為系統運行周期，本文取 T = 100 ms。
預測狀態量和預測最小均方差矩陣方程分別為：
計算誤差增益、最優估計狀態量方程和均方差誤差更新矩陣分別為：
可以通過 R、Q 的大小調節觀測值的可信度。Q 值越大，預測結果越接近預測值，R 值越大，預測結果越接近觀測值。
4. 試驗結果
首先分析單幀雷達數據，利用 DBSCAN、模糊線段等演算法，準確提取了障礙物的幾何特徵，與文獻 [1]、文獻 [3] 和文獻 [8] 中的演算法相比，改進的自適應閾值 DBSCAN 演算法解決了由於雷達數據分布不均造成的聚類不準確等問題，經過大量的試驗測試，正確率達到 98.2%；模糊線段與最小凸包相結合的方法，解決了由於模板匹配造成的障礙物角度匹配精度不高的問題。基於兩幀提取的障礙物特徵，採用多假設跟蹤演算法進行數據關聯，關聯正確率達到 99%。
圖 11 無人自主車
將本文提出的演算法應用在武漢理工大學智能車團隊搭建的實時運算平台上，試驗結果表明，該演算法能夠以每幀 100 ms 的速度實現準確、穩定的檢測和跟蹤多個動態障礙物。該平台硬體配置為 INTEL 8 核 3.4 GHz 處理器，內存 16 GB，軟體採用 Ubuntu下的 ROS 機器人實時操作系統。圖 11 為自行搭建的無人車平台。
圖 12 攝像頭拍攝實景
圖 13 雷達跟蹤結果
圖 12 為攝像頭拍攝實景，圖 13 為文中演算法實時連續跟蹤和檢測動態障礙物 25 幀的結果。工況為校園路況，圖中同時跟蹤車輛和行人，目標車輛正在遠離無人車，行人正在接近無人車，車輛表示為較大的矩形，行人表示為較小的矩形，圖中顯示障礙物在第 2、12、22 幀時刻的位置。「+」表示障礙物每幀跟蹤點的位置坐標，折線表示經過卡爾曼濾波器濾波後的運動軌跡，可以觀察出，未經濾波，跟蹤點變動較大，經過卡爾曼器濾波後軌跡曲線趨於平滑。
由圖 13 可知，雷達投射到車輛上的激光點分布不均勻，自適應閾值的 DBSCAN 演算法可以對障礙點進行準確聚類，目標車輛在經過周圍停放車輛時，雖然相距較近，本文聚類演算法也可以準確分辨目標車輛。隨著目標車輛的遠離，激光雷達投射到車上的激光點逐漸減少，最終導致目標車輛的輪廓逐漸變小，如圖中第 22 幀檢測到的目標車輛，但並不影響對目標車輛的跟蹤。
從圖 13 可知，目標車輛與 x 軸方向並不是平行的，本文所採用的模糊線段與最小凸包相結合的演算法可以精確地提取到這一特徵，並且車輛外接矩形框的方向也與目標車輛運動方向相符。圖中可以觀察出較長一段行人運動軌跡附近沒有出現「+」，由於測量誤差以及樹葉的遮擋，對目標行人的檢測突然「消失」，本文中 MHT 演算法繼續保持障礙物丟失前的特徵信息，並利用卡爾曼濾波器繼續跟蹤，結果顯示，恢復了對目標行人的跟蹤狀態。
圖 14 速度對比結果
本文通過所跟蹤車輛的航向角和速度來體現跟蹤效果的穩定性和準確性。將使用毫米波雷達的跟蹤效果與本文演算法的跟蹤效果進行對比，速度跟蹤對比結果如圖 14 所示，圖中同時檢測了車輛和行人的速度。運動車輛行進約 20 m，車輛在超車過程中速度變慢，之後速度持續加快，由曲線可以看出，本文演算法對於障礙物速度檢測與毫米波雷達所檢測的速度最大相差 0.432 m/s，驗證了該演算法對速度跟蹤的準確性。
圖 15 航向角對比結果
圖 15 為運動車輛航向角跟蹤對比結果，將從障礙點特徵提取的航向角與跟蹤演算法計算的航向角融合，使用卡爾曼濾波器進行濾波，並與圖中毫米波雷達檢測到的航向角進行對比，由於感測器誤差的存在，單純從分析障礙點特徵獲取的航向角變化較為劇烈，但經過卡爾曼濾波器後，輸出的航向角度與毫米波雷達檢測的角度基本一致。兩感測器測量值最大相差 0.32°，驗證了本文演算法對航向角跟蹤的準確性。
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 Intelligent Vehicles 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
編輯整理：厚勢分析師拉里佩
轉載請註明來自厚勢和厚勢公號：iHoushi
-END-
文章精選
企業家
智能駕駛
新能源汽車
項目和評論
TAG:厚勢 |