作者：Chao Qin1 Haoyang Ye Christian E. Pranata, Jun Han, Shuyang Zhang, and Ming Liu

來源：ICRA 2020

摘要

LINS是以濾波為主的IMU、激光雷達緊耦合的激光SLAM算法。該算法的主要創新點就是用以 IESKF(即迭代誤差卡爾曼)為框架，融合 IMU 與激光雷達。該算法于發表于 2020 年 ICRA， 由于網上已經有些同學對算法做了介紹，一些基礎的知識本文不再贅述，本文將詳細圍繞以下兩個問題介紹，希望對讀者理解算法有所幫助: 1、LINS 是如何將激光觀測融入濾波框架的? 2、濾波框架與優化框架有何不同?

算法框架

為了講述通暢還是先介紹下 LINS 算法的主要框架，算法流程圖如下:

結合代碼不難發現，算法的特征提取、以及 Mapping 部分都是直接沿用了 Lego-Loam 算法的，不再贅述。算法的核心創新點為前端激光里程計部分，對應代碼的 lins_fusion_node 節點。

理論與代碼分析

（一）LINS 是如何將激光觀測引入濾波框架的?1、從代碼中可以看出，LINS只是前端里程計部分采用了濾波框架(即 scan-to-scan 估計幀間位姿)，而后端 scan-to-map部分仍然采用優化方式求解(即用 LM 方法求解位姿)。2、將點線/點面距離殘差作為觀測方程引入到代價函數中。根據論文公式 12，可以看到代價函數分為兩個部分，前面是最小化誤差狀態(即最小化后驗與后先驗狀態之差)，后面的復合函數 f(x)表示點線/面距離，即最小化點線/面距離。也就是說把點線/面距離殘差作為觀測方程，有了觀測方程，然后帶入卡爾曼濾波相關公式即可求解。不過，作為一個創新點，作者采用了 IESKF，即迭代誤差卡爾曼作為濾波框架。

IESKF 與 ESKF 最大的區別就是，在做狀態更新時是采用迭代求解的方式計算的，類似高斯牛頓等優化方法求解時需要多次迭代直到收斂。通過迭代的求解的方式，與 ESKF 相比可以得到更精確的解。對應論文公式 16。

3、LINS 代碼實現。LINS 中濾波相關的核心部分在 include/StateEstimator.hpp 中的 performIESKF()函數中實現。

代碼中基本是按照論文中的公式 12-18 實現的，比較好讀懂。代碼中的 findCorrespondingSurfFeatures(), findCorrespondingCornerFeatures() 函數即為 costfunction 中的點面/線距離殘差計算函數，基本沿用了loam的實現方式，其中jacobianCoffSurfs, jacobianCoffCorns 變量為點面/線距離殘差關于點的雅可比矩陣，該過程在之前推送的文章《泡泡點云時空 LOAM 專題-3】LOAM 代價函數設計與雅可比求解詳細推導(上)》有詳細的推導，感興趣的可以去看下。

代碼中，Hk_為點面/線距離殘差關于誤差狀態的雅可比矩陣，作者是用 BCH 近似求解的，其實含義上與 LOAM 算法在 Odometry，Mapping 部分最終求解得到的是一樣的，都是點面/線距離關于誤差狀態的雅可比矩陣，只不過求解方式表達形式不同，LOAM 中姿態是用歐拉角表示的，LINS 是四元數。

performIESKF()函數的剩余部分就是論文中公式 15-18 的實現，比較易懂就不贅述了。 （二）濾波與優化有何不同 其實，討論濾波框架與優化框架的不同是一個相對比較大的問題，為了更具體一些，我們僅就 LINS 算法使用的 IESKF 做的濾波的 LIO 前端與 LIO-SAM 等以高斯牛頓或LM優化為框架的 LIO 前端做對比(當然嚴格意義上來說，LIO-SAM 的前端其實只有預積分，省略了激光里程計，使用 scan-to-map 匹配校正 imu 零偏，但是不影響我們的分析過程)。 1、框架差異。從框架上說，LINS 使用的 IESKF 是將所有的狀態一起放到濾波框架里估計，而以 LIO-SAM 為代表的算法前端，是預積分+scan-to-map 算法，其中 scan-to-map 模塊使用 LM方法計算幀圖位姿，得到計算出的位姿后，輸入預積分模塊校正imu 零偏。也就是說 imu 零偏的估計與激光匹配位姿計算是分開的，而 LINS 使用了 IESKF 將二者當作狀態一起估計。 2、理論差異。其實在 1993 年的論文[2]中就已經證明了 IKF 的更新方程與 Gauss-Newton 理論上是等價的。詳細的推導過程可以參考論文，為了方便理解，我們在這里做一個簡化的推導。 參照 LINS 論文中公式 15-17

當我們使用 Gauss-Newton 求解待估狀態時，通常用到的公式是:

其實從方程的形式上，怎么看這個方程也與 LINS 中的公式 16 差別挺大。下面我們嘗試盡量直白的解釋下 Gauss-Newton 與 IESKF 更新方程的等價性。 對于一些剛接觸SLAM的同學來說，可能有一個疑問是Gauss-Newton 求解時很少用到協方差矩陣，但是卡爾曼的更新方程中的卡爾曼增益是需要計算協方差矩陣的，如何理解? 其實 Gauss-Newton 只是一個公式，當我們設計誤差函數時帶有協方差矩陣，那么 Gauss-Newton 求解過程中就會有協方差矩陣的計算，實際上論文[2]中也正是通過這種方式證明的等價性。由于我們實際在用 Gauss-Newton 求解時基本不考慮協方差矩陣(指點云匹配求解位姿時)，為了更好的理解，我們假設噪聲相關的矩陣都為單位矩陣，那么 LINS 論文中的公式 15 變為:

那么公式 16 變為

到這一步可以看到跟 Gauss-Newton 的方程不一致。這個時候要借助一個重要的公式實現轉換，即 matrix inversion lemma，即矩陣求逆引理，即如果 A,C 為非奇異矩陣，那么有

那么公式 16 可以變為:

到這里，其實我們看這個形式跟優化的方法已經很像了，但是跟標準的高斯牛頓求解方程還是不一致。現在我們定義如下觀測方程:

則其對應的雅可比矩陣為，

因此，

可以看到此時方程出現了我們熟悉的高斯牛頓的公式。因為濾波的框架里面其實是同時考慮狀態方程與觀測方程，因此使用最小二乘的框架構建的時候需要構建成如上形式。 個人的觀點是，如果單從最終的公式來看，使用高斯牛頓求解兩幀點云的相對位姿，與 LINS 中使用 IESKF 求解結果并不完全相等，因為求解出來的狀態更新方程并不是完全一樣，但是從理論上說，IESKF 的更新方程可以統一到最小二乘的框架里，因此效果上二者應該是近似的。

算法效果

根據論文提供的實驗結果，如果考慮前端+后端的整體處理效果，算法在公園與林間場景表現較好，但是在城區以及港口效果不及 LIOM。如果只考慮前端激光里程計，算法在除了港口場景外都是最優的，畢竟論文的主要貢獻在前端。

總結

本文通過理論分析與代碼對比介紹了 LINS 算法，其中主要介紹了 LINS 如何使用濾波框架做激光 SLAM，以及優化與濾波的區別。主要是把前端激光里程計部分改為了濾波框架，并且引入了 IMU 做緊耦合。同時，我們也推導了濾波的更新方程其實可以由高斯牛頓法推導而來。希望對讀者有所幫助，如果錯誤煩請指出。

審核編輯：郭婷