朱 株,劉濟(jì)林
(浙江大學(xué) 信息與電子工程學(xué)系,浙江 杭州310027)
精確感知三維環(huán)境是地面自主智能車(chē)輛的一項(xiàng)重要工作.為保證安全行駛,自主車(chē)輛通常裝有激光雷達(dá)和相機(jī)組成的傳感器系統(tǒng),用于探測(cè)障礙區(qū)域和地面區(qū)域.
早期自主機(jī)器人平臺(tái)一般采用單線激光雷達(dá)作為距離傳感器,具有數(shù)據(jù)量小及響應(yīng)速度快的優(yōu)點(diǎn),但是存在探測(cè)范圍有限的缺點(diǎn).隨著VELODYNE 64線激光雷達(dá)的問(wèn)世,這種三維激光雷達(dá)開(kāi)始替代單線激光雷達(dá),廣泛用于自主車(chē)平臺(tái)[1-3].
現(xiàn)有的三維激光雷達(dá)地面分割算法主要有如下2類(lèi).
1)基于柵格地圖的地面分割算法.這類(lèi)算法將數(shù)據(jù)柵格化,投影到一個(gè)柵格地圖中,分析每個(gè)柵格中的三維點(diǎn)云特性來(lái)確定該柵格的障礙物屬性.Kammel等[4]提出了一種基于柵格中點(diǎn)云最大高程差的地面檢測(cè)方法,如果柵格最大高程差小于某個(gè)閾值,柵格將被標(biāo)記為地面區(qū)域.Himmelsbach等[5]將三維數(shù)據(jù)以極坐標(biāo)的方式柵格化,并且用非參數(shù)的方法來(lái)擬合地平面,分離地面和障礙物.Guo等[6]將柵格圖以無(wú)向圖的方式處理,用馬爾科夫隨機(jī)場(chǎng)將柵格分為4類(lèi),從中選取地面.雖然基于柵格的檢測(cè)方法較為穩(wěn)定,但是該方法的檢測(cè)精度取決于柵格大?。ㄍ?0 cm×20 cm),相比于原始三維點(diǎn)云數(shù)據(jù)精度(0.2 cm),柵格算法精度較小;受限于計(jì)算速度,柵格地圖范圍不大,典型的250×150柵格圖相當(dāng)于50 m×30 m的檢測(cè)范圍,相比于原始數(shù)據(jù)200 m×200 m的檢測(cè)范圍,大量數(shù)據(jù)丟失;由于三維數(shù)據(jù)分布的非均勻性,大量柵格內(nèi)并沒(méi)有數(shù)據(jù),造成了存儲(chǔ)和處理的浪費(fèi).
2)基于圖的地面檢測(cè).三維激光雷達(dá)數(shù)據(jù)結(jié)構(gòu)的特點(diǎn):任意三維點(diǎn)與其在同一掃描線上相鄰角度的點(diǎn)以及前后掃描線相同角度的點(diǎn)組成4鄰域系統(tǒng),可以將三維點(diǎn)云以無(wú)向圖的方式組織起來(lái).Montemerlo等[7]使用三維數(shù)據(jù)環(huán)之間的距離來(lái)判斷單個(gè)三維點(diǎn)是否屬于地面區(qū)域(三維激光雷達(dá)地面反射點(diǎn)在二維平面上呈現(xiàn)出環(huán)的樣式,障礙物點(diǎn)的環(huán)間距要遠(yuǎn)小于地的面點(diǎn)間距).Moosmann等[8]使用局部凹凸性分割不同物體.Douillard等[9]采用Mesh方式構(gòu)造圖,并且通過(guò)計(jì)算梯度來(lái)確定地面點(diǎn).
相比于柵格類(lèi)的分割算法,基于圖的算法精度較高,并且能處理全部雷達(dá)數(shù)據(jù),但是現(xiàn)有算法均使用單個(gè)三維點(diǎn)作為圖的節(jié)點(diǎn),特征單一,容易受到噪聲干擾,在起伏道路環(huán)境中魯棒性較差.
本文提出一種新的基于圖的算法.將掃描線χ-y平面上的投影曲線進(jìn)行分段,構(gòu)建以線段為節(jié)點(diǎn)的馬爾科夫隨機(jī)場(chǎng),通過(guò)對(duì)線段特征分析建立能量函數(shù),并采用圖分割算法求全局最優(yōu)解.算法不僅能在100 ms內(nèi)完成,而且在不同場(chǎng)景下分割效果都優(yōu)于現(xiàn)有基于圖的分割算法.
觀察雷達(dá)數(shù)據(jù)在χ-y平面上的投影可以發(fā)現(xiàn),地面數(shù)據(jù)和障礙物數(shù)據(jù)在線型上的明顯區(qū)別.地面區(qū)域如圖1(a)環(huán)狀圖案所示,單條掃描線障礙區(qū)域放大后如圖1(b)、(c)所示:在同一掃描線上呈斷開(kāi)狀或是折角狀.據(jù)此,本文提出的地面分割算法采用線段特征.
圖1 掃描線在x-y平面上投影的樣式Fig.1 Projection of scan lines onχ-y plane
圖2 地面分割算法流程圖Fig.2 Flow chart of ground segmentation algorithm
圖2為地面分割算法的流程框圖,該流程分為3個(gè)步驟:掃描線投影分段;線段端點(diǎn)精確定位;建立無(wú)向圖馬爾科夫隨機(jī)場(chǎng)并且求解.
首先輸入激光雷達(dá)原始數(shù)據(jù),并且進(jìn)行預(yù)處理,包括異常距離三維點(diǎn)濾除以及采用GPS進(jìn)行點(diǎn)云位姿修正.
考慮到在χ-y平面的投影中每條掃描線處于同一直線上的相鄰點(diǎn)屬性相同,算法首先對(duì)每條掃描線進(jìn)行快速分割.由于存在噪聲,分割后相鄰線段端點(diǎn)(角點(diǎn))可能出現(xiàn)定位錯(cuò)誤,通過(guò)角點(diǎn)檢測(cè),準(zhǔn)確選取端點(diǎn),增加算法的精確度.采用線段作為圖節(jié)點(diǎn),不僅大大減少了節(jié)點(diǎn)數(shù)量(從每幀大約130 000個(gè)點(diǎn)減少到20 000個(gè)點(diǎn)左右)加快圖分割的處理速度,而且能獲取多個(gè)基于線段的特征,增加了算法的魯棒性.最后通過(guò)圖分割算法,獲取全局優(yōu)化后的分割結(jié)果,提升算法的性能.
由于地面不平整以及激光雷達(dá)本身具有的檢測(cè)噪聲,掃描線在χ-y平面上投影含有噪聲.采用基于最大模糊線段的方法對(duì)掃描線投影進(jìn)行快速分割.
模糊線段(blurred segment,BS)定義為介于直線l1:aχ+by=μ以及直線l2:aχ+by=μ+ω之間的有限離散二維點(diǎn)集,如圖3所示.ν為BS的寬度:
圖3 模糊線段Fig.3 Blurred segment
一個(gè)二維點(diǎn)集模糊線段的參數(shù)可以通過(guò)計(jì)算該點(diǎn)集凸包絡(luò)的支持線來(lái)獲?。?0],計(jì)算復(fù)雜度與點(diǎn)的數(shù)目成線性關(guān)系.分割算法流程如下:
1)將單條掃描線的映射點(diǎn)順序輸入,如果當(dāng)前點(diǎn)和前一個(gè)點(diǎn)的距離大于閾值T p-d,則將前一個(gè)點(diǎn)作為當(dāng)前線段的終止端點(diǎn),當(dāng)前點(diǎn)作為新一條線段的起始端點(diǎn).相鄰2個(gè)點(diǎn)的屬性都標(biāo)記為分離點(diǎn).
2)若當(dāng)前點(diǎn)和前一個(gè)點(diǎn)的距離小于閾值Tp-d,則進(jìn)行模糊線段參數(shù)計(jì)算.如果模糊線段的寬度大于閾值Tv,則將前一個(gè)點(diǎn)作為當(dāng)前線段的終止端點(diǎn),當(dāng)前點(diǎn)作為新一條線段的起始端點(diǎn).相鄰2個(gè)點(diǎn)的屬性都標(biāo)記為連接點(diǎn).
三維激光雷達(dá)在不同距離下有不同的分辨率,Tp-d和Tv均為距離相關(guān)的自適應(yīng)參數(shù):
式中:D為當(dāng)前點(diǎn)在χoy平面的投影到原點(diǎn)的距離,μ1、μ2為比例因子.由于 VELODYNE HDL64E雷達(dá)在不同距離,掃描線在地面上的點(diǎn)間隔不同,對(duì)此閾值使用比例因子與距離的乘積.考慮到在10 Hz運(yùn)行下,激光雷達(dá)角分辨率最大為0.18°,2個(gè)連續(xù)掃描點(diǎn)間距理論上為D×0.18π/180.作為閾值將該值適當(dāng)放大,取3倍值,計(jì)算中μ1=0.009 42.式(3)用于確定分割線段的最大寬度.該閾值不僅受到距離的影響,也受到不同地形環(huán)境的影響.本文使用經(jīng)驗(yàn)值,平坦地形下μ2=0.003,在顛簸環(huán)境下μ2=0.005.
分割結(jié)果見(jiàn)圖4,相鄰的線段用2種不同的灰度表示.
圖4 模糊線段分割結(jié)果Fig.4 Segmentation by blurred segment
模糊線段有一定的寬度,當(dāng)連接點(diǎn)很多時(shí)不能準(zhǔn)確定位在局部曲率最大的角點(diǎn)上.可以通過(guò)角點(diǎn)檢測(cè)算法重新定位連接點(diǎn).
實(shí)空間R上的算子A被定義為以下形式:
二維曲線Γ定義為參數(shù)化方程a(s),核函數(shù)φ定義為高斯函數(shù),其中u為均值,t為方差:
將a(u)在u=0附近進(jìn)行泰勒展開(kāi):
可得:
積分算子在χ處的值在曲線法方向nχ上正比于曲線曲率κχ,選擇局部曲率最大的點(diǎn)作為角點(diǎn).角點(diǎn)定位前后分割結(jié)果如圖5(a)、(b)所示.
圖5 角點(diǎn)定位前后不同的分割結(jié)果Fig.5 Segment results before and after corner detection
馬爾科夫隨機(jī)場(chǎng)框架建立了全局標(biāo)記優(yōu)化模型,常被用于解決場(chǎng)景分類(lèi)問(wèn)題[11].定義G= (V,E)為無(wú)向圖,節(jié)點(diǎn)v i∈V;邊 (vi,v j)∈E對(duì)應(yīng)于相鄰的節(jié)點(diǎn)對(duì).每條邊(vi,vj)∈E被賦以非負(fù)權(quán)重W(v i,v j),用于描述相鄰節(jié)點(diǎn)vi及v j間的相似度.
標(biāo)記推測(cè)算法能為每個(gè)節(jié)點(diǎn)找到概率最大的標(biāo)記,例如圖分割[12].定義L為一個(gè)有限的標(biāo)記集合,f為標(biāo)記賦值函數(shù),f(v i)為每個(gè)節(jié)點(diǎn)賦以標(biāo)記.假設(shè)標(biāo)記在區(qū)域內(nèi)變化緩慢,而在區(qū)域邊緣變化劇烈.標(biāo)記賦值的準(zhǔn)確度由能量方程來(lái)衡量:
式(8)中等號(hào)右側(cè)第一項(xiàng)是數(shù)據(jù)項(xiàng),是對(duì)節(jié)點(diǎn)本身的約束;第二項(xiàng)是平滑項(xiàng),是對(duì)能量方程解的平滑性約束[13].
本文算法利用激光雷達(dá)原始數(shù)據(jù)結(jié)構(gòu),構(gòu)建無(wú)向圖.把每條線段當(dāng)作一個(gè)節(jié)點(diǎn),所有包含與當(dāng)前節(jié)點(diǎn)線段相鄰三維點(diǎn)的線段,與該節(jié)點(diǎn)線段組成一個(gè)相鄰集合.定義與節(jié)點(diǎn)相鄰的同一掃描線線段為側(cè)鄰線段(SN),定義與節(jié)點(diǎn)相鄰的前一掃描線線段為上鄰線段(UN),定義與節(jié)點(diǎn)相鄰的后一掃描線線段為下鄰線段(DN).根據(jù)定義,每條線段只有左右2條側(cè)鄰線段,可以有多條上下鄰線段.
根據(jù)線段內(nèi)點(diǎn)數(shù)依據(jù)閾值Tp-n將線段分為2類(lèi):長(zhǎng)線段(Sl)和短線段(Ss).由于地面較為平坦,長(zhǎng)線段大多屬于高概率地面區(qū)域(Agh),只有在以下情況長(zhǎng)線段被定義為高概率的障礙區(qū)域(Aoh):
1)長(zhǎng)線段端點(diǎn)標(biāo)記為分離點(diǎn),且該點(diǎn)距雷達(dá)原點(diǎn)的距離與側(cè)鄰線段分離點(diǎn)距原點(diǎn)距離的差大于閾值Td.
2)長(zhǎng)線段端點(diǎn)標(biāo)記為連接點(diǎn),且它與側(cè)鄰線段夾角與直角的差小于閾值Ta.
3)長(zhǎng)線段與上鄰線段的梯度大于閾值Tg.梯度定義為線段包含三維點(diǎn)平均高度差與線段在χ-y平面上的平均距離的比值.
短線段大多屬于高概率障礙區(qū)域,只有在以下情況長(zhǎng)線段被定義為高概率的地面區(qū)域:
1)短線段端點(diǎn)標(biāo)記為連接點(diǎn),與側(cè)鄰定義為高概率地面區(qū)域的長(zhǎng)線段夾角與直角的差值大于閾值Ta.
2)短線段與上鄰定義為高概率地面區(qū)域的長(zhǎng)線段的梯度小于閾值Tg.
本文采用線段點(diǎn)數(shù)閾值的經(jīng)驗(yàn)值Tp-n=6.Td、Tg均為障礙物閾值,障礙物會(huì)造成同一掃描線上線段的分離以及相鄰掃描線上線段間產(chǎn)生梯度.這2個(gè)閾值的取值與地形或者障礙物區(qū)分敏感度有關(guān).實(shí)驗(yàn)中,城市道路的Td=40,Tg=tan 12°(0.212);鄉(xiāng)村顛簸道路的Td=60,Tg=tan 25°(0.466).Ta主要用于區(qū)分城市道路中的馬路牙子和地面,實(shí)驗(yàn)中Ta=30.
根據(jù)以上準(zhǔn)則,可得:
η是符合判斷準(zhǔn)則下,標(biāo)記為地面的先驗(yàn)概率值,實(shí)驗(yàn)中η=0.8.
為保證局部區(qū)域標(biāo)記的一致性,定義平滑項(xiàng):
φ(Δh)是根據(jù)相鄰線段在三維空間中的平均高度差Δh來(lái)度量相似性的函數(shù),k為經(jīng)驗(yàn)系數(shù)(實(shí)驗(yàn)中k=0.01):
最后使用圖分割的方法(Graph-Cut)優(yōu)化能量方程,得出標(biāo)記結(jié)果.
實(shí)驗(yàn)平臺(tái)如圖6所示.三維激光雷達(dá)安裝在車(chē)頂部,除少部分盲區(qū)外,能較為全面地獲取車(chē)體周?chē)娜S環(huán)境信息.
圖6 三維激光雷達(dá)與自主車(chē)Fig.6 3D LIDAR and autonomous landing vehicle
分別在城市平坦路面場(chǎng)景和鄉(xiāng)村起伏道路場(chǎng)景采集數(shù)據(jù),將本文算法結(jié)果與局部凹凸性算法[8]、Mesh梯度算法[9]以及人工標(biāo)記的結(jié)果進(jìn)行對(duì)比.圖7中第1行是城市平坦路面場(chǎng)景;第2行是鄉(xiāng)村起伏道路場(chǎng)景;圖7(a)、(f)是對(duì)2種場(chǎng)景的相機(jī)圖片;圖7(b)、(g)是三維激光雷達(dá)數(shù)據(jù)人工標(biāo)記的結(jié)果,在本文中該圖為灰度圖,實(shí)際中該圖的其中地面區(qū)域標(biāo)記為綠色,障礙區(qū)域標(biāo)記為紅色,黑色區(qū)域?yàn)槲粗獏^(qū)域;圖7(c)、(h)為局部凹凸性算法的結(jié)果;圖7(d)、(i)為 Mesh梯度算法結(jié)果;圖7(e)、(j)為本文算法的結(jié)果.
在城市平坦路面場(chǎng)景下,Mesh梯度算法(圖7(d))結(jié)果和本文算法(圖7(e))的結(jié)果都較好,接近人工標(biāo)記的結(jié)果(圖7(a)).局部凹凸性算法(圖7(c))結(jié)果較差,以局部平面的法向量方向作為判斷準(zhǔn)則,用單個(gè)三維點(diǎn)的鄰接4點(diǎn)擬合平面.因此,在構(gòu)建圖時(shí),刪除了長(zhǎng)度大于某個(gè)閾值的邊,以防止遠(yuǎn)距離點(diǎn)用于平面擬合,造成障礙物邊緣判斷錯(cuò)誤.由于在遠(yuǎn)距離,激光雷達(dá)相鄰點(diǎn)本身相距很遠(yuǎn),導(dǎo)致刪除長(zhǎng)邊后的圖出現(xiàn)大量未知區(qū)域,影響了檢測(cè)效果.
在鄉(xiāng)村起伏道路場(chǎng)景下,地面起伏變化大,使得基于單個(gè)三維點(diǎn)梯度特征的Mesh梯度算法(圖7(i))檢測(cè)效果下降,出現(xiàn)了大量地面區(qū)域漏檢點(diǎn);路邊大量低矮植物造成了地面區(qū)域誤檢點(diǎn).局部凹凸性算法(圖7(h))不僅在遠(yuǎn)距離存在未知區(qū)域;在近距離,由于地面不平整,使得局部平面的法向量變化很大,導(dǎo)致地面區(qū)域漏檢,路邊大量低矮植物又造成局部平面的法向量變化緩慢,產(chǎn)生地面區(qū)域誤檢點(diǎn).本文算法(圖7(j))效果優(yōu)于Mesh梯度算法和局部凹凸性算法:由于本算法使用線段的梯度特征,減少噪聲對(duì)單點(diǎn)梯度的影響,線段夾角特征和分離點(diǎn)距離特征的判斷增加了地面區(qū)域判斷的可靠性,最后使用馬爾科夫隨機(jī)場(chǎng)進(jìn)行全局優(yōu)化使得結(jié)果具有更好的魯棒性.
圖7 2種場(chǎng)景的分割結(jié)果Fig.7 Segmentation results of two kinds of scenes
式中:NTP為地面點(diǎn)被正確標(biāo)記的數(shù)目,NFP為障礙點(diǎn)被標(biāo)記為地面點(diǎn)的數(shù)目,NFN為地面點(diǎn)被標(biāo)記為障礙點(diǎn)的數(shù)目,NTN為障礙點(diǎn)正確標(biāo)記的數(shù)目.
城市場(chǎng)景下的算法準(zhǔn)確率如圖8所示,橫坐標(biāo)為連續(xù)幀的編號(hào),縱坐標(biāo)為準(zhǔn)確率.Mesh梯度算法和本文算法都有較高的準(zhǔn)確率.
鄉(xiāng)村起伏道路場(chǎng)景下的算法準(zhǔn)確率如圖9所示,本文算法相比于Mesh梯度算法和局部凹凸性算法,不僅準(zhǔn)確率高于這2種算法,而且穩(wěn)定性也要好于這2種算法.在起伏道路環(huán)境中,地面梯度變化劇烈,基于單點(diǎn)局部特征的算法穩(wěn)定性差不適用于這類(lèi)場(chǎng)景.本文算法通過(guò)多特征全局優(yōu)化,獲得了較高的穩(wěn)定性,更適合鄉(xiāng)村起伏道路場(chǎng)景.
本文算法使用C++實(shí)現(xiàn),在Intel酷睿i5雙核2.8 GHz CPU、2 G內(nèi)存的自主車(chē)平臺(tái)上運(yùn)行時(shí)間
圖8 城市場(chǎng)景下算法準(zhǔn)確率Fig.8 Accuracy of algorithms under urban scene
圖9 鄉(xiāng)村起伏道路場(chǎng)景下算法準(zhǔn)確率Fig.9 Accuracy of algorithms under rural bumpy scene
在2種場(chǎng)景下運(yùn)行100幀數(shù)據(jù)序列,通過(guò)正確率(R)對(duì)不同算法的結(jié)果與人工標(biāo)記的真值進(jìn)行量化比較.為每幀61 ms,其中原始激光雷達(dá)距離數(shù)據(jù)轉(zhuǎn)換成三維點(diǎn)云25 ms,掃描線分割15 ms,馬爾科夫隨機(jī)場(chǎng)建立和求解21 ms,符合實(shí)時(shí)運(yùn)行的要求.
本文提出了一種適用于多類(lèi)場(chǎng)景的實(shí)時(shí)三維激光雷達(dá)地面分割算法.算法首先使用最大模糊線段方法對(duì)三維掃描先在平面上的投影曲線進(jìn)行分割,然后采用角點(diǎn)檢測(cè)算法重新精確定位線段端點(diǎn),最后建立無(wú)向圖馬爾科夫隨機(jī)場(chǎng)并求解標(biāo)記地面區(qū)域.在城市平坦路面和鄉(xiāng)村起伏路面下的實(shí)驗(yàn)結(jié)果表明:本文算法相比于其他算法具有較高的魯棒性,能在起伏路面場(chǎng)景下穩(wěn)定可靠地為自主車(chē)輛提供路面區(qū)域信息.
(
):
[1]GLENNIE C.Calibration and kinematic analysis of the Velodyne HDL-64E S2 Lidar sensor[J].Photogrammetric Engineering and Remote Sensing,2012,78(4):339- 347.
[2]HASELICH M,BING R,PAULUS D.Calibration of multiple cameras to a 3D laser range finder[C]∥International Conference on Emerging Signal Processing Applications(ESPA).Las Vegas:IEEE,2012:25- 28.
[3]楊飛,朱株,龔小謹(jǐn),等.基于三維激光雷達(dá)的動(dòng)態(tài)障礙實(shí)時(shí)檢測(cè)與跟蹤[J].浙江大學(xué)學(xué)報(bào):工學(xué)版,2012,46(9):1565- 1571.YANG Fei,Zhu Zhu,GONG Xiao-jin,et al.Real-time dynamic obstacle detection and tracking using 3D Lidar[J].Journal of Zhejiang University:Engineering Science,2012,46(9):1565- 1571.
[4]KAMMEL S,PITZER B.Lidar-based lane marker detection and mapping[C]∥Intelligent Vehicles Symposium.Eindhoven:IEEE,2008:1137- 1142.
[5]HIMMELSBACH M,HUNDELSHAUSEN F,WUENSCHE H.Fast segmentation of 3D point clouds for ground vehicles[C]∥Intelligent Vehicles Symposium(IV).San Diego:IEEE,2010:560- 565.
[6]GUO C,SATO W,HAN L,et al.Graph-based 2D road representation of 3D point clouds for intelligent vehicles[C]∥Intelligent Vehicles Symposium(IV).Detroit:IEEE,2011:715- 721.
[7]MONTEMERLO M,BECHER J,BHAT S,et al.Junior:The stanford entry in the urban challenge[J].Journal of Field Robotics,2008,25(9):569- 597.
[8]MOOSMANN F,PINK O,STILLER C.Segmentation of 3D lidar data in non-flat urban environments using a local convexity criterion[C]∥Intelligent Vehicles Symposium.Xi’an:IEEE,2009:215- 220.
[9]DOUILLARD B,UNDERWOOD J,KUNTZ N,et al.On the segmentation of 3D LIDAR point clouds[C]∥International Conference on Robotics and Automation(ICRA).Shanghai:IEEE,2011:2798- 2805.
[10]DEBLED RENNESSON I,F(xiàn)ESCHET F,ROUYERDEGLI J.Optimal blurred segments decomposition in linear time[C]∥ Discrete Geometry for Computer Imagery.Berlin Heidelberg:Springer,2005:371- 382.
[11]BESAG J.Spatial interaction and the statistical analysis of lattice systems[J].Journal of the Royal Statistical Society.Series B(Methodological),1974,36(2):192- 236.
[12]BOYKOV Y,VEKSLER O,ZABIH R.Fast approximate energy minimization via graph cuts[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2001,23(11):1222- 1239.
[13]BOYKOV Y,KOLMOGOROV V.An experimental comparison of min-cut/max-flow algorithms for energy minimization in vision[J].IEEE Transactions on Pattern Analysis and Machine Intelligenc e,2004,26(9):1124- 1137.