本文提出的立體視覺+慣導(dǎo)+激光雷達(dá)的SLAM系統(tǒng)，在比如隧道一些復(fù)雜場景下能夠?qū)崿F(xiàn)良好性能。VIL-SLAM通過將緊密耦合的立體視覺慣性里程計(jì)（VIO）與激光雷達(dá)建圖和激光雷達(dá)增強(qiáng)視覺環(huán)路閉合相結(jié)合來實(shí)現(xiàn)這一目標(biāo)。該系統(tǒng)實(shí)時(shí)生成環(huán)閉合校正的6自由度激光雷達(dá)姿態(tài)和接近實(shí)時(shí)的1cm體素稠密點(diǎn)云。與最先進(jìn)的激光雷達(dá)方法相比，VIL-SLAM顯示了更高的精確度和魯棒性。

（a）傳感器設(shè)備（b）構(gòu)建室外場景模型

主要內(nèi)容

VIL-SLAM系統(tǒng)圖。傳感器為灰色，模塊為綠色。箭頭指示消息在系統(tǒng)中的流動(dòng)方式。深色粗箭頭表示系統(tǒng)實(shí)時(shí)輸出，淺色粗箭頭表示近實(shí)時(shí)后處理生成的輸出。

系統(tǒng)有四個(gè)模塊，如圖2所示。視覺前端從立體攝像機(jī)獲取立體圖像。它執(zhí)行幀到幀的跟蹤和立體幀匹配，并輸出立體匹配結(jié)果作為視覺測(cè)量。立體VIO采用立體匹配和IMU測(cè)量，在位姿圖上執(zhí)行IMU預(yù)積分和平滑緊耦合結(jié)果。該模塊根據(jù)IMU和攝像機(jī)輸出VIO姿態(tài)。LiDAR建圖模塊使用VIO的運(yùn)動(dòng)估計(jì)，并執(zhí)行LiDAR點(diǎn)去噪和掃描以進(jìn)行地圖配準(zhǔn)?；丨h(huán)閉合模塊進(jìn)行視覺環(huán)路檢測(cè)和初始環(huán)路約束估計(jì)，并通過稀疏點(diǎn)云ICP對(duì)準(zhǔn)進(jìn)一步精細(xì)配準(zhǔn)。對(duì)約束所有LiDAR姿態(tài)的全局姿態(tài)圖進(jìn)行增量式優(yōu)化，得到全局修正軌跡和實(shí)時(shí)LiDAR姿態(tài)修正。它們被送回激光雷達(dá)建圖模塊進(jìn)行地圖更新和重新定位。在后處理中，將激光雷達(dá)掃描幀與最佳估計(jì)的激光雷達(dá)姿態(tài)拼接，以獲得稠密的點(diǎn)云地圖結(jié)果

圖6 實(shí)驗(yàn)裝置 使用兩個(gè)VPs校準(zhǔn)相機(jī)機(jī)

視覺前端

對(duì)立體視覺進(jìn)行立體匹配，本文使用Kanade Lucas Tomasi（KLT）特征跟蹤器來跟蹤先前立體匹配中的所有特征點(diǎn)，無論是在左圖像還是右圖像中。只有當(dāng)他們都被跟蹤才會(huì)輸出。較大的立體基線有助于尺度估計(jì)，并減少由遠(yuǎn)距離特征引起的退化問題。我們使用基于特征的方法，比KLT更適合處理長基線。如果跟蹤的立體匹配數(shù)低于閾值，使用Shi-Tomashi角點(diǎn)檢測(cè)器進(jìn)行特征提取，然后進(jìn)行特征消除過程，刪除與任何小于閾值的現(xiàn)有特征的像素距離的特征。ORB描述子后計(jì)算所有幸存特征，然后進(jìn)行暴力立體匹配以獲得新的匹配結(jié)果。系統(tǒng)通過對(duì)第一幀立體視覺進(jìn)行立體匹配來初始化系統(tǒng)。

視覺慣導(dǎo)里程計(jì)

立體VIO的目標(biāo)是在相對(duì)較高的頻率上提供實(shí)時(shí)準(zhǔn)確的狀態(tài)估計(jì)，作為LiDAR建圖算法的運(yùn)動(dòng)模型。在位姿圖上運(yùn)行的緊耦合固定滯后平滑器在精度和效率之間是一個(gè)很好的權(quán)衡?；趦?yōu)化的方法通常允許多次線性化以接近全局最小值。固定滯后位姿圖優(yōu)化器進(jìn)一步限定了變量的最大數(shù)目，因此計(jì)算成本是有界的。由于糟糕的視覺測(cè)量會(huì)導(dǎo)致收斂問題，對(duì)視覺測(cè)量強(qiáng)制執(zhí)行嚴(yán)格的異常值拒絕機(jī)制。系統(tǒng)通過檢查平均重投影誤差來消除異常值。

該算法以IMU預(yù)積分因子和無結(jié)構(gòu)視覺因子作為約束條件

激光雷達(dá)建圖

激光雷達(dá)建圖在進(jìn)行激光雷達(dá)點(diǎn)提取和掃描幀點(diǎn)云到地圖配準(zhǔn)之前，使用高頻IMU速率VIO姿態(tài)作為運(yùn)動(dòng)。將掃描c表示為從一個(gè)完整的激光雷達(dá)旋轉(zhuǎn)獲得的點(diǎn)云。從c中提取幾何特征，包括銳邊上的點(diǎn)和平面上的點(diǎn)。然后，基于當(dāng)前掃描到地圖的特征點(diǎn)（所有先前的特征點(diǎn)），通過最小化特征點(diǎn)形成的歐幾里德距離殘差，將其作為一個(gè)優(yōu)化問題來解決。

基于雷達(dá)增強(qiáng)的回環(huán)檢測(cè)

回環(huán)檢測(cè)對(duì)任何一個(gè)SLAM系統(tǒng)都是至關(guān)重要的，因?yàn)殚L期運(yùn)行會(huì)引入漂移。環(huán)路閉合的目的是通過對(duì)全局位姿圖進(jìn)行優(yōu)化來消除漂移，該全局位姿圖結(jié)合了LiDAR建圖的回環(huán)約束和相對(duì)變換信息。為了更好地輔助激光雷達(dá)制圖，校正后的激光雷達(dá)姿態(tài)被實(shí)時(shí)發(fā)送回來，以便新掃描的特征點(diǎn)被配準(zhǔn)到重新訪問的地圖上。本文提出在視覺詞袋回環(huán)檢測(cè)和PnP回環(huán)約束公式的基礎(chǔ)上增加ICP對(duì)齊。該系統(tǒng)采用增量求解器iSAM2對(duì)全局姿態(tài)圖進(jìn)行優(yōu)化，實(shí)現(xiàn)了實(shí)時(shí)性

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

評(píng)估了VIL-SLAM，并將其與基于最佳實(shí)時(shí)激光雷達(dá)的系統(tǒng)LOAM2在自定義數(shù)據(jù)集上進(jìn)行了比較。還使用EuRoC MAV數(shù)據(jù)集評(píng)估立體VIO子模塊（VIL-VIO）。

這里提供了五個(gè)典型環(huán)境的結(jié)果，包括無特色的走廊、雜亂無章的高臺(tái)、隧道和室外環(huán)境。所有這些序列的數(shù)據(jù)收集在同一點(diǎn)開始和結(jié)束?；谧罱K漂移誤差（FDE）對(duì)里程計(jì)（LiDAR建圖姿態(tài)）進(jìn)行評(píng)估。以Faro掃描為基本真實(shí)值，用平均配準(zhǔn)誤差（MRE）對(duì)建圖結(jié)果進(jìn)行評(píng)估。首先將地圖與模型對(duì)齊（Faro掃描），然后計(jì)算地圖點(diǎn)與模型中最近點(diǎn)之間的歐幾里德距離。里程表FDE和建圖結(jié)果如表一所示，較好的用粗體顯示。

地圖比較：VIL-SLAM（右）和LOAM（左）與模型相比的地圖配準(zhǔn)誤差。0.3m以上的誤差（a-b）用紅色表示，0.5m以上的誤差（c）用紅色表示。藍(lán)色和綠色內(nèi)部不連續(xù)的紅色區(qū)域是由于掃描設(shè)備的遮擋導(dǎo)致的模型缺失。

總結(jié)

VIL-SLAM是一種先進(jìn)的里程計(jì)和地圖構(gòu)建系統(tǒng)，旨在在不同的環(huán)境中長期穩(wěn)定地運(yùn)行。當(dāng)前的框架將VIL-VIO和激光雷達(dá)建圖松耦合在一起。我們正在將其擴(kuò)展到一個(gè)緊密耦合的框架中，以便從LiDAR建圖得到的精確姿態(tài)估計(jì)可以用于IMU偏差校正。在循環(huán)閉合中對(duì)掃描幀之間的稀疏特征點(diǎn)進(jìn)行ICP精細(xì)化獲得更好的回環(huán)約束。

責(zé)任編輯：gt