九種3D Lidar-SLAM算法評估

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

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

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

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

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

01介紹

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

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

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

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

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

這項(xiàng)工作的主要貢獻(xiàn)是：

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

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

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

02SLAM算法

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

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

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

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

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

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

2.2 BLAM

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

2.3 LOAM

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

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

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

2.4 ISC-LOAM

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

2.5 hdl graph slam

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

2.6 LeGO-LOAM

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

2.7 Cartographer

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

2.8 LIO-mapping

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

2.9 LIO-SAM

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

2.10 FAST-LIO

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

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

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

03實(shí)驗(yàn)評估和討論

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

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

3.1 軌跡的評估和比較

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

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

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

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

3.2 點(diǎn)云的評估與比較

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

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

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

04結(jié)論

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

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

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

審核編輯：劉清