九種3D Lidar-SLAM算法評估

當機器人處在照明條件不足且無法使用GPS的地下（SubT）環(huán)境中，其自主導航是一項極具挑戰(zhàn)性的任務(wù)，這也促進了姿勢估計和建圖算法的研究。

受在此類環(huán)境中實際部署自主機器人需求的啟發(fā)，本文介紹了3D SLAM算法的實驗比較研究。該研究側(cè)重于具有開源實現(xiàn)的最先進的激光雷達SLAM算法，這些算法是

i）僅激光雷達，如BLAM，LOAM，A-LOAM，ISC-LOAM和HDL圖SLAM

ii）激光雷達慣性，如LeGO-LOAM，Cartographer，LIO-mapping和LIO-SAM。

這些方法的評估是在地下隧道執(zhí)行任務(wù)期間從配備3D激光雷達Velodyne Puck Lite和IMU Vectornav VN-100的Boston Dynamics Spot機器人收集的數(shù)據(jù)集進行的。在評估過程中，將機器人位姿和SLAM算法的3D隧道重建相互比較，以找到在位姿精度和地圖質(zhì)量方面性能最可靠的方法。

01介紹

同時定位與地圖構(gòu)建（SLAM）是一項具有挑戰(zhàn)性的任務(wù)，它解決了未知環(huán)境下機器人自主導航的問題。在SLAM過程中，機器人使用車載感知和慣性傳感器獲取環(huán)境地圖，同時嘗試將自己定位在該地圖內(nèi)。魯棒姿態(tài)估計是移動機器人控制的一項關(guān)鍵任務(wù)。在實驗室環(huán)境中，它可以通過動作捕捉系統(tǒng)來解決。但并無法滿足將其部署到機器人導航的任何目標區(qū)域。因此，機器人的目標之一是能夠獨立于基礎(chǔ)設(shè)施進行自主導航。在戶外、開闊地帶，機器人可以使用全球定位系統(tǒng)（GPS）進行姿態(tài)估計。

然而，在地下區(qū)域等GPS無效的環(huán)境中，這種解決方案將不起作用，盡快有時可以選擇安裝額外的設(shè)備，但是這并不是適用于任何情況的。目前，已經(jīng)有了基于相機和測距傳感器的SLAM算法，不過，這些算法通常需要快速移動計算機來提供實時地圖構(gòu)建和姿態(tài)估計。

隨著便攜CPU、傳感器小型化和長續(xù)航電池的技術(shù)發(fā)展，SLAM算法可以在板上運行，這有助于將機器人應(yīng)用帶入具有挑戰(zhàn)性的SubT環(huán)境。在這些環(huán)境中，關(guān)鍵因素是人類安全，可以通過機器人進行自主檢查來提高人類安全，并通過提供環(huán)境重建來提高人類工人對環(huán)境的態(tài)勢感知。這對地圖繪制質(zhì)量和定位精度提出了很高的要求。

在照明較差的SubT環(huán)境中，視覺SLAM方法往往表現(xiàn)出較差的性能，這是不可接受的。與之相反，基于激光雷達的方法可以在姿態(tài)估計和環(huán)境地圖顯示方面提供可靠的性能。然而，由于SubT環(huán)境的特殊性，如無特征、自相似的隧道區(qū)域、多塵隧道和傳感器限制，它們的性能可能會隨著時間的推移而下降。

在本研究中，我們將在地下環(huán)境中對九種開源的ROS兼容的3D Lidar-SLAM算法進行實驗評估。

這項工作的主要貢獻是：

（1）評估了九種基于SOTA激光雷達的3D SLAM方法，使用SubT數(shù)據(jù)集來證明它們在此類環(huán)境中的性能。

（2） 對所有方法的姿態(tài)估計和生成的環(huán)境3D圖進行定量和定性比較，這將使機器人開發(fā)團隊易于評估和理解其優(yōu)缺點，包括為該應(yīng)用選擇SLAM算法框架。

注：評估數(shù)據(jù)集是在波士頓動力點的勘探任務(wù)期間沿著一個有多個隧道的地下區(qū)域收集的。車載傳感器套件由Velodyne Puck Lite激光雷達和Vectornav vn-100 IMU組成，前者是一種用于自主導航的SoA傳感器，后者是多個機器人系統(tǒng)中常用的硬件，與SubT研究工作高度相關(guān)。

02SLAM算法

在這項研究中，我們選擇了所有主流的SLAM算法，這些算法具備實時操作能力，并使用3D激光雷達的點云作為輸入，也可以將其與IMU測量相耦合。本章節(jié)先介紹了機器人平臺，其次介紹了SubT環(huán)境，最后介紹了SLAM方法。所有選擇的方法都用ROS Melodic和Ubuntu 18.04進行了測試。

2.1 機器人平臺和數(shù)據(jù)集

使用了由波士頓動力公司開發(fā)的機器人平臺用于數(shù)據(jù)收集。該機器人能夠以高達1.6米/秒的速度移動，同時攜帶高達14公斤的有效載荷，并穿越具有挑戰(zhàn)性的地形。在Spot的頂部放置的有效載荷包括3D激光雷達、IMU、spotCore和電池，如圖1所示。板載計算機采用Intel Core i5 CPU，16GB RAM，Ubuntu 18.04和ROS Melodic。為了為3D激光雷達提供暢通無阻的視野，它被安裝為一個柱狀結(jié)構(gòu)，位于前端，帶有獨立的LED燈條Lustreon DV12V 10W，尺寸為170×15 mm，指向前、左、右。

數(shù)據(jù)集是從瑞典Lulea港口的地下隧道收集的，該隧道具有手動控制的Spot，如圖2所示。它被記錄在一次通過中，將來自IMU和3D激光雷達的測量結(jié)果存儲在ROS bag文件1中。3D重建地圖的俯視圖如圖2所示。數(shù)據(jù)是用圖1所示的傳感器配置收集的，IMU發(fā)布速率設(shè)置為200Hz，3D激光雷達發(fā)射速率設(shè)為10Hz。在SubT環(huán)境中獲得基本事實是一項具有挑戰(zhàn)性的任務(wù)，因此在本研究中只提供了算法的相對比較。

圖2 3D激光雷達生成的地圖俯視圖（數(shù)據(jù)從現(xiàn)場測試環(huán)境中掃描而來）。實線表示穿過隧道的導線，而箭頭表示導線的方向。

在下面的小節(jié)中，我們將簡要介紹所選擇的算法。

2.2 BLAM

Berkeley localization and mapping（BLAM）是一個開源ROS包，用于基于圖的激光SLAM。它通過使用附近姿勢的掃描進行迭代最近點（ICP）掃描匹配來計算環(huán)路。對于地圖優(yōu)化，它使用Georgia技術(shù)平滑和建圖（GTSAM）庫。BLAM能夠在線構(gòu)建非常稠密和精確的地圖，這也使得它成為一種計算成本高昂的方法Nava Chocron（2019）。

2.3 LOAM

Laser Odometry and Mapping（LOAM）或LOAM velodyne是一種實時方法，能夠同時估計里程計并使用3D激光雷達繪制地圖。該方法通過將SLAM任務(wù)分解為兩種算法來解決SLAM任務(wù)。一個用于計算里程計，另一個用于增量地圖構(gòu)建，此外它還估計激光雷達的速度。LOAM沒有閉環(huán)檢測，這使它無法識別以前訪問過的區(qū)域，相反，它實現(xiàn)了特征點匹配，從而確?？焖俚睦锍逃嬘嬎愫蜏蚀_的地圖構(gòu)建。該方法具有IMU支持，與僅使用激光雷達相比，可以獲得更高的精度。

A-LOAM是LOAM的高級實現(xiàn)，它使用特征庫進行線性代數(shù)運算，并使用Ceres Solver 解決相應(yīng)的優(yōu)化問題。

Fast LiDAR Odometry and Mapping（F-LOAM）是LOAM和ALOAM的優(yōu)化版本，其基于非迭代兩階段畸變補償方法，可降低計算時間。F-LOAM結(jié)合了特征提取、畸變補償、姿態(tài)優(yōu)化和建圖。

2.4 ISC-LOAM

Intensity Scan Context based Full SLAM Implementation（ISC-LOAM）是另一種為3D激光雷達設(shè)計的算法。它結(jié)合了一個全局描述子，該描述子包含幾何體和強度特征。所提出的閉環(huán)檢測方法基于用于位置識別的兩階段分層強度掃描上下文（ISC），這可以提高計算性能。ISC結(jié)合了基于快速二進制運算的幾何索引和強度結(jié)構(gòu)重新識別。

2.5 hdl graph slam

hdl graph slam是一個開源ROS包，用于與3D激光雷達SLAM。該方法基于Pose Graph SLAM，其中閉環(huán)檢測是基于連續(xù)幀之間的正態(tài)分布變換（NDT）掃描匹配。與其他算法相比，無損檢測方法在三維激光雷達應(yīng)用中具有更好的掃描匹配性能。其中，無跡卡爾曼濾波器用于姿態(tài)估計。

2.6 LeGO-LOAM

Lightweight and ground optimized lidar odometry and mapping（LeGO LOAM）是一種用于在地形變化復雜的環(huán)境中使用無人車進行姿態(tài)估計的實時建圖方法。它通過進行點云分割來利用地面分離，這可以過濾表示不可靠特征的點。LeGOLOAM使用兩步優(yōu)化進行姿態(tài)估計。在第一步中，從地面提取平面特征以獲得z、滾轉(zhuǎn)、俯仰，在第二步中，通過匹配從點云提取的特征來獲得剩余的x、y、偏航。該方法還支持使用ICP實現(xiàn)的環(huán)路檢測。

2.7 Cartographer

Cartographer是谷歌開發(fā)的一個系統(tǒng)，用于實時同時定位和構(gòu)建2D和3D地圖。它支持獨特的傳感器配置。對于3D SLAM，它需要具有初始猜測所需的IMU數(shù)據(jù)，以確定激光雷達掃描的方向。在最佳姿態(tài)估計下，它從激光雷達中獲取掃描結(jié)果，并將其轉(zhuǎn)換為概率網(wǎng)格，用于構(gòu)建子地圖。最近完成的子地圖構(gòu)建和掃描可以通過掃描匹配來進行回環(huán)檢測。掃描匹配依賴于branch-and-boun算法。Cartographer將局部和全局SLAM方法分別結(jié)合起來。在局部SLAM中，Ceres匹配器用于找到與子地圖最佳匹配的姿勢。這個過程中慢慢積累的誤差，可以通過基于稀疏姿態(tài)調(diào)整（SPA）的閉環(huán)機制進行消除。

2.8 LIO-mapping

LIO-mapping是一種用于3D姿態(tài)估計和建圖的實時方法。在這種方法中，IMU與激光雷達緊密耦合，共同降低激光雷達和IMU測量的成本。該方法使用滑動窗口方法，通過在窗口中包括新的姿態(tài)估計和棄用最舊的姿態(tài)估計來限制計算數(shù)量。盡管如此，LIO-mapping仍然是一種計算成本很高的實時導航方法，使用16線3D激光雷達同時進行里程估計和建圖需要超過0.2秒的時間。

2.9 LIO-SAM

LIO-SAM是一個實時緊耦合激光雷達慣性里程計工具包，由LeGoLOAM構(gòu)建，是一種基于ICP的方法。這種方法會構(gòu)建一個因子圖，使得加入額外的傳感器變得容易。在其實現(xiàn)中，LIO-SAM在貝葉斯樹的增量平滑和映射方法中添加了IMU預集成。值得注意的是，這種方法處理數(shù)據(jù)的速度是實時數(shù)據(jù)的13倍。

2.10 FAST-LIO

FAST-LIO是一個激光雷達慣性里程計框架，其中使用緊耦合迭代卡爾曼濾波器將激光雷達特征點與IMU測量值融合。在這種方法中，從激光雷達點云中提取平面和邊緣特征，在下一步，這些特征與IMU測量一起用于狀態(tài)估計，然后使用估計的姿態(tài)將特征點匹配到全局幀中并更新全局地圖。

FAST-LIO的全局幀被定義為第一個IMU的幀。這意味著IMU在一開始的x-y-z軸將是全局幀的x-y-z軸。所以機器人的方向可能不像你想象的那樣。

表1總結(jié)了所有SLAM方法、所需硬件及其功能，下一步是在SLAM方法上運行記錄的數(shù)據(jù)。

03實驗評估和討論

在本節(jié)中，基于第2.1小節(jié)中引入的收集數(shù)據(jù)集，在SubT環(huán)境中對SLAM方法進行評估。

SLAM方法的基準測試是在配備Intel i7第9代CPU、64GB RAM、Ubuntu 18.04和ROS Melodic的計算機上進行的。對于算法的比較，我們已經(jīng)盡了最大努力，根據(jù)自定義的package給定硬件配置調(diào)整所有方法。

3.1 軌跡的評估和比較

從地下隧道收集的數(shù)據(jù)集代表了基于激光雷達的SLAM算法在缺乏特征、重復性和窄尺寸方面具有挑戰(zhàn)性的環(huán)境，而另外兩個閉環(huán)可以用來評估方法的回環(huán)檢測性能。我們盡最大努力對所有SLAM方法進行了徹底調(diào)整，然而，根據(jù)多次評估，我們得出結(jié)論，VLP16-Lite的視場不足以在垂直維度上捕獲足夠的數(shù)據(jù)，這導致z軸的高度不確定性，即使使用IMU也無法補償，如圖3所示。

根據(jù)我們用DeWALT激光水準儀從隧道入口到隧道中心50米的坡度測量，我們得出隧道具有正坡度，傾角等于0.95度。然而，即使在隧道的直線部分，也沒有一種備選的方法能夠正確估計傾角，例如，對于Cartographer來說，傾角估計約為2.91度，對于hdl graph slam來說，傾角約為0.1度，對于LIO-SAM來說，傾角大約為-1.4度，對于Fast LIO來說，傾角為-3.7度。值得注意的是，除Cartographer和hdl graph slam外，所有方法都估計了負斜率，這可能意味著其它的融合IMU數(shù)據(jù)的方法強烈依賴于激光雷達傳感器，而不是IMU。

與垂直維度類似，在水平維度中，VLP16-Lite具有高數(shù)據(jù)冗余，這允許所有方法來處理x和y 2D姿態(tài)估計，如圖4所示。

從圖2可以看出，所有SLAM方法估計的2D姿態(tài)與隧道地圖一致?？偟膩碚f，所有算法都能夠估計隧道沿線的po-e，hdl graph slam作為一個明顯的異常值。我們將在下一小節(jié)中繼續(xù)進行這一分析，我們將在其中評估生成的地圖。對于相關(guān)方法的評估，我們計算并比較了總行駛距離，如表2所示，并計算了[251:20 256:66]米的置信區(qū)間，置信水平為95%。由此可以看出，BLAM、A-LOAM、LeGO LOAM、Cartographer和FastLIO距離測量具有更值得信賴的價值。

3.2 點云的評估與比較

建圖結(jié)果可以看到有環(huán)路閉合和沒有環(huán)路閉合的方法之間的明顯差異，這可能從定位結(jié)果中不那么明顯。在圖5中，描述了每個算法生成的地圖。基于此，可以說loam velodyne、A-loam、F-loam和LIO-mapping中缺乏閉環(huán)，導致點云建圖錯位。根據(jù)這組方法，F(xiàn)astLIO雖然沒有循環(huán)閉合功能，但可以有效地處理環(huán)境并生成正確的建圖。

在實現(xiàn)了回環(huán)檢測能力的方法中，可以說除了BLAM之外的所有方法都可以提供正確的建圖。hdl graph slam雖然能夠產(chǎn)生正確的建圖，但在姿態(tài)估計中仍然是一個異常值，盡管這一發(fā)現(xiàn)可能是由于我們調(diào)整的潛在缺陷。根據(jù)點云配準方法的不同，在默認配置中，SLAM方法會產(chǎn)生以下數(shù)量的點，如表2所示，從中我們可以看出，存儲地圖的內(nèi)存占用最少的方法是ISC-LOAM，而Fast LIO是需要最大的內(nèi)存。盡管如此，地圖中的總點數(shù)可以通過額外的方法配置來減少，例如在圖5中，LIO-SAM方法的地圖大小減少到4820點。

因此，如果我們在隧道的連接區(qū)域重新繪制圖4，并只保留在圖6中所示的在姿態(tài)估計和建圖中提供可靠結(jié)果的方法，可以說所有方法都表現(xiàn)出了良好的穩(wěn)定性和性能。然而，正如可以注意到的那樣，在交界區(qū)域，LeGO LOAM執(zhí)行了環(huán)路檢測，因此對地圖進行了校正，但沒有對軌跡進行校正。值得注意的是，IMU會影響方法的性能，因為Fast LIO與具有回環(huán)檢測的方法的性能相同。

04結(jié)論

在本文中，我們比較了最新的和SoA激光雷達SLAM方法在苛刻的SubT環(huán)境中的性能。為此，我們使用配備了自主軟件包的Spot機器人收集了數(shù)據(jù)集，并分別配置了所有SLAM方法?；诒疚闹羞M行的SLAM方法比較，可以得出結(jié)論，所配備的3D激光雷達不足以進行3D姿態(tài)估計，由于缺乏特征，導致z軸上的顯著漂移。

因此，我們的分析集中在2D姿態(tài)比較上。評估結(jié)果表明，BLAM、A-LOAM、LeGO LOAM、Cartographer和Fast LIO比其他方法產(chǎn)生了更可信的結(jié)果。盡管與Fast LIO相比，生成的地圖非常稀疏，但ISC-LOAM是用于地圖存儲的內(nèi)存成本最低的方法。此外，可以得出結(jié)論，將IMU與激光雷達融合有助于校正姿態(tài)估計。

我們之后的目標是使用具有更寬視場的激光雷達傳感器，并在更大的環(huán)境和不平坦的地形中進行評估，這對腿式機器人來說是一個挑戰(zhàn)。

審核編輯：劉清