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

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

I 引言

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

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

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

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

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

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

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

II 相關(guān)工作

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

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

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

III 數學(xué)模型

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

A. Intensity Calibration

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

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

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

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

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

B. Salient Point Selection and Feature Extraction

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

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

C. Intensity Map Building

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

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

D. Scan to Map Matching

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

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

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

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

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

3)位姿估計：可通過(guò)最小化幾何殘余和強度殘余來(lái)估計最終位姿：

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

E. Loop Closure Detection & Global Optimization

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

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

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

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

IV 實(shí)驗評估

B. Evaluation Metric

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

實(shí)驗結果：

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

2）KITTI數據集中05序列多種方法的對比

3）數據集上強度輔助方法和非強度輔助方法的結果分析

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

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

V 結論

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