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

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

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

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

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

01介紹

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

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

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

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

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

這項工作的主要貢獻是：

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

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

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

02SLAM算法

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

2.1 機器人平臺和數據集

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

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

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

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

2.2 BLAM

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

2.3 LOAM

Laser Odometry and Mapping（LOAM）或LOAM velodyne是一種實時方法，能夠同時估計里程計并使用3D激光雷達繪制地圖。該方法通過將SLAM任務分解為兩種算法來解決SLAM任務。一個用于計算里程計，另一個用于增量地圖構建，此外它還估計激光雷達的速度。LOAM沒有閉環檢測，這使它無法識別以前訪問過的區域，相反，它實現了特征點匹配，從而確保快速的里程計計算和準確的地圖構建。該方法具有IMU支持，與僅使用激光雷達相比，可以獲得更高的精度。

A-LOAM是LOAM的高級實現，它使用特征庫進行線性代數運算，并使用Ceres Solver 解決相應的優化問題。

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

2.4 ISC-LOAM

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

2.5 hdl graph slam

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

2.6 LeGO-LOAM

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

2.7 Cartographer

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

2.8 LIO-mapping

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

2.9 LIO-SAM

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

2.10 FAST-LIO

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

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

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

03實驗評估和討論

在本節中，基于第2.1小節中引入的收集數據集，在SubT環境中對SLAM方法進行評估。

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

3.1 軌跡的評估和比較

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

根據我們用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數據的方法強烈依賴于激光雷達傳感器，而不是IMU。

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

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

3.2 點云的評估與比較

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

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

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

04結論

在本文中，我們比較了最新的和SoA激光雷達SLAM方法在苛刻的SubT環境中的性能。為此，我們使用配備了自主軟件包的Spot機器人收集了數據集，并分別配置了所有SLAM方法。基于本文中進行的SLAM方法比較，可以得出結論，所配備的3D激光雷達不足以進行3D姿態估計，由于缺乏特征，導致z軸上的顯著漂移。

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

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

審核編輯：劉清