一種快速的激光視覺(jué)慣導(dǎo)融合的slam系統(tǒng)

本文提出了一種快速的激光視覺(jué)慣導(dǎo)融合的slam系統(tǒng)，可以分為L(zhǎng)IO和VIO兩個(gè)緊耦合的子系統(tǒng)。LIO直接把當(dāng)前的掃描點(diǎn)和增量構(gòu)建的地圖對(duì)齊，地圖點(diǎn)也會(huì)輔助基于直接法的VIO系統(tǒng)進(jìn)行圖像對(duì)齊。為了進(jìn)一步提高vio系統(tǒng)的魯棒性和準(zhǔn)確性，作者提出了一種新的方法來(lái)剔除邊緣或者在視覺(jué)中遮擋的地圖點(diǎn)。

本文方法可以適用于機(jī)械雷達(dá)和固態(tài)雷達(dá)，并能實(shí)時(shí)的ARM和Intel的處理器上運(yùn)行，作者已經(jīng)開(kāi)源了代碼。

代碼地址：https://github.com/hku- mars/FAST- LIVO

本文的主要貢獻(xiàn)有：

一個(gè)建立在兩個(gè)基于直接法的緊耦合的完整的激光視覺(jué)慣導(dǎo)融合的slam框架；

一個(gè)直接高效的最大程度重用LIO構(gòu)建的地圖的VIO框架，具體來(lái)說(shuō)利用地圖點(diǎn)和觀測(cè)到的圖像像素塊結(jié)合后投影到一個(gè)新的圖像上通過(guò)最小化光度誤差來(lái)得到全部狀態(tài)的位姿估計(jì)結(jié)果。

通過(guò)在視覺(jué)中使用雷達(dá)點(diǎn)云可以避免特征的提取和三角化，同時(shí)可以在測(cè)量層對(duì)視覺(jué)和激光雷達(dá)進(jìn)行融合。

開(kāi)源了這項(xiàng)偉大的工作。

這項(xiàng)工作的系統(tǒng)框架如下所示：



LIO：利用后向遞推的方法剔除點(diǎn)云的運(yùn)動(dòng)畸變，利用去畸變的點(diǎn)云基于點(diǎn)到平面的距離進(jìn)行幀到地圖的匹配。

VIO：視覺(jué)基于當(dāng)前的FOV從全局的視覺(jué)地圖中選取當(dāng)前能觀測(cè)到的子地圖并剔除被遮擋和深度不連續(xù)的點(diǎn)，然后基于稀疏光流進(jìn)行幀到地圖點(diǎn)匹配。

最后激光點(diǎn)到平面的殘差和視覺(jué)的光度誤差及IMU前向傳播的值放到基于誤差狀態(tài)的迭代卡爾曼濾波器中得到準(zhǔn)確的位姿，并利用該位姿把新的觀測(cè)加到地圖中。

狀態(tài)估計(jì)： 系統(tǒng)利用緊耦合的ESIKF來(lái)進(jìn)行狀態(tài)估計(jì)，首選需要知道兩個(gè)運(yùn)算的定義：



其中Exp和Log表示旋轉(zhuǎn)矩陣和旋轉(zhuǎn)向量之間的基于羅德里格斯公式的映射關(guān)系。

狀態(tài)轉(zhuǎn)移模型： 在本文的系統(tǒng)中假設(shè)激光雷達(dá)，相機(jī)和imu之間的時(shí)間offset是已知的，定義imu的第一幀為全局坐標(biāo)系，三個(gè)傳感器之間固聯(lián)且外參已知。第i幀imu在離散模型下的狀態(tài)轉(zhuǎn)移方程為：



T表示imu采樣的時(shí)間間隔，x是狀態(tài)，u是輸入，w是噪聲，f的具體形式為：



前兩個(gè)狀態(tài)分別表示imu在全局坐標(biāo)系下的姿態(tài)和平移，最后一個(gè)表示重力在全局坐標(biāo)系下的方向。 前向傳播： 利用前向傳播來(lái)得到i+1時(shí)刻的狀態(tài)和協(xié)方差矩陣，具體形式為：



在前向傳播中把噪聲項(xiàng)設(shè)置為0，其中Q是噪聲的協(xié)方差矩陣。大家應(yīng)該知道下尖是后驗(yàn)，就是已經(jīng)融合了視覺(jué)和激光雷達(dá)觀測(cè)的結(jié)果，通過(guò)運(yùn)動(dòng)方程我們可以得到新的視覺(jué)或者激光雷達(dá)來(lái)的時(shí)候的先驗(yàn)的狀態(tài)，然后等激光或者視覺(jué)幀來(lái)的時(shí)候進(jìn)行對(duì)應(yīng)的量測(cè)更新。

（這里多說(shuō)一點(diǎn)，在做自動(dòng)駕駛的時(shí)候，由于觀測(cè)后的補(bǔ)償量可能較大導(dǎo)致位姿產(chǎn)生小范圍的跳變，所以我們一般都會(huì)把大的補(bǔ)償量分成小的補(bǔ)償量進(jìn)行補(bǔ)償，雖然這種做法不嚴(yán)密但是能保證位姿的平滑性，大家也可以想想有什么更好的辦法）。

幀到地圖的量測(cè)更新：

激光雷達(dá)的測(cè)量模型：

新的激光幀來(lái)之后首先進(jìn)行點(diǎn)云運(yùn)動(dòng)畸變矯正，當(dāng)進(jìn)行幀到地圖匹配到時(shí)候我們假設(shè)新觀測(cè)的點(diǎn)在和他近鄰的地圖中的平面上（用方向向量和中心點(diǎn)表示），如果先驗(yàn)的位姿是準(zhǔn)的可以得到如下約束：



這個(gè)約束是把當(dāng)前的點(diǎn)利用先驗(yàn)的位姿投影到地圖上，找到最近的平面后投影點(diǎn)應(yīng)該在平面上，所以兩個(gè)點(diǎn)相減得到的向量為平面上的向量，和平面的法向量垂直，點(diǎn)乘為0。

實(shí)際上，為了找到距離該點(diǎn)最近的平面，利用先驗(yàn)的位姿把點(diǎn)投影到地圖中找到距離該投影點(diǎn)最近的五個(gè)點(diǎn)（地圖點(diǎn)是用ikd_tree維護(hù)的）來(lái)擬合平面，為了考慮雷達(dá)點(diǎn)的測(cè)量噪聲，會(huì)加上一個(gè)矩陣表示每個(gè)點(diǎn)的權(quán)重。

2.視覺(jué)的測(cè)量模型：

當(dāng)接收到一幀新的圖像，我們從全局的視覺(jué)地圖中提取落在當(dāng)前視野內(nèi)的地圖點(diǎn)。對(duì)于地圖中的點(diǎn)，已經(jīng)被先前的幀觀測(cè)過(guò)很多次，我們找到和當(dāng)前觀測(cè)角度相近的一幀作為參考幀，然后把地圖點(diǎn)投影到當(dāng)前幀獲取地圖點(diǎn)的光度值，應(yīng)該和參考幀中的patch獲取的光度值一樣，以此構(gòu)建殘差：



預(yù)印版沒(méi)有解釋A，我猜測(cè)因?yàn)槭窍袼貕K進(jìn)行光度匹配，所以A矩陣是權(quán)重矩陣，patch中心點(diǎn)權(quán)重高，周圍點(diǎn)權(quán)重低，大家可以看代碼驗(yàn)證一下。 基于迭代的卡爾曼濾波器更新：?通過(guò)公式3我們可以得到先驗(yàn)的狀態(tài)和協(xié)方差的值，先驗(yàn)的分布可以表示為：



當(dāng)視覺(jué)和激光的觀測(cè)來(lái)的時(shí)候我們可以進(jìn)行量測(cè)更新以得到狀態(tài)量后驗(yàn)的結(jié)果：



上式為非凸的函數(shù)，可以基于高斯牛頓的方法優(yōu)化求解最小值，但是高斯牛頓和基于迭代的卡爾曼濾波器是等價(jià)的，參考文獻(xiàn)的21有證明。為了保證流型的約束，在每次迭代的時(shí)候，都把誤差狀態(tài)參數(shù)化到切空間中（通過(guò)第一個(gè)公式定義的方法），得到的誤差狀態(tài)更新到狀態(tài)量然后進(jìn)行下次迭代直到收斂，收斂的狀態(tài)和協(xié)方差用于imu的前向遞推，也用于視覺(jué)地圖和激光雷達(dá)地圖點(diǎn)增量更新。

地圖管理： 地圖主要有LIO構(gòu)建的雷達(dá)點(diǎn)云圖和VIO構(gòu)建的用patchs表示的視覺(jué)全局地圖。

1.雷達(dá)地圖管理： 激光雷達(dá)點(diǎn)云圖的管理和FAST-LIO2一致，利用ikd_tree進(jìn)行管理，ikd_tree提供了一些查詢、插入和刪除的接口，還可以根據(jù)配置參數(shù)下采樣地圖，同時(shí)新幀來(lái)的時(shí)候基于kd_tree的數(shù)據(jù)結(jié)構(gòu)可以大大縮短最近點(diǎn)查找的時(shí)間（基于并行化加速后，2000個(gè)點(diǎn)大概0.6ms）。

2.視覺(jué)全局地圖管理： 視覺(jué)的全局地圖是原來(lái)觀測(cè)過(guò)的雷達(dá)點(diǎn)云的集合，每個(gè)雷達(dá)點(diǎn)都對(duì)應(yīng)著多個(gè)觀測(cè)到這個(gè)激光點(diǎn)點(diǎn)視覺(jué)幀到多個(gè)像素塊。

視覺(jué)全局地圖的數(shù)據(jù)結(jié)構(gòu)和更新的方法如下： 數(shù)據(jù)結(jié)構(gòu)：為了快速找到落在當(dāng)前視野內(nèi)的地圖點(diǎn)，我們利用體素保存視覺(jué)全局地圖。

體素通過(guò)哈希表來(lái)管理，每個(gè)體素中保存點(diǎn)的位置，多個(gè)觀測(cè)到該點(diǎn)的像素的patch的金字塔和每個(gè)patch金字塔的相機(jī)位姿。

視覺(jué)的子地圖和外點(diǎn)剔除：即使體素的數(shù)量比視覺(jué)地圖定的數(shù)量少的多，但是確定他們中的哪些在當(dāng)前的視野中仍然非常耗時(shí)，尤其是體素?cái)?shù)量很大時(shí)。為了解決這個(gè)問(wèn)題，作者針對(duì)最近的雷達(dá)掃描的每個(gè)點(diǎn)基于哈希表查找這些體素。如果相機(jī)FoV和雷達(dá)大致對(duì)齊，則落在相機(jī)FoV中的地圖點(diǎn)很可能包含在這些體素中。

因此，視覺(jué)子圖可以通過(guò)這些體素包含的點(diǎn)進(jìn)行FoV檢查獲得。 視覺(jué)子圖可能包含在當(dāng)前的圖像幀中被遮擋或具有不連續(xù)深度的地圖點(diǎn)，這會(huì)降低VIO的精度。為了解決這個(gè)問(wèn)題，作者基于當(dāng)前的狀態(tài)量將視覺(jué)子圖中的所有點(diǎn)投影到當(dāng)前幀并在每個(gè)40x40的像素網(wǎng)格中保留深度最小的點(diǎn)。

此外，作者將當(dāng)前幀雷達(dá)掃描點(diǎn)投影到當(dāng)前幀，并檢查他們的深度來(lái)檢查他們是否遮擋了投影到9x9領(lǐng)域內(nèi)的其他地圖點(diǎn)。被遮擋的點(diǎn)也會(huì)被剔除。 視覺(jué)子地圖更新：在對(duì)齊新的圖像幀后，我們將當(dāng)前圖像中的patch附加到FoV內(nèi)的地圖點(diǎn)中，這樣地圖點(diǎn)就可能具有均勻分布視角的有效patch。

具體而言，作者在幀對(duì)齊后選擇具有高光度誤差的patch，如果距離上次添加patch超過(guò)20幀，或者當(dāng)前幀中patch距離上次添加了patch的參考幀中的像素位置超過(guò)40像素，則將向地圖點(diǎn)中添加新的patch。從當(dāng)前圖像中提取新的大小為8×8像素。并構(gòu)建金字塔，并保存相機(jī)的位姿。

除了向地圖點(diǎn)添加patch之外，還需要向視覺(jué)全局地圖添加新的地圖點(diǎn)。為此作者將當(dāng)前圖像分成40×40像素的網(wǎng)格，并在其上投影最近一次激光雷達(dá)掃描中的點(diǎn)。每個(gè)網(wǎng)格中具有最高梯度的投影激光雷達(dá)點(diǎn)將添加到視覺(jué)全局地圖中，以及在其中提取的patch和相機(jī)位姿。為了避免將邊緣上的激光雷達(dá)點(diǎn)添加到視覺(jué)地圖中，跳過(guò)了具有高局部曲率的邊緣點(diǎn)。

審核編輯：劉清