ICRA2021| Intensity-SLAM：基于強度輔助的大規(guī)模環(huán)境定位和建圖

摘要：SLAM是一項基于傳感器感知（如LiDAR和相機）可用于估計機器人位置并重建環(huán)境的任務。它廣泛應用于自動駕駛等機器人應用場景。傳統(tǒng)基于 LiDAR 的 SLAM 算法主要利用場景上下文中的幾何特征，而忽略了來自 LiDAR 的強度信息。最近一些基于深度學習的 SLAM 算法會考慮強度特征并以端到端的方式訓練位姿估計網(wǎng)絡(luò)。然而，它們需要大量的數(shù)據(jù)收集工作，并且它們對訓練環(huán)境以外的環(huán)境的普遍性仍不清楚。本文中，我們接收了基于強度特征的 SLAM 系統(tǒng)。我們提出了一種新穎的完整 SLAM 框架，它利用了幾何和強度特征。所提出的 SLAM 涉及基于強度的前端里程計估計和基于強度的后端優(yōu)化。進行了包括室外自動駕駛和室內(nèi)倉庫機器人實驗。其結(jié)果表明，所提出的方法優(yōu)于現(xiàn)有的僅幾何的 LiDAR SLAM 方法。

I 引言

定位是機器人技術(shù)的基本和基本主題之一。隨著機器人工業(yè)的發(fā)展，機器人定位在過去的幾十年里變得更加具有挑戰(zhàn)性：從已知到未知環(huán)境，從簡單到復雜環(huán)境，從靜態(tài)到動態(tài)環(huán)境[1]，從短期到長期定位[2]。通常，固定錨（或路由器）設(shè)置在預定義的區(qū)域中，機器人位姿通過使用機器人與多個錨的距離來獲得，例如超寬帶(UWB)[3]和WI-FI定位[4]。但是，這些方法依賴于外部設(shè)置，并且主要用于小規(guī)模環(huán)境。為了解決傳統(tǒng)定位方法的局限性，引入了同時定位和建圖 (SLAM) 來估計機器人位姿 [5]。它獨立于外部設(shè)置，因此在機器人應用中很有前景。根據(jù)使用的感知系統(tǒng)，SLAM可以進一步分為Visual SLAM（V-SLAM）和LiDAR SLAM。與 Visual SLAM 相比，LiDAR SLAM 對天氣和光照等環(huán)境變化更加準確和魯棒 [6]。

傳統(tǒng)的 LiDAR SLAM 方法，如 LOAM[7] 和 HDL-Graph-SLAM [10]，主要關(guān)注僅依靠幾何信息以最小化點云差異。這樣，強度信息常常被忽略。請注意，強度信息與材料的反射率有關(guān)，并且對于不同的物體是不同的，這對于定位和物體識別很有用。因此，我們認為一個強大的 LiDAR SLAM 系統(tǒng)應該考慮到這樣的強度信息。雖然最近有一些工作旨在使用強度信息來提高準確性，但它們主要采用卷積神經(jīng)網(wǎng)絡(luò)（CNN）來擬合原始激光掃描數(shù)據(jù)，沒有任何特定的強度分析或公式化。此外，需要收集大量數(shù)據(jù)來訓練神經(jīng)網(wǎng)絡(luò)，這在實際應用中可能很麻煩。

在之前的工作 [8] 中，我們已經(jīng)證明強度信息對于穩(wěn)健的回環(huán)檢測非常有用。本文中，我們進一步利用強度信息來提高 SLAM 系統(tǒng)的定位精度。我們提出了一種新穎的 SLAM 框架，它使用幾何和強度信息進行里程計估計。我們首先分析強度測量的物理模型。然后，我們在里程計估計公式中為現(xiàn)有的僅依靠幾何的代價引入了額外的強度代價。最后，我們結(jié)合了基于強度的閉環(huán)檢測和后端優(yōu)化來進一步提高性能。所提出的框架使用室內(nèi)倉庫環(huán)境機器人和室外自動駕駛汽車進行了測試。結(jié)果表明，我們的方法在多種環(huán)境中提供了可靠和準確的定位，并且優(yōu)于僅依靠幾何的方法。本文的主要貢獻如下：

1）我們提出了一種新穎的 SLAM 框架，它使用強度和幾何信息進行定位估計，包括前端里程計估計和后端因子圖優(yōu)化。所提出的方法是開源的。

2）我們建議構(gòu)建一個強度圖來揭示強度分布，并將強度代價引入現(xiàn)有的僅依賴幾何形狀的 LiDAR SLAM 以估計機器人的位置。

3）對所提出的方法進行了全面的評估。更具體地說，我們的方法在倉庫和自動駕駛場景中進行了測試。結(jié)果表明，對于多種場景，我們的方法優(yōu)于現(xiàn)有的僅依靠幾何方法。

本文的組織如下：第二部分回顧了現(xiàn)有 LiDAR SLAM 方法的相關(guān)工作。第三節(jié)描述了所提出方法的細節(jié)，包括強度校準、特征選擇、里程計估計和基于強度掃描上下文 (ISC) 的圖形優(yōu)化。第四部分展示了實驗結(jié)果并與一些現(xiàn)有工作進行了比較，然后是第五部分的結(jié)論。

II 相關(guān)工作

大多數(shù)現(xiàn)有的 LiDAR SLAM 工作都集中在環(huán)境的幾何信息上。最流行的點云匹配方法之一的迭代最近點 (ICP) 方法，該方法將當前點與目標幀中的最近點進行匹配 [9]。ICP 通過最小化點對之間的歐幾里德距離來迭代地尋找最佳點對應關(guān)系，直到變換矩陣收斂。該算法廣泛用于 SLAM 系統(tǒng)，例如 HDL-Graph SLAM [10] 和 LiDAR-Only 里程計、定位[11]。然而，所有點都用于計算，這對于每次掃描數(shù)萬個點的 LiDAR 來說計算成本很高。ICP 對噪聲也很敏感。在自動駕駛等實際應用中，測量噪聲（例如，路邊樹木的測量）可能很重要，隨后會導致定位漂移。一種更健壯且計算效率更高的方法是利用幾何特征。在 LiDAR Odometry And Mapping (LOAM) [7] 中，通過分析局部平滑度引入了一種簡單的特征提取策略。根據(jù)局部平滑度將特征點分割為邊緣特征和平面特征，然后分別通過邊緣到邊緣和平面到平面的匹配計算機器人位姿。類似的想法也用于輕型和地面優(yōu)化的 LiDAR 里程計 (LeGO-LOAM) [12]，其目標是為無人地面車輛 (UGV) 提供計算效率高的 LiDAR SLAM。激光掃描首先被分割成地面點和非地面點。從地面點提取平面特征，從非地面點提取邊緣特征。平面特征用于估計滾轉(zhuǎn)和俯仰角以及 z 平移。結(jié)果隨后用于匹配非地面點以計算 x 和 y 平移以及偏航角。對于接地機器人，LeGO-LOAM 實現(xiàn)了比 LOAM 更高的定位精度。然而，強度信息被忽略，在這些方法中只使用幾何信息。

近年來，一些工作試圖通過深度學習將強度信息引入 SLAM。在 DeepICP 中，提出了一種基于端到端學習的 3D 點云配準來尋找機器人位姿[13]。強度信息和幾何通道都被引入深度特征提取 (DFE) 層以找到關(guān)鍵點。不是搜索最近的點，而是使用對應點生成 (CPG) 層根據(jù)學習到的一組候選者之間的匹配概率生成關(guān)鍵點對應關(guān)系，這使得反向傳播成為可能。Deep-ICP 使用 KITTI [14] 和 Apollo-SouthBay 數(shù)據(jù)集 [15] 進行了驗證，并且優(yōu)于現(xiàn)有方法，例如 Generalized-ICP [16] 和正態(tài)分布變換 (NDT) [17]。Chen等還引入了一個端到端的 CNN 框架來識別后端 SLAM 的兩次激光掃描的重疊 [18]。幾何信息和強度信息通過 11 層網(wǎng)絡(luò)進行訓練以生成 3 維特征。通過對提取的特征應用另一個較小的層來估計重疊，而通過訓練特征的互相關(guān)來估計偏航角變化。在 KITTI 數(shù)據(jù)集上的實驗表明，從強度和幾何信息估計的偏航角優(yōu)于僅基于幾何的深度學習框架?；谏疃葘W習的方法采用端到端訓練，無需進一步分析強度信息。然而，在實踐中收集、標記和訓練數(shù)據(jù)通常是困難和耗時的。此外，環(huán)境變化下的性能可能不一致，即訓練后的網(wǎng)絡(luò)可能在與訓練數(shù)據(jù)相似的環(huán)境中表現(xiàn)良好，但在轉(zhuǎn)移到另一個環(huán)境時會失敗。

其他的一些工作嘗試使用強度信息并將強度集成到位姿估計中，而無需端到端訓練。Tian等提出了一種強度輔助 ICP方法，用于 2D-LiDAR 的快速配準 [19]。與傳統(tǒng)的ICP算法相比，引入了目標函數(shù)，根據(jù)空間距離和強度殘差確定初始剛體變換估計。迭代次數(shù)減少了 10 倍，因此所提出的算法能夠在單核CPU 下實時運行。在 [20] 中，強度模型被合并到稀疏集束調(diào)整 (SBA) [21] 估計問題中。作者分析了強度測量的物理模型，并提出了一種增強 3D 定位、強度和表面法線的新測量模型。仿真結(jié)果表明，強度測量的添加以更少的地標實現(xiàn)了類似的 SLAM 精度。這項工作在 [22] 中進一步擴展，使用飛行時間 (ToF) 相機進行實驗。關(guān)鍵點是通過尺度不變特征變換 (SIFT) [23] 或二元穩(wěn)健不變可縮放關(guān)鍵點 (BRISK) [24] 從強度假想中提取特征來選擇和跟蹤的。該實驗主要側(cè)重于通過計算機視覺技術(shù)進行的視覺里程計 (VO)，但并未展示 LiDAR SLAM 在大規(guī)模環(huán)境中的性能。Khan等提出了一種數(shù)據(jù)驅(qū)動的方法來校準強度信息 [25]。然后通過最小化點對的強度殘差而不是幾何殘差來解決掃描匹配。隨后將其集成到 HectorSLAM [26] 中，與原始 Hector SLAM 相比，顯著降低了漂移誤差。但是，它僅限于小規(guī)模環(huán)境中的 2D 定位和建圖。

III 數(shù)學模型

所提出的強度框架包括前端激光里程計和后端閉環(huán)檢測。系統(tǒng)概述如圖2所示。前端包括強度校準、特征提取和里程計估計，后端包括閉環(huán)檢測和位姿圖優(yōu)化。

A. Intensity Calibration

激光雷達****激光束，測量到達時間和反射信號的能量。物體在傳感器坐標中的位置由****角度和距離（即到達時間）確定。強度值由從反射信號接收的能量與****的激光功率之比確定。接收功率Pr的物理原理可確定為[21]：

其中Pe是****激光束的功率，Dr是接收器孔徑，ηsys是系統(tǒng)****因子，ηatm是大氣****因子，α是物體表面與激光束之間的入射角，ρ是物體的材料反射率。測量距離R和入射角α為外部參數(shù)。而ηsys和ηatm是常數(shù)參數(shù)。因此，強度測量I由[21]確定：

其中，ηall是一個常數(shù)。因此，表面反射率ρ僅與入射角α和測量距離R通過：

對于激光雷達掃描，該距離可以很容易地測量到。因此，可以通過分析局部法線來估計入射角。對于每個點p∈P3，我們可以搜索最近的兩點p1和p2，以便局部表面法線n可以表示為：

因此，我們可以從具有幾何讀數(shù)P和強度讀數(shù)I的原始掃描中推導出校準強度掃描I?，特別是在一些傳感器中默認使用距離測量部分校準，因此我們只將校準應用于入射角度。此外，低強度值往往會導致較低的信噪比(SNR)，這也降低了在低值[27]下進行強度測量的測距精度。低強度的點在實踐中被忽略了。

B. Salient Point Selection and Feature Extraction

激光雷達掃描通常由數(shù)萬個點組成。使用ICP等原始點云匹配方法的計算效率較低。此外，原始數(shù)據(jù)包含自動駕駛從路邊樹木上測量的點，這將降低匹配精度。因此，將點云與[7]，[13]特征相匹配更健壯和計算效率。本文使用了基于幾何形狀和強度信息的特征，而不是只使用幾何形狀的特征。校準的強度信息包含揭示不同物體分布的環(huán)境的反射率輪廓。因此，強度信息也有助于識別在多個幀中的相同特征。對于點Pi∈P及其強度值ηi?∈i?，我們搜索附近的Ni∈P，并計算局部距離分布σgi和強度分布σIi通過：

其中，wg和wI分別是幾何形狀和強度分布的權(quán)重。請注意，對于墻等平面，平滑度值較??；而對于拐角點或邊緣點，平滑度值較大。因此，對于每次掃描，邊緣特征從大σ點中選擇Pε∈P，并從低σ點中選擇平面特征Ps∈P。

C. Intensity Map Building

強度地圖M包含了周圍環(huán)境的反射率分布。對于大多數(shù)只有幾何的SLAM，地圖通過占用網(wǎng)格[28]或Octomap [29]維護和更新。三維空間被分割成網(wǎng)格單元，每個單元都由一個概率函數(shù)表示。類似的想法可以用于構(gòu)造和更新一個強度圖。我們使用強度測量I(ηi|z1:t)來表示每個網(wǎng)格單元mi[26]，而不是概率函數(shù)。更具體地說，對于時間t觀察網(wǎng)格單元，表面反射率可以通過：

其中M(mi|z1:t)是當前的強度觀測值，nmi是對mi的總觀測次數(shù)。要注意，如果網(wǎng)格不包含對象，則強度將標記為0，因為沒有反射信號。

D. Scan to Map Matching

激光里程計是估計當前幀到全局圖之間的變換矩陣T∈SE(3)的任務。通過最小化幾何誤差和強度誤差來計算最佳位姿估計：

1)幾何殘差：與LOAM[7]類似，通過將當前邊緣和平面特征與全局地圖匹配來計算幾何誤差。它可以通過最小化點到邊和點到平面的殘差來實現(xiàn)。給定邊緣特征 pi∈Pε和變換點 p?i = Tpi，我們可以從全局地圖中搜索兩個最近的點 pε1 和 pε2。點到邊殘差定義為：

2)強度殘差：將特征與強度圖匹配計算強度殘差?？梢酝ㄟ^最小化當前點pi（包括邊緣特征和平面特征）與強度圖中的轉(zhuǎn)換點p?i之間的強度殘差來實現(xiàn)：

其中M(p?i)是強度圖M中的點強度值，為了在強度圖中搜索強度信息，我們引入了三線性插值。雖然用最近的網(wǎng)格單元來表示強度更直接，但強度信息不太精確，特別是對于網(wǎng)格分辨率較低的大尺度建圖。對于每個轉(zhuǎn)換點p?i=[xi，yi，zi]T，我們可以找到周圍的八個網(wǎng)格單元。如圖3所示，這些網(wǎng)格單元的強度測量結(jié)果標注為M1（p?i）、M2（p?i）、……、M8（p?i）。使單元1的中心位置（最接近原點）為p1=[x1、y1、z1]T，并且單元8的中心位置（離原點最遠）為p2=[x2、y2、z2]T，其中x2?x1、y2?y1和z2?z1分別為每個網(wǎng)格單元的寬度、高度和深度。目標點的強度估計計算為：

同樣地，我們也可以推導出M34、M56和M78。

3)位姿估計：可通過最小化幾何殘余和強度殘余來估計最終位姿：

它可以用LM算法[30]來求解。注意，通過假設(shè)恒定的角速度和線速度，相比假設(shè)相同的變換矩陣可以增加收斂速度。

E. Loop Closure Detection & Global Optimization

閉環(huán)檢測的目標是從歷史數(shù)據(jù)中識別出重新訪問的場景。對于僅前端的 SLAM 系統(tǒng)，不可避免地會出現(xiàn)測量噪聲的漂移。為了減少定位漂移，后端 SLAM 通過識別相同的位置來回環(huán)檢測。我們使用關(guān)鍵幀選舉來減少檢索時間，因為閉環(huán)檢測的計算成本通常很高。關(guān)鍵幀的選擇基于以下標準：(1) 機器人的位移是顯著的，即大于預先定義的閾值；(2)旋轉(zhuǎn)角度變化顯著；(3)經(jīng)過的時間超過一定時間。對于大規(guī)模環(huán)境，閾值設(shè)置得更高以降低計算成本。所有關(guān)鍵幀都存儲在后端維護的位姿圖中。

對于每個關(guān)鍵幀，我們使用強度掃描上下文 (ISC) [9] 將當前幀提取為全局簽名。與 GLAROT3D [31]、NBLD [32] 和掃描上下文 [33] 等僅幾何描述符相比，ISC 在識別回環(huán)閉合時對旋轉(zhuǎn)變化具有魯棒性。ISC 是一個二維矩陣，通過將極坐標在方位角和徑向方向上等分為 Ns 個扇區(qū)和 Nr 個環(huán)來計算。每個子空間由區(qū)域內(nèi)點的最大強度表示。給定一個關(guān)鍵幀，我們可以從強度和幾何信息中提取 ISC 描述符Ω。為了將查詢 ISC 描述符Ωq與候選 ISC 描述符Ωc進行比較，讓 Vqi 和 Vci 是 Ωq和Ωc 的第 i 列。相似度分數(shù)φ(Ωq,Ωc) 是通過取每個扇區(qū)的平均余弦距離來找到的：

可以通過在φ(Ωq,Ωc)上設(shè)置一個閾值來確定回環(huán)檢測。

回環(huán)檢測能夠有效地識別環(huán)路對。然而，錯誤檢測會導致位姿圖優(yōu)化失敗。為了防止誤報，使用幾何一致性驗證來檢查候選幀的相似性。對于候選循環(huán)幀，我們從全局地圖中搜索附近的線和平面信息。并且對于當前幀，通過最小化點到邊緣和點到平面的距離來提取邊緣和平面特征并將其匹配到相應的全局線和平面。當兩幀不相關(guān)時，距離的總和通常很高。因此，可以通過對其設(shè)置閾值來過濾掉誤報。確定重新訪問的位置后，我們可以將兩幀之間的邊添加到位姿圖中，并且可以應用全局優(yōu)化來校正漂移[34]。

IV 實驗評估

B. Evaluation Metric

為了評估定位的準確性，我們使用平均平移誤差(ATE)和平均旋轉(zhuǎn)誤差(ARE)[15]：

實驗結(jié)果：

1）在KITTI數(shù)據(jù)集上研究結(jié)果。從左到右為序列00、02、05、07和09。強度-SLAM的結(jié)果為綠色，參考值為紅色。

2）KITTI數(shù)據(jù)集中05序列多種方法的對比

3）數(shù)據(jù)集上強度輔助方法和非強度輔助方法的結(jié)果分析

4）KITTI數(shù)據(jù)集中05序列上不同方法的性能比較

5）我們在倉庫環(huán)境中的實際機器人上測試我們的方法的性能。圖6(a)為實物圖；圖6(b)為SLAM結(jié)果，將機器人的軌跡繪制為紅色。

V 結(jié)論

本文中，我們?yōu)榛?LiDAR 的定位系統(tǒng)提出了一種新穎的強度輔助完整 SLAM 框架。現(xiàn)有方法主要利用僅依靠幾何特征來估計位置并忽略強度信息。我們從強度測量的物理模型開始，并說明強度信息的使用。我們引入了用于位姿估計的強度圖和強度殘差，以提高定位精度。此外，我們提出了一個由前端和后端系統(tǒng)組成的完整 SLAM 結(jié)構(gòu)，即 Intensity-SLAM。在不同場景下進行了實驗，包括倉庫中的室內(nèi) AGV 和市區(qū)的室外自動駕駛。實驗結(jié)果表明，通過集成強度信息，定位精度得到顯著提高。與最先進的僅依靠幾何方法相比，所提出的方法還實現(xiàn)了較低的平移誤差和旋轉(zhuǎn)誤差。