Tightly Coupled LiDAR Inertial Odometry and Mapping源碼解析（一）

• 1. LiDAR inertial odometry and mapping簡介
• 2. Tightly coupled LiDAR inertial odometry
• • 2.1 LiDAR-IMU odometry overview
  • 2.2 IMU and pre-integration
  • 2.3 De-skewing and feature extraction
  • 2.4 Relative lidar measurements

1. LiDAR inertial odometry and mapping簡介

在利用3D LiDAR進行SLAM計算時，有時會遇到一些問題，例如兩幀之間的運動較為劇烈(大轉角)可能會導緻點雲配準算法失效，在某些場景如長廊場景下有效測量點雲較少使運動估計精度下降等，這些現象影響了SLAM算法的魯棒性。而IMU(Inertial Measurement Unit)作為一種慣性測量單元，可以精确地測量載體的三軸加速度以及三軸角速度，在上述的場景中(大轉角，長廊環境)可以為LiDAR提供額外的先驗資訊，進而增強LiDAR算法的魯棒性，是以LiDAR-IMU fusioin成為一種近年來備受關注的SLAM方法。事實上，camera-IMU融合已經有一些開源的SLAM算法，例如VINS Mono。在ICRA 2019 上，終于迎來了第一個開源的LiDAR-IMU SLAM算法LIOM(Lidar-Inertial Odometry and mapping)，由香港科技大學的劉明老師及學生共同完成，其主要貢獻如下：

• 提出了一種緊耦合的LiDAR-IMU裡程計算法，可實作實時、高精度以及高更新率(High update rate)的裡程計計算
• 利用LiDAR-IMU裡程計的先驗資訊，基于旋轉限制的優化方法進一步優化位姿和點雲地圖，以生成全局一緻、魯邦的地圖和位姿估計
• 進行了大量的室内外實驗測試，實驗結果優于LiDAR-only或LiDAR-IMU松耦合方法
• 第一個開源的LiDAR-IMU緊耦合SLAM算法(LIO-Mapping)

2. Tightly coupled LiDAR inertial odometry

2.1 LiDAR-IMU odometry overview

整個LiDAR-IMU裡程計的算法流程如下圖所示：

為了更好的了解LIO算法，我們一起看下面這幅時序圖：

假設目前的時間戳是 i i i，則由于一般IMU裝置的頻率(100-1000Hz)會遠遠高于LiDAR點雲資料的更新頻率(10Hz)，是以在下一幀點雲即時間戳 j j j到來之前，會有大量的IMU資料 τ i , j \tau_{i,j} τi,j​讀入，如上圖中紫色線條所示。則可以根據這段時間内IMU的資料對IMU的狀态進行估計，即流程圖中的state-prediction部分，同時利用這段時間間隔内的IMU測量量進行預積分操作，即對應流程圖中的Pre-integration。當到達時間戳 j j j也即新的雷射雷達資料幀過來後，由于雷射雷達為連續測量，即在時間戳 i i i到時間戳 j j j過程中，雷射雷達的測量是在不斷運動且位姿不斷變化的過程中測量得到的，是以會産生一定的運動畸變(motion distortion)。此時，在獲得時間戳 j j j對應的雷射點雲資料 S j S_j Sj​後，應首先根據IMU的state-prediction消除運動畸變即流程圖中的De-skewing部分進而得到未畸變的點雲 S ‾ j \overline{S}_j Sj​。

在得到畸變矯正後的點雲 S ‾ j \overline{S}_j Sj​後，為減少計算量，采用了LOAM中的方法從點雲提取了兩種特征 F L j F_{L_j} FLj​​，分别是角點以及平面特征點，即對應流程圖中的Feature Extraction部分。然後就是根據局部地圖 M L o , i L p M_{L_{o,i}}^{L_p} MLo,i​Lp​​，确定特征與局部地圖的對應關系，也即流程圖中的Find relative lidar measurements部分。所謂的局部地圖，也就是将上圖所示的從時間戳 o o o到時間戳 i i i的特征點都投影到一個坐标系下形成一個點雲局部地圖，在本文中，每次都是投影到中間的時間戳即 p p p對應的坐标系下，即對應流程圖中的Local map management。最後在确定了測量限制和IMU的預積分限制後，建立非線性最小二乘的目标函數并進行聯合優化求解，即對應流程圖中的Joint non-linear optimization部分。接下來，我們對LIO中的每一個子部分進行詳細解析。

2.2 IMU and pre-integration

一般來講，一個三軸正交的IMU裝置可以測量三軸的加速度以及角速度，由牛頓運動定律，若初始狀态(位置，姿态，速度)已知，則可以根據加速度以及角速度的測量跟蹤一個物體的狀态(位置，姿态，速度)，是以廣泛的應用于慣性導航(Inertial Navigation System)中。在涉及到IMU的導航系統中，通常IMU的狀态可以模組化為如下向量：

X B i W = [ p B i W T , v B i W T , q B i W T , b a i T , b ω i T ] T X_{B_i}^{W}=[{p_{B_i}^W}^T,{v_{B_i}^W}^T,{q_{B_i}^W}^T,b_{a_i}^T,b_{\omega_i}^T]^T XBi​W​=[pBi​W​T,vBi​W​T,qBi​W​T,bai​T​,bωi​T​]T

其中 p B i W T {p_{B_i}^W}^T pBi​W​T表示的是IMU在 i i i時刻在世界坐标系 W W W下的位置， v B i W T {v_{B_i}^W}^T vBi​W​T表示的是IMU在 i i i時刻在世界坐标系 W W W下的速度，而 q B i W T {q_{B_i}^W}^T qBi​W​T表示的是IMU在 i i i時刻在世界坐标系 W W W下的姿态(四元數)，最後兩個 b a i T , b ω i T b_{a_i}^T,b_{\omega_i}^T bai​T​,bωi​T​則是IMU原始測量資料即加速度以及角速度測量的偏置，可以了解為一種IMU原始測量資料的誤差模組化。在本文中，由于涉及到LiDAR-IMU融合，LiDAR與IMU之間為剛性連結且未先前标定，是以狀态向量除了IMU狀态向量 X B i W X_{B_i}^{W} XBi​W​外，還要估計LiDAR和IMU之間的相對位姿關系，即:

T B L = [ p B L , q B L ] T T_B^L=[p_B^L,q_B^L]^T TBL​=[pBL​,qBL​]T

根據高中學過的牛頓運動定律 x = x 0 + v 0 t + 1 2 a t 2 , v = v 0 + a t x=x_0+v_0t+\frac{1}{2}at^2,v=v_0+at x=x0​+v0​t+21​at2,v=v0​+at，我們可以根據IMU的原始測量資料以及前一時刻 i i i，IMU的狀态，對目前時刻 j j j，IMU的狀态進行更新，即：

p B j W = p B i W + ∑ k = i j − 1 ( v k Δ t + 1 2 ( g W + R k ( a ^ k − b a k ) ) Δ t 2 ) v B j W = v B i W + ∑ k = i j − 1 ( g W + R k ( a ^ k − b a k ) ) Δ t q B j W = q B i W ∏ k = i j − 1 δ q k p_{B_j}^W=p_{B_i}^W+\sum_{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}(g^W+R_k(\hat{a}_k-b_{a_k}))\Delta t^2)\\ v_{B_j}^W=v_{B_i}^W+\sum_{k=i}^{j-1}(g^W+R_k(\hat{a}_k-b_{a_k}))\Delta t\\ q_{B_j}^W=q_{B_i}^W\prod_{k=i}^{j-1}\delta q_k pBj​W​=pBi​W​+k=i∑j−1​(vk​Δt+21​(gW+Rk​(a^k​−bak​​))Δt2)vBj​W​=vBi​W​+k=i∑j−1​(gW+Rk​(a^k​−bak​​))ΔtqBj​W​=qBi​W​k=i∏j−1​δqk​

是以，時間戳 i i i到時間戳 j j j内IMU的運動可以利用預積分模組化為：

z i , j = { Δ p i j , Δ v i j , Δ q i j } z_{i,j}=\{\Delta p_{ij},\Delta v_{ij},\Delta q_{ij}\} zi,j​={Δpij​,Δvij​,Δqij​}

2.3 De-skewing and feature extraction

這一部分主要實作點雲畸變矯正以及從畸變矯正的點雲中提取特征，主要借鑒了LOAM的畸變矯正以及特征提取方法，是以在這裡僅簡單描述一下畸變矯正以及特征提取的思路，細節還請閱讀LOAM源碼。

我們都知道，雷射雷達的測量與攝像頭的測量不太相同。攝像頭的拍照可以了解為one-shot，即快門曝光的時刻。而雷射雷達的測量則是連續測量，也即每一幀點雲中的不同點其實都是在不同時刻測量得到的。而車載的雷射雷達又都是在不斷運動的，這就導緻該幀點雲中的每一個點其實都是在不同的時刻測量得到的。是以需要根據IMU的加速度以及角速度資訊(插值等方法)預測每一個雷射掃描點測量時，該點對應的雷射雷達的位置。具體這部分具體實作也可以參考LOAM源碼。

2.4 Relative lidar measurements

這部分非常有趣，剛看這篇論文時十分困惑，我去relative lidar measurements，以前沒聽過啊我靠，好高端，得好好研究研究，甚至去逐行看了源碼(源碼品質不錯)。研究了一番後，發現其實也不過是為了給優化函數添加限制建立了一種特征點到局部地圖的關聯罷了，其實從這個角度看還是蠻好了解的。

如下圖所示，這篇論文采用了滑動視窗(sliding window)的方法，圖中B表示的是IMU坐标系，L表示的是雷射雷達坐标系，在運動過程中假設兩者之間的相對位姿關系不發生變化(剛性連接配接)。藍色的大視窗即是論文中說的sliding window，在源碼中該window設定為包含了15個雷射雷達幀大小的視窗，其中從 o o o到 p p p為之前優化過的幀，而從 p p p到 i i i對應的幀為待優化的幀。

為了建立特征點到局部地圖的關聯關系，當然應該首先建立局部地圖對吧。有的同學可能會問了，直接進行點雲幀之間的關聯不就可以了嗎，這樣不就是一個簡答的雷射裡程計嗎？問題在于啊，一幀的點雲通常還是比較稀疏的，尤其是線數比較低的點雲，建立稠密局部地圖可以使得特征之間的比對更為準确。現在假設我們根據2.3節中所述LOAM的特征提取方法，分别提取出來了每一幀的平面特征點，但是這些特征點都是定義在自己的局部雷射雷達坐标系 O L i O_{L_i} OLi​​内的哦。是以為了建立坐标系統一的局部地圖，我們首先應該要找一個基準，然後大家把自己的點雲都投影到這個基準下面。在論文中，這個基準就是 p p p時刻對應的雷達坐标系 O L p O_{L_p} OLp​​。接下來的問題就是怎麼投影了，其實就是一些簡單的坐标變化關系，我們一起來看一下。

現在已知 i i i時刻和 p p p時刻IMU坐标系到世界坐标系的位姿變化關系即 T B i W T_{B_i}^{W} TBi​W​和 T B p W T_{B_p}^{W} TBp​W​，且已知雷射雷達到IMU坐标系之間的變換關系 T L B T_L^B TLB​(外參：待标定)，則根據坐标變換的鍊式法則，可以得到 i i i時刻雷射雷達坐标系到 p p p時刻雷射雷達坐标系之間的變換關系為：

T L P = T L B T B i W T B p W − 1 T L B − 1 T_L^P=T_L^BT_{B_i}^W{T_{B_p}^W}^{-1}{T_L^B}^{-1} TLP​=TLB​TBi​W​TBp​W​−1TLB​−1

在建立完局部地圖(Local map)後，就要尋找特征到局部地圖的對應關系了。對于待優化視窗(紅色)内的某一幀，我們可以根據2.2節中IMU的預積分得到該幀的IMU位姿，然後就可以根據上式的坐标變換關系将該幀内的特征點 F l F_l Fl​也投影到 p p p對應的雷射雷達坐标系内。對于 F l F_l Fl​内的每一個平面特征點，我們需要确定其與特征地圖的對應關系。也就是說，如果所有測量都沒有誤差，由于局部地圖是由平面特征點構成的，則 F l F_l Fl​中的每一個特征點都應該落在局部地圖的平面上。然而，實際情況是IMU和雷達的測量都會存在誤差，導緻預估的位姿不準确，同時雷射雷達與IMU的相對位姿關系也存在誤差，是以該特征點不是嚴格的落在局部地圖的平面上。是以我們可以建立點到面的對應關系，首先在局部地圖内搜尋5個緊鄰點，然後這5個近鄰點可以拟合出一個平面，這樣就可以用點到面的距離來作為限制啦。看起來比較簡單，不過實作的時候還是有很多細節的，将在後面的源碼部落格中一一介紹，目前這篇部落格先專注于LiDAR-IMU緊耦合的原理。

我們再回頭看2.1節中的算法流程圖，似乎隻有聯合優化(Joint non-linear optimization)沒有講了，考慮到這部分比較重要，且這篇部落格篇幅有點長，将在下一篇部落格中介紹。