詳解激光SLAM框架LeGO-LOAM

LOAM存在的問題

LeGO-LOAM全稱為：Lightweight and Groud-Optimized Lidar Odometry and Mapping on Variable Terrain，從標(biāo)題可以看出 LeGO-LOAM 為應(yīng)對(duì)可變地面進(jìn)行了地面優(yōu)化，同時(shí)保證了輕量級(jí)。 LeGO-LOAM是專門為地面車輛設(shè)計(jì)的SLAM算法，要求在安裝的時(shí)候Lidar能以水平方式安裝在車輛上；如果是傾斜安裝的話，也要進(jìn)行位姿轉(zhuǎn)換到車輛上。

而LOAM對(duì)Lidar的安裝方式?jīng)]有要求，即使手持都沒有關(guān)系。 作者的實(shí)驗(yàn)平臺(tái)是一個(gè)移動(dòng)小車（UGA），掛載了一個(gè)Velodyne VLP-16 線激光雷達(dá)，還配有一個(gè)低精度的 IMU；選用的硬件平臺(tái)是 Nvidia Jetson TX2（ARM Cortex-A57 CPU）；整體負(fù)載是 20Kg；移動(dòng)速度為：2.0m/s；測(cè)試場(chǎng)景為：地面不平（比較顛簸）的草地。

圖1 硬件平臺(tái) LOAM框架在這樣的硬件環(huán)境和使用場(chǎng)景中會(huì)存在一些問題：

由于搭載的是嵌入式系統(tǒng)，計(jì)算能力將會(huì)受限，LOAM的計(jì)算需要將難以滿足，致使無法做到實(shí)時(shí)；

如果使用LOAM框架，系統(tǒng)計(jì)算每個(gè)點(diǎn)曲率的處理頻率（數(shù)據(jù)量很大，VLP-16一條線是1800個(gè)點(diǎn)）將難以跟上傳感器的更新頻率；

UGA行駛的路面是非平滑連續(xù)的（運(yùn)動(dòng)是顛簸的），采集的數(shù)據(jù)將會(huì)失真（運(yùn)動(dòng)畸變，勻速運(yùn)動(dòng)模型無法適用于顛簸場(chǎng)景），使用LOAM很難在兩幀之間找到可靠的特征對(duì)應(yīng)。

在噪雜的環(huán)境中操作UGV也會(huì)給LOAM帶來一些挑戰(zhàn)，例如：浮動(dòng)的草叢和擺動(dòng)的樹葉的點(diǎn)云將被誤提取為角點(diǎn)或面點(diǎn)，這些特征是不可靠的，難以在連續(xù)幀之間獲取準(zhǔn)確的匹配，從而會(huì)造成較大的漂移。

LeGO-LOAM的設(shè)計(jì)思路

對(duì)地面點(diǎn)云的配準(zhǔn)主要使用的是面點(diǎn)特征；在分割后的點(diǎn)云配準(zhǔn)主要使用的是邊緣點(diǎn)和面點(diǎn)特征。從中可以看出使用邊緣點(diǎn)的數(shù)量是要遠(yuǎn)小于平面點(diǎn)的數(shù)量，這也是能實(shí)現(xiàn)加速的主要原因。

LeGO_LOAM的軟件系統(tǒng)輸入 3D Lidar 的點(diǎn)云，輸出 6 DOF 的位姿估計(jì)。整個(gè)軟件系統(tǒng)分為 5 個(gè)部分：

第一部分：Segmentation： 這一部分的主要操作是分離出地面點(diǎn)云；同時(shí)對(duì)剩下的點(diǎn)云進(jìn)行聚類，濾除數(shù)量較少的點(diǎn)云簇。

第二部分：Feature Extraction： 對(duì)分割后的點(diǎn)云（已經(jīng)分離出地面點(diǎn)云）進(jìn)行邊緣點(diǎn)和面點(diǎn)特征提取，這一步和LOAM里面的操作一樣。

第三部分：Lidar 里程計(jì)： 在連續(xù)幀之間進(jìn)行（邊緣點(diǎn)和面點(diǎn)）特征匹配找到連續(xù)幀之間的位姿變換矩陣。

第四部分：Lidar Mapping： 對(duì)feature進(jìn)一步處理，然后在全局的 point cloud map 中進(jìn)行配準(zhǔn)。

第五部分：Transform Integration： Transform Integration 融合了來自 Lidar Odometry 和 Lidar Mapping 的 pose estimation 進(jìn)行輸出最終的 pose estimate。

LeGO-LOAM的算法細(xì)節(jié)

圖3 噪聲環(huán)境中一條scan的特征提取過程

A. Segmentation

提取地面點(diǎn)之后，再對(duì)剩下的距離圖像進(jìn)行聚類（分簇），過濾掉點(diǎn)云數(shù)量小于 30 的點(diǎn)云簇，對(duì)保留下來的點(diǎn)云簇分配不同的標(biāo)簽。地面點(diǎn)云屬于特殊的一類點(diǎn)云簇（一開始我們就提取出來了）。對(duì)點(diǎn)云進(jìn)行聚類再處理，可以提高運(yùn)行效率和提取更穩(wěn)定的特征。

例如，小車運(yùn)行于嘈雜的環(huán)境，樹葉將會(huì)產(chǎn)生不可靠的特征，相同的葉子不太可能在連續(xù)兩幀掃描中看到。圖3(a)是原始點(diǎn)云，包含了很多植被點(diǎn)云；經(jīng)過處理之后變成了圖3(b)，只剩下大物體點(diǎn)云，例如：樹干。地面點(diǎn)云將被保留以作進(jìn)一步處理。

此時(shí)，保留下來的每個(gè)點(diǎn)將具備三種屬性：（1）點(diǎn)云的標(biāo)簽；（2）在距離圖像中的行列數(shù)；（3）距離值。

B. Feature Extraction

這一步主要是從地面點(diǎn)云和分割出來的點(diǎn)云中提取特征，與LOAM中的操作一樣。主要要搞清楚幾個(gè)特征點(diǎn)集合的概念。

為了從各個(gè)方向均勻地提取特征，我們將距離圖像水平分割成幾個(gè)相等的子圖像，將360°均勻分成6等分，每一等分的精度是 300 imes 16（因?yàn)閂LP-16線Lidar，一條scan是1800個(gè)點(diǎn)）。

計(jì)算子圖中每一行的點(diǎn)的曲率值，對(duì)曲率值進(jìn)行排序分類，> Cth，分為邊緣點(diǎn)特征；< Cth 分為面點(diǎn)特征。設(shè)置以下幾個(gè)集合(論文中這部分語句相近，以下是我個(gè)人的理解，可能不準(zhǔn)確)：

C. Lidar Odometry

1）Label Matching：LeGO-LOAM對(duì)點(diǎn)云進(jìn)行了聚類分簇，不同的點(diǎn)云簇具有不同的Label。Label 信息可以作為兩幀匹配的約束條件，連續(xù)兩幀之間只有同類標(biāo)簽點(diǎn)云簇才能進(jìn)行配準(zhǔn)。這種方式可以提高配準(zhǔn)的精度和效率。

D. Lidar Mapping

LeGO-LOAM的性能表現(xiàn)

作者為測(cè)試LeGO-LOAM的性能，設(shè)計(jì)了一系列的實(shí)驗(yàn)來對(duì)比LeGO-LOAM和LOAM的表現(xiàn)性能。 作者在戶外的小場(chǎng)景和大場(chǎng)景中，分別對(duì)LeGO-LOAM和LOAM測(cè)試平臺(tái)執(zhí)行激烈和平緩的控制，來查看兩者的建圖效果和效率。 在小場(chǎng)景的激烈運(yùn)動(dòng)過程中：LOAM 會(huì)把草叢、樹葉提取為邊緣點(diǎn)特征（草叢和樹葉是不穩(wěn)定特征的主要來源）；而LeGOU-LOAM會(huì)過濾掉這些不穩(wěn)定的特征，只會(huì)在樹干，地面，臺(tái)階等上提取穩(wěn)定特征。如下圖所示（綠色是邊緣點(diǎn)，粉色是面點(diǎn)）：

在LOAM框架中，劇烈的運(yùn)動(dòng)容易造成點(diǎn)云地圖的發(fā)散，如下圖中(a)LOAM，有三個(gè)樹干。

作者在大場(chǎng)景的都市環(huán)境（是一個(gè)學(xué)校，不同地點(diǎn)的海拔誤差在19m之內(nèi)）里也進(jìn)行多種測(cè)試以驗(yàn)證建圖的精度，有人行道，水泥路，土路和草叢。 LOAM 在人行道上的建圖效果并不好，可能是一端存在樹木葉子的干擾，如下圖所示：

在其他三種場(chǎng)景中，LeGO-LOAM的偏差(終點(diǎn)相對(duì)于初始位置的偏差)表現(xiàn)都要優(yōu)于LOAM。

整個(gè)測(cè)試的結(jié)果如下：

特征點(diǎn)數(shù)量對(duì)比：LeGO-LOAM特征點(diǎn)整體下降幅度超過：29%，40%，68%，72%。

迭代次數(shù)對(duì)比：里程計(jì)的迭代次數(shù)降低了34%，48%。

運(yùn)行時(shí)間對(duì)比：降低了 60%

位姿誤差對(duì)比：LeGOLOAM可以用更少的計(jì)算時(shí)間實(shí)現(xiàn)可比或更好的位置估計(jì)精度。

審核編輯：劉清