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

來(lái)源：ICRA 2020

摘要

LINS是以濾波為主的IMU、激光雷達(dá)緊耦合的激光SLAM算法。該算法的主要?jiǎng)?chuàng)新點(diǎn)就是用以 IESKF(即迭代誤差卡爾曼)為框架，融合 IMU 與激光雷達(dá)。該算法于發(fā)表于 2020 年 ICRA， 由于網(wǎng)上已經(jīng)有些同學(xué)對(duì)算法做了介紹，一些基礎(chǔ)的知識(shí)本文不再贅述，本文將詳細(xì)圍繞以下兩個(gè)問(wèn)題介紹，希望對(duì)讀者理解算法有所幫助: 1、LINS 是如何將激光觀測(cè)融入濾波框架的? 2、濾波框架與優(yōu)化框架有何不同?

算法框架

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

結(jié)合代碼不難發(fā)現(xiàn)，算法的特征提取、以及 Mapping 部分都是直接沿用了 Lego-Loam 算法的，不再贅述。算法的核心創(chuàng)新點(diǎn)為前端激光里程計(jì)部分，對(duì)應(yīng)代碼的 lins_fusion_node 節(jié)點(diǎn)。

理論與代碼分析

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

IESKF 與 ESKF 最大的區(qū)別就是，在做狀態(tài)更新時(shí)是采用迭代求解的方式計(jì)算的，類似高斯牛頓等優(yōu)化方法求解時(shí)需要多次迭代直到收斂。通過(guò)迭代的求解的方式，與 ESKF 相比可以得到更精確的解。對(duì)應(yīng)論文公式 16。

3、LINS 代碼實(shí)現(xiàn)。LINS 中濾波相關(guān)的核心部分在 include/StateEstimator.hpp 中的 performIESKF()函數(shù)中實(shí)現(xiàn)。

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

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

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

當(dāng)我們使用 Gauss-Newton 求解待估狀態(tài)時(shí)，通常用到的公式是:

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

那么公式 16 變?yōu)?/p>

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

那么公式 16 可以變?yōu)?

到這里，其實(shí)我們看這個(gè)形式跟優(yōu)化的方法已經(jīng)很像了，但是跟標(biāo)準(zhǔn)的高斯牛頓求解方程還是不一致?，F(xiàn)在我們定義如下觀測(cè)方程:

則其對(duì)應(yīng)的雅可比矩陣為，

因此，

可以看到此時(shí)方程出現(xiàn)了我們熟悉的高斯牛頓的公式。因?yàn)闉V波的框架里面其實(shí)是同時(shí)考慮狀態(tài)方程與觀測(cè)方程，因此使用最小二乘的框架構(gòu)建的時(shí)候需要構(gòu)建成如上形式。 個(gè)人的觀點(diǎn)是，如果單從最終的公式來(lái)看，使用高斯牛頓求解兩幀點(diǎn)云的相對(duì)位姿，與 LINS 中使用 IESKF 求解結(jié)果并不完全相等，因?yàn)榍蠼獬鰜?lái)的狀態(tài)更新方程并不是完全一樣，但是從理論上說(shuō)，IESKF 的更新方程可以統(tǒng)一到最小二乘的框架里，因此效果上二者應(yīng)該是近似的。

算法效果

根據(jù)論文提供的實(shí)驗(yàn)結(jié)果，如果考慮前端+后端的整體處理效果，算法在公園與林間場(chǎng)景表現(xiàn)較好，但是在城區(qū)以及港口效果不及 LIOM。如果只考慮前端激光里程計(jì)，算法在除了港口場(chǎng)景外都是最優(yōu)的，畢竟論文的主要貢獻(xiàn)在前端。

總結(jié)

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

審核編輯：郭婷