01 傳感器介紹

IMU(加速度計)的測量頻率高，即可以精確的測量到物體的姿態運動，對運動靈敏，同時成本低，體積小，抗干擾能力強，基本上在多傳感器融合中是一個必備的傳感器。但IMU也有局限性，第一點是測量值存在隨機游走的bias，第二點是由于通過積分的形式計算出那個姿態，因此會存在累計誤差，導致長時間的積分結果不可靠。因此常是IMU+雷達或IMU+視覺相機進行計算。

因為視覺相機比較輕小，價格便宜，所以一般是基于視覺相機的SLAM被廣泛應用在無人機，AR/VR等輕載重的平臺上。但視覺相機也存在局限性，第一是單目相機無法直接獲得可靠的3D幾何信息，第二是基于多視圖的方法可以獲得3D幾何信息，但需要消耗大量的計算資源，而且在視覺紋理缺失、大量紋理重復場景下的效果比較差，第三是相機的SLAM方法對環境光照的要求會比較苛刻。

激光雷達(LiDAR)傳感器可以提供高精度稠密的3D點云，因此可以直接獲得六自由度的狀態估計，還能重建出周圍環境高精度的3D地圖。但同樣也存在局限性，首先是激光雷達傳感器本身就又貴又大又重，使得無法用在一些輕量的平臺上，第二是分辨率相比視覺相機是比較低，同時刷新率也低，約為12Hz，第三是幀內的點云會存在運動畸變，第四點是在 周圍環境特征不足的情況下，會發生退化現象。

在市區里面，到底是用視覺相機跑SLAM還是用激光雷達跑SLAM實驗更合適點？這里更偏向采用視覺相機+激光雷達的方式，相機用來觀測周圍環境，激光雷達直接測量環境的3D幾何形狀。在特性排序上，相機獲得的圖像是有序的，連續的2D測量，雷達獲得的是無序的，離散的3D測量。

02 激光雷達(慣導)SLAM

首個基于固態激光雷達的SLAM系統(loam-livox)

近期激光雷達的發展一般都致力于降低設備成本的同時提高有效性，其中備受關注的是各種固態雷達，比如微電機系統掃描，光相位陣列，Risley棱鏡等。因此，在這樣的背景下，loam-livox工作是以livox-MID40為代表的首個固態激光雷達的SLAM框架。livox-MID40和傳統雷達對比如圖1所示，可以看出來紅色livox-MID40的FoV比藍色的傳統雷達是小的多，右圖中是livox-MID40的掃描軌跡圖。

圖1 livox-MID40和傳統雷達對比

在此項工作之前，有許多優秀的方法，比如:2014年提出的使用激光雷達完成定位與三維建圖的LOAM算法, 提出了一種特征點的提出方法，使用點-面ICP的形式，完成了基于一個單線激光的3D定位和建圖工作; 2018年提出LeGO-LOAM，其相對于LOAM的提升主要在于輕量級和地面優化，即在前端使用了分割模塊來區分地面點和非地面點，接著對這些點進行特征提取，后端是設計了一個加速算法，進一步提高算法的運行速度。使得LeGO-LOAM在各種平臺甚至是Nvidia的Tx2平臺上實現了實時的定位和建圖。

由于固態雷達的特性，例如非重復掃描的模式不能作frame-to-frame的匹配，小FoV的雷達對feature點要求更高，對異常值更敏感，更容易受動態物體的影響，因此這些優秀的算法并不能直接適用了固態激光雷達。所以，實現了首個適用于固態激光雷達，能夠實時魯棒低漂移的SLAM系統。系統框圖如圖2所示，先做特征點提取，然后進行面到面的匹配后去更新地圖，重建KD樹以供下一次的搜索。

圖2 loam-livox系統框圖

點云去幀內運動模糊如圖3所示，一開始使用線性插值(圖3(b))，發現效果不太好，后面使用分段處理(圖3(c))，即把一幀分為三段，然后去重新估計姿態，因為三小段上幀內的運動模糊不明顯。圖3(d)是開源手持設備和采集場景。

圖3 去幀內運動模糊

激光雷達-慣導緊耦合的里程計(FAST-LIO)

在此項工作之前，也存在許多優秀的工作。LIO-mapping 是香港科技大學劉明組所提出的一個緊耦合的雷達慣導融合方法，在LiDAR Odometry 融合了IMU信息以進一步提高定位精度和魯棒性。此外，為了獲取雷達位姿的更可靠估計，提出了一種旋轉約束精煉算法(LIO-mapping)來進一步對齊雷達位姿和和全局地圖。LIO-Mapping借鑒LOAM和VINS-Mono的思想進行的一個聯合狀態估計問題。

工程層面和VINS-Mono一樣，只不過前端視覺部分換成了lidar的前端進行特征關聯和odometry的位姿估計。其主要分為兩個部分：tightly coupled lidar-IMU odometry與the rotation constrained refinement。LIO-SAM是Tixiao Shan，在LeGO的基礎上，提出了一種緊耦合激光-慣性里程計方法，通過優化包含LiDAR里程計因子，IMU預積分因子，GPS因子和回環因子來得到機器人的全局一致的位姿。作者使用幀-局部地圖匹配代替LOAM的幀-全局地圖匹配，提高了幀圖匹配的效率。但是他們都或多或少存在一些問題，導致他們在實際使用中并不夠魯棒，精度不夠高，運行速度慢的問題。

因此在此類背景下提出了FAST-LIO系列。FAST-LIO是一個高效的緊耦合的雷達慣導融合框架，是基于迭代卡爾曼濾波器，濾波器里提供雷達觀測和IMU觀測，主要貢獻在于降低了大規模觀測數量導致的計算量。此外，還提出了FAST-LIO2，其主要創新在于使用了一個增量式KD樹去維護地圖的更新，有效解決了傳統方法中重建時導致地圖越來越大的問題。

FAST-LIO框架如圖4(a)所示，首先以雷達點云作為數據，然后進行特征提取后進行去幀內點云模糊(圖4(b))，一幀內的點云通過IMU預積分的方式將其統一到幀末，以此消除掉真正的點云運動模糊。最后通過迭代卡爾曼濾波器方法去緊耦合的融合激光雷達和IMU觀測。

圖4 FAST-LIO框架

實驗是在實驗室中拿一個手持設備不斷地劇烈運動來得到周圍環境建圖，結果如圖5所示，可以看到無論是精度上，還有計算效率上都會比同行的方法有比較明顯的優勢。

圖5 FAST-LIO實驗對比

FAST-LIO2的系統框圖如圖6所示，其主要創新的第一點是引入增量式更新的KD樹，第二點是使用的原始的雷達點，沒有特定點提取的步驟，由于使用了更多的點，反而在精度上比使用特征點的方法有所提升，第三點是重力加速度上使用了S2更新。

圖6 FAST-LIO2框架

增量式更新的KD樹之所以會比同行的方法有明顯的優勢，主要因為增量式K的引進。傳統的KD樹里，去搜索K最近鄰的點，然后構成一個點到面的殘差時，會用到KD樹，它是一種平衡的數據結構，也是最優的knn搜索，但并不支持增加式的。于是，提出了增量式K的數據結構，首先是支持增量式的更新，其次是可以在線的做動態的樹平衡。

模型耗時評估結果如圖7所示，即計算比較每雷達幀的處理時間。通過跑相同處理的雷達幀的結果對比，可以發現存在非常明顯的優勢。

圖7 模型耗時評估對比

03 多傳感器融合(激光雷達-慣導-視覺)

首個開源的多傳感器緊耦合方案(R2LIVE)

現在的激光雷達--慣導—視覺的融合還是學術界的一個非常熱門的方向，很多地方的問題還解決的不是很徹底。LIC-Fusion 是19年提出的雷達-慣導-相機的多傳感器融合算法，可以有效地融合IMU測量，稀疏的視覺特征和提取的激光雷達點。此外，還能夠在三個異步傳感器之間執行在線的空間和時間傳感器標定，以補償可能的標定偏差。隨后，LIC-Fusion 2.0又提出了一種新的平面特征跟蹤算法來更加有效地處理激光雷達測量，以進一步提高算法的精度和魯棒性。LVI-SAM是在LIO-SAM的基礎上，融合了VINS-MONO的觀測，整個系統分為VIO和LIO系統，VIO系統通過單獨處理相機+IMU數據給出一個里程計結果, LIO系統通過提取匹配雷達特征給出一個雷達里程計結果，最后，通過聯合優化完成系統的狀態估計。

在這樣的背景下，提出了R2LIVE的多傳感器融合方案，系統框圖如圖8所示。該模型也是VIO和LIO兩個子系統，LIO跟FAST-LIO框架差別不大，VIO是有兩步觀測，第一步是用迭代卡爾曼濾波器完成3D點的觀測，第二步去更新相機的狀態估計。由于卡爾曼濾波器耗時比較低，所以能夠及時的計算出相機姿態，然后后端有個圖優化是因為需要不斷優化視覺的3D點位置，否則更新出來的地圖會產生較大誤差。

圖8 R2LIVE框架

迭代卡爾曼濾波器如圖9所示，當輸入一幀雷達點云時，上一幀是相機的更新幀，預積分到一個時刻，然后提供一個姿態估計的先驗，之后雷達點云到達時就完成完成一次迭達卡爾曼濾波器的更新。當幾個狀態估計完成之后，下一幀如果來的還是那個視覺的觀測，那就把雷達觀測的預積分跟到相機來的時間戳上，再去完成更新。要做視覺和雷達的融合的原因在于提高系統整體的魯棒性。

圖9 迭代卡爾曼濾波器示意圖

實時真彩地圖重建(R3LIVE)

R3LIVE的系統框架如圖10所示，也是分為LIO和VIO兩個子系統，LIO是基于FAST-LIO2改進的一個系統，VIO是考慮的一個融合視覺相機的方案。R3LIVE相比于R2LIVE的區別在于，R3LIVE會考慮一些相機的光度學模型，即相機的鏡頭陰影、相機的響應函數等方面。

圖10 R3LIVE系統框架

光度學模型如圖11所示，在建圖中，認為一個點在不同相機觀測中的顏色值是一樣的，但是沒有考慮相機曝光的影響或鏡頭陰影及響應應函數的影響，導致它有可能一個點插在不同的相機幀里。R3LIVE++每個點是重建的輻射光場，即反射自然光中的一個點的輻射程度通過彩色相機的模型，然后去最終計算出相機的灰度值觀測。

圖11 R3LIVE++光度學模型

VIO中存在兩個更新，一個是frame-to-frame(圖12左)，還有一個是frame-to-map(圖12右)。之所以需要frame-to-frame更新是因為直接使用frame-to-map的更新是比較慢的，因為一開始可能沒有收斂到一個合適的收斂值。frame-to-frame的VIO更新是上一幀跟蹤的點，通過光流的方法去預測下一幀的當前幀，通過迭代優化全狀態，使得兩個觀測點的像素距離最小，最后完成更新。

圖12 VIO中的兩個更新方式

04 激光雷達的實時定位和網格(mesh)重建系統(ImMesh)

ImMesh的介紹和實驗演示

R3LIVE重建的彩色點云是要得到雷達在地圖里面全環境的輻射信息，才能夠重建出地圖的紋理，且必須要求每個點覆蓋到了才會有。然而，點云的密度有限，即不能在沒有點的地方看著這個點，現有一種解決方案是重建地圖的網格(mesh)模型，然后用相機得到圖像RGB信息后貼到網格上，這樣能夠無損的去重建地圖紋理。在這個背景下，做了一個在線激光雷達的實時定位和網格重建系統。主要貢獻是完成了一個實時的、在線的網格重建。

網格重建的相關工作首先一個是泊松重建，其原理是通過一個3D的標量方程，然后去擬合3D空間中的點云，使得擬合的標量方程在每個采樣點的梯度方向上和輸入點的靶向量是盡可能的匹配，再通過優化3D的標量方程，之后去提取等值面的方式把這個面給提取出來，示意圖如圖13所示。但是泊松重建的速度比較慢，另一方面是在點間距的邊緣上會出現一些莫名凸包。

圖13 泊松重建示意圖

離線方法中比較典型的是德勞內三角剖分+圖分割的方法，如圖14所示，基本原理是第一步先用3D的德勞內三角剖分對3D空間做四面體的切割，然后再通過相機和觀測點的可視化關系，再去做圖分割，以此區分去面片的inside和outside，最后是提取這些等值面。這些等值面就是重建的出來的mesh。

圖14 離線重建方法

在線方法比較經典的是TSDF+Marching cubes,如圖15所示，其原理是通過并行更新每個體素的TSDF，后面用Marching cubes的方法把面片給提取出來。TSDF的更新可以做到實時，如采用GPU加速，但在提取表面時候的速度會比較慢一些，因為是隱式的重建方法。

圖15 TSDF+Marching cubes在線重建

盡管現在有這些工作，但它們在激光雷達上依舊無法做到實時性。因此，希望去構建一個實時的網格重建，因為一方面是是效率快，第二方面是能夠實時的給你一個預覽反饋。ImMesh系統框架如圖16所示，它是直接以雷達和轉換器為輸入，如果有IMU則也會拿來做雷達點云的去運動模糊。定位模塊第一步是去點云的運動模糊，第二步是做狀態估計，然后第三步是做點云配準，完成之后存入數據結構中，即啟動在線的match模塊，首先取點云，然后進行降維處理，第三步是做增量式的三角形面片更新，最后把更新結果推送到全局地圖里。所以，ImMesh能夠同時輸出系統的一個實時狀態，還有實時輸出雷達匹配準之后的點云，以及重建的三角形面片。

圖16 ImMesh系統框架

這里在線網格重建的方法先是使用分層體素，如圖17所示，首先是用分層體素對空間進行一些劃分，比如說有L1，L2，L3，之后用哈希表的形式將這些體素給儲存起來，然后分類管理。所以，每次雷達點云配準之后，會更新一些局部的地圖，這時每個體素里存在一個狀態，如果體素中插入新的頂點，體素中會有一個標志位去標志，讓網格重建模塊去更新這個體素里面數據。

圖17 分層體素示意圖

在取出點之后，將3D的一個個點通過圖形的方法去投影到一個合適的平面內，這個合適的平面是由voxel-map估計的一個地圖在此點的平面法向量，然后進入到那個平面去。因為雷達點本身對環境進行測量的時候，這個點本來是在一個連續的平面上，也在一個區域內，如果這時在3D上直接做match，復雜度會很高，而且目前還沒有成熟的方法。所以，這里采用直接用2D的視角去解決3D的這個問題，完成降維操作，之后再去進行局部區域的更新，最后利用增量式的machine進行同步到全局地圖里面。

這個系統能夠實現在數據采集的時候，同時對周圍環境做在線的網格重建，演示如圖18所示。這里有三個視圖，藍色框的是第三人稱拍的，紅色框是雷達點云的錄入，黃色框是前置相機的預覽，然后把這三個視圖通過視頻剪輯的方式將時間給合在一起，進行同步。

圖18 系統演示

該方法不僅僅適用于固態雷達，還有旋轉機械式雷達等。在不同的數據集上測試了這個算法的性能，結果如圖19所示。可以發現在不同的數據集，不同的場景和不同的雷達上對該算法進行測試，結果表明這個算法都能夠在這些測試區域中實現一個實時的重建。

圖19 實驗結果

基于ImMesh開發的應用

（1）激光雷達點云增強

激光雷達點云增強應用如圖20所示，第二行的雷達的掃描是一種玫瑰花色的掃描，導致這種雷達做learning算法的時候會出現一些問題。由于該系統能夠實時的重建出周圍環境的三角形面片，那就可以把這個三角形面片給觀察化到當前幀的坐標系里面，這時就能夠得到一個規則的深度圖，然后再從深度圖里面反投影。所以，雷達增強之后的點云就是一個一個規則排列的增強點云。如果相機的四張角變得更大，那么就可以獲得更大范圍的3D感知的雷達點云，就可以對遠處觀測的更稠密的一些。

圖20 激光雷達點云增強

（2）快速無損的場景紋理重建

剛才提到R3LIVE是需要每個點去記錄環境的紋理，如果有match的形式，可以直接把采集的圖像給貼圖到面片上，因此就能夠無損的對環境進行紋理重建，這里在線的快速紋理重建如圖21所示。

圖21 快速場景紋理重建