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

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

前言

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

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

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

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

1 點云預處理

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

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

圖1 柵格增補結(jié)果

2 障礙物檢測

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

2.1 靜態(tài)障礙物檢測

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

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

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

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

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

圖2 靜態(tài)障礙物2幀融合示意圖

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

式中R1為旋轉(zhuǎn)平移變換矩陣。

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

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

圖3 靜態(tài)障礙物檢測結(jié)果

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

2.2 動態(tài)障礙物檢測

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

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

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

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

圖4 模板匹配算法檢測結(jié)果

圖5 可行駛區(qū)域檢測示意圖

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

圖6 動態(tài)障礙物檢測結(jié)果

3 動態(tài)障礙物跟蹤

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

將上一幀的動態(tài)障礙物與當前幀的動態(tài)障礙物進行數(shù)據(jù)關(guān)聯(lián),在進行數(shù)據(jù)關(guān)聯(lián)時,采用邊界位置相似度關(guān)聯(lián)方法,將在第t幀第i個動態(tài)障礙物的4條邊界位置定義為:

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

式中:m為前一幀中動態(tài)障礙物的個數(shù)；n為當前幀中動態(tài)障礙物的個數(shù)。

在無人車所在車道的可行駛區(qū)域進行數(shù)據(jù)關(guān)聯(lián)時,前后兩幀間的任意2個動態(tài)障礙物之間的相似度sij為

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

3.2 動態(tài)障礙物運動狀態(tài)估計

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

將動態(tài)障礙物的運動狀態(tài)表示為

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

圖7 動態(tài)障礙物跟蹤示意圖

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

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

4 實驗結(jié)果與分析

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

圖8 實驗平臺

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

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

圖9 動態(tài)障礙物連續(xù)跟蹤結(jié)果

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

圖10 1號動態(tài)障礙物速度跟蹤結(jié)果

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

圖11 1號動態(tài)障礙物航向角跟蹤結(jié)果

5 結(jié)論

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

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