一文詳解激光SLAM框架LeGO-LOAM

作者介紹：Zach，移動(dòng)機器人從業(yè)者，熱愛(ài)移動(dòng)機器人行業(yè)，立志于科技助力美好生活。

LOAM存在的問(wèn)題

LeGO-LOAM全稱(chēng)為：Lightweight and Groud-Optimized Lidar Odometry and Mapping on Variable Terrain，從標題可以看出 LeGO-LOAM 為應對可變地面進(jìn)行了地面優(yōu)化，同時(shí)保證了輕量級。

LeGO-LOAM是專(zhuān)門(mén)為地面車(chē)輛設計的SLAM算法，要求在安裝的時(shí)候Lidar能以水平方式安裝在車(chē)輛上；如果是傾斜安裝的話(huà)，也要進(jìn)行位姿轉換到車(chē)輛上。而LOAM對Lidar的安裝方式?jīng)]有要求，即使手持都沒(méi)有關(guān)系。

作者的實(shí)驗平臺是一個(gè)移動(dòng)小車(chē)（UGA），掛載了一個(gè)Velodyne VLP-16 線(xiàn)激光雷達，還配有一個(gè)低精度的 IMU；選用的硬件平臺是 Nvidia Jetson TX2（ARM Cortex-A57 CPU）；整體負載是 20Kg；移動(dòng)速度為：2.0m/s；測試場(chǎng)景為：地面不平（比較顛簸）的草地。

圖1 硬件平臺

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

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

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

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

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

LeGO-LOAM的設計思路

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

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

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

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

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

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

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

LeGO-LOAM的算法細節

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

A. Segmentation

提取地面點(diǎn)之后，再對剩下的距離圖像進(jìn)行聚類(lèi)（分簇），過(guò)濾掉點(diǎn)云數量小于 30 的點(diǎn)云簇，對保留下來(lái)的點(diǎn)云簇分配不同的標簽。地面點(diǎn)云屬于特殊的一類(lèi)點(diǎn)云簇（一開(kāi)始我們就提取出來(lái)了）。對點(diǎn)云進(jìn)行聚類(lèi)再處理，可以提高運行效率和提取更穩定的特征。例如，小車(chē)運行于嘈雜的環(huán)境，樹(shù)葉將會(huì )產(chǎn)生不可靠的特征，相同的葉子不太可能在連續兩幀掃描中看到。圖3(a)是原始點(diǎn)云，包含了很多植被點(diǎn)云；經(jīng)過(guò)處理之后變成了圖3(b)，只剩下大物體點(diǎn)云，例如：樹(shù)干。地面點(diǎn)云將被保留以作進(jìn)一步處理。此時(shí)，保留下來(lái)的每個(gè)點(diǎn)將具備三種屬性：（1）點(diǎn)云的標簽；（2）在距離圖像中的行列數；（3）距離值。

B. Feature Extraction

這一步主要是從地面點(diǎn)云和分割出來(lái)的點(diǎn)云中提取特征，與LOAM中的操作一樣。主要要搞清

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

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

C. Lidar Odometry

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

D. Lidar Mapping

LeGO-LOAM的性能表現

作者為測試LeGO-LOAM的性能，設計了一系列的實(shí)驗來(lái)對比LeGO-LOAM和LOAM的表現性能。

作者在戶(hù)外的小場(chǎng)景和大場(chǎng)景中，分別對LeGO-LOAM和LOAM測試平臺執行激烈和平緩的控制，來(lái)查看兩者的建圖效果和效率。

在小場(chǎng)景的激烈運動(dòng)過(guò)程中：LOAM 會(huì )把草叢、樹(shù)葉提取為邊緣點(diǎn)特征（草叢和樹(shù)葉是不穩定特征的主要來(lái)源）；而LeGOU-LOAM會(huì )過(guò)濾掉這些不穩定的特征，只會(huì )在樹(shù)干，地面，臺階等上提取穩定特征。如下圖所示（綠色是邊緣點(diǎn)，粉色是面點(diǎn)）：

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

作者在大場(chǎng)景的都市環(huán)境（是一個(gè)學(xué)校，不同地點(diǎn)的海拔誤差在19m之內）里也進(jìn)行多種測試以驗證建圖的精度，有人行道，水泥路，土路和草叢。

LOAM 在人行道上的建圖效果并不好，可能是一端存在樹(shù)木葉子的干擾，如下圖所示：

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

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

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

迭代次數對比：里程計的迭代次數降低了34%，48%。

運行時(shí)間對比：降低了 60%

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

參考資料

https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

備注：作者也是我們「3D視覺(jué)從入門(mén)到精通」特邀嘉賓：一個(gè)超干貨的3D視覺(jué)學(xué)習社區