立體視覺+慣導+激光雷達SLAM系統(tǒng)

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

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

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

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

圖6 實驗裝置? ?使用兩個VPs校準相機機

視覺前端

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

視覺慣導里程計

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

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

激光雷達建圖

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

基于雷達增強的回環(huán)檢測

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

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

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

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

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

交流群

歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學影像、GAN、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~