在大規模場景下的地圖構建時候，純鐳射雷達感測器無法很好的完成地圖構建工作，這時需要考慮使用不同的感測器，獲取多種資料來進行感測器的融合，實現位姿的矯正，最終完成複雜大規模場景下的地圖構建，這種方法也被稱之為多感測器融合。

本篇文章要講述的是發表在ICRA2020上的一篇多感測器融合實現點雲地圖構建的文章。

涵蓋的知識面有：ESKF，IKF，IMU，Quaternion等。

論文地址：

https://arxiv.org/abs/1907.02233

1、論文概覽

R-LINS的本質是一個以機器人為中心的鐳射慣導狀態估計器。它使用以下兩種感測器來估計機器人的運動姿態：

6軸 IMU：高頻，聚焦自身運動，不採集外界環境資料 3D LiDAR：低頻，聚焦車體運動，採集外界環境資料 這兩種感測器想必大家都不陌生。但是，這兩個感測器在複雜大規模領域裡，單獨的依靠自己的資料是很難實現地圖構建的。所以，為了能夠在具有挑戰性的環境下也能保持魯棒性和計算效率，本文使用了迭代的誤差狀態卡爾曼濾波器(ESKF)來透過重複生成新的對應特徵來遞迴的修正機器人的狀態資訊。

為了防止濾波發散和降低計算成本，本文采用了一種新的以機器人為中心的公式，該公式重新定義了一個移動的區域性幀的狀態估計量，而不是像標準的以世界為中心的LIO中的固定全域性幀的狀態估計量。

這裡需要注意的是，LINS和LIO的區別如下：

本篇文章相比於LIO，降低了計算的時間消耗，是第一個使用ESKF的LIO框架。

2、雷達里程計分類

現在基於鐳射雷達里程計的劃分主要有三大類：

純雷達模型 松耦合的LiDAR-IMU模型 緊耦合的LiDAR-IMU模型 下面將依次介紹下對應的模型。

2.1、純雷達模型

一般來說純雷達的里程計模型大部分都是基於幀間匹配的，主要分為兩大類：

基於迭代最近點法（ICP） 基於正態分佈變換（NDT） 所以，現階段基於迭代最近點法的純雷達模型，比較出名的是LOAM框架，主要為LOAM和LeGO-LOAM

而基於正態分佈變換的純雷達模型，比較出名的是HDL-Localization。

這些演算法都是基於單獨的鐳射雷達感測器資料而進行的里程計推演，實現地圖構建。

2.2、松耦合的LiDAR-IMU模型

純雷達模型使用的感測器是鐳射雷達，可以很好的探測到外界的環境資訊。但是，同樣的，也會受到這些資訊的干擾，在長時間的運算中會產生一定的累計誤差。為了防止這種誤差干擾到後續的地圖構建中，需要使用另一種感測器來矯正機器人自身的位姿資訊。

在實際使用中，為了矯正鐳射雷達感測器所得到的位姿資訊，一般使用的是IMU感測器。

IMU感測器由於是自身運動估計的感測器，所以，採集的都是自身運動的姿態資訊。可以很好的矯正鐳射雷達里程計的位姿資訊。所以，通常使用鐳射雷達和慣導來進行資料融合，實現姿態資訊的矯正，這種模型也被稱之為LiDAR-Inertial Odometry（LIO） 。

這裡主要介紹的是松耦合的LiDAR-IMU模型。

高斯粒子濾波就是比較常見的松耦合模型。

松耦合模型由於是對兩個感測器的資料直接進行使用，得到新的姿態解算後的資料，並沒有構建新的損失函式，所以，得到的結果相對來說也不是特別的精準。

但是，由於其模型簡單，可以快速實現，所以，松耦合的多感測器融合模型，也有著自己的用處。

2.3、緊耦合的LiDAR-IMU模型

緊耦合餓到多感測器融合模型，是將各個感測器的資料都輸入到了一個模型之中，構建約束關係，使用最佳化方法最小化約束，得到最終的結果。其方法可以分為：

基於最佳化的緊耦合模型 基於EKF的緊耦合模型 基於最佳化的緊耦合模型目前比較熱門的是LIO演算法。

基於EKF的緊耦合模型目前比較熱門的是LIOM演算法。

本文提出的基於迭代ESKF的R-LINS是基於EKF模型的變種的一種緊耦合模型。

由於EKF模型是將非線性模型區域性線性化，所以，對於雷達觀測約束這種典型的非線性模型而言，容易出現效能低下甚至發散的情況，所以，需要使用一種針對非線性模型的卡爾曼濾波模型來替換EKF。本文使用的是誤差狀態卡爾曼濾波模型（ESKF）來最小化非線性約束，從而實現姿態的更新，這也是R-LINS的一個創新點。

3、LiDAR-Inertial建圖

本章節將著重講解LiDAR和IMU感測器融合實現地圖構建的部分。本文的系統架構如圖所示：

一共分為三大塊：

特徵提取：從原始點雲中提取穩定的特徵 LIO：狀態傳遞和狀態更新模組組成，使用迭代ESKF，輸出純里程資料和不失真的點雲特徵 地圖構建：細化純里程資料，得到全域性位姿資訊並輸出全域性地圖 其中，特徵提取使用的是LeGO-LOAM部分的特徵提取模組，LIO模組和之前的LIO論文不一樣，改變了ESKF作為約束最佳化的方法，地圖構建使用的是LeGO-LOAM部分的地圖構建模組。

由於本文的重點是LIO模組，所以，這裡直接略過了特徵提取和地圖構建，只講解LIO模組的內容。

這裡用到了一個迭代卡爾曼濾波。迭代卡爾曼濾波器主要目的在於克服EKF線性化過程中對高階誤差的捨棄而造成的估計不準確。“迭代“主要是指在量測更新過程中添加了一步迭代，直到狀態收斂。

3.1、狀態定義

這裡和VIO中表示使用誤差向量中的區域性重力不同。我們使用估計的roll和pitch，將重力從世界幀轉換到區域性幀，來獲取重力資訊。

本文使用的方法會更加合理一點，假設在極短的時間內，roll和pitch不會急劇變化，則在地圖閉環後，得到的結果有著很小的漂移，就可以得到比較精準的重力資訊了。

3.2、狀態遞推

可以推導得到：

其中，有個關係，大家需要注意：

估計值 = 測量值 - 偏差

透過上面的公式(1)，我們可以得到

3.3、狀態更新

我們知道，卡爾曼濾波（KF）是有著五大方程的，分別是：

預測：狀態預測方程，協方差預測方程

更新：狀態更新方程，協方差更新方程

卡爾曼增益方程

誤差狀態卡爾曼濾波（ESKF）也有著上面的五大方程組。唯一的區別在於卡爾曼濾波針對的是物體的運動狀態，而誤差狀態卡爾曼濾波針對的是物體運動狀態的誤差狀態。

在前面3.2部分，我們已經得到了狀態預測方程（公式2）和協方差預測方程（公式3）。剩下的就是要得到狀態更新方程，協方差更新方程和卡爾曼增益方程了。

本文用的是迭代卡爾曼濾波（IKF）。這裡就可以將狀態更新問題變成了一個最佳化問題

這裡其實就是LOAM的點線距離和點面距離的公式了：

所以，上面的式子其實可以拆解為：

這樣看的話，其實就能理解上面公式是什麼意思了。

其中，ESKF的卡爾曼增益方程和狀態更新方程：

這就是迭代的卡爾曼濾波和常規的卡爾曼濾波的區別之處。

當迭代終止的時候，假定終止下標為

，我們就可以算出協方差更新方程為：

3.4 狀態合成

我們前面算出來的是以機器人為中心的參照系下的位姿資訊。所以，為了得到全域性座標資訊，需要進行狀態合成，合成公式為：

3.5、初始化

我們使用的感測器為IMU和鐳射雷達，所以有一些引數資訊需要提前知曉：

4、地圖構建

透過前面的部分，我們已經得到了每個時刻精準的狀態估計資訊。就可以使用LeGO-LOAM進行地圖構建，得到全域性地圖。

但是也有一些不同。本文使用地圖構建模組匯出的roll和pitch作為LIO模組的反饋輸入，這種策略被證明可以有效地提高系統的魯棒性。

原文連結 https://zhuanlan.zhihu.com/p/134621840