（每日一讀2019.10.21）緊耦合的3D雷達慣性里程計及建圖（LIO）

論文：pdf
代碼：github
視頻：video

摘要

自身運動估計是大部分移動機器人應用的基本需求。通過傳感器融合，我們能夠補償單個傳感器的缺陷並且提供更可靠的估計。在本文中，我們引入一種緊耦合的雷達-慣導融合的方法。通過聯合最小化雷達和慣導的測量損失，雷達-慣導里程計(LIO)在長期的試驗後，在可接受的漂移，甚至是在激光雷達測量會降級的具有挑戰性的情況下表現不錯。此外，爲了獲取雷達位姿的更可靠的估計，我們還提出了一種旋轉約束精煉算法(LIO-mapping)來進一步對齊雷達位姿和和全局地圖。

I 介紹

自身運動估計在許多導航任務中起着重要作用，是自主機器人的關鍵問題之一。它提供了機器人姿態的認知，並能向姿態控制器提供即時反饋。此外，它與感知環境的各種傳感器一起，爲同時定位和映射（slam）提供了關鍵信息。準確估計機器人的姿態有助於降低風險，並有助於成功規劃。激光雷達傳感器可以爲周圍環境提供距離測量，在機器人系統中得到了廣泛的應用。具體來說，典型的三維激光雷達可以在360度的水平視場（FOV）下以10Hz左右的頻率來感知周圍環境。作爲一種主動傳感器，它對光照具有不變性。激光雷達傳感器具有高可靠性和高精度，我們很多人都會使用這種傳感器。
儘管激光雷達傳感器具有許多優點，但它還不夠完善，存在一些缺點。

• 從激光雷達本身來看，它的垂直分辨率較低，得到的稀疏點雲提供的特徵有限，使得特徵跟蹤成爲一個棘手的問題（跟蹤算法）。
• 安裝在移動機器人上的激光雷達也會受到運動畸變的影響，（需要去除運動畸變算法）這直接影響到傳感精度。
• 在實際場景中，存在一些激光雷達退化（獲得的數據無效）的情況，其中激光雷達接收的點很少或丟失。例如，在狹窄的走廊環境中，3d lidar只能接收很少的可用點。接收點主要來自側牆，只有一小部分是從天花板和地板觀察到的。在這種情況下，匹配的激光雷達特徵很容易導致非約束姿態估計。
• 更新率低。這限制了它在需要快速響應的任務中的應用，例如機器人姿態的控制。

本文針對上述問題，提出了一種緊耦合的三維激光雷達imu姿態估計算法。激光雷達和慣性測量單元的測量結果用於聯合優化。爲了實現實時和更一致的估計，採用固定滯後平滑和邊緣化舊的姿態，然後進行旋轉約束優化。我們工作的主要貢獻如下：

• 提出了一種緊密耦合的激光雷達里程計算法。它提供了實時準確的狀態估計和高更新率。
• 考慮到來自激光雷達IMU里程計的先驗信息，旋轉約束優化方法進一步優化了最終姿勢和生成的點雲地圖。即使在某些激光雷達退化的情況下，它也能確保一致和穩健的估計。
• 通過廣泛的室內和室外測試驗證了該算法。它的性能優於目前最先進的純激光雷達或鬆耦合雷達+imu算法。
• 在線提供源代碼1。這是第一個可供社區使用的用於緊密耦合的lidar和imu融合的開源實現。

II 相關研究

有幾種方法與imu和lidar測量的融合有關。一個重要的範疇是鬆耦合融合。這類方法分別考慮了激光雷達的估計和慣性測量單元的估計。在文獻[1]中，利用imu輔助的lidar里程計依賴於imu計算的方向，並且在使用加速度時假設速度爲零。它將激光雷達和慣性測量單元的測量數據解耦，主要以慣性測量單元爲先驗，無法利用慣性測量單元進一步優化。在文獻[2]中，採用鬆耦合擴展卡爾曼濾波器（ekf）融合二維情況下的imu和lidar，但它不能處理三維或更復雜的環境。Lynen等人[3]提出了一種模塊化的方法，在三維情況下，通過ekf將imu測量與其他相對姿態測量（例如，從相機、激光雷達甚至壓力傳感器）融合。這種鬆散耦合的方法具有計算效率，但是比緊耦合方法[4]更不精確，因爲它將里程計部分作爲一個黑盒，並且不使用來自IMU的測量更新它。緊耦合方法是另一個重要的範疇。對於二維平面運動估計，Soloviev等人[5]提出了一種在二維激光雷達掃描中提取和匹配直線的方法，其中傾斜的激光雷達由IMU的預測方向進行補償。在激光雷達測量領域中，採用卡爾曼濾波對imu狀態進行校正。Hemann等人[6]提出了一種以誤差狀態卡爾曼濾波器的形式將IMU傳播與積累的激光雷達高度圖緊密耦合的方法。利用激光雷達高度圖與先驗數字高程模型（dem）的匹配更新狀態修正。該方法在環境已知的情況下，顯示了遠距離gps拒絕導航的能力，但沒有先驗的地圖信息，該方法無法正常工作。在[7]和[8]中，直接從慣性測量單元得到的原始測量值和從連續軌跡得到的預測慣性測量值用於計算待優化的殘差。這些方法不涉及狀態的轉換和估計，這使得即使使用額外的攝像機，系統在快速運動下也不可行[9]。受其他視覺慣性工作[10]、[11]的啓發，我們設計了一種緊密耦合的激光雷達imu融合方法。我們“預集成”並使用原始IMU測量和激光雷達測量來優化整個系統的狀態，這可以在激光退化情況下或在運動迅速時工作。據我們所知，我們是少數幾個適合複雜三維環境的三維激光雷達imu融合算法之一。

III 符號註釋

載體運動過程中從第i幀到第j幀的預積分。

IV緊耦合激光雷達里程計

爲了確保有效的估計，許多關於lidar建圖的工作，如[7]、[1]和[13]，將任務分爲兩部分，里程計和建圖。受這些工作的啓發，提出的系統由兩個並行部分組成。第一部分是緊密耦合的激光雷達IMU里程計，它優化了一個局部窗口內的所有狀態。第二部分是旋轉約束優化（導致全局一致的建圖過程），它使用來自優化姿勢和重力約束的信息將激光雷達掃描與全局地圖對齊。

A 雷達-IMU 里程計框圖

圖1提供了雷達-IMU里程計的簡要概述。

• （1）我們可以基於之前的數據的狀態估計，將lidar的原始數據$S_j$和IMU的原始輸入i和j（從上一個時間戳i到當前時間戳j）來進行新的狀態優化。
  在$S_j$到達之前IMU狀態通過公式2進行更新。
• （2）同時這些輸入被預積分用於聯合優化。
• （3）當收到最新的激光雷達掃描$S_j$時先對原始數據進行去除運動畸變處理
• （4）應用特徵提取來降低數據的維度（這裏不清楚具體如何提取來降低維度）並提取最重要的特徵點$F_{L_j}$
• （5）根據先前相應的優化狀態，局部窗口內之前的lidar特徵點$F_{L_{o,i}}$被邊緣化(marged)爲局部地圖$M_{L_{o,i}}^{L_p}$
• （6）利用預測的$F_j$激光雷達姿態，我們可以找到相對激光雷達測量$m_{L_{p+1,j}}$
• （7）最後一步是聯合非線性優化，採用相對激光雷達測量和IMU預積分，獲得局部窗口內狀態的MAP估計。將第一步中的優化結果應用於更新預測狀態，以避免imu漂移。

B 去傾斜和特徵提取

三維激光雷達內部有旋轉機構，可接收一整週的數據。當3d激光雷達運動時，來自它的原始數據$S_j$會受到運動畸變的影響，這使得掃描點與真實位置不同。爲了解決這一問題，我們利用imu傳播對lidar運動$T_{L_j}^{L_j}$的預測，並假設掃描過程中是線性運動模型。然後，對每個點$x(t)∈S_j∈R^3$進行$T_{L_j}^{L_j}$的線性插值校正，得到去除傾斜的掃描數據，其中$t∈ t_{j^`},t_j$是掃描點的時間戳，t_{j^`}和t_j分別是掃描開始和結束的時間戳。

C 激光相對測量

通過IMU和另一個能夠提供傳感器對相對姿態的傳感器的融合，估計出的狀態，如果我們固定第一個參考系，$X_B^W$和$T_B^L$將是局部可觀測的。爲了適當地融合來自imu的預積分，我們建議使用掃描之間的相對lidar測量來約束lidar姿態，作爲算法1。在找到點對應之前，我們先建立一個局部地圖，因爲一次掃描中的點密度不足以計算精確的對應。
局部地圖包含來自nm離散時間戳{o，···，p，···，i}的lidar特徵點，其中o，p和i分別是窗口內第一次lidar掃描、pivot lidar掃描和最後處理的lidar掃描的時間戳，如圖2所示。局部地圖$M^{L_p}_{L_{o,i}}$是在pivot lidar的框架內從特徵掃描而來。$F^{L_p}_{L_γ}，γ∈{o,····,i}$，通過先前優化的激光雷達姿態$T_{L_p}^{L_γ}$進行變換。待估計狀態是ns時間戳 {p+1,····,i,j} 處的狀態，其中p+1和j是激光雷達掃掠 pivot1旁邊的時間戳和窗口中當前激光雷達掃描的時間戳。

D 雷達幀匹配

激光雷達相對測量可以提供pivot雷達姿態和以下激光雷達姿態之間的相對約束。我們的方法優化了優化窗口中的所有姿態，包括第一姿態$T^W_{L_p}$，即$F_{Lp}$不固定。因此，激光雷達代價函數中的每一項都涉及兩個激光雷達掃描的姿態，$T^W_{L_p}$和$T^W_{L_α}$，α∈{p+1,···,j}。優化pivot姿態有助於更好地減小預積分誤差，並確保傳感器對與重力對準。我們估計的狀態是imu的狀態，因此需要引入外部參數來表示imu狀態對lidar的約束。從後一個激光雷達姿態到窗口中的軸姿態的相對轉換可以定義爲

根據前面的對應關係，每個相對激光雷達測量的殘差m=[x，ω，d]∈$m_{L_α}$，α∈{p+1，···，j}可以表示爲點到平面的距離.

V 旋轉約束的優化

將特徵點加入到全局地圖而不是局部地圖可以將激光雷達姿態約束到一致的世界幀$F_W$。我們的改進方法使用相對激光雷達測量值$m_L$。由於全局地圖是精細化的副產品，我們也把它稱爲建圖方法。最近的激光雷達特徵點與全局地圖對齊後形成的代價函數如下：

式中$T=T^W_L$是最新估計的激光雷達姿態，$m$是特徵點$x$在$F_L$和係數$ω$，$d$在$F_W$中定義的相對激光雷達測量。然後我們可以使用類似的高斯牛頓法來最小化$C_M$。利用殘差$C_M$和雅可比$J^C_p$和$J^C_θ$進行優化，其中$θ$是相應四元數q的誤差狀態，但由於累積的旋轉誤差，經過長期的運算，合併後的全局地圖不能與重力精確對齊。這可能導致進一步的建圖錯誤地與傾斜的地圖對齊。受文獻[16]在SE2約束下優化SE3的啓發，我們提出了一種約束建圖策略。該策略利用了來自激光雷達imu里程計的旋轉約束，確保最終地圖始終與重力保持一致。圖3示出了旋轉約束建圖的結構。
給定沿z軸的方向具有更高的不確定性，並且方向的其它兩個DOF更接近於真值，我們可以通過修改方向的雅可比（12）中的詳細推導來約束代價函數，

其中表示最後一次迭代中狀態的估計，z是w.r.t fw方向的信息矩陣的近似，x和y可以通過fw中x軸和y軸方向與z軸方向的信息比得到。
之後，我們使用$J^C_p$和$J^C_{θ_z}$作爲雅可比，這是優化步驟所需要的。增量的激光雷達姿態可以用$\delta θ_z$和$\delta θ_p$表示，從而得到最新的激光雷達狀態p和q

VI 運行

本節介紹了不同的傳感器配置、系統初始化以及室內外測試的不同參數。

A 不同傳感器的配置

傳感器對的配置可以不同。對於手持式傳感器對，例如圖4a中的傳感器對，lidar和imu彼此接近。因此，管道與之前介紹的一樣。但對於安裝在汽車上的傳感器對，這兩個傳感器通常彼此相距較遠。例如，圖4b示出了安裝在汽車的基座連桿上方的imu，而激光雷達則安裝在汽車的前部。在公式（6）中增加了一個用於汽車測試的外部平移參數的先驗項，而不是自動校準所有平移參數。

B 初始化

首先，沒有對激光雷達特徵點進行姿態估計。因此，需要大致精確的匹配算法。在初始化階段採用了[1]的激光雷達里程計。在提供的激光雷達姿態下，需要傳感器對的充分運動，以使IMU狀態可見[14]。然後利用激光雷達的姿態和imu的測量值來初始化imu的狀態，這些狀態可以用文[10]和文[17]中介紹的方法來求解。在幾秒鐘內進行測試。vii，我們遵循[10]中的初始化方法，該方法也對外部參數進行線性初始化。接下來，利用初始狀態和新的測量值，在局部窗口內迭代進行非線性優化以估計狀態。

VII 測試和分析

A 定量分析

爲了定量分析我們的方法，使用圖4a所示的傳感器對。帶有16條線路的Velodyne VLP-16激光雷達安裝在XSENS MTI-100 IMU上方。反射標記可以通過運動捕捉系統提供地面真實姿態。激光雷達配置爲10Hz更新速率，IMU更新爲400Hz。使用[18]將不同方法估計的軌跡與地面真實情況對齊。
不同運動條件下的試驗：表1顯示了不同運動速度和不同方法下的均方根誤差（rmse）結果，其中loam[1]被視爲基線。lio是我們的局部窗口優化的里程計方法。lio-raw和lio-no ex分別與lio期望的運動補償或在線外部參數估計被截斷相同。lio映射是由具有旋轉約束的建圖結果得到的。最好的兩個結果用粗體顯示。結果表明，在任何情況下，lio映射都能提供平移（位置）和旋轉（方向）狀態的精確估計。lio在運動速度較快時有較好的性能，產生更多的imu激勵。但是，如果運動緩慢，它會受到漂移的影響，因爲此時的局部地圖相對稀疏。該表還顯示，通過運動補償和在線外部參數估計，lio可以提供更好的性能，特別是在運動速度很快的情況下。

B 定性分析

爲了顯示在具有挑戰性的場景中的改進，進行了幾種不同傳感器配置和環境的測試，包括室內手持和室外校園高爾夫球車測試（配置如圖4所示），以及在Kaist Urban數據集上的測試[19]。由於空間有限，這些姿態估計和映射結果顯示在補充視頻中。

VIII 總結

本文提出了一種新的緊密耦合激光雷達里程計方法。它包括里程計的狀態優化和帶旋轉約束的優化。結果表明，我們的方法優於目前最先進的激光雷達方法和鬆耦合方法。儘管所提出的方法需要初始化，但即使在具有挑戰性的測試場景（例如快速運動情況、激光雷達退化情況和有限重疊的激光雷達掃描）下，通過充分的IMU激勵，我們的方法確實顯示了具有快速更新率的魯棒姿態估計結果。