基于三維激光雷達的無人車障礙物檢測與跟蹤?

謝德勝,徐友春,王任棟,蘇致遠

(1.軍事交通學院研究生管理大隊,天津 300161； 2.軍事交通學院軍用車輛系,天津 300161)

前言

近年來,隨著人工智能和機器人技術的不斷發展,無人駕駛汽車作為人工智能領域的一個重要的分支已經成為國內外的研究熱點。無人車主要通過使用相機、激光雷達、GPS等傳感器對周圍環境進行實時感知[1]。障礙物檢測和跟蹤是無人車環境感知的重要內容,對無人車的路徑規劃和決策控制有重要影響。

國內外對障礙物檢測和跟蹤的研究主要集中在基于計算機視覺的方法[2]和基于激光雷達的方法[3-4]上。基于深度學習的視覺目標檢測和跟蹤技術已經獲得了很大的發展[5],但基于計算機視覺的方法易受光線影響而使檢測和跟蹤效果不佳。激光雷達因其能夠獲得障礙物的基本形貌、距離和位置等信息,同時具有精度高等優點,而被廣泛運用于無人裝備的障礙物檢測和跟蹤中。

文獻[6]中建立了包含目標中心點位置、速度等用以表示動態障礙物信息的盒子模型,并采用了MHT(多假設跟蹤算法)在一定程度上解決了多運動目標的跟蹤問題。但未考慮在跟蹤過程中點云會隨動態障礙物運動而變化,且在跟蹤過程中,所采用方法的最小變化量為1個柵格,導致跟蹤結果不理想。文獻[7]中將四線激光雷達與三維激光雷達HDL-64E相結合實現了對動態障礙物的檢測和跟蹤,一定程度上改善了文獻[6]中存在的問題,但未能很好地解決障礙物運動過程中點云變化致使跟蹤點不穩定的問題,其實驗結果中速度方向的劇烈變化反映了此點。文獻[8]中則采用定向包圍體來描述目標障礙物,首先除去異值點云,然后用RANSAC算法找出剩余點云的主成分分布方向,進而得到障礙物的航向角。這種做法得到的結果比文獻[6]中方法的結果更加穩定,但實際情況中受車輛遮擋或是自身遮擋因素的影響易出現過擬合的情況。

為解決上述文獻方法中存在的瑕疵,本文中提出了一種基于三維激光雷達障礙物檢測和跟蹤的方法。首先將路面分割后的點云柵格化,再進行柵格增補。在障礙物聚類之后,結合無人車RTK-GPS數據和INS的航向角數據實現了多幀融合的靜態障礙物檢測,并利用靜態障礙物檢測結果形成的可行駛區域和動態障礙物模板匹配算法檢測出了動態障礙物。最后,利用標準卡爾曼濾波器對動態障礙物的運動狀態進行了平滑濾波,得到了穩定可靠的跟蹤結果。

1 點云預處理

本文中所采用的HDL-64E三維激光雷達由Velodyne公司生產。每幀可以產生近13萬個點云,這些點云是在同一空間參考坐標系下可以表征障礙物空間分布特征的大量點的集合。面對如此巨大的數據量,須對點云進行預處理以方便后續進行障礙物檢測和跟蹤。

先采用文獻[9]中路面分割的方法去除了路面點云,在無人車前后左右各40m范圍內對剩余的點云采用40cm×40cm大小的柵格進行柵格化。柵格內點云數量小于2個的視為非障礙物柵格,柵格內點云數量大于等于2個的柵格視為障礙物柵格。當兩個障礙物柵格中間存在非障礙柵格時,若二者的最大點云高度相近,則將中間的非障礙物柵格變為障礙物柵格,并將原兩端障礙物柵格的屬性值賦給非障礙物柵格,沿無人車縱向增補時,如果中間非障礙物柵格的數量超過3個則不再增補,沿無人車橫向增補時,如果中間非障礙物柵格的數量超過1個則不再增補。柵格增補可以極大地增強一幀點云反映無人車周邊環境的能力,從而完成一幀點云的柵格化,圖1是去除地面點云后進行柵格增補前后的對比。

圖1 柵格增補結果

2 障礙物檢測

利用文獻[10]中的聚類算法可實現對障礙物柵格的聚類。在完成聚類之后,先進行靜態障礙物的檢測,再進行動態障礙物的檢測。目前,單幀靜態障礙物的檢測結果中往往存在漏檢的情況,因此提出在單幀靜態障礙物檢測結果的基礎上采用多幀融合的方法來檢測靜態障礙物。在進行動態障礙物檢測時,單一依靠模板匹配算法[6]易出現誤檢的情況,本文中提出先利用靜態障礙物檢測結果確定出可行駛區域,然后再結合動態障礙物模板匹配算法來對可行駛區域內的動態障礙物進行檢測的新思路,有效地避免了部分動態障礙物的誤檢。

2.1 靜態障礙物檢測

單幀點云中靜態障礙物的聚類結果主要包含2種:(1)第1種聚類結果包含路邊界及與之相鄰接的靜態障礙物如建筑物等,這類聚類結果的長度和寬度明顯大于車輛、行人等動態障礙物的長度和寬度。(2)第2種聚類結果則是只包含路邊界,這類聚類結果的顯著特征是狹長,且高度明顯低于較長的公交車和高速公路上的貨物運輸車。

通過靜態障礙物聚類結果的幾何特征能檢測出單幀中的部分靜態障礙物。因運動的車輛等動態障礙物的遮擋和HDL-64E激光雷達不同激光線之間間距的影響,將使路邊界等靜態障礙物易出現漏檢的情況,提出在單幀靜態障礙物檢測結果的基礎上采用多幀融合的方法來檢測靜態障礙物。

采用多幀融合檢測靜態障礙物須先進行2幀的融合,在此基礎上再實現連續多幀的融合。利用文獻[11]中經緯度轉平面坐標的方法將記錄各幀點云數據時無人車RTK-GPS的經緯度轉到大地平面直角坐標系XOY下,可以得到無人車第t幀時在坐標系XOY下的平面坐標(xt,yt),大地平面直角坐標系XOY的y軸始終指向正北方向,x軸始終指向正東方向,坐標系原點O為無人車第一次記錄RTKGPS時的位置,進而可得任意連續2幀中無人車的相對平移量(Δxt,Δyt):

式中(xt-1,yt-1)為無人車第t-1幀時在坐標系XOY下的平面坐標。

由于在記錄各幀點云時,無人車的航向不同,INS提供的航向角數據會不一樣,將2幀點云融合時需考慮二者之間航向角β的關系,參考圖2,首先將第t-1幀的車體坐標系ot-1-xyz下靜態障礙物的點云先旋轉到第t-1幀與大地平面直角坐標系XOY坐標軸方向相同的局部車體坐標系ot-1-x′y′z下,然后根據2幀的相對平移量(Δxt,Δyt)平移到第t幀的坐標系ot-x′y′z下,最后再從第t幀的坐標系otx′y′z旋轉到第 t幀的車體坐標系 ot-xyz下,完成2幀靜態障礙物點云的融合。

圖2 靜態障礙物2幀融合示意圖

若第t-1幀無人車的航向角為βt-1,第t幀無人車的航向角為βt,定義為無人車運動方向與坐標系XOY正北方向的順時針夾角,將第t-1幀屬于靜態障礙物的一個掃描點A點()旋轉平移到第t幀的局部車體坐標系 ot-x′y′z下的坐標():

式中R1為旋轉平移變換矩陣。

經旋轉平移后得到此掃描點在第t幀的局部車體坐標系 ot-x′y′z 下的坐標(),然后再將此掃描點旋轉回第t幀的車體坐標系ot-xyz。通過式(4)得到第t-1幀的車體坐標系ot-1-xyz下的一個靜態障礙物掃描點A點()在第t幀的車體坐標系ot-xyz下的坐標():

一幀點云包含13萬個點,若對所有屬于靜態障礙物的點云都進行旋轉平移變換,則計算量龐大,部分靜態障礙物柵格里的點云在旋轉平移變換后仍會落到同一個柵格里,所以隨機選取每個靜態障礙物柵格里的3個掃描點(不足3個的取所有掃描點)作為變換點,不再對靜態障礙物柵格里所有點云做處理,將原來屬于靜態障礙物的掃描點在旋轉平移變換后所在的柵格標記為靜態障礙物柵格。本文中在2幀融合的基礎上,融合了連續6幀的數據。

圖3 靜態障礙物檢測結果

城市場景環境下動態障礙物多,靜態障礙物的遮擋情況較其他場景環境下更嚴重,圖3為采用本文方法在城市場景環境下多幀融合的靜態障礙物檢測結果。對比圖3中1號和2號橢圓形,可以明顯看到多幀融合的靜態障礙物檢測方法可避免因車輛遮擋導致靜態障礙物漏檢情況地發生,同時有效地保留了歷史幀的靜態障礙物檢測結果,這對短期內無人車路徑規劃和控制決策起到了積極的作用。

2.2 動態障礙物檢測

基于單一幾何特征的動態障礙物檢測算法[12]可實現部分動態障礙物的檢測,但這種檢測方法在非封閉的場景環境下易出現誤檢的情況,本文中利用多幀融合的靜態障礙物檢測結果,確定了對無人車路徑規劃和決策控制(如超車、跟車、換道等)影響較大的本車道的可行駛區域,將所有位于本車道可行駛區域內已檢測出來的動態障礙物視為需要被跟蹤的動態障礙物,其它不在可行駛區域內的障礙物視為普通障礙物,僅將其位置信息傳給無人車,不再進行跟蹤,這樣做可很大程度上地避免誤檢情況的出現,同時可很好地滿足無人車進行路徑規劃和決策控制的感知需求。

根據特定的動態障礙物模板[6]進行匹配,可將動態障礙物模板分為4類:(1)“L”型動態障礙物,這類動態障礙物多為在城市道路上行駛的汽車,因激光雷達只掃描到汽車的兩面,而呈現出類似“L”的形狀；(2)“I”型動態障礙物,這類障礙物多為行駛的過程中位于無人車正前方或者正后方的車輛,因自遮擋而致使障礙物點云出現類似“I”的形狀；(3)“口”型動態障礙物,這類模板能較好地反映障礙物的真實形狀；(4)行人和騎車人員動態障礙物,這類障礙物點云形狀接近“I”型。通過將聚類結果與特定的動態障礙物模板進行匹配,可以初步確定出絕大部分動態障礙物。

但這種檢測方法并不能將所有類型的動態障礙物囊括進來,易出現誤檢的情況,如圖4所示,1～4號橢圓形內黑色的障礙物聚類結果即為采用文獻[6]中模板匹配算法誤檢為動態障礙物的障礙物,所以本文中利用多幀融合的靜態障礙物檢測結果,確定了無人車所在車道的可行駛區域并結合動態障礙物模板匹配算法確定了可行駛區域內的動態障礙物,避免了誤檢情況地發生。

在確定本車道可行駛區域時(見圖5),由無人車所在柵格O的位置出發先沿正前方和正后方做縱向搜索,遇到靜態障礙物柵格或末端柵格則終止搜索,確定出正前方終止柵格A和正后方終止柵格B。之后,按照從O到A和B的順序依次向兩側做橫向搜索,遇到靜態障礙物柵格則終止本次搜索。由于4級及以上公路車道寬度應在3m以上,如果前一次橫向搜索終止位置與本次橫向搜索終止位置相差3m以上則認為未找到靜態障礙物柵格,如柵格C,但如果此時遇到普通障礙物柵格則將普通障礙物柵格作為搜索終止柵格,如柵格D,否則將前一次終止柵格的位置作為本次搜索終止的位置,由此可以得到圖5中多邊形所包圍的本車道的可行駛區域,受益于靜態障礙物多幀融合的良好檢測效果,可很好地確定出無人車所在車道的可行駛區域。

圖4 模板匹配算法檢測結果

圖5 可行駛區域檢測示意圖

動態障礙物檢測結果如圖6所示。圖中被小矩形包圍的障礙物是在確定了本車道可行駛區域后再結合模板匹配算法確定的動態障礙物,可以看出這樣做可很好地檢測出動態障礙物,同時有效地避免了文獻[6]中方法在復雜的城市道路環境下誤檢情況地發生,提高了算法的魯棒性。

圖6 動態障礙物檢測結果

3 動態障礙物跟蹤

3.1 數據關聯

將上一幀的動態障礙物與當前幀的動態障礙物進行數據關聯,在進行數據關聯時,采用邊界位置相似度關聯方法,將在第t幀第i個動態障礙物的4條邊界位置定義為:

可得到前后2幀之間各個動態障礙物之間的相似度矩陣 Sm×n:

式中:m為前一幀中動態障礙物的個數；n為當前幀中動態障礙物的個數。

在無人車所在車道的可行駛區域進行數據關聯時,前后兩幀間的任意2個動態障礙物之間的相似度sij為

在數據關聯時,會有3種情況:(1)上一幀和當前幀中同一個動態障礙物確實仍然存在；(2)上一幀中的動態障礙物在當前幀中已經不存在；(3)在當前幀中新出現的動態障礙物,而上一幀沒有此動態障礙物。因此即使是相似度最大,也不一定是同一個動態障礙物,所以可通過設置合適的閾值(本文中的合適閾值為0.005)來區別無人車所在車道可行駛區域的同一個動態障礙物是否同時存在于上一幀和當前幀中。

3.2 動態障礙物運動狀態估計

跟蹤點的選擇對跟蹤效果影響很大,在使用HDL-64E三維激光雷達進行跟蹤時,利用障礙物點云形狀結合車輛形狀的先驗知識可以反推車輛中心位置[6-7,9],但實際上,受動態障礙物遮擋或是自遮擋的影響,同時各種動態障礙物的形狀尺寸并不嚴格滿足特定的模型,這種推導出來的中心位置存在一定的偏差,本文中借鑒了文獻[13]中跟蹤點的選取思想,并結合RTK-GPS數據和INS航向角數據來計算動態障礙物的運動狀態,而不通過點云分布特征來確定動態障礙物的航向角,可很好地避免文獻[8]中在動態障礙物運動過程中因點云分布不均致使錯誤估計動態障礙物運動狀態的情況出現,同時也使得跟蹤結果更穩定。

將動態障礙物的運動狀態表示為

動態障礙為跟蹤示意圖如圖7所示,圖中障礙物為第i個動態障礙物。式(9)中的(是第 t幀時第i個動態障礙物跟蹤點在大地平面直角坐標系XOY下的橫坐標和縱坐標,由式(10)求出；和是動態障礙物分別在正東方向上和正北方向上的速度；是動態障礙物的合速度,由式(11)求出；是動態障礙物的航向角,定義為動態障礙物運動方向與正北方向的順時針夾角,由式(12)和式(13)求出。

圖7 動態障礙物跟蹤示意圖

式中T為連續2幀的時間間隔,T＝0.1s。

將式(9)所述狀態量代入標準卡爾曼濾波器[14],并將標準卡爾曼濾波器中狀態轉移矩陣A定義為式(14)中的4階方陣,將觀測矩陣H定義為4階單位矩陣,這樣可確保在狀態轉移時是從t-1時刻的位置轉移到t時刻的位置,其中σ為高斯白噪聲。同時優化了噪聲矩陣參數,最終將狀態轉移噪聲矩陣Q定義為均值為0、標準差為0.37m的高斯白噪聲協方差4階對角陣,觀測噪聲矩陣R定義為均值為0、標準差為0.75m的高斯白噪聲協方差4階對角陣,利用上述標準卡爾曼濾波器可以穩定地跟蹤動態障礙物。

4 實驗結果與分析

實驗平臺是課題組自主研發的“JJUV-5”智能車(見圖8),主要傳感器包括有1臺Velodyne HDL-64E激光雷達、1套RTK-GPS系統和1臺Inertial+慣導,計算機CPU是主頻為3.1GHz的Intel i5-3350p處理器,內存為4G。

圖8 實驗平臺

為了檢驗所提障礙物檢測與跟蹤方法的有效性,利用“JJUV-5”無人車在真實城市環境中進行了大量的測試,本節選擇其中一段動態障礙物較多的具有代表性的路段進行介紹和分析,并與文獻[8]中采用的方法進行了比較。

圖9為動態障礙物連續跟蹤的結果。由圖可見,本文方法可以一直對同一個動態障礙物的顏色和編號保持不變,如程序中標為“1號”的障礙物(本文中的1號障礙物)和“0號”的障礙物(本文中的2號障礙物)在整個跟蹤過程中都始終保持各自的顏色,且始終是同一個編號,這表明本文方法可以很穩定地跟蹤同一個障礙物。同時,在動態障礙物較多的時候,如圖9(e)～圖9(h),本文方法仍能準確地跟蹤各個動態障礙物,如1號障礙物和2號障礙物。圖9的8幅圖所展現的8幀障礙物的檢測與跟蹤結果中,幾乎所有的靜態障礙物和不在可行駛區域的動態障礙物都為黑色,這表明了本文方法可有效地區分靜態障礙物、非可行駛區域動態障礙物和需跟蹤的動態障礙物。選取1號障礙物為代表來說明本文方法對動態障礙物的跟蹤能力。

圖9 動態障礙物連續跟蹤結果

圖10分別為采用本文中方法和采用文獻[8]中方法對1號動態障礙物的進行跟蹤的速度結果。1號動態障礙開始時速度高于無人車速度,離無人車越來越遠,但隨著1號障礙物速度越來越低,其距離無人車也越來越近,從圖10的障礙物速度估計過程可看出本文中方法和文獻[8]中方法都可有效地反映動態障礙物的實時速度。

圖10 1號動態障礙物速度跟蹤結果

圖11為1號動態障礙物航向角跟蹤結果。根據圖9,1號動態障礙物與無人車的運動方向基本上一致,反映在圖11上,可以看到1號動態障礙物的航向角與無人車航向角幾近相同,在第125幀至第160幀左右,1號動態障礙物向無人車右側行駛,此時圖11上1號動態障礙物的航向角明顯高于無人車的航向角。本文方法和文獻[8]的方法都可有效地反映動態障礙物的航向變化情況。根據圖10的1號框和2號框,以及圖11的3號框和4號框,可以看到本文方法相比于文獻[8]的方法能更為平穩地估計動態障礙物的速度和航向角,在障礙物出現遮擋或是自遮擋時能在一定程度上避免因點云分布不均而致使錯誤估計的情況,使得跟蹤結果更為穩定。在240幀中,平均每幀的檢測和跟蹤耗時為104.165ms,滿足無人車對算法的實時性要求。

圖11 1號動態障礙物航向角跟蹤結果

5 結論

本文中主要研究了基于三維激光雷達的障礙物檢測和跟蹤問題。在靜態障礙物檢測上,結合無人車RTK-GPS數據和INS航向角數據實現了多幀融合的靜態障礙物檢測,即使是在靜態障礙物遮擋較嚴重的城市環境下,也能有效地檢測出靜態障礙物。在動態障礙物檢測上,利用靜態障礙物檢測結果形成的可行駛區域與模板匹配算法實現了對動態障礙物的檢測,有效降低了動態障礙物的誤檢率。最后,結合RTK-GPS數據和INS航向角數據來估計動態障礙物運動狀態的方法和標準卡爾曼濾波器的有效運用提高了對動態障礙物的跟蹤能力并獲得了穩定可靠的跟蹤結果,可靠的實驗結果表明本文中基于三維激光雷達的無人車障礙物檢測和跟蹤方法的可行性和有效性。

本研究尚未考慮動態障礙物在完全遮擋后再次重現時的情況,同時柵格化尺度過大,當不同障礙物靠得較近時,會將兩個不同的障礙物聚類成一個障礙物,下一步將著重解決這些細節問題。