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

李皓，范玉卿，劉豪杰

（中國礦業大學（北京）機電與信息工程學院，北京，100083）

0 引言

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

1 LOAM算法與體素柵格濾波

■1.1 LOAM算法的實時性計算優化

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

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

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

■1.2 體素柵格濾波算法

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

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

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

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

計算每個體素柵格的重心，記為cijk，以重心代替該體素內所有點云數據。當重心不存在時，以體素內在距離上最接近重心點的點云數據代替體素內所有數據。

其中，n為體素柵格內數據點個數，ip為數據點。

經以上步驟，點云數據完成體素柵格濾波過程。

2 實驗與分析

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

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

表1 Velodyne VLP-16參數

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

表2 室外實驗結果

圖1 室內環境深度地圖

圖3 室外環境深度地圖

在表4中，當體素邊長由10cm變為5cm時，建圖算法CPU最大占用率沒有明顯上升，點云數量卻增加了54%，這是由于體素設置過小，點云總體數量過大，體素數量超過Voxel_Grid函數設定值，導致數據溢出。因此在大場景中使用體素柵格濾波時，需要根據實際情況設定體素大小，或將點云分塊處理，避免數據溢出情況的發生。

表3 走廊實驗結果

表4 室內實驗結果

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

圖2 走廊環境深度地圖

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

圖4 小場景實驗環境

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

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

圖5 室內環境建圖效果

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

圖6 走廊環境建圖效果

3 結語

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