0.摘要 Abstract

提出一種緊耦合的平滑建圖激光慣導(dǎo)里程計(jì)框架，LIO-SAM审胚，完成高準(zhǔn)確度动羽、實(shí)時(shí)的移動(dòng)機(jī)器人軌跡估計(jì)和地圖構(gòu)建。LIO-SAM在因子圖上制定了一個(gè)激光慣導(dǎo)里程計(jì)若皱，允許許多相對(duì)和絕對(duì)測(cè)量镊叁，包括回環(huán)，從不同的來源作為因子圖的輸入納入系統(tǒng)中走触。來源于IMU預(yù)積分的運(yùn)動(dòng)估計(jì)對(duì)點(diǎn)云進(jìn)行去畸變并產(chǎn)生激光里程計(jì)優(yōu)化的初始估計(jì)晦譬。得到的激光雷達(dá)里程計(jì)解決方案用于估計(jì)IMU的零偏。為了確保在實(shí)時(shí)環(huán)境下得到較好的性能互广，在姿態(tài)優(yōu)化的時(shí)候邊緣化掉最老的激光雷達(dá)掃描敛腌，而不是將激光雷達(dá)掃描匹配到全局地圖中。局部尺度而不是全局尺度上的掃描匹配可以顯著高系統(tǒng)的實(shí)時(shí)性能惫皱，選擇性引入關(guān)鍵幀以及有效的滑動(dòng)窗口方法像樊，將新的關(guān)鍵幀注冊(cè)到固定大小的“子關(guān)鍵幀”中。提出的方法旅敷，在不同尺度和環(huán)境下生棍，從三個(gè)平臺(tái)搜集的數(shù)據(jù)集上，進(jìn)行了廣泛的評(píng)估媳谁。

1.引言 Introduction

狀態(tài)估計(jì)涂滴、定位與建圖是一個(gè)成功的智能移動(dòng)機(jī)器人的基礎(chǔ)前提，這是反饋控制晴音、避障和規(guī)劃等許多其他功能所必需的柔纵。利用基于視覺和基于激光雷達(dá)的感知，人們已經(jīng)付出了巨大的努力致力于實(shí)現(xiàn)移動(dòng)機(jī)器人的高性能實(shí)時(shí)同步定位與建圖锤躁，它可以支持移動(dòng)叫人的六自由度的狀態(tài)估計(jì)搁料。基于視覺的方法通常采用單目或者深度相機(jī)进苍，并通過連續(xù)圖像的三角化特征來確定相機(jī)運(yùn)動(dòng)加缘。盡管基于視覺的方法特別適合位置識(shí)別，但是視覺方法對(duì)初始化觉啊、光照、距離的敏感性沈贝，使得它們?cè)趩为?dú)用于支持自動(dòng)導(dǎo)航系統(tǒng)時(shí)不是很可靠杠人。另一方面，基于激光雷達(dá)的方法在很大程度上不受光照變化的影響宋下。特別是隨著最近提出的遠(yuǎn)距離嗡善、高分辨率三維激光雷達(dá)，比如Velodyne128線以及Ouster128線学歧，激光雷達(dá)變得更適合直接捕捉三維空間環(huán)境中的細(xì)節(jié)罩引。因此，本文著重研究基于激光雷達(dá)的狀態(tài)估計(jì)和建圖方法枝笨。

最近二十年袁铐，許多基于激光雷達(dá)的狀態(tài)估計(jì)和建圖方法提出揭蜒。其中，LOAM方法剔桨，在低偏移和實(shí)時(shí)狀態(tài)估計(jì)和建圖中屉更，是應(yīng)用最廣泛的。LOAM采用激光雷達(dá)和IMU達(dá)到了最好的性能洒缀，并自從KITTI里程計(jì)Benchmark榜單發(fā)布以來一直被列為最好的激光雷達(dá)方法瑰谜。盡管LOAM取得了成功，但還是存在一些限制：通過將數(shù)據(jù)保存在全局體素地圖中树绩，通常很難執(zhí)行回環(huán)檢測(cè)以及結(jié)合其他萨脑，比如GPS，絕對(duì)測(cè)量值用于姿態(tài)校正饺饭。當(dāng)體素地圖在特征豐富的環(huán)境中變得稠密時(shí)赶袄，LOAM的在線優(yōu)化過程就會(huì)變得效率很低。LOAM在大規(guī)模測(cè)試中也存在漂移問題琼稻，因?yàn)樗暮诵氖且环N基于scan-match掃描匹配的方法燎含。

本文中，提出一個(gè)通過平滑和建圖的緊耦合激光慣導(dǎo)里程計(jì)框架军援，LIO-SAM仅淑，用來解決上面提到的問題。我們假設(shè)一個(gè)點(diǎn)云去畸變后的非線性運(yùn)動(dòng)模型胸哥，使用原始IMU測(cè)量數(shù)據(jù)估計(jì)激光雷達(dá)掃描過程中的傳感器運(yùn)動(dòng)涯竟。除了點(diǎn)云去畸變，運(yùn)動(dòng)估計(jì)作為激光雷達(dá)里程計(jì)優(yōu)化的初始估計(jì)空厌。得到的激光雷達(dá)里程計(jì)解決方案用于估計(jì)因子圖中IMU零偏庐船。通過引入機(jī)器人軌跡估計(jì)的全局因子圖，我們利用激光雷達(dá)和IMU測(cè)量有效地進(jìn)行傳感器融合嘲更，結(jié)合機(jī)器人姿態(tài)之間的位置識(shí)別筐钟，并在可用的時(shí)候，引入GPS定位或者指南針航向等絕對(duì)測(cè)量赋朦。不同來源因子的集合用于因子圖的聯(lián)合優(yōu)化篓冲。此外，我們邊緣化舊的激光雷達(dá)掃描來優(yōu)化姿態(tài)宠哄，而不是類似LOAM那樣將掃描匹配到全局地圖中壹将。局部尺度而不是全局尺度上的掃描匹配scan-match可以顯著提高系統(tǒng)的實(shí)時(shí)性能，以及選擇性地引入關(guān)鍵幀和一種有效的滑動(dòng)窗口方法毛嫉，將新的關(guān)鍵幀注冊(cè)到之前的“子關(guān)鍵幀”集合中诽俯。

本文工作的主要貢獻(xiàn)總結(jié)如下：

• 在因子圖上建立一個(gè)緊耦合的激光慣導(dǎo)里程計(jì)框架，適用于多傳感器融合和全局優(yōu)化承粤。

• 提出一種高效的暴区，基于局部滑動(dòng)窗口的掃描匹配方法闯团，通過有選擇性地挑選新關(guān)鍵幀注冊(cè)到固定大小的先前的子關(guān)鍵幀集合上，來達(dá)到實(shí)時(shí)的性能颜启。

• 提出的框架在不同尺度偷俭、車輛以及環(huán)境中的測(cè)試得到了廣泛的驗(yàn)證。

2.相關(guān)工作 Related Work

激光雷達(dá)里程計(jì)通常是通過ICP【3】或者GICP【4】等scan-match掃描匹配算法查找兩個(gè)連續(xù)幀之間的相對(duì)轉(zhuǎn)換來進(jìn)行的缰盏。除了匹配一整片點(diǎn)云涌萤，基于特征的匹配方法已經(jīng)成為一種流行的替代方法。例如口猜，文獻(xiàn)【5】中提出的基于平面的實(shí)時(shí)激光雷達(dá)匹配方法负溪。假設(shè)在結(jié)構(gòu)化環(huán)境中進(jìn)行操作，它從點(diǎn)云中提取平面济炎，并通過解決最小二乘問題對(duì)它們進(jìn)行匹配川抡。文獻(xiàn)【6】中提出一種基于項(xiàng)圈線的里程計(jì)估計(jì)方法。在該方法中须尚，從原始點(diǎn)云中隨機(jī)生成線段崖堤，并用于之后的點(diǎn)云配準(zhǔn)。然而耐床，由于現(xiàn)代三維激光雷達(dá)的旋轉(zhuǎn)機(jī)制和傳感器的運(yùn)動(dòng)密幔，掃描的點(diǎn)云通常都是傾斜的。僅僅使用激光雷達(dá)進(jìn)行姿態(tài)估計(jì)并不是理想的撩轰，因?yàn)槭褂命c(diǎn)云或特征進(jìn)行配準(zhǔn)最終會(huì)導(dǎo)致大的漂移胯甩。

因此，激光雷達(dá)通常與其他傳感器聯(lián)合使用堪嫂，比如IMU或者GPS偎箫，用于狀態(tài)估計(jì)以及建圖。這種利用傳感器融合的設(shè)計(jì)方案皆串，通逞桶欤可以分為兩類：松耦合融合以及緊耦合融合。在LOAM中恶复，引入IMU用來激光雷達(dá)掃描數(shù)據(jù)的去畸變娇唯，并給出掃描匹配的運(yùn)動(dòng)先驗(yàn)信息。然而寂玲，IMU并沒有涉及到算法的優(yōu)化過程。因此LOAM可以歸類為松耦合方法梗摇。文獻(xiàn)【7】中提出一種用于地面車輛建圖任務(wù)的輕量級(jí)地面優(yōu)化激光雷達(dá)里程計(jì)與建圖算法LeGO-LOAM拓哟。其對(duì)IMU數(shù)據(jù)的融合過程與LOAM中一致。EKF擴(kuò)展卡爾曼濾波是另一種更加流行的松耦合融合方法伶授。例如断序，文獻(xiàn)【9】到文獻(xiàn)【13】在優(yōu)化階段使用EKF結(jié)合激光雷達(dá)和IMU/GPS觀測(cè)流纹，來用于機(jī)器人狀態(tài)估計(jì)。

緊耦合系統(tǒng)通常提供更高的精度违诗，并且是目前正在研究的主要熱點(diǎn)漱凝。文獻(xiàn)【15】中，IMU預(yù)積分被用到點(diǎn)云去畸變中诸迟。在文獻(xiàn)【16】中提出一種機(jī)器人激光慣導(dǎo)狀態(tài)估計(jì)器RLINS茸炒。它以一種緊耦合的方式遞歸地采用錯(cuò)誤狀態(tài)卡爾曼濾波器去矯正機(jī)器人的狀態(tài)估計(jì)。由于缺乏其他用于狀態(tài)估計(jì)的傳感器阵苇，它在長期導(dǎo)航過程中容易出現(xiàn)漂移壁公。文獻(xiàn)【17】中引入一種緊耦合的激光慣導(dǎo)里程計(jì)與建圖框架，LIOM绅项。其是LIO-Mapping的縮寫紊册，聯(lián)合優(yōu)化了激光雷達(dá)和IMU的測(cè)量結(jié)果，與LOAM相比獲得了差不多甚至更好的精度快耿。因?yàn)長IOM方法被設(shè)計(jì)用來處理所有的傳感器測(cè)量值囊陡，因此不能達(dá)到實(shí)時(shí)性--在我們的測(cè)試中大約跑了六成的實(shí)時(shí)速度。

3.基于平滑和建圖的激光慣導(dǎo)里程計(jì) Lidar Inertial Odometry Via Smoothing and Mapping

A. 系統(tǒng)總覽 System Overview

首先我們定義整個(gè)論文中使用的坐標(biāo)系與變量符號(hào)掀亥。我們將世界坐標(biāo)系定義為撞反，機(jī)器人坐標(biāo)系為。為了方便起見铺浇，我們還假設(shè)IMU坐標(biāo)系與機(jī)器人本體坐標(biāo)系重合痢畜。機(jī)器人的狀態(tài)可以寫為：

其中是定義在特殊正交群上的旋轉(zhuǎn)矩陣，是平移向量鳍侣，是速度丁稀，是IMU的零偏。變換矩陣表示從機(jī)器人機(jī)體坐標(biāo)系到世界坐標(biāo)系的特殊歐式群倚聚，表示為

LIO-SAM的系統(tǒng)結(jié)構(gòu)线衫。接收3D激光雷達(dá)、IMU以及GPS作為輸入惑折。引入四種因子去構(gòu)建因子圖授账。a)IMU預(yù)積分因子，b) 激光里程計(jì)因子惨驶，c) GPS因子白热，d) 閉環(huán)因子。這些因子如何產(chǎn)生描述在第III節(jié)中

該系統(tǒng)從激光雷達(dá)粗卜、IMU以及可選的GPS中接收傳感器數(shù)據(jù)屋确。我們?cè)噲D利用這些傳感器的觀測(cè)來估計(jì)機(jī)器人的狀態(tài)及其軌跡。狀態(tài)估計(jì)問題可以表示為一個(gè)最大后驗(yàn)MAP問題。我們使用因子圖去建模這個(gè)問題攻臀，因?yàn)榕c貝葉斯網(wǎng)絡(luò)相比焕数，因子圖更適合執(zhí)行推理任務(wù)。在高斯噪聲模型的假設(shè)下刨啸，我們問題的最大后驗(yàn)估計(jì)等價(jià)于求解一個(gè)非線性最小二乘問題堡赔。注意，如果不失一般性设联，我們提出的系統(tǒng)還可以包含來自其他傳感器的測(cè)量善已，比如高度計(jì)的海拔高度以及指南針的航向等。我們介紹四種類型的因子factors仑荐，以及一種因子圖構(gòu)造的變量類型variable雕拼。該變量表示機(jī)器人狀態(tài)在特定時(shí)間的狀態(tài)歸屬于因子圖中的節(jié)點(diǎn)nodes。
這四種因子分別是：

• 1. IMU預(yù)積分因子
• 2. 激光雷達(dá)里程計(jì)因子
• 3. GPS因子
• 4. 閉環(huán)因子

當(dāng)機(jī)器人姿態(tài)的變化超過用戶定義的閾值時(shí)粘招，圖中會(huì)添加一個(gè)新的機(jī)器人狀態(tài)節(jié)點(diǎn)啥寇。在使用貝葉斯樹（iSAM2）增量式平滑和建圖過程插入一個(gè)新的節(jié)點(diǎn)時(shí)，因子圖會(huì)隨之進(jìn)行優(yōu)化洒扎。

B. IMU預(yù)積分因子 IMU Preintegration Factor

IMU角速度和加速度的觀測(cè)公式定義如下：

其中和是機(jī)體坐標(biāo)系下t時(shí)刻的IMU觀測(cè)源數(shù)據(jù)辑甜。和均受到緩慢變化的IMU零偏以及白噪聲的影響。是從世界坐標(biāo)系到機(jī)體坐標(biāo)系的旋轉(zhuǎn)矩陣袍冷。是世界坐標(biāo)系下的重力常數(shù)向量磷醋。

現(xiàn)在我們可以采用IMU測(cè)量去推斷出機(jī)器人的運(yùn)動(dòng)。機(jī)器人在時(shí)刻的速度胡诗、位置以及旋轉(zhuǎn)計(jì)算如下：

其中邓线。這里我們假設(shè)機(jī)器人角速度和加速度在積分過程中保持勻速不變。

然后我們采用文獻(xiàn)【20】提出的IMU預(yù)積分去獲取兩個(gè)時(shí)間戳之間的相對(duì)運(yùn)動(dòng)煌恢。預(yù)積分得到的第和第時(shí)刻之間的速度測(cè)量骇陈、位置測(cè)量以及旋轉(zhuǎn)測(cè)量計(jì)算如下：

由于篇幅限制，我們讓讀者參考來自文獻(xiàn)【20】的描述瑰抵，來得到公式【2-9】的詳細(xì)推導(dǎo)你雌。除了IMU預(yù)積分的效率之外，應(yīng)用IMU預(yù)積分也可以很自然地給出IMU預(yù)積分因子在因子圖中的約束條件二汛。IMU零偏因子與激光雷達(dá)里程計(jì)因子在因子圖中一起進(jìn)行聯(lián)合優(yōu)化婿崭。

C.激光雷達(dá)里程計(jì)因子 Lidar Odometry Factor

當(dāng)新的一圈激光雷達(dá)掃描到來時(shí)，首先執(zhí)行特征提取肴颊。通過評(píng)估局部區(qū)域上點(diǎn)的粗糙度/曲率來提取邊緣和平面特征氓栈。粗糙度/曲率較大的點(diǎn)被劃分為邊緣特征。類似地婿着，一個(gè)平面特征也可以按一個(gè)小的粗糙度/曲率進(jìn)行分類颤绕。 我們將從激光雷達(dá)在時(shí)刻掃描點(diǎn)云中提取的邊緣特征以及平面特征分別定義為以及幸海。在時(shí)刻提取的所有特征構(gòu)成了一個(gè)激光雷達(dá)點(diǎn)云幀，其中奥务。注意一個(gè)激光雷達(dá)點(diǎn)云幀是表示在機(jī)器人坐標(biāo)系下的。特征提取步驟的更多詳細(xì)描述可以在文獻(xiàn)【1】或如果用到距離圖像也可以參考文獻(xiàn)【7】袜硫。

如果將每一幀激光雷達(dá)點(diǎn)云都拿來計(jì)算因子并添加到因子圖中是難以計(jì)算的氯葬，因此我們采用了關(guān)鍵幀選擇 的概念，其被廣泛應(yīng)用于視覺SLAM領(lǐng)域婉陷。使用一種簡單但有效的啟發(fā)式方法帚称，當(dāng)機(jī)器人姿態(tài)的變化與先前的狀態(tài)相比，如果超過了用戶定義的閾值時(shí)秽澳，我們選擇此時(shí)的激光雷達(dá)幀作為關(guān)鍵幀闯睹。新保存的關(guān)鍵幀與因子圖的中新的機(jī)器人狀態(tài)節(jié)點(diǎn)相關(guān)聯(lián)。然后丟棄掉兩個(gè)關(guān)鍵幀之間的其他激光雷達(dá)幀担神。這樣添加的關(guān)鍵幀不僅可以實(shí)現(xiàn)地圖密度和內(nèi)存消耗之間的平衡楼吃，而且有助于保持一個(gè)相對(duì)稀疏的因子圖結(jié)構(gòu)，適用于實(shí)時(shí)的非線性優(yōu)化妄讯。在我們的工作中孩锡，添加新關(guān)鍵幀的平移和旋轉(zhuǎn)的變化閾值選擇為1米和10度。

假設(shè)我們希望在因子圖中添加一個(gè)新的狀態(tài)節(jié)點(diǎn)亥贸。那么與該狀態(tài)相關(guān)聯(lián)的激光雷達(dá)關(guān)鍵幀為躬窜。激光雷達(dá)里程計(jì)因子的生成過程描述如下：

• 1）建立子關(guān)鍵幀的體素地圖 sub-keyframes for voxel map

我們實(shí)現(xiàn)了一種滑動(dòng)窗口方法去構(gòu)建點(diǎn)云地圖，其中包含了固定數(shù)目的最近激光雷達(dá)的掃描點(diǎn)云炕置。我們沒有優(yōu)化兩個(gè)連續(xù)的激光雷達(dá)掃描點(diǎn)云幀之間的變換關(guān)系荣挨，而是提取了n個(gè)最近的關(guān)鍵幀，sub-keyframes子關(guān)鍵幀朴摊，來進(jìn)行估計(jì)默垄。采用變換關(guān)系將關(guān)聯(lián)的子關(guān)鍵幀集合轉(zhuǎn)換到世界坐標(biāo)系中。轉(zhuǎn)換后的子關(guān)鍵幀會(huì)被合并成一個(gè)體素地圖仍劈。由于我們?cè)谥暗奶卣魈崛〔襟E中提取了兩種特征：邊緣特征以及平面特征厕倍，因此體素地圖由兩個(gè)子體素地圖構(gòu)成，定義邊緣特征體素地圖為贩疙，平面特征體素地圖為讹弯。激光雷達(dá)幀以及體素地圖之間的關(guān)系如下：

其中：

而和是轉(zhuǎn)換到世界坐標(biāo)系下的邊緣特征和平面特征。然后對(duì)子體素地圖和進(jìn)行下采樣这溅，消除落在同一個(gè)體素單元中的重復(fù)冗余特征组民。本文中，選擇25悲靴。和下采樣的分辨率分別設(shè)為0.2m和0.4m臭胜。

• 2）掃描匹配 scan-matching

我們將新獲得的激光雷達(dá)關(guān)鍵幀通過掃描匹配到世界坐標(biāo)系下的體素地圖中。為此，可以使用各種掃描匹配scan-match方法耸三，比如ICP【3】和GICP【4】乱陡。這里，我們選擇使用文獻(xiàn)【1】中提出的方法仪壮，因?yàn)樗邆涑錾?jì)算效率和在各種有挑戰(zhàn)性的環(huán)境中有很好的魯棒性憨颠。

我們首先將激光雷達(dá)關(guān)鍵幀從機(jī)器人坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系中，得到轉(zhuǎn)換后的關(guān)鍵幀集合為积锅。初始的變換關(guān)系是通過從IMU預(yù)測(cè)的機(jī)器人運(yùn)動(dòng)中得到的爽彤。對(duì)于每幀或者中的邊緣特征和平面特征，我們?nèi)缓笳业剿鼈儗?duì)應(yīng)的子體素地圖和中的邊緣和平面缚陷。為了簡潔起見适篙，找到對(duì)應(yīng)關(guān)系的詳細(xì)過程在這里省略，文獻(xiàn)【1】中給出了詳細(xì)的描述箫爷。

• 3）相對(duì)變換關(guān)系 relative transformation

邊緣特征與平面特征和它們對(duì)應(yīng)的邊緣或平面塊之間的距離可以通過如下的公式進(jìn)行計(jì)算：

其中嚷节，是在邊緣特征或平面特征對(duì)應(yīng)集合中的索引號(hào)。對(duì)于在轉(zhuǎn)換后的邊緣關(guān)鍵幀中的邊緣特征來說蝶缀，和是形成邊緣子體素地圖中相對(duì)應(yīng)的邊緣線上的點(diǎn)丹喻。而對(duì)于在轉(zhuǎn)換后的平面關(guān)鍵幀中的平面特征來說，翁都，以及形成了平面子體素地圖中對(duì)應(yīng)的平面塊碍论。然后采用高斯牛頓法來求解最優(yōu)的變換矩陣關(guān)系，通過最小化下述公式：

最后柄慰，我們可以得到狀態(tài)節(jié)點(diǎn)與狀態(tài)節(jié)點(diǎn)之間的相對(duì)變換關(guān)系鳍悠，它是連接這兩種位姿的激光里程計(jì)因子：

我們注意到，獲得的另一種方法是將子關(guān)鍵幀轉(zhuǎn)換到狀態(tài)節(jié)點(diǎn)的坐標(biāo)系下坐搔。換句話說藏研，就是將與在狀態(tài)節(jié)點(diǎn)坐標(biāo)系下表示的體素地圖進(jìn)行匹配。這種方式下概行，可以直接得到實(shí)施相對(duì)變換關(guān)系蠢挡。由于轉(zhuǎn)換后的特征和可以多次重復(fù)使用，所以我們選擇采用第III節(jié)-C.1中描述的方法凳忙，因?yàn)樗挠?jì)算效率很高业踏。

D. GPS 因子 GPS Factor

雖然僅僅利用IMU預(yù)積分和激光里程計(jì)因子就可以獲得可靠的狀態(tài)估計(jì)以及建圖，但是這個(gè)系統(tǒng)在長期的導(dǎo)航任務(wù)中涧卵，仍然存在漂移問題勤家。為了解決這個(gè)問題，我們可以引入提供消除漂移的絕對(duì)測(cè)量值的傳感器柳恐。這些傳感器包括高度計(jì)伐脖、指南針羅盤以及GPS全球定位系統(tǒng)热幔。這里為了闡述清楚，我們討論了其中GPS讼庇，因?yàn)樗诂F(xiàn)實(shí)導(dǎo)航系統(tǒng)中被廣泛使用绎巨。

當(dāng)我們接收GPS測(cè)量時(shí)，首先使用文獻(xiàn)【21】中提到的方法將GPS坐標(biāo)轉(zhuǎn)換到局部笛卡爾坐標(biāo)系下巫俺。在因子圖中添加一個(gè)新的節(jié)點(diǎn)后认烁，我們將新的GPS因子和這個(gè)新節(jié)點(diǎn)關(guān)聯(lián)起來。如果GPS信號(hào)與激光雷達(dá)沒有做硬件同步介汹，那么我們根據(jù)激光雷達(dá)的時(shí)間戳在GPS中進(jìn)行線性插值。

我們注意到舶沛，當(dāng)GPS接收可用時(shí)嘹承，不斷添加GPS因子是沒有必要的，因?yàn)榧す鈶T導(dǎo)里程計(jì)漂移增加是非常緩慢的如庭。在實(shí)踐中叹卷，我們只需要在估計(jì)的位置協(xié)方差大于接收到的GPS位置協(xié)方差的時(shí)候，才去增加一個(gè)GPS因子坪它。

E. 閉環(huán)因子

由于利用了因子圖骤竹，閉環(huán)也可以無縫地插入到提出的系統(tǒng)中，而不是跟LOAM和LIO Mapping那樣往毡。為了說明蒙揣，我們描述并實(shí)現(xiàn)了一種樸素但有效的基于歐氏距離的閉環(huán)檢測(cè)方法。我們還注意到开瞭，我們提出的框架與其他的閉環(huán)檢測(cè)方法是兼容的懒震，例如文獻(xiàn)【22】【23】提到的方法，他們生產(chǎn)了一個(gè)點(diǎn)云描述符并將它用于位置識(shí)別嗤详。

當(dāng)一個(gè)新的狀態(tài)被加入到因子圖中个扰，我們首先在因子圖中進(jìn)行搜索，找到歐式空間中接近狀態(tài)的先驗(yàn)狀態(tài)葱色。如圖1所示递宅，狀態(tài)節(jié)點(diǎn)是其中一個(gè)返回的候選對(duì)象。然后我們嘗試用掃描匹配scan-match方式將關(guān)鍵幀與子關(guān)鍵幀集進(jìn)行匹配苍狰。注意办龄，關(guān)鍵幀和過去的子關(guān)鍵幀在掃描匹配scan-match之前已經(jīng)先被轉(zhuǎn)換到世界坐標(biāo)系W中 。然后我們就得到了相對(duì)變換舞痰，并將其作為一個(gè)閉環(huán)因子添加到因子圖中土榴。在本文中，我們選擇索引數(shù)m為12响牛，閉環(huán)與新狀態(tài)的搜索范圍設(shè)為15m玷禽。

實(shí)際中赫段，我們發(fā)現(xiàn)，當(dāng)GPS是唯一可用的絕對(duì)位置測(cè)量傳感器的時(shí)候矢赁，添加閉環(huán)因子對(duì)于修正機(jī)器人高程上的漂移特別有用糯笙。這是由于GPS的高程測(cè)量是非常不準(zhǔn)確的，在我們的測(cè)試中撩银，沒有閉環(huán)情況下给涕，會(huì)導(dǎo)致接近100米的高程誤差。

4.實(shí)驗(yàn) Experiments

我們現(xiàn)在描述一系列的實(shí)驗(yàn)來定性和定量地分析我們提出的框架额获。本文采用的傳感器套件包括一個(gè)Velodyne 16線激光雷達(dá)够庙，一個(gè)MicroStrain 3DM-GX5-25的IMU以及一個(gè)Reach M的GPS。為了進(jìn)行實(shí)驗(yàn)驗(yàn)證抄邀，我們?cè)诓煌叨仍耪！⑵脚_(tái)和環(huán)境中搜集了5個(gè)不同的數(shù)據(jù)集。這些數(shù)據(jù)集分別為Rotation境肾，Walking剔难，Campus，Park奥喻，Amsterdam偶宫。傳感器安裝平臺(tái)如圖2所示。

圖2. 在三個(gè)平臺(tái)上搜集的數(shù)據(jù)集：a) 一個(gè)定制的手持式設(shè)備环鲤；b) 一輛無人駕駛的地面汽車-ClearPath Jackal纯趋；c) 一艘電動(dòng)船-Duffy

前三個(gè)數(shù)據(jù)集是使用MIT校園里定制的手持式設(shè)備搜集的。Park數(shù)據(jù)集是在一個(gè)植被覆蓋的公園里采集的楔绞，使用了一輛無人地面車輛（UGV）- Clearpath Jackal结闸。最后一個(gè)數(shù)據(jù)集，Amsterdam酒朵，是通過將傳感器安裝在一艘電動(dòng)船上桦锄，并在阿姆斯特丹運(yùn)河中巡航采集的。這些數(shù)據(jù)集的詳細(xì)信息顯示在下表中蔫耽。

將我們提出的LIO-SAM框架與LOAM和LIO Mapping進(jìn)行比較结耀。在所有實(shí)驗(yàn)中，LOAM和LIO-SAM實(shí)時(shí)的運(yùn)行了匙铡。而LIO-Mapping相反图甜，給了無限多的時(shí)間來處理每個(gè)傳感器的觀測(cè)值。所有的方法都在C++中實(shí)現(xiàn)鳖眼，并在配備了Intel i7-10710U CPU的筆記本上運(yùn)行黑毅，使用的平臺(tái)是是Ubuntu中的ROS機(jī)器人操作系統(tǒng)。我們注意到钦讳，只有CPU適用于計(jì)算矿瘦，沒有啟用并行計(jì)算枕面。我們的LIO-SAM的實(shí)現(xiàn)可以在我們的Github項(xiàng)目中免費(fèi)下載。執(zhí)行實(shí)驗(yàn)的補(bǔ)充細(xì)節(jié)缚去，包括所有測(cè)試的完整視頻潮秘，可以在我們的YouTube中找到：

A. Rotation數(shù)據(jù)集

在這個(gè)測(cè)試中，當(dāng)只使用IMU預(yù)積分以及激光里程計(jì)因子被添加到因子圖中時(shí)易结，重點(diǎn)評(píng)估我們提出的LIO-SAM框架的魯棒性枕荞。Rotation數(shù)據(jù)集是人手持傳感器套件采集的，在原地靜止不移動(dòng)條件下搞动，只進(jìn)行純旋轉(zhuǎn)操作躏精。本實(shí)驗(yàn)中最大的旋轉(zhuǎn)速度為133.7度每秒。測(cè)試環(huán)境如圖3(a)所示鹦肿。LOAM和LIO-SAM中生成的地圖如圖3(b)玉控，圖3(c)所示。

圖3. LOAM和LIO-SAM在Rotation純旋轉(zhuǎn)數(shù)據(jù)集上的建圖結(jié)果狮惜。LIO-Mapping沒有建出好結(jié)果。

由于LIO-Mapping使用了文獻(xiàn)【25】中相同的初始化過程碌识，繼承了視覺慣導(dǎo)SLAM類似的對(duì)初始化的敏感性碾篡，并且無法使用這個(gè)Rotation數(shù)據(jù)完成正確的初始化。由于LIO-Maping沒能產(chǎn)生有意義的結(jié)果筏餐，它的地圖就沒有顯示出來开泽。從圖中可以看出，與LOAM算法相比魁瞪，LIO-SAM構(gòu)建的地圖保留了更精細(xì)的環(huán)境結(jié)構(gòu)細(xì)節(jié)穆律。這是由于LIO-SAM能夠在SO(3)特殊正交群中精確的注冊(cè)每個(gè)激光雷達(dá)幀點(diǎn)云，即使當(dāng)機(jī)器人發(fā)生了快速旋轉(zhuǎn)的情況下导俘。

B. Walking數(shù)據(jù)集

這個(gè)測(cè)試設(shè)計(jì)的目的在于評(píng)估我們提出的方法在SE(3)特殊歐式群中系統(tǒng)發(fā)生劇烈平移和旋轉(zhuǎn)的情況下的性能峦耘。這個(gè)數(shù)據(jù)集上遇到的最大平移速度和旋轉(zhuǎn)速度分別為1.8米每秒和213.9度每秒。在數(shù)據(jù)采集期間旅薄，人拿著圖2(a)那樣的手持式傳感器辅髓，然后快速行走在MIT校園中，如圖4(a)中少梁。這個(gè)實(shí)驗(yàn)中洛口，LOAM建的地圖，如圖4(b)所示凯沪，當(dāng)遇到主動(dòng)旋轉(zhuǎn)時(shí)候第焰，會(huì)在多個(gè)位置出現(xiàn)發(fā)散。LIO-Mapping在這個(gè)數(shù)據(jù)集上要比LOAM表現(xiàn)的出色妨马。然而LIO-Mapping建的地圖挺举，如圖4(c)所示杀赢，在不同位置仍然存在不一樣，并且由很多模糊的結(jié)構(gòu)組成豹悬。由于LIO-Mapping被設(shè)計(jì)用來處理所有傳感器的測(cè)量葵陵，因此它旨在0.56倍的實(shí)時(shí)條件下運(yùn)行，而其他兩種方法都可以實(shí)時(shí)運(yùn)行瞻佛。最后脱篙，LIO-SAM的性能要優(yōu)于其他兩種方法，并構(gòu)建了一張與可用的谷歌地圖一致的地圖伤柄。

圖4. LOAM, LIO-Mapping和LIO-SAM在Walking數(shù)據(jù)集上的建圖結(jié)果绊困。當(dāng)遇到主動(dòng)旋轉(zhuǎn)時(shí)，圖b的LOAM建的圖會(huì)多次發(fā)散适刀。LIO-Mapping建圖性能要優(yōu)于LOAM秤朗。但是，LIO-Mapping建的地圖由于點(diǎn)云配準(zhǔn)的不準(zhǔn)確笔喉，造成了許多模糊結(jié)構(gòu)取视。LIO-SAM在未使用GPS的情況下，生成了一張與谷歌地球圖像一致的地圖

C. Campus數(shù)據(jù)集

本數(shù)據(jù)集上的測(cè)試是設(shè)計(jì)用來展示引入GPS和閉環(huán)因子后的建圖效果常挚。為了做到這一點(diǎn)作谭，我們故意未添加GPS和閉環(huán)因子到因子圖中。當(dāng)GPS和閉環(huán)因子禁用后奄毡，我們提出的方法命名為LIO-Odom折欠，也就是只利用IMU預(yù)積分和激光里程計(jì)因子。當(dāng)使用GPS因子時(shí)吼过，我們提出的方法定義為LIO-GPS锐秦，其使用IMU預(yù)積分，激光里程計(jì)以及GPS因子用于建圖盗忱。LIO-SAM則是在圖中添加了所有的因子酱床。為了收集這個(gè)數(shù)據(jù)集，研究人員使用手持式傳感器設(shè)備在MIT校園周圍走動(dòng)售淡，并返回到相同位置斤葱。由于在建圖區(qū)域存在許多建筑物和樹木，GPS接收幾乎不可用揖闸，而且大多數(shù)時(shí)候也不準(zhǔn)確揍堕。在過濾掉不一致的GPS觀測(cè)后，可用的GPS如圖5(a)綠色部分所示汤纸。這些區(qū)域?qū)?yīng)于少數(shù)沒有建筑物或樹木包圍的區(qū)域衩茸。

圖5. 各種方法在MIT校園中采集的Campus數(shù)據(jù)集上的結(jié)果。紅點(diǎn)表示起始位置和結(jié)束位置贮泞。軌跡的方向是按照時(shí)鐘排列的楞慈。LIO-Mapping方法由于沒有產(chǎn)生有意義的軌跡結(jié)果幔烛，沒有顯示在里面

LOAM、LIO-Odom囊蓝、LIO-GPS以及LIO-SAM估計(jì)出來的軌跡如圖5(a)所示饿悬。由于LIO-Mapping沒能正確地初始化，并且未能產(chǎn)生有意義的結(jié)果聚霜，因此LIO-Mapping的結(jié)果沒有展示出來狡恬。如圖上所示，和其他其他方法相比蝎宇，LOAM的軌跡有明顯的漂移弟劲。由于沒有對(duì)GPS數(shù)據(jù)進(jìn)行修正，在地圖的右下角LIO-Odom的軌跡開始出現(xiàn)明顯的漂移姥芥。加入了GPS數(shù)據(jù)后兔乞，LIO-GPS可以在GPS可用時(shí)修正漂移。但是凉唐，在數(shù)據(jù)集的后半部分GPS都是不可用的庸追。因此，當(dāng)由于漂移導(dǎo)致機(jī)器人返回到起始位置時(shí)台囱，LIO-GPS無法檢測(cè)到閉環(huán)锚国。另一方面，LIO-SAM可以通過向因子圖中添加閉環(huán)因子來消除軌跡漂移玄坦。LIO-SAM建的地圖與谷歌地球上的圖可以很好的對(duì)齊，如圖5(b)所示绘沉。當(dāng)機(jī)器人回到原點(diǎn)時(shí)煎楣，所有方法的相對(duì)平移誤差如下表所示。

D. Park 數(shù)據(jù)集

在本次測(cè)試中车伞，我們將傳感器安裝在一輛UGV無人車上择懂，沿著森林徒步路線行駛。機(jī)器人在行駛40分鐘后回到出發(fā)位置另玖。無人車在三種路面上行駛：瀝青路面困曙、草坪以及泥地。由于缺乏懸掛系統(tǒng)谦去，這個(gè)機(jī)器人在非瀝青路面行駛過程中慷丽，會(huì)出現(xiàn)低幅度高頻率的振動(dòng)。

為了模擬具有挑戰(zhàn)性的建圖場(chǎng)景鳄哭，我們只在機(jī)器人處于開放的區(qū)域時(shí)使用GPS測(cè)量值要糊，比如圖6(a)中的綠色部分。這種建圖場(chǎng)景一類任務(wù)的代表妆丘，即機(jī)器人必須在沒有GPS的區(qū)域進(jìn)行建圖锄俄，并定期返回有GPS信號(hào)的區(qū)域來糾正漂移局劲。

圖6. 使用美國新澤西州的Pleasant Valley Park收集Park數(shù)據(jù)集上所有方法的軌跡對(duì)比結(jié)果。紅點(diǎn)表示起始位置和結(jié)束位置奶赠。軌跡方向是順時(shí)針方向鱼填。

與前面實(shí)驗(yàn)結(jié)果類似，LOAM毅戈、LIO-Mapping和LIO-Odom都出現(xiàn)了很明顯的軌跡漂移创译，因?yàn)樗麄儧]有使用絕對(duì)校正數(shù)據(jù)眨八。此外，LIO-Mapping只在0.67倍實(shí)時(shí)速度下運(yùn)行，而其他方法是實(shí)時(shí)運(yùn)行的蛔翅。雖然LIO-GPS和LIO-SAM的軌跡在水平面保持一致，但它們的相對(duì)平移誤差是不一樣的如表II所示哎迄。由于沒有可靠的絕對(duì)高度測(cè)量值谭贪，LIO-GPS在高度上有漂移，并且在返回到機(jī)器人的起始位置時(shí)無法閉環(huán)令花。LIO-SAM沒有出現(xiàn)這樣的問題阻桅，因?yàn)樗瞄]環(huán)因子來消除軌跡漂移。

E. Amsterdam 數(shù)據(jù)集

最后兼都，我們把傳感器套件安裝到電動(dòng)船上嫂沉，沿著阿姆斯特丹的運(yùn)河巡航3小時(shí)。雖然在這次測(cè)試中扮碧，傳感器的移動(dòng)相對(duì)平穩(wěn)趟章，但由于存在若干個(gè)原因，給運(yùn)河整體建圖仍然具有挑戰(zhàn)性慎王。運(yùn)河上的許多橋梁都出現(xiàn)了退化的情況蚓土，因?yàn)楫?dāng)船出現(xiàn)在橋下時(shí)，幾乎沒有什么有用的特征赖淤，類似于穿過一條很長又毫無特色的走廊蜀漆。由于水里不存在地面，平面特征的數(shù)量也明顯較少咱旱。當(dāng)陽光直射到傳感器的視野范圍內(nèi)時(shí)确丢，我們觀察到許多激光雷達(dá)的錯(cuò)誤檢測(cè)，這發(fā)生在數(shù)據(jù)采集總時(shí)長的20%處吐限。由于電動(dòng)船上方橋梁和城市建筑的存在鲜侥，我們也只獲得了間歇性的GPS信號(hào)的接收。

由于存在這樣那樣的挑戰(zhàn)诸典，LOAM剃毒、LIO-Mapping和LIO-Odom都未能在這個(gè)測(cè)試中產(chǎn)生有意義的結(jié)果。與Park公園數(shù)據(jù)集中遇到的問題類似，LIO-GPS由于高度值上出現(xiàn)漂移赘阀，在返回機(jī)器人的初始位置時(shí)無法閉環(huán)益缠，這進(jìn)一步激發(fā)我們?cè)贚IO-SAM中使用閉環(huán)因子來消除軌跡上的漂移。

F. 基準(zhǔn)測(cè)試結(jié)果 Benchmarking Results

由于完整的GPS覆蓋只在Park數(shù)據(jù)集中可用基公，因此我們顯示了關(guān)于GPS測(cè)量歷史的均方根誤差(RMSE)結(jié)果幅慌，可以把它看作Ground Truth真值。這里均方根誤差沒有考慮沿z軸發(fā)生的誤差轰豆。如下表所示胰伍，LIO-GPS和LIO-SAM在GPS的Ground Truth真值上都達(dá)到了差不多的RMSE均方根誤差。需要注意的是酸休，我們可以通過讓這兩種方法使用所有GPS測(cè)量值骂租，來使其均RMSE方根誤差減少至少一個(gè)數(shù)量級(jí)。然而斑司，在許多建圖配置中渗饮，完整GPS觀測(cè)并不總是可用的。我們的目的是設(shè)計(jì)一個(gè)可以在各種具有挑戰(zhàn)性的環(huán)境中都可運(yùn)行的魯棒系統(tǒng)宿刮。

LOAM互站、LIO-Mapping以及LIO-SAM這三種對(duì)比方法，在上述5個(gè)數(shù)據(jù)集上處理一個(gè)激光雷達(dá)點(diǎn)云幀的平均運(yùn)行時(shí)間如下表所示僵缺。在所有測(cè)試中胡桃，LOAM和LIO-SAM是實(shí)時(shí)運(yùn)行的。換句話說磕潮，當(dāng)激光雷達(dá)的旋轉(zhuǎn)速率為10Hz時(shí)翠胰，如果運(yùn)行時(shí)間需要超過100ms，那么一些激光雷達(dá)幀點(diǎn)云就會(huì)被丟棄自脯。我們給LIO-Mapping方法無限長的時(shí)間來處理每個(gè)激光雷達(dá)幀亡容。如圖中所示，LIO-SAM的運(yùn)行時(shí)間明顯少于其他兩種方法冤今，這使得它更適合部署在低功耗的嵌入式系統(tǒng)上。

我們還通過比實(shí)時(shí)速度更快的輸入數(shù)據(jù)到LIO-SAM中茂缚，對(duì)其進(jìn)行壓力測(cè)試戏罢。當(dāng)LIO-SAM在加速播放與1倍速回放數(shù)據(jù)的結(jié)果進(jìn)行比較，可以順利達(dá)到相似的性能時(shí)脚囊，記錄下最大數(shù)據(jù)回放速度并顯示在上表的最后一列中龟糕。從圖中可以看出，LIO-SAM能夠以高達(dá)13倍速來處理數(shù)據(jù)悔耘。

我們注意到讲岁，LIO-SAM的運(yùn)行時(shí)間明顯會(huì)受到特征地圖密度的影響，但受到因子圖中節(jié)點(diǎn)數(shù)和因子數(shù)的影響比較小。例如缓艳，Park數(shù)據(jù)集是在一個(gè)特征豐富的環(huán)境中采集的校摩，其中的綠植產(chǎn)生了大量的特征，而Amsterdam數(shù)據(jù)集產(chǎn)生的特征地圖相對(duì)更稀疏阶淘。Park數(shù)據(jù)集實(shí)驗(yàn)中的因子圖包含4573個(gè)節(jié)點(diǎn)和9365個(gè)因子衙吩，而Amsterdam數(shù)據(jù)集實(shí)驗(yàn)中的因子圖則有23304個(gè)節(jié)點(diǎn)和49617個(gè)因子。盡管如此溪窒，與Park數(shù)據(jù)集測(cè)試相比坤塞，LIO-SAM在Amsterdam數(shù)據(jù)集測(cè)試中使用的時(shí)間更少。

5.結(jié)論與討論 Conclusion and Discussion

我們提出了LIO-SAM澈蚌，一個(gè)通過平滑和建圖實(shí)現(xiàn)緊耦合激光雷達(dá)慣性里程計(jì)框架摹芙，用于復(fù)雜環(huán)境中執(zhí)行實(shí)時(shí)狀態(tài)估計(jì)和建圖。通過在因子圖上建立激光雷達(dá)-慣性里程計(jì)宛瞄，LIO-SAM特別適用于多傳感器融合浮禾。額外的傳感器測(cè)量值可以很容易地作為新的因子加入到框架中。提供絕對(duì)測(cè)量的傳感器坛悉，比如GPS伐厌、羅盤指南盤或高度計(jì)，可以用來消除激光雷達(dá)慣性里程計(jì)在長時(shí)間運(yùn)行或特征比較少的環(huán)境中累計(jì)的軌跡漂移裸影。

位置識(shí)別也可以很容易地融入到系統(tǒng)中挣轨。為了提高系統(tǒng)的實(shí)時(shí)性能，我們提出了一種滑動(dòng)窗口方法轩猩，邊緣化舊的激光雷達(dá)幀進(jìn)行scan-matching掃描匹配卷扮。關(guān)鍵幀被有選擇性地添加到因子圖中，并且當(dāng)同時(shí)產(chǎn)生激光雷達(dá)里程計(jì)因子和閉環(huán)因子時(shí)均践，新的關(guān)鍵幀只關(guān)聯(lián)到固定大小的子關(guān)鍵幀集中晤锹。這種局部尺度而不是全局尺度的scan-match掃描匹配方法提高了LIO-SAM算法框架的實(shí)時(shí)性能。提出的LIO-SAM方法在不同環(huán)境的三個(gè)移動(dòng)平臺(tái)中采集的數(shù)據(jù)集上進(jìn)行了全面的評(píng)估彤委。結(jié)果表明鞭铆，與LOAM和LIO-Mapping相比，LIO-SAM可以獲得相似或更好的精度焦影。未來的工作包括在無人機(jī)上測(cè)試我們提出的LIO-SAM系統(tǒng)车遂。