基于體素柵格濾波的點云地圖處理方法

李皓，范玉卿，劉豪杰

（中國礦業(yè)大學(xué)（北京）機電與信息工程學(xué)院，北京，100083）

0 引言

激光SLAM以激光雷達作為主要傳感器[1]，由于激光雷達掃描得到的激光點云僅攜帶深度信息，因此應(yīng)用激光SLAM算法生成的地圖對環(huán)境信息的表達十分有限。為了方便幀間匹配，一些3D激光SLAM算法[2-3]會從激光點中提取帶有明顯幾何特征的點，并將這些特征點應(yīng)用到最后的建圖算法中去。在建圖算法中，常使用濾波算法減少地圖中點云的數(shù)量以降低數(shù)據(jù)存儲壓力，這就導(dǎo)致最后生成的地圖中的點云更加稀疏。隨著多傳感器融合SLAM算法[4-7]的發(fā)展，激光雷達掃描得到的激光點或激光SLAM算法構(gòu)建的點云圖被應(yīng)用到視覺SLAM圖像像素深度值的獲取上。ChenMengxiao[8]等人提出使用2D激光雷達獲取3D點云并與圖像關(guān)鍵幀融合，利用基于相機的視覺SLAM回環(huán)進行后端優(yōu)化，建立全局地圖。ZhangJi[9]等人提出了DEMO，將激光里程計部分使用激光雷達獲取的周圍環(huán)境的點云圖用于視覺里程計部分為部分圖像像素特征點提供深度信息。Johannes Graeter[10]等人提出了LIMO，使用單幀激光點云數(shù)據(jù)提取圖像特征點的深度。Jason Ku[11]等人提出使用激光雷達數(shù)據(jù)為部分圖像像素提供深度值，并使用圖像處理算法進行深度補全。在激光點云深度信息與圖像信息融合構(gòu)建地圖時，如果建立稀疏地圖，激光點云深度圖的稀疏性對建圖效果影響有限，算法整體上計算量不大，可以實時運行；在建立稠密地圖時，時效性并非首選項[12-13]，因此可以相應(yīng)增加激光點云處理量，提高點云密度，從而提供更加稠密的點云地圖。以LOAM算法[3]為例，利用該算法為圖像像素提供深度信息，在對激光點云進行處理時，使用體素柵格濾波算法，通過設(shè)置合理的體素大小，在不考慮實時性建圖的前提下，達到稠密化點云深度地圖的目的，為圖像提供更多的深度信息。

1 LOAM算法與體素柵格濾波

■1.1 LOAM算法的實時性計算優(yōu)化

LOAM是一種實時的激光雷達里程計與建圖算法。在地圖漂移的問題上，LOAM優(yōu)先考慮算法實時性，選擇了在線方式解決。該算法將定位與建圖過程分離，其中里程計算法計算頻率高但精度低，目的是通過快速計算找到相鄰幀特征點之間的對應(yīng)關(guān)系，建圖算法計算頻率低但精度高，通過檢測局部點云簇的幾何分布，計算相關(guān)特征值與特征向量確定對應(yīng)關(guān)系，得到高精度位姿與點云地圖。定位與建圖相分離、高低精度搭配的方式降低了計算復(fù)雜度，使算法可以實時運行，緩解地圖漂移問題，得到理想的機器人位姿與點云地圖。

為了減少計算復(fù)雜度，保證算法實時性，LOAM主要采取了兩種方式：限制特征點的提取數(shù)量、使用體素網(wǎng)格方法進行下采樣。在激光里程計算法中，提取完特征點后對less_flat點進行體素柵格濾波，減少需要處理的點云的數(shù)量。在建圖算法中，使用體素柵格濾波對角點、平面點、地圖進行了下采樣，減小了點云密度，提高了算法運行速度。

激光雷達與相機融合建立致密的環(huán)境地圖或進行三維重建時，算法實時性并非必需選項，離線的處理方式可以提供足夠的時間對數(shù)據(jù)進行處理和優(yōu)化，從而取得更好的定位與建圖效果。因此合理配置體素柵格大小，在不考慮算法實時性的基礎(chǔ)上，讓激光SLAM部分提供更加致密的深度信息，是一個值得研究的方向。

■1.2 體素柵格濾波算法

體素是三維的一個個小立體空間，將點云數(shù)據(jù)劃分出一個個3D體素，在每個體素內(nèi)，用重心近似所有的點。這種方法可以減少點云的數(shù)量，減小計算復(fù)雜度，保持點云的形狀特征，在提高配準(zhǔn)、曲面重建、形狀識別等方面有著重要作用[14]。

體素柵格濾波通過Voxel_Grid函數(shù)實現(xiàn)。當(dāng)接收到點云坐標(biāo)數(shù)據(jù)后，首先求得X、Y、Z坐標(biāo)軸上最大值xmax、ymax、zmax和最小值xmin、ymin、zmin。然后根據(jù)各坐標(biāo)軸上的最大值和最小值求出能夠包圍點云數(shù)據(jù)的最小體積的各邊長lx、ly、lz。

給定體素柵格大小v,則X、Y、Z坐標(biāo)軸可以等分為L、M、N份，則一共產(chǎn)生L·M·N個體素柵格。

對所有體素柵格進行編號，記為（i, j, k）,將每個數(shù)據(jù)點劃分到所屬體素柵格內(nèi)。

計算每個體素柵格的重心，記為cijk，以重心代替該體素內(nèi)所有點云數(shù)據(jù)。當(dāng)重心不存在時，以體素內(nèi)在距離上最接近重心點的點云數(shù)據(jù)代替體素內(nèi)所有數(shù)據(jù)。

其中，n為體素柵格內(nèi)數(shù)據(jù)點個數(shù)，ip為數(shù)據(jù)點。

經(jīng)以上步驟，點云數(shù)據(jù)完成體素柵格濾波過程。

2 實驗與分析

在使用體素柵格濾波進行下采樣時，體素的大小選擇很重要，過大容易出現(xiàn)點云失真，無法準(zhǔn)確還原環(huán)境的特征。在對LOAM算法的一些改進中，下采樣時多采取較大的體素，雖然提高了算法實時性，卻損失了一些有利用價值的深度信息。本實驗分別選取不同邊長的體素，以里程計算法中處理完成的角點、平面點以及最終點云深度地圖為下采樣對象。設(shè)定變量T=（c,f,m），其中c、f、m分別代表角點、平面點、點云地圖的體素大小。體素選取值為20cm、10cm、5cm、2cm，分別觀察LOAM算法在室外大場景、走廊以及室內(nèi)小場景中執(zhí)行下采樣后的點云數(shù)量以及建圖算法（Lasermpping）的CPU占用率峰值。里程計算法中l(wèi)ess_flat點下采樣體素大小不變，設(shè)定為2cm。

實驗所用激光雷達為Velodyne公司生產(chǎn)的VLP-16，可以直接獲取3D激光雷達點云數(shù)據(jù)，具體參數(shù)如表1所示。實驗在PC端進行,程序基于Ubuntu16.04上的ROS系統(tǒng)。PC所用CPU為Inter(R) Core(TM) i7-756U @3.5GHz，4核8線程。實驗所需數(shù)據(jù)為提前錄制的數(shù)據(jù)包，在運行LOAM算法時用0.5倍速播放。

表1 Velodyne VLP-16參數(shù)

如圖1～圖3所示，在室內(nèi)、走廊兩種小場景以及室外大場景的實驗中，隨著體素不斷減小，地圖上的深度點不斷增多，最大可增加54.87倍。結(jié)合表2至表4可以發(fā)現(xiàn)，在數(shù)據(jù)包0.5倍速播放的情況下，體素越小，建圖算法CPU最大占用率越大，最大接近100%，由此可以得出，當(dāng)數(shù)據(jù)包以正常速度播放時，CPU占用率將超過100%，此時LOAM算法將不再具有實時性。

表2 室外實驗結(jié)果

圖1 室內(nèi)環(huán)境深度地圖

圖3 室外環(huán)境深度地圖

在表4中，當(dāng)體素邊長由10cm變?yōu)?cm時，建圖算法CPU最大占用率沒有明顯上升，點云數(shù)量卻增加了54%，這是由于體素設(shè)置過小，點云總體數(shù)量過大，體素數(shù)量超過Voxel_Grid函數(shù)設(shè)定值，導(dǎo)致數(shù)據(jù)溢出。因此在大場景中使用體素柵格濾波時，需要根據(jù)實際情況設(shè)定體素大小，或?qū)Ⅻc云分塊處理，避免數(shù)據(jù)溢出情況的發(fā)生。

表3 走廊實驗結(jié)果

表4 室內(nèi)實驗結(jié)果

在圖1和圖2中，當(dāng)體素大小設(shè)定為20cm時，可以觀察到點云地圖深度信息較為稀疏，無法真實反映出環(huán)境特征。當(dāng)體素逐漸變小，環(huán)境特征逐漸清晰，此時并未發(fā)生下采樣數(shù)據(jù)溢出，因此在小場景中，可以適當(dāng)減小體素大小，獲得更加豐富的環(huán)境深度信息。

圖2 走廊環(huán)境深度地圖

將以上實驗結(jié)果應(yīng)用在融合算法中，設(shè)定體素大小，使用體素柵格濾波算法對LOAM算法得到的點云地圖進行處理，為圖像像素提供深度信息，然后輸出帶有色彩信息的點云地圖。如圖4為實驗所選取的小場景環(huán)境，其中圖4(a)為室內(nèi)環(huán)境，圖4(b)為走廊環(huán)境。在兩種環(huán)境中分別用邊長為20cm、5cm的體素對LOAM建圖效果進行處理，并將5cm體素處理后的結(jié)果用于融合算法建圖。

圖4 小場景實驗環(huán)境

實驗結(jié)果如圖5、圖6所示。其中(a)為20cm體素處理后的LOAM建圖效果、(b)為5cm的體素處理后的LOAM建圖效果，(c)為融合算法建圖效果。

通過對比可以發(fā)現(xiàn)，圖5雖然是室內(nèi)環(huán)境，激光雷達無需掃描遠距離物體，各掃描點之間的夾角相對較小，由于使用了較大體素，點云圖對環(huán)境輪廓的表達不夠清晰，從圖5(a)無法判斷出物體的輪廓。圖像圖5(b)由于使用了較小體素，從點云圖中可以相對清晰地看出門框、書架輪廓，經(jīng)融合算法賦予色彩信息后，能夠比較清楚地識別門、墻壁、標(biāo)定板等物體的形狀、顏色。

圖5 室內(nèi)環(huán)境建圖效果

走廊環(huán)境建圖效果如圖6所示。圖6(a)僅能依稀辨別走廊的柱狀輪廓，對走廊中存在的門窗等物體表達模糊；經(jīng)小體素處理后，圖6(b)中點云數(shù)量足以顯示走廊中門窗的形態(tài)，對于走廊整體輪廓的表達更加清晰；將圖6(b)中結(jié)果用融合算法處理，賦予門窗、地板顏色信息，走廊盡頭的壁掛電視清晰可見，整體上對走廊環(huán)境信息的表達更加豐富。

圖6 走廊環(huán)境建圖效果

3 結(jié)語

選擇不同的體素大小，會給激光點云地圖帶來不同的顯示效果，較小的體素可以顯著增加激光點云地圖的密度，使地圖對環(huán)境信息的表達更加細膩，在激光雷達與相機的數(shù)據(jù)融合算法中可以為更多圖像像素提供深度信息。相比于大場景，使用體素柵格濾波算法在走廊、室內(nèi)等小場景中對于點云密度的增加更明顯，且不容易發(fā)生體素數(shù)量溢出的情況。較小的體素會增加處理器運算量，可能會出現(xiàn)無法實時建圖的情況，因此需要在算法實時性與點云密度之間進行選擇。