點擊上方“3D視覺工坊”，選擇“星標”
乾貨第一時間送達
作者：robot L | 來源：知乎
https://zhuanlan.zhihu.com/p/137954753
本文僅做學術分享，如有侵權，請聯繫刪除。
摘要
本文中，我們提出一種新的激光慣性里程計和建圖方法來達到實時，低漂移和魯棒的位姿估計在大尺度高速環境中。提出的方法主要由四部分組成，幀預處理模塊，動態物體檢測模塊，激光-慣性里程計模塊和激光建圖模塊。幀預處理模塊使用慣性測量來補償每個激光幀的運動擾動。然後，動態物體檢測模塊被用於檢測和移除每幀的動態物體通過應用CNN分割網絡。在獲得沒有運動物體的未擾動點雲後，激光慣性里程計模塊使用誤差狀態卡爾曼濾波器來融合激光和IMU的數據並以高頻率輸出粗糙的位姿估計。最終，激光建圖模塊執行精準的處理步驟同時“幀-模型”掃描匹配策略被用於創造一個靜止全局地圖。我們比較我們方法的表現和兩個SOTA方法，LOAM和SuMa，使用KITTI數據集和真實高速場景數據集。實驗結果展示了我們的方法效果優於SOTA方法在真實高速場景並達到了具有競爭力的表現在KITTI數據集中。
S. Zhao, Z. Fang, H. Li, S. Scherer. A Robust Laser-Inertial Odometry and Mapping Method for Large-Scale Highway Environments. IEEE/RSJ International Conference on Intelligent Robots and Systems, 1285-1292, 2019.
1. 介紹
對於智能移動機器人，SLAM是一個基本技術以達到自運動估計和建圖在未知環境中。自運動估計對於汽車控制是重要的，其中地圖對於障礙物感知和路徑規劃非常重要。最近幾年，大量基於視覺[1]-[3]和激光[4]的SLAM方法被提出，其提供了許多令人滿意的解決方案。與基於視覺的SLAM方法相比，基於激光的方法更可靠在視覺退化的環境中，並提供更魯棒和準確的位姿估計。然而，基於激光的SLAM算法仍然不能在所有場景中達到魯棒的位姿估計。其中一個具有挑戰性的場景是高速環境。
高速環境帶來了許多挑戰。例如
速度快：速度一般會達到70km/h，其導致點雲在每幀中的擾動並影響建圖的準確率。
缺乏幾何特徵：大多數高速環境缺少垂直方向的幾何特徵。
高運動物體：高速上有很多運動的車輛。
激光迴環區域：高速上很少有迴環場景，迴環檢測策略很難修正軌跡誤差。
不幸的是，大多數激光SLAM方法假設環境是靜態的。同時，因爲這些方法依賴激光提供的測量，它們很難處理劇烈運動在幾何退化的環境中。因爲這兩種限制，這些方法很難提供魯棒和準確的運動估計在高速場景中。
受上述討論的啓發，在本工作中我們展示了一種新穎的激光-慣性里程計和建圖方法（LIOM），其不僅達到了魯棒的位姿估計而且建立了一個靜止的高速地圖，如圖1所示。主要貢獻如下：
我們提供了一個實時的激光里程計和建圖流程，同時利用CNN分割網絡和激光-慣性框架。CNN分割網絡移除了動態物體在每幀激光測量中的的影響，激光-慣性框架克服了劇烈運動並達到了高速環境的運動估計。
爲了驗證我們提出的解決方案，大量實驗被執行在各種挑戰環境中，例如具有運動物體，劇烈運動和大尺度的的環境。實驗結果展示了我們的方法能夠同時在靜止和動態高速環境中工作。
剩餘部分如下：第2節展示了大量與3D激光掃描儀相關的位姿估計算法。第3節描述了提出方法的細節。定量和定性評估在第5節展示。第6節總結全文。
2. 相關工作
爲了允許移動機器人達到長期自主在挑戰性環境中，許多研究人員致力於如何提高激光位姿估計方法的魯棒性。以高速環境中的運動估計爲例，兩種相關的問題需要被解決：如何解決劇烈運動，如何排除動態物體以達到魯棒的運動估計。
A. 處理劇烈運動
由於激光掃描儀的低掃描幀率，一幀不能被看作是一個剛體當傳感器的外部運動非常劇烈時。然而，大多數激光SLAM方法假設一幀中的激光點共享一個座標系，其等價於認爲在掃描過程中沒有外部運動。這種假設是不合理的，並且會導致運動擾動[6]。
爲了解決該問題，Zlot[7]使用IMU技術作爲運動模型並匹配連續的激光幀來達到運動估計。Le[6]提出一種外參標定框架，使用IMU測量並建模點雲的運動擾動通過預積分技術。Ji[8]4]展示了一種魯棒的多傳感器融合流程以估計自運動並建立一個一致的地圖在多種挑戰性環境中。該方法使用一個IMU用於運動預測和一個視覺-慣性耦合方法來解決劇烈運動問題。然而，因爲上述方法主要考慮靜止環境，它很難達到好的結果在高速場景中。
B. 處理動態物體
有大量SLAM的解決方案在動態環境中。但是，大多數方法基於視覺傳感器，例如RGB-D相機[2][3]或雙目相機[1]。相反地，動態環境中基於激光的方法相對較少。大多數基於激光的SLAM方法假設周圍環境是靜態的，導致動態環境中的一系列問題，例如位姿估計和迴環檢測的失效。
爲了解決該問題，Walcott[9]考慮地圖構建過程中的時間因素，並維護一個準確的地圖在動態環境中。但是，這種方法主要被設計爲低動態環境，其不適合高動態環境例如高速場景。Fehr[10]使用貝葉斯模型來獲取點是動態還是靜態的，其能夠構建一個一致的地圖在動態環境中。Rendong[11]和Masoud[112]使用提高的RANSAC算法來跟蹤運動物體並達到運動估計在動態環境中。但是，因爲上述方法不能整合IMU測量，這些方法很難處理劇烈運動並達到準確的運動估計在高速場景中。Jiang[13]提供了一種激光-相機SLAM系統，使用基於稀疏子空間聚類的運動分割方法來構建動態環境中的靜止地圖。儘管如此，提出的系統不能實時運行並需要足夠的環境光照。因此，應用場景非常有限。Johannes[14]提供一種激光-相機SLAM方法，拒絕運動物體通過語義標號。但是，缺點是它僅使用了lidar來提供視覺特徵的深度測量，並且沒有利用lidar來達到獨立的運動估計。因此，該方法不能達到魯棒的位姿估計在視覺退化的環境中。
在最近幾年，一些其他方法被提出用於3D激光。Jens[15]提供了一種稠密的基於面元的方法用於運動估計和建圖。Young-Sik[16]提供了一種激光-相機SLAM系統，基於直接法並達到了好的實時表現。Jean-Emmanuel[17]提供了一種新的激光SLAM方法，使用特定的採樣策略和新的幀圖匹配方法。但是，上述方法假設環境靜止，很難達到魯棒的運動估計在高速環境中。
從上述的討論中，我們可以發現基於LiDAR的位姿估計和建圖方法在高速環境中沒有被解決。本文致力於達到實時，低漂移和魯棒的位姿估計在大尺度高速環境中。
3. 激光-慣性里程計和建圖
在本節，我們介紹我們Laser-Inertial Odometry and Mapping（LIOM）方法，如圖2所示，由四部分組成。上述模塊允許魯棒的運動估計和建圖在高速環境中。
圖2. LIOM綜述
A. 掃描預處理模塊
由於車輛在高速環境中的劇烈運動，點雲擾動存在於每一幀，影響位姿估計的準確度。因此，移除點雲擾動非常必要。這裏，我們主要考慮由於車輛的非均勻運動導致的點雲運動擾動。實時步驟如下：
第1步：用時間戳標記激光幀中的激光點，如圖3所示。根據等式1，每一幀的所有激光點都被標記時間戳：
其中 
第2步：找到從時間戳 
第3步：通過整個過程，我們可以獲得IMU在時刻 
第4步：使用線性插值方法以確定IMU在時刻 
第5步：我們要解決的是當前激光點相對開始點由於非均勻運動導致的運動擾動。由於當前激光點和開始點的位置 
第6步：變換開始點座標系中的所有激光點，並且對每個激光點，提取它們的點雲運動擾動
B. 動態物體檢測模塊
在該步驟，一個全卷積神經網絡（FCNN）[18]被應用來準確檢測和分割運動物體，例如車輛，行人和自行車。該過程可以被分爲四個連續步驟：1. 通道特徵提取。2. 基於CNN的障礙物預測。3. 障礙物聚類。4. 後處理。
通道特徵提取：在該步驟，輸入點雲被投影到2D X-Y平面。根據點的X和Y座標，每個點被分到一個2D柵格。然後每個柵格內的點集的八個統計量被計算並喂入FCNN。我們建議讀者參考[18]進一步瞭解八個統計量的細節。
基於CNN的障礙物預測：FCNN被用於預測逐柵格的障礙物性質，包括中心偏置，物體類別，置信度，物體高度和類別概率。
物體聚類：上述五個柵格物體屬性被用於生成一個障礙物物體。然後一個壓縮的並查集被使用來尋找物體類別的候選。
後處理：在障礙物聚類後，我們獲得一組候選物體簇。後處理進一步精修潛在的候選簇並輸出最終的簇通過預先設計的參數。
這四個步驟將會移除動態物體，並保持靜態背景點雲用於位姿估計和建圖。
C. 激光-慣性里程計模塊
爲了魯棒的位姿估計，我們使用誤差狀態卡爾曼濾波器（ESKF）來實現激光掃描儀和IMU的傳感器融合。與傳統的卡爾曼濾波器方法相比，該方法主要有兩個優點。首先，誤差狀態很小，意味着我們將忽略二階乘積並減少計算量。第二，朝向誤差狀態很小，避免了過參數化和萬向鎖問題。
在本節，我們將給出簡單的運動估計和測量更新的描述。運動預測被用於提供狀態預測通過IMU測量，同時測量更新被用於修正預測的狀態。更多細節被發現在[19][2]。
1）運動預測：運動預測由兩部分組成：誤差狀態動力學和傳播過程。
a）誤差狀態動力學：根據之前的工作[20]，真實狀態 
這裏 
誤差動力學系統如下：
其中 
b）傳播過程：傳播過程包含預測狀態傳播和誤差協方差傳播。我們直接使用歐拉積分來傳播預測狀態。誤差協方差矩陣被推導從線性化誤差狀態運動學開始。傳播規則的表達式在等式6和7中：
其中 
2）測量更新：測量更新由四部分組成：觀測模型，恢復測量，修正和重置名義狀態。
a）觀測模型：在誤差狀態形式中，觀測模型可以被寫爲：
b）恢復測量：在運動預測過程中，我們獲得了位姿先驗 
其中 
c）修正：一旦測量 
d) 重置名義狀態：最終，名義狀態被更新通過等式(14)
在完成上述過程後，魯棒的激光-慣性里程計以高頻率（100Hz）被獲得。
D. 激光建圖模塊
爲了進一步提高位姿估計的準確率，我們採用近似LOAM算法[8]的建圖策略。主要不同是我們應用了多線程NDT算法[21]（代碼：koide3/ndt_omp）以完成“幀-模型”掃描匹配而不是基於特徵的方法。原因是NDT方法更魯棒相對於特徵方法來說。因此，它能夠提供更準確的運動估計在幾何退化的環境中，特別是高速公路場景。
運行時間展示
推薦閱讀：
重磅！3DCVer-學術論文寫作投稿 交流羣已成立
掃碼添加小助手微信，可申請加入3D視覺工坊-學術論文寫作與投稿 微信交流羣，旨在交流頂會、頂刊、SCI、EI等寫作與投稿事宜。
同時也可申請加入我們的細分方向交流羣，目前主要有3D視覺、CV&深度學習、SLAM、三維重建、點雲後處理、自動駕駛、CV入門、三維測量、VR/AR、3D人臉識別、醫療影像、缺陷檢測、行人重識別、目標跟蹤、視覺產品落地、視覺競賽、車牌識別、硬件選型、學術交流、求職交流等微信羣，請掃描下面微信號加羣，備註：”研究方向+學校/公司+暱稱“，例如：”3D視覺 + 上海交大 + 靜靜“。請按照格式備註，否則不予通過。添加成功後會根據研究方向邀請進去相關微信羣。原創投稿也請聯繫。
![]( "3D視覺工坊小助理微信.jpg.jpg")
▲長按加微信羣或投稿
![]()
▲長按關注公衆號
3D視覺從入門到精通知識星球：針對3D視覺領域的知識點彙總、入門進階學習路線、最新paper分享、疑問解答四個方面進行深耕，更有各類大廠的算法工程人員進行技術指導。與此同時，星球將聯合知名企業發佈3D視覺相關算法開發崗位以及項目對接信息，打造成集技術與就業爲一體的鐵桿粉絲聚集區，近1000+星球成員爲創造更好的AI世界共同進步，知識星球入口：
學習3D視覺核心技術，掃描查看介紹，3天內無條件退款
![]()
圈裏有高質量教程資料、可答疑解惑、助你高效解決問題
