01 傳感器介紹

IMU(加速度計(jì))的測(cè)量頻率高，即可以精確的測(cè)量到物體的姿態(tài)運(yùn)動(dòng)，對(duì)運(yùn)動(dòng)靈敏，同時(shí)成本低，體積小，抗干擾能力強(qiáng)，基本上在多傳感器融合中是一個(gè)必備的傳感器。但I(xiàn)MU也有局限性，第一點(diǎn)是測(cè)量值存在隨機(jī)游走的bias，第二點(diǎn)是由于通過積分的形式計(jì)算出那個(gè)姿態(tài)，因此會(huì)存在累計(jì)誤差，導(dǎo)致長(zhǎng)時(shí)間的積分結(jié)果不可靠。因此常是IMU+雷達(dá)或IMU+視覺相機(jī)進(jìn)行計(jì)算。

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

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

在市區(qū)里面，到底是用視覺相機(jī)跑SLAM還是用激光雷達(dá)跑SLAM實(shí)驗(yàn)更合適點(diǎn)？這里更偏向采用視覺相機(jī)+激光雷達(dá)的方式，相機(jī)用來觀測(cè)周圍環(huán)境，激光雷達(dá)直接測(cè)量環(huán)境的3D幾何形狀。在特性排序上，相機(jī)獲得的圖像是有序的，連續(xù)的2D測(cè)量，雷達(dá)獲得的是無序的，離散的3D測(cè)量。

02 激光雷達(dá)(慣導(dǎo))SLAM

首個(gè)基于固態(tài)激光雷達(dá)的SLAM系統(tǒng)(loam-livox)

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

圖1 livox-MID40和傳統(tǒng)雷達(dá)對(duì)比

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

由于固態(tài)雷達(dá)的特性，例如非重復(fù)掃描的模式不能作frame-to-frame的匹配，小FoV的雷達(dá)對(duì)feature點(diǎn)要求更高，對(duì)異常值更敏感，更容易受動(dòng)態(tài)物體的影響，因此這些優(yōu)秀的算法并不能直接適用了固態(tài)激光雷達(dá)。所以，實(shí)現(xiàn)了首個(gè)適用于固態(tài)激光雷達(dá)，能夠?qū)崟r(shí)魯棒低漂移的SLAM系統(tǒng)。系統(tǒng)框圖如圖2所示，先做特征點(diǎn)提取，然后進(jìn)行面到面的匹配后去更新地圖，重建KD樹以供下一次的搜索。

圖2 loam-livox系統(tǒng)框圖

點(diǎn)云去幀內(nèi)運(yùn)動(dòng)模糊如圖3所示，一開始使用線性插值(圖3(b))，發(fā)現(xiàn)效果不太好，后面使用分段處理(圖3(c))，即把一幀分為三段，然后去重新估計(jì)姿態(tài)，因?yàn)槿《紊蠋瑑?nèi)的運(yùn)動(dòng)模糊不明顯。圖3(d)是開源手持設(shè)備和采集場(chǎng)景。

圖3 去幀內(nèi)運(yùn)動(dòng)模糊

激光雷達(dá)-慣導(dǎo)緊耦合的里程計(jì)(FAST-LIO)

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

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

因此在此類背景下提出了FAST-LIO系列。FAST-LIO是一個(gè)高效的緊耦合的雷達(dá)慣導(dǎo)融合框架，是基于迭代卡爾曼濾波器，濾波器里提供雷達(dá)觀測(cè)和IMU觀測(cè)，主要貢獻(xiàn)在于降低了大規(guī)模觀測(cè)數(shù)量導(dǎo)致的計(jì)算量。此外，還提出了FAST-LIO2，其主要?jiǎng)?chuàng)新在于使用了一個(gè)增量式KD樹去維護(hù)地圖的更新，有效解決了傳統(tǒng)方法中重建時(shí)導(dǎo)致地圖越來越大的問題。

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

圖4 FAST-LIO框架

實(shí)驗(yàn)是在實(shí)驗(yàn)室中拿一個(gè)手持設(shè)備不斷地劇烈運(yùn)動(dòng)來得到周圍環(huán)境建圖，結(jié)果如圖5所示，可以看到無論是精度上，還有計(jì)算效率上都會(huì)比同行的方法有比較明顯的優(yōu)勢(shì)。

圖5 FAST-LIO實(shí)驗(yàn)對(duì)比

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

圖6 FAST-LIO2框架

增量式更新的KD樹之所以會(huì)比同行的方法有明顯的優(yōu)勢(shì)，主要因?yàn)樵隽渴終的引進(jìn)。傳統(tǒng)的KD樹里，去搜索K最近鄰的點(diǎn)，然后構(gòu)成一個(gè)點(diǎn)到面的殘差時(shí)，會(huì)用到KD樹，它是一種平衡的數(shù)據(jù)結(jié)構(gòu)，也是最優(yōu)的knn搜索，但并不支持增加式的。于是，提出了增量式K的數(shù)據(jù)結(jié)構(gòu)，首先是支持增量式的更新，其次是可以在線的做動(dòng)態(tài)的樹平衡。

模型耗時(shí)評(píng)估結(jié)果如圖7所示，即計(jì)算比較每雷達(dá)幀的處理時(shí)間。通過跑相同處理的雷達(dá)幀的結(jié)果對(duì)比，可以發(fā)現(xiàn)存在非常明顯的優(yōu)勢(shì)。

圖7 模型耗時(shí)評(píng)估對(duì)比

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

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

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

在這樣的背景下，提出了R2LIVE的多傳感器融合方案，系統(tǒng)框圖如圖8所示。該模型也是VIO和LIO兩個(gè)子系統(tǒng)，LIO跟FAST-LIO框架差別不大，VIO是有兩步觀測(cè)，第一步是用迭代卡爾曼濾波器完成3D點(diǎn)的觀測(cè)，第二步去更新相機(jī)的狀態(tài)估計(jì)。由于卡爾曼濾波器耗時(shí)比較低，所以能夠及時(shí)的計(jì)算出相機(jī)姿態(tài)，然后后端有個(gè)圖優(yōu)化是因?yàn)樾枰粩鄡?yōu)化視覺的3D點(diǎn)位置，否則更新出來的地圖會(huì)產(chǎn)生較大誤差。

圖8 R2LIVE框架

迭代卡爾曼濾波器如圖9所示，當(dāng)輸入一幀雷達(dá)點(diǎn)云時(shí)，上一幀是相機(jī)的更新幀，預(yù)積分到一個(gè)時(shí)刻，然后提供一個(gè)姿態(tài)估計(jì)的先驗(yàn)，之后雷達(dá)點(diǎn)云到達(dá)時(shí)就完成完成一次迭達(dá)卡爾曼濾波器的更新。當(dāng)幾個(gè)狀態(tài)估計(jì)完成之后，下一幀如果來的還是那個(gè)視覺的觀測(cè)，那就把雷達(dá)觀測(cè)的預(yù)積分跟到相機(jī)來的時(shí)間戳上，再去完成更新。要做視覺和雷達(dá)的融合的原因在于提高系統(tǒng)整體的魯棒性。

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

實(shí)時(shí)真彩地圖重建(R3LIVE)

R3LIVE的系統(tǒng)框架如圖10所示，也是分為L(zhǎng)IO和VIO兩個(gè)子系統(tǒng)，LIO是基于FAST-LIO2改進(jìn)的一個(gè)系統(tǒng)，VIO是考慮的一個(gè)融合視覺相機(jī)的方案。R3LIVE相比于R2LIVE的區(qū)別在于，R3LIVE會(huì)考慮一些相機(jī)的光度學(xué)模型，即相機(jī)的鏡頭陰影、相機(jī)的響應(yīng)函數(shù)等方面。

圖10 R3LIVE系統(tǒng)框架

光度學(xué)模型如圖11所示，在建圖中，認(rèn)為一個(gè)點(diǎn)在不同相機(jī)觀測(cè)中的顏色值是一樣的，但是沒有考慮相機(jī)曝光的影響或鏡頭陰影及響應(yīng)應(yīng)函數(shù)的影響，導(dǎo)致它有可能一個(gè)點(diǎn)插在不同的相機(jī)幀里。R3LIVE++每個(gè)點(diǎn)是重建的輻射光場(chǎng)，即反射自然光中的一個(gè)點(diǎn)的輻射程度通過彩色相機(jī)的模型，然后去最終計(jì)算出相機(jī)的灰度值觀測(cè)。

圖11 R3LIVE++光度學(xué)模型

VIO中存在兩個(gè)更新，一個(gè)是frame-to-frame(圖12左)，還有一個(gè)是frame-to-map(圖12右)。之所以需要frame-to-frame更新是因?yàn)橹苯邮褂胒rame-to-map的更新是比較慢的，因?yàn)橐婚_始可能沒有收斂到一個(gè)合適的收斂值。frame-to-frame的VIO更新是上一幀跟蹤的點(diǎn)，通過光流的方法去預(yù)測(cè)下一幀的當(dāng)前幀，通過迭代優(yōu)化全狀態(tài)，使得兩個(gè)觀測(cè)點(diǎn)的像素距離最小，最后完成更新。

圖12 VIO中的兩個(gè)更新方式

04 激光雷達(dá)的實(shí)時(shí)定位和網(wǎng)格(mesh)重建系統(tǒng)(ImMesh)

ImMesh的介紹和實(shí)驗(yàn)演示

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

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

圖13 泊松重建示意圖

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

圖14 離線重建方法

在線方法比較經(jīng)典的是TSDF+Marching cubes,如圖15所示，其原理是通過并行更新每個(gè)體素的TSDF，后面用Marching cubes的方法把面片給提取出來。TSDF的更新可以做到實(shí)時(shí)，如采用GPU加速，但在提取表面時(shí)候的速度會(huì)比較慢一些，因?yàn)槭请[式的重建方法。

圖15 TSDF+Marching cubes在線重建

盡管現(xiàn)在有這些工作，但它們?cè)诩す饫走_(dá)上依舊無法做到實(shí)時(shí)性。因此，希望去構(gòu)建一個(gè)實(shí)時(shí)的網(wǎng)格重建，因?yàn)橐环矫媸鞘切士欤诙矫媸悄軌驅(qū)崟r(shí)的給你一個(gè)預(yù)覽反饋。ImMesh系統(tǒng)框架如圖16所示，它是直接以雷達(dá)和轉(zhuǎn)換器為輸入，如果有IMU則也會(huì)拿來做雷達(dá)點(diǎn)云的去運(yùn)動(dòng)模糊。定位模塊第一步是去點(diǎn)云的運(yùn)動(dòng)模糊，第二步是做狀態(tài)估計(jì)，然后第三步是做點(diǎn)云配準(zhǔn)，完成之后存入數(shù)據(jù)結(jié)構(gòu)中，即啟動(dòng)在線的match模塊，首先取點(diǎn)云，然后進(jìn)行降維處理，第三步是做增量式的三角形面片更新，最后把更新結(jié)果推送到全局地圖里。所以，ImMesh能夠同時(shí)輸出系統(tǒng)的一個(gè)實(shí)時(shí)狀態(tài)，還有實(shí)時(shí)輸出雷達(dá)匹配準(zhǔn)之后的點(diǎn)云，以及重建的三角形面片。

圖16 ImMesh系統(tǒng)框架

這里在線網(wǎng)格重建的方法先是使用分層體素，如圖17所示，首先是用分層體素對(duì)空間進(jìn)行一些劃分，比如說有L1，L2，L3，之后用哈希表的形式將這些體素給儲(chǔ)存起來，然后分類管理。所以，每次雷達(dá)點(diǎn)云配準(zhǔn)之后，會(huì)更新一些局部的地圖，這時(shí)每個(gè)體素里存在一個(gè)狀態(tài)，如果體素中插入新的頂點(diǎn)，體素中會(huì)有一個(gè)標(biāo)志位去標(biāo)志，讓網(wǎng)格重建模塊去更新這個(gè)體素里面數(shù)據(jù)。

圖17 分層體素示意圖

在取出點(diǎn)之后，將3D的一個(gè)個(gè)點(diǎn)通過圖形的方法去投影到一個(gè)合適的平面內(nèi)，這個(gè)合適的平面是由voxel-map估計(jì)的一個(gè)地圖在此點(diǎn)的平面法向量，然后進(jìn)入到那個(gè)平面去。因?yàn)槔走_(dá)點(diǎn)本身對(duì)環(huán)境進(jìn)行測(cè)量的時(shí)候，這個(gè)點(diǎn)本來是在一個(gè)連續(xù)的平面上，也在一個(gè)區(qū)域內(nèi)，如果這時(shí)在3D上直接做match，復(fù)雜度會(huì)很高，而且目前還沒有成熟的方法。所以，這里采用直接用2D的視角去解決3D的這個(gè)問題，完成降維操作，之后再去進(jìn)行局部區(qū)域的更新，最后利用增量式的machine進(jìn)行同步到全局地圖里面。

這個(gè)系統(tǒng)能夠?qū)崿F(xiàn)在數(shù)據(jù)采集的時(shí)候，同時(shí)對(duì)周圍環(huán)境做在線的網(wǎng)格重建，演示如圖18所示。這里有三個(gè)視圖，藍(lán)色框的是第三人稱拍的，紅色框是雷達(dá)點(diǎn)云的錄入，黃色框是前置相機(jī)的預(yù)覽，然后把這三個(gè)視圖通過視頻剪輯的方式將時(shí)間給合在一起，進(jìn)行同步。

圖18 系統(tǒng)演示

該方法不僅僅適用于固態(tài)雷達(dá)，還有旋轉(zhuǎn)機(jī)械式雷達(dá)等。在不同的數(shù)據(jù)集上測(cè)試了這個(gè)算法的性能，結(jié)果如圖19所示?？梢园l(fā)現(xiàn)在不同的數(shù)據(jù)集，不同的場(chǎng)景和不同的雷達(dá)上對(duì)該算法進(jìn)行測(cè)試，結(jié)果表明這個(gè)算法都能夠在這些測(cè)試區(qū)域中實(shí)現(xiàn)一個(gè)實(shí)時(shí)的重建。

圖19 實(shí)驗(yàn)結(jié)果

基于ImMesh開發(fā)的應(yīng)用

（1）激光雷達(dá)點(diǎn)云增強(qiáng)

激光雷達(dá)點(diǎn)云增強(qiáng)應(yīng)用如圖20所示，第二行的雷達(dá)的掃描是一種玫瑰花色的掃描，導(dǎo)致這種雷達(dá)做learning算法的時(shí)候會(huì)出現(xiàn)一些問題。由于該系統(tǒng)能夠?qū)崟r(shí)的重建出周圍環(huán)境的三角形面片，那就可以把這個(gè)三角形面片給觀察化到當(dāng)前幀的坐標(biāo)系里面，這時(shí)就能夠得到一個(gè)規(guī)則的深度圖，然后再?gòu)纳疃葓D里面反投影。所以，雷達(dá)增強(qiáng)之后的點(diǎn)云就是一個(gè)一個(gè)規(guī)則排列的增強(qiáng)點(diǎn)云。如果相機(jī)的四張角變得更大，那么就可以獲得更大范圍的3D感知的雷達(dá)點(diǎn)云，就可以對(duì)遠(yuǎn)處觀測(cè)的更稠密的一些。

圖20 激光雷達(dá)點(diǎn)云增強(qiáng)

（2）快速無損的場(chǎng)景紋理重建

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

圖21 快速場(chǎng)景紋理重建