謝德勝,徐友春,王任棟,蘇致遠(yuǎn)

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

前言

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

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

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

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

1 點(diǎn)云預(yù)處理

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

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

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

2 障礙物檢測

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 動(dòng)態(tài)障礙物運(yùn)動(dòng)狀態(tài)估計(jì)

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

將動(dòng)態(tài)障礙物的運(yùn)動(dòng)狀態(tài)表示為

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

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

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

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

4 實(shí)驗(yàn)結(jié)果與分析

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

圖8 實(shí)驗(yàn)平臺(tái)

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

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

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

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

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

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

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

5 結(jié)論

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

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