基于網(wǎng)格單元的點(diǎn)云降維處理算法

段志國(guó), 吳灝, 侯陽, 王少博, 辛慶山, 張明路

(1.國(guó)網(wǎng)石家莊供電公司, 石家莊 050000; 2.河北工業(yè)大學(xué)機(jī)械工程學(xué)院, 天津 300401)

點(diǎn)云是實(shí)際物體三維信息的統(tǒng)計(jì)無序數(shù)集,包含了物體的三維坐標(biāo)值,顏色值和灰度值[1]。近年來,學(xué)者們針對(duì)點(diǎn)云的預(yù)處理和障礙物包圍盒的建立進(jìn)行了大量的研究。密度聚類是一種無監(jiān)督聚類算法,能在“噪聲”數(shù)據(jù)中識(shí)別出任意形狀的聚類[2-7]但由于密度聚類在去除點(diǎn)云噪聲時(shí)需要對(duì)點(diǎn)云進(jìn)行逐個(gè)遍歷,利用歐式距離進(jìn)行鄰域點(diǎn)判斷收斂時(shí)間較長(zhǎng),不適合海量點(diǎn)云的去噪處理。文獻(xiàn)[8]用K-means聚類算法,根據(jù)點(diǎn)到聚類中心的歐式距離和臨近點(diǎn)曲率變化判斷每個(gè)類中的點(diǎn)是否為噪聲點(diǎn);文獻(xiàn)[9]提出基于改進(jìn)隨機(jī)抽樣一致的點(diǎn)云分割算法實(shí)現(xiàn)點(diǎn)云分割的同時(shí)有效去除噪聲點(diǎn);文獻(xiàn)[10]通過枚舉繞軸旋轉(zhuǎn)的角度,對(duì)比旋轉(zhuǎn)后的包圍盒體積,最終獲得體積最小的包圍盒。雖然已有算法已經(jīng)在一定程度上提高了點(diǎn)云聚類和包圍盒建立的精度,但是仍然存在一些問題。例如,枚舉算法的復(fù)雜度較高,一些算法對(duì)初始值較敏感,依賴法向量、曲率等信息,因此點(diǎn)云處理效果難以把控。

針對(duì)上述問題,在點(diǎn)云分割完成的基礎(chǔ)上,現(xiàn)提出一種高效的點(diǎn)云聚類與包圍盒建立方法。該方法基于二進(jìn)制占用網(wǎng)格對(duì)點(diǎn)云進(jìn)行降維處理,將點(diǎn)云映射到網(wǎng)格單元中實(shí)現(xiàn)不同物體點(diǎn)云的快速聚集。并尋找點(diǎn)云輪廓生成的二進(jìn)制占用網(wǎng)格主方向,基于主方向旋轉(zhuǎn)點(diǎn)云從而建立更加緊致隨動(dòng)的包圍盒。

1 基于二進(jìn)制占用網(wǎng)格的點(diǎn)云聚類

點(diǎn)云分割完成后,需要將不同障礙物的散落點(diǎn)集集合成點(diǎn)云簇,將障礙物點(diǎn)云圖形轉(zhuǎn)化為若干個(gè)對(duì)象。采用二進(jìn)制網(wǎng)格的方式聚類,通過對(duì)點(diǎn)云進(jìn)行降維處理,再將點(diǎn)云映射到網(wǎng)格單元中,查找并分別儲(chǔ)存已占用網(wǎng)格的連通域,從而實(shí)現(xiàn)不同物體點(diǎn)云的快速聚集。

1.1 網(wǎng)格生成與點(diǎn)云儲(chǔ)存

對(duì)點(diǎn)云空間創(chuàng)建二維極坐標(biāo)網(wǎng)格,極坐標(biāo)網(wǎng)格劃分包含扇形區(qū)域和子區(qū)域兩個(gè)部分,首先將極坐標(biāo)區(qū)域劃分為扇形區(qū)域,取扇形圓心角αpl,將極坐標(biāo)平面分割為m個(gè)扇形,公式為

(1)

再繼續(xù)對(duì)每個(gè)扇形區(qū)域劃分子區(qū)域,取最大距離為rmax,步長(zhǎng)距離為rpl,將每一個(gè)扇形沿扇形半徑均等劃分為n份,公式為

(2)

這樣就對(duì)點(diǎn)云空間創(chuàng)建了一個(gè)m×n的極坐標(biāo)網(wǎng)格平面。

網(wǎng)格劃分完畢后,將點(diǎn)云儲(chǔ)存到網(wǎng)格中,由于點(diǎn)云具有三維信息,將點(diǎn)云信息進(jìn)行降維存儲(chǔ),即直接將點(diǎn)的坐標(biāo)(x,y,z)簡(jiǎn)化為(x,y)。原因在于一方面點(diǎn)云聚類并不關(guān)心點(diǎn)云簇在z方向的排列與順序,若兩個(gè)點(diǎn)的x、y值一致而z不同,則視為兩點(diǎn)在z方向疊加,它們屬于同一障礙物的點(diǎn)云;另一方面能夠加快聚類速度,滿足聚類算法在動(dòng)態(tài)變化的環(huán)境中的實(shí)時(shí)性要求。確定點(diǎn)pi=(xi,yi,-)所屬的扇形區(qū)域sa,公式為

(3)

(4)

確定扇形區(qū)域后,再計(jì)算點(diǎn)b(pi)所述的子區(qū)域bb,公式為

(5)

(6)

將各點(diǎn)映射到網(wǎng)格中后,對(duì)網(wǎng)格進(jìn)行賦值。若網(wǎng)格中未包含點(diǎn),則記網(wǎng)格的值為0,其像素顏色為黑色;若網(wǎng)格中包含點(diǎn),則記網(wǎng)格的值為1,其像素顏色為白色。從而生成二進(jìn)制占用網(wǎng)格。這樣就將三維點(diǎn)云處理轉(zhuǎn)化為二維圖像處理,并且二進(jìn)制占用網(wǎng)格組成簡(jiǎn)單,占用空間小,處理難度大大降低。

1.2 二進(jìn)制占用網(wǎng)格膨脹處理

對(duì)二進(jìn)制圖像進(jìn)行圖像膨脹處理,圖像膨脹是將圖像在保持原有形狀不變的情況下,對(duì)圖像的白色部分進(jìn)行擴(kuò)張。其主要目的是將白色區(qū)域內(nèi)部孤立的黑色的像素點(diǎn)去除。定義一個(gè)大小為3×3的正方形的核,其原點(diǎn)位于中心位置。設(shè)輸入二進(jìn)制圖像像素集合為A,核像素集合為B,原點(diǎn)為中心點(diǎn),則圖像膨脹定義為

A⊕B={x|(B)x∩A≠?}

(7)

式(7)中:?為空集。

核的原點(diǎn)從二進(jìn)制圖像的原點(diǎn)出發(fā),使核在圖像上進(jìn)行逐個(gè)像素點(diǎn)的移動(dòng),掃描其3×3范圍內(nèi)的圖像像素點(diǎn)。若核中掃描的所有像素點(diǎn)都為0,則錨點(diǎn)處的像素點(diǎn)記為0,否則記為1。遍歷每一個(gè)像素點(diǎn)即可得到膨脹圖像,與原圖像相比它具有更大且更連續(xù)的白色區(qū)域。

1.3 二進(jìn)制占用網(wǎng)格連通區(qū)域分析

連通區(qū)域分析是一種計(jì)算機(jī)圖像分析與處理的常見方法,其目的是將前景目標(biāo)物體從背景中提取出來以便后續(xù)處理的使用。連通區(qū)域一般是指圖像中具有相同像素且位置相鄰的像素點(diǎn)組成的圖形區(qū)域,它是一個(gè)像素集合。連通區(qū)域分析是指將圖像中的各個(gè)連通區(qū)域找出并標(biāo)記。

在對(duì)二值圖像進(jìn)行膨脹后,得到存放障礙物點(diǎn)云的白色像素塊,下一步是識(shí)別不同障礙物的像素塊并分類儲(chǔ)存,使用連通區(qū)域分析算法中的種子填充法對(duì)二值圖像進(jìn)行進(jìn)一步的處理。其步驟如下。

(1)定義一個(gè)四鄰域的像素相鄰關(guān)系,它表示從目標(biāo)像素點(diǎn)出發(fā),對(duì)其上、下、左、右4個(gè)相鄰的像素點(diǎn)進(jìn)行檢測(cè)。

(2)從二值圖像原點(diǎn)開始,逐個(gè)像素點(diǎn)掃描圖像,直到當(dāng)前掃描像素點(diǎn)p1為白色像素點(diǎn)。對(duì)該白色像素點(diǎn)的位置賦予一個(gè)label,label是從1開始逐漸增大的自然數(shù),被用來對(duì)白色像素點(diǎn)進(jìn)行標(biāo)記。

(3)將p1鄰域內(nèi)的點(diǎn)p2、p3、p4、p5按先后順序放入一個(gè)集合M中,如式(8)所示,從左到右分別代表放入的先后順序,最后放入的點(diǎn)為p5。

M={p2,p3,p4,p5}

(8)

(4)將p5取出進(jìn)行檢測(cè),若p5為白色像素點(diǎn),則對(duì)p5賦予與p1相同的label,并將p5中所有鄰域內(nèi)的點(diǎn)按先后順序加入M;若p5為黑色像素點(diǎn),則舍棄p5并取出p4進(jìn)行檢測(cè)。

(5)重復(fù)步驟(4),直到取出M中所有的點(diǎn)。此時(shí)便得到了label=1的一個(gè)連通區(qū)域。

(6)重復(fù)步驟(2),當(dāng)掃描結(jié)束時(shí),就可以得到圖像中的所有連通區(qū)域。

查找每一label標(biāo)記的點(diǎn)集分別標(biāo)記上不同的顏色,得到不同障礙物的點(diǎn)云簇,點(diǎn)云聚類流程示意如圖1所示。

圖1 點(diǎn)云聚類流程示意圖

2 基于點(diǎn)云主方向旋轉(zhuǎn)的包圍盒建立

2.1 基于二進(jìn)制占用網(wǎng)格點(diǎn)云主方向識(shí)別

點(diǎn)云主方向是指點(diǎn)云數(shù)據(jù)變化最大的朝向,它決定了包圍盒的方向與大小,判定點(diǎn)云主方向關(guān)鍵在于如何得到點(diǎn)云邊框在x-y平面內(nèi)的最大分布?；舴蛑本€檢測(cè)是一種經(jīng)典的直線特征提取技術(shù),它廣泛應(yīng)用于具有輪廓特征的二進(jìn)制占用網(wǎng)格內(nèi),即從圖像中獲取直線。霍夫直線檢測(cè)是通過累加器在特定類型的形狀內(nèi)找到直線,直線候選對(duì)象由累加數(shù)的局部極大值確定?；舴蛑本€檢測(cè)不僅能檢測(cè)出圖像的直線輪廓,還能定位到該直線的位置和角度等。其檢測(cè)方程為

ρ=xcosθ+ysinθ

(9)

式(9)中:ρ與θ為極坐標(biāo)下點(diǎn)的極徑與極角,霍夫直線檢測(cè)的優(yōu)點(diǎn)在于:給定x-y平面中的單個(gè)點(diǎn),那么通過該點(diǎn)的所有直線的集合對(duì)應(yīng)于θ-ρ平面中的一條曲線,且點(diǎn)與曲線的對(duì)應(yīng)關(guān)系是唯一的。兩個(gè)或多個(gè)點(diǎn)形成的一條直線將產(chǎn)生在該線的(θ,ρ)處交叉的一條或多條曲線。因此,檢測(cè)共線點(diǎn)的問題可以轉(zhuǎn)化為在θ-ρ平面內(nèi)找到曲線交點(diǎn)個(gè)數(shù)的問題。

點(diǎn)云主方向的算法流程如下。

(1)提取點(diǎn)云邊框,因?yàn)辄c(diǎn)云是一種類似于空殼的圖像,只反映被拍攝物體的表面信息,且對(duì)于一般障礙物而言,其橫向尺寸變化不大。故去除z方向較大的點(diǎn)云并投影到x-y平面后,可得到點(diǎn)云的邊框圖像。其判定如式(10)所示,保留滿足式(10)的所有點(diǎn)pi。

z(pi)-zmin<0.7(zmax-zmin)

(10)

式(10)中:z(pi)為點(diǎn)的z方向大小。

(11)

(12)

取邊長(zhǎng)為g的正方形建立柵格,則該柵格圖的大小為rx×ry,將點(diǎn)云投影到柵格地圖中將包含點(diǎn)的柵格值記為1,未包含點(diǎn)的柵格值記為0,生成點(diǎn)云邊框的二進(jìn)制占用網(wǎng)格。

(3)讀取點(diǎn)云邊框二進(jìn)制占用網(wǎng)格,掃描其像素值為1的像素點(diǎn),并以柵格邊長(zhǎng)作為霍夫直線檢測(cè)的變換步長(zhǎng),其總檢測(cè)長(zhǎng)度為點(diǎn)云邊框的總周長(zhǎng),則檢測(cè)次數(shù)nt為

nt=2(rx+ry)/g

(13)

(4)對(duì)霍夫空間設(shè)置累加器Num(θ,ρ)并初始化Num(θ,ρ)=0。對(duì)于二進(jìn)制占用網(wǎng)格中每一個(gè)像素點(diǎn)(x,y),在參數(shù)空間中找出所有滿足ρ=xcosθ+ysinθ的(θ,ρ),然后令Num(θ,ρ)=Num(θ,ρ)+1。

(5)統(tǒng)計(jì)所有Num(θ,ρ)的大小,取出其最大值,該情況所對(duì)應(yīng)的直線即為點(diǎn)云主方向?qū)?yīng)直線。計(jì)算點(diǎn)云主方向?qū)?yīng)直線與基坐標(biāo)系的角度α,則α為點(diǎn)云主方向。

計(jì)算點(diǎn)云主方向流程示意如圖2所示。

圖2 計(jì)算點(diǎn)云主方向流程示意圖

2.2 包圍盒頂點(diǎn)確定

在得到了點(diǎn)云主方向以后,可以根據(jù)點(diǎn)云主方向建立包圍盒。這種包圍盒的特點(diǎn)是與障礙物之間貼合緊密,間隙較小,能夠更準(zhǔn)確地描述障礙物的尺寸與朝向。該過程可分為以下步驟進(jìn)行。

(1)計(jì)算障礙物點(diǎn)云集中心位置坐標(biāo)(xc,yc),如式(14)所示,并計(jì)算出以中心點(diǎn)為起點(diǎn)指向點(diǎn)云集各點(diǎn)的向量pi如式(15)所示,其中xmax、xmin、ymax、ymin代表障礙物點(diǎn)云集在x、y、z方向上的最大值和最小值。

(14)

pi=(xi-xc,yi-yc)

(15)

(2)將向量pi繞中心點(diǎn)處z軸旋轉(zhuǎn)-α角得到向量p′i,使點(diǎn)云主方向?qū)ζ浠鴺?biāo)x軸,即

(16)

(3)遍歷旋轉(zhuǎn)后的點(diǎn)云集,得到旋轉(zhuǎn)后點(diǎn)云在x、y、z方向的最大值和最小值x′max、x′min、y′max、y′min、z′max、z′min得到包圍盒的8個(gè)頂點(diǎn)坐標(biāo)box′i,即

(17)

(4)將得到的包圍盒頂點(diǎn)坐標(biāo)box′i通過旋轉(zhuǎn)變換回原點(diǎn)集,得到原點(diǎn)集包圍盒的8個(gè)頂點(diǎn)坐標(biāo)boxi,即

(18)

點(diǎn)云建立包圍盒流程示意如圖3所示。

圖3 點(diǎn)云建立包圍盒流程示意圖

3 實(shí)驗(yàn)結(jié)果及分析

實(shí)驗(yàn)采用RS-LiDAR-M1激光雷達(dá)進(jìn)行環(huán)境采集并獲取點(diǎn)云數(shù)據(jù)視頻,該場(chǎng)景中包含兩個(gè)小型路障,距激光雷達(dá)較近的記為小型路障1,較遠(yuǎn)的記為小型路障2。一輛大型貨車和行人等4個(gè)障礙物,其中行人向著遠(yuǎn)離激光雷達(dá)的方向移動(dòng)。其實(shí)驗(yàn)場(chǎng)景如圖4所示,輸出點(diǎn)云數(shù)據(jù)如圖5所示。

圖4 點(diǎn)云采集實(shí)驗(yàn)場(chǎng)景

圖5 輸出點(diǎn)云數(shù)據(jù)

將地面點(diǎn)云去除,保留障礙物點(diǎn)云。取參數(shù)αpl=0.65°、rmax=200、rpl=0.2。任取點(diǎn)云視頻中的多幀進(jìn)行檢驗(yàn),經(jīng)過二進(jìn)制網(wǎng)格聚類后點(diǎn)云如圖6所示,從圖6中可以看出,該方法能夠有效分辨場(chǎng)景中障礙物的個(gè)數(shù),并且不隨幀數(shù)的變化而出現(xiàn)漏判或多判的情況。

圖6 點(diǎn)云聚類結(jié)果

對(duì)視頻中的各類障礙物進(jìn)行點(diǎn)數(shù)統(tǒng)計(jì)并計(jì)算最大誤差。由于行人隨著時(shí)間的變換距離激光雷達(dá)越來越遠(yuǎn),因此行人點(diǎn)云的點(diǎn)數(shù)會(huì)隨著時(shí)間的變化較大,故只對(duì)靜態(tài)障礙物進(jìn)行統(tǒng)計(jì),從而檢驗(yàn)聚類算法的準(zhǔn)確性。統(tǒng)計(jì)結(jié)果如表1所示。

表1 各障礙物聚類點(diǎn)數(shù)統(tǒng)計(jì)

由表1可以看出,隨著點(diǎn)云幀數(shù)的變化,各靜態(tài)障礙物的點(diǎn)數(shù)變化最大誤差均在5%以內(nèi),說明基于二進(jìn)制占用網(wǎng)格的聚類算法能準(zhǔn)確地將不同障礙物所屬的點(diǎn)集進(jìn)行聚類。此外大型貨車聚類最大誤差為1.58%,而小型障礙物的聚類誤差為2.99%和3.64%,說明該算法對(duì)于點(diǎn)數(shù)越多的障礙物,其聚類效果越好。

基于二進(jìn)制占用網(wǎng)格的聚類算法與傳統(tǒng)歐式聚類算法對(duì)比如表2所示,兩種算法均能檢測(cè)出場(chǎng)景中的4個(gè)障礙物,但基于二進(jìn)制占用網(wǎng)格的聚類方法運(yùn)算更迅速,所需時(shí)間更短。原因在于一方面對(duì)點(diǎn)云采用降低維度的方式進(jìn)行處理,即由三維點(diǎn)云處理轉(zhuǎn)換為二維圖像處理,且二進(jìn)制占用網(wǎng)格是一種非常簡(jiǎn)單的二維圖像;另一方面該算法無需迭代,只需要對(duì)二進(jìn)制占用網(wǎng)格進(jìn)行掃描,大大降低了時(shí)間復(fù)雜度,對(duì)于場(chǎng)景中運(yùn)動(dòng)狀態(tài)變換較快的物體具有良好的實(shí)時(shí)性。兩種算法對(duì)比如表2所示。

表2 二進(jìn)制占用網(wǎng)格聚類與歐式聚類算法性能比較

對(duì)聚類后的障礙物建立包圍盒如圖7所示,其中數(shù)字代表障礙物的標(biāo)號(hào)。從圖7中可以看出,隨著幀數(shù)的改變,包圍盒能始終跟隨并包絡(luò)障礙物點(diǎn)云,且標(biāo)號(hào)保持一致,具有良好的隨動(dòng)性。對(duì)于靜態(tài)障礙物來說,由于主方向不發(fā)生改變,其包圍盒的朝向與大小未發(fā)生改變。而對(duì)于行人這類動(dòng)態(tài)障礙物來說,由于在行走過程中存在位置和速度改變,故點(diǎn)云主方向也隨之變動(dòng),圖7中顯示該包圍盒建立方法能夠?qū)崟r(shí)測(cè)量點(diǎn)云主方向的變換,進(jìn)而改變包圍盒的朝向和大小,使得包圍盒始終緊密貼合行人點(diǎn)云,證明了該方法的有效性。

圖7 障礙物包圍盒建立

基于點(diǎn)云主方向旋轉(zhuǎn)的包圍盒建立算法與AABB包圍盒算法對(duì)比如表3所示,AABB包圍盒是直接以障礙物點(diǎn)集在x、y、z方向上的最大和最小值為頂點(diǎn)建立包圍盒。從表3可以看出,兩類算法的包圍效果與包圍盒高相差不大,但基于點(diǎn)云主方向旋轉(zhuǎn)的包圍盒長(zhǎng)、寬均小于AABB包圍盒算法,證明該方法所建立包圍盒更加緊致,能夠更精確地反映障礙物的大致尺寸,證明了該方法的優(yōu)越性。

4 結(jié)論

提出了基于二進(jìn)制占用網(wǎng)格的點(diǎn)云聚類算法,首先對(duì)點(diǎn)云進(jìn)行降維處理,將三維點(diǎn)云簡(jiǎn)化為二維像素圖像處理,并且對(duì)點(diǎn)云輪廓生成二進(jìn)制占用網(wǎng)格從而尋找點(diǎn)云主方向,基于主方向旋轉(zhuǎn)點(diǎn)云從而建立包圍盒。實(shí)驗(yàn)結(jié)果表明,該方法能夠在保證聚類精度的同時(shí)大大提高運(yùn)算速度,其建立包圍盒能夠更加準(zhǔn)確的反應(yīng)障礙物的尺寸,具有良好的實(shí)時(shí)性與隨動(dòng)性,對(duì)之后的機(jī)械臂自主避障提供了可靠的信息。