編者按：在定位導航與實時建圖問題中，多源信息的融合是目前流行的解決方案，即綜合不同傳感器的信息進行處理，并得到融合定位結果，常用有全球衛(wèi)星導航系統(tǒng)、慣性里程計、激光雷達、攝像頭等。而因子圖（Factor Graph）優(yōu)化是近年來使用頻繁的后端優(yōu)化方法。因子圖是種用于描述事物關系的有效模型，基于該模型的優(yōu)化方法能夠從全局的定位結果進行優(yōu)化。本篇文章基于激光雷達、視覺信息以及慣性里程計，運用因子圖進行信息融合，實現(xiàn)高效精準的SLAM算法，同時提出“點、線、面”等不同的地標追蹤因子，提升SLAM算法的精度，具有極高的現(xiàn)代性與前瞻性。

本文譯自：

《Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry》

文章來源：

IEEE Robotics and Automation Letters, 2021, 6(2): 1004-1011.

作者：

David Wisth，Marco Camurri，Sandipan Das，Maurice Fallon

原文鏈接：

https://ieeexplore.ieee.org/abstract/document/9345356

摘要：我們提出了一個有效的多傳感器里程測量系統(tǒng)的移動平臺，聯(lián)合優(yōu)化視覺，激光雷達和慣性信息在一個單一的綜合因子圖。這將使用固定的延遲平滑以全幀率實時運行。為了實現(xiàn)這種緊密集成，提出了一種從激光雷達點云中提取三維線段、平面基元的新方法。該方法克服了典型的幀對幀跟蹤方法的缺點，將基元視為地標并通過多次掃描跟蹤它們。通過對激光雷達和相機幀進行微妙的被動同步，實現(xiàn)了激光雷達特征與標準視覺特征和IMU的真正集成。3D功能的輕量級形式允許在單個CPU上實時執(zhí)行。我們提出的系統(tǒng)已經在多種平臺和場景中進行了測試，包括使用有腿機器人進行地下勘探和使用動態(tài)移動手持設備進行戶外掃描，總持續(xù)時間為96分鐘，行走距離為2.4公里。在這些測試序列中，僅使用一個外部傳感器就會導致失敗，原因要么是幾何約束不足(影響激光雷達)，要么是過度光照變化導致的無紋理區(qū)域(影響視覺)。在這些條件下，我們的因子圖自然地使用了從每個傳感器模態(tài)獲得的最佳信息，而沒有任何外部干預與硬性條件判斷。

關鍵詞：傳感器融合，視覺慣性SLAM，定位

1 引言

多源傳感器融合是自動導航系統(tǒng)在現(xiàn)實場景中成功部署的關鍵能力?？赡艿膽梅秶軓V，從自主地下探測到室外動態(tài)測繪(見圖1)。激光雷達硬件的最新進展促進了激光雷達-慣性導航融合[1]-[4]的研究。激光雷達傳感器廣泛的視場、密度、范圍和精度使得它們適合于導航、定位和測繪任務。

然而，在復雜的環(huán)境中，如長隧道或開闊空間，光雷達方法可能會失敗。由于單獨的IMU積分遞推不能提供超過幾秒鐘的可靠姿態(tài)估計，系統(tǒng)故障通常是不可恢復的。為了應付這些情況，還需要與更多的傳感器，特別是照相機進行融合。雖然過去已經通過松耦合方法[5]實現(xiàn)了視覺-慣性-激光雷達的融合，但像增量平滑這樣的緊耦合方法由于其優(yōu)越的魯棒性而更受歡迎。

在平滑方法方面，視覺慣性導航系統(tǒng)(VINS)的研究已經成熟，激光慣性導航系統(tǒng)的研究也越來越普遍。然而，三種傳感器模式的緊密融合仍然是一個有待解決的研究問題。

A. 出發(fā)點

IMU、激光雷達和相機傳感融合的兩個主要挑戰(zhàn)是: 1)在移動平臺有限的計算預算下實現(xiàn)實時性; 2)在不同頻率和采集方法下實現(xiàn)三種信號的適當同步。

先前的工作通過采用松散耦合的方法[5]，[8]，[9]或運行兩個獨立的系統(tǒng)(一個用于激光雷達慣性，另一個用于視覺慣性測程)[10]來解決這兩個問題。

相反，我們通過以下方法來解決這些問題:1)提取和跟蹤稀疏輕量級基元，2)開發(fā)一個相干因子圖，利用IMU預積分來動態(tài)地將去變形的點云轉換為附近攝像機幀的時間戳。前者避免匹配整個點云(如ICP)或跟蹤數(shù)百個特征點(如LOAM[1])。后者使所有傳感器的實時平滑成為可能。

B. 貢獻

本篇論文的主要貢獻如下：

• 一種新的因子圖公式，緊密融合視覺，激光雷達和IMU測量在一個單一的一致的優(yōu)化過程;

• 一種有效的提取激光雷達特征的方法，然后優(yōu)化為地標。激光雷達和視覺特征共享一個統(tǒng)一的表示，因為所有的地標都被處理為三維參數(shù)流形(即點、線、面)。這種緊湊的表現(xiàn)方式允許我們以名義幀率處理所有的激光雷達掃描;

• 在一系列場景中進行的廣泛實驗評估表明，與在單個傳感器模式失敗時困難的更典型的方法相比，該方法具有更好的魯棒性。

我們的工作建立在我們之前的工作[11]，[12]中引入的VILENS估計系統(tǒng)的基礎上，增加了激光雷達特征跟蹤和激光雷達輔助視覺跟蹤。攝像頭和激光雷達的結合使便攜式設備即使在快速移動時也能使用，因為它可以自然地處理場景中的退化(由于缺乏激光雷達或視覺特征)。

圖1：我們使用DARPA SubT挑戰(zhàn)賽中的ANYmal四足機器人[6](上圖，由蘇黎世聯(lián)邦理工學院提供)和一個位于牛津新學院的手持繪圖設備[7](下圖)的數(shù)據(jù)測試了我們的多傳感器里程表算法。

2 相關工作

先前的多模態(tài)傳感器融合工作使用了激光雷達、相機和IMU傳感的組合，可以被描述為松散或緊密耦合，如表I所示。松散耦合系統(tǒng)分別處理來自每個傳感器的測量數(shù)據(jù)，并將其融合到一個濾波器中。在那里他們被邊緣化以得到當前的狀態(tài)估計。另外，緊密耦合系統(tǒng)可以聯(lián)合優(yōu)化過去和當前的測量數(shù)據(jù)，以獲得完整的軌跡估計。

另一個重要的區(qū)別是里程計系統(tǒng)和SLAM系統(tǒng)之間的區(qū)別。在后者中，執(zhí)行回環(huán)檢測以在訪問同一地點兩次后保持估計的全局一致性。盡管表格中的一些工作也包括姿態(tài)圖SLAM后端，但我們在這里主要對高頻里程計感興趣。

A. 松耦合激光雷達-慣性里程計

基于激光雷達的里程測量技術得到了廣泛的應用，這要歸功于Zhang等人[1]提出的LOAM算法。他們的關鍵貢獻之一是定義邊界和平面三維特征點，跟蹤幀到幀。兩幀之間的運動是線性插值使用IMU運行在高頻。該運動先驗用于特征的精細匹配和配準，以實現(xiàn)高精度的測程。Shan等人[2]提出了LeGO-LOAM，通過優(yōu)化地面平面的估計，進一步提高了地面車輛LOAM的實時性。然而，這些算法在無結構環(huán)境或退化場景[20](由于激光雷達的范圍和分辨率有限，如長高速公路、隧道和開放空間，無法找到約束條件)中將難以穩(wěn)定運行。

B. 松耦合視覺-慣性-激光雷達里程計

在最近的許多工作中，[5]，[8]，[9]，[15]視覺與激光雷達和IMU以一種松散耦合的方式結合，用于里程估計，以提供互補的傳感器模態(tài)，既避免退化，又在激光雷達-慣性系統(tǒng)上獲得更平滑的估計軌跡。

LOAM的作者擴展了他們的算法，將V -LOAM[8]中的單目攝像機的特征跟蹤與IMU相結合，從而為激光雷達掃描匹配生成視覺慣性測程先驗。然而，操作仍然是逐幀執(zhí)行，并沒有保持全局一致性。為了提高一致性，Wang等人引入了視覺-慣性-激光雷達SLAM系統(tǒng)。[9]使用基于V-LOAM的方法進行里程估計，并通過維護關鍵幀數(shù)據(jù)庫實現(xiàn)全局位姿圖優(yōu)化。Khattak等人[15]提出了另一種類似于V-LOAM的松散耦合方法，利用視覺/熱紅外慣性先驗進行激光雷達掃描匹配。為了克服簡并，作者使用視覺和熱紅外慣性里程計，以便在沒有照明的長隧道中操作。在Pronto[5]中，作者使用視覺-慣性腿里程計作為激光雷達里程計系統(tǒng)的運動先驗，并將視覺和激光雷達里程計的姿態(tài)修正集成在一起，以一種松散耦合的方式糾正姿態(tài)漂移。

C. 緊耦合慣性雷達里程計

早期的激光雷達和IMU緊密融合方法之一是在LIPS[3]中提出的，這是一種基于圖的優(yōu)化框架，它優(yōu)化了由最近點到平面表示和預集成IMU測量得到的三維平面因子。Y等人[4]以類似的方式提出了LIOM，一種聯(lián)合最小化激光雷達特征損失方程和預積分IMU測量的方法，在快速移動的情況下，這導致了比LOAM更好的里程表估計。Shan等人提出了LIO-SAM, LIO-SAM通過引入局部尺度而不是全局尺度的掃描匹配來適應LOAM框架。這允許新的關鍵幀被注冊到先前“子關鍵幀”的滑動窗口合并成一個體素貼圖。該系統(tǒng)在手持設備、地面和漂浮車輛上進行了廣泛的測試，突出了SLAM系統(tǒng)重建的質量。對于長時間航行，他們還使用了閉環(huán)因子和GPS因子來消除漂移。

但是，由于缺乏視覺，上述算法可能難以在退化場景下穩(wěn)健執(zhí)行。

D. 緊耦合視覺雷達里程計

為了避免退化和使系統(tǒng)更魯棒，多模態(tài)傳感能力(視覺、激光雷達和IMU)的緊密集成在最近的一些工作[10]，[16]-[19]中進行了探索。在LIMO[16]中，作者提出了一種基于束調整的視覺里程表系統(tǒng)。他們通過將激光雷達測量的深度重新投射到圖像空間，并將其與視覺特征聯(lián)系起來，從而幫助保持精確的尺度。Shao等人引入了VIL-SLAM，他們將VIO和基于激光雷達的里程表作為單獨的子系統(tǒng)來組合不同的傳感器模式，而不是進行聯(lián)合優(yōu)化。

為了進行聯(lián)合狀態(tài)優(yōu)化，許多方法[17]-[19]使用了多狀態(tài)約束卡爾曼濾波(MSCKF)框架[21]。[17]將3.5m范圍內RGB-D傳感器的平面特征與使用MSCKF的視覺和IMU測量的點特征緊密結合。為了限制狀態(tài)向量的大小，大部分點特征被視為MSCKF特征并被線性邊緣化，而只有少數(shù)強制平面點約束的點特征被保留在狀態(tài)向量中作為SLAM特征。Zuo等人提出的LIC-Fusion[18]將IMU測量數(shù)據(jù)緊密結合，利用MSCKF融合框架提取激光雷達邊緣特征，以及稀疏的視覺特征。然而，在最近的一項后續(xù)工作LIC-Fusion 2.0[19]中，作者引入了一種基于滑動窗口的平面特征跟蹤方法，用于高效處理三維激光雷達點云。

與之前的工作相比，我們在一個單一的、一致的因子圖優(yōu)化框架中聯(lián)合優(yōu)化了上述三種傳感器模式。為了實時處理激光雷達數(shù)據(jù)，我們直接從激光雷達點云中提取并跟蹤線、面等三維基元，而不是執(zhí)行“點對面”或“點對線”的成本函數(shù)。這允許以類似于視覺跟蹤的方式在多幀上進行自然跟蹤，并在退化場景中限制運動。

3 問題描述

我們的目標是估計一個移動平臺(在我們的實驗中，一個腿式機器人或一個手持傳感器負載)的位置、方向和線速度，該平臺裝備有IMU、激光雷達和一個具有低延遲和全傳感器率的單目或立體聲攝像機。

圖2：ANYmal平臺和手持設備的參考框架約定。世界坐標系W為固定架，底座坐標系B、光學相機坐標系C、IMU坐標系、I、激光雷達坐標系L分別安裝在移動機器人的底盤或設備上。為簡單起見，C和B在手持設備上是一致的。

相關的參考坐標系在圖2中指定，并且包括機器人固定的基準坐標系B、左攝像機坐標系C、IMU坐標系I和激光雷達坐標系L。我們希望估計相對于固定的世界坐標系W的基準幀的位置。

除非另有說明，否則基礎的位置wPWB方向RWB (以

為對應的齊次變換)以世界坐標表示；速度

、

以基礎坐標系表示，IMU偏移bg和ba以IMU坐標系表示。

A. 狀態(tài)量定義

在時間

的移動平臺狀態(tài)定義如下：  

其中：Ri是方位，pi是位置，vi是線速度，最后兩個元素是通常的IMU陀螺儀和加速度計偏差。

除了狀態(tài)之外，我們還關注了三個n維流形的參數(shù)：點、線和面。點地標ml是視覺特征，而直線ll和平面pl地標是從激光雷達中提取的。我們估計的目標是截至當前時間i的所有可見狀態(tài)和地標：

其中Xk,Mk,Pk,Lk是在固定滯后平滑窗口內跟蹤的所有狀態(tài)和地標的列表。

B. 測量定義

來自單目或立體攝像機C、IMU I、和激光雷達L的測量在不同的時間和頻率被接收。我們將zk定義為在平滑窗口內接收的全套測量值。第V-B1節(jié)解釋了如何將測量結果集成到因子圖中，以便在固定頻率下執(zhí)行優(yōu)化。

C. 最大后驗估計

我們根據(jù)給定的歷史狀態(tài)Xk中最大化測量Zk的可能性：

這些測量值被表示為有條件地獨立并且受到高斯白噪聲的影響。因此，公式（3）可以表示為以下最小二乘最小化問題[22]：

其中Iij是ti和tj之間的IMU測量值，并且Kk是tk之前的所有關鍵幀索引。每一項都是與一種因子類型相關的殘差，由其協(xié)方差矩陣的倒數(shù)加權。殘差包括：IMU、激光雷達平面和線特征、視覺地標和先驗狀態(tài)。

4 因子圖公式

現(xiàn)在我們描述圖中各因子的測量值、殘差和協(xié)方差，如圖3所示。為方便起見，我們在第IV-A節(jié)中總結了IMU因子；然后，我們在第IV-B和IV-C節(jié)中介紹了視覺-激光雷達地標因子，而第IV-D節(jié)描述了我們新的平面和線狀地標因子。

圖3：VILENS滑動窗口因子圖結構，顯示先驗的、視覺的、平面的、線的和預集成的IMU因子。隨著時間的推移跟蹤地標，通過允許新的測量來提高過去狀態(tài)的準確性，從而提高了估計的準確性。

A. IMU預積分因子

我們遵循現(xiàn)在標準的IMU測量預積分方式[23]來約束圖中兩個連續(xù)節(jié)點之間的姿態(tài)、速度和偏差，并在節(jié)點之間提供高頻狀態(tài)更新。殘差的形式為：

有關殘差的定義，請參見[23]。

B. 使用激光雷達深度的單目地標因子

為了充分利用視覺和激光雷達傳感模式的融合，我們跟蹤單目視覺特征，但使用激光雷達的重疊視場來提供深度估計，如[16]所示。為了匹配分別工作在10 Hz和30 Hz的激光雷達和相機的測量結果，我們使用了第V-B1節(jié)中描述的方法。

設

是歐氏空間中的一個視覺地標，

函數(shù)在給定平臺姿態(tài)Ti的情況下將一個地標投影到圖像平面上(為簡單起見，我們省略了底座、激光雷達和照相機之間的固定變換)，以及

在圖像平面上檢測ml (圖4中的黃點，右)。我們首先用

 將激光雷達在時間ti和ti+1之間獲取的所有點

 (圖4中的綠點，右圖中的綠點)投影到像平面上。然后，我們求出投影點

。最接近(ul,vl)。在3個像素的鄰域內的圖像平面上。最后，殘差計算為：

當我們無法將激光雷達深度與視覺特征相關聯(lián)時(由于激光雷達和相機傳感器的分辨率不同)，或者如果它不穩(wěn)定(即，當幀之間的深度由于動態(tài)障礙物或噪聲而變化大于0.5m時)，我們將恢復到立體匹配，如下一節(jié)所述。

C. 立體地標因子

地標ml在狀態(tài)xi的殘差為[12]：

其中(uL,v)，(uR,v)是檢測到的地標的像素位置，并且根據(jù)0.5像素的不確定性計算Σm最后，如果只有單目攝像機可用，則只使用公式7中的第一個和最后一個元素。

D. 平面地標因子

我們使用黑森范式將無限平面

參數(shù)化為單位法線

和表示其到原點的距離的標量d：

設?是對平面p上的所有點應用齊次變換T的算子，并且?將兩個平面(pi,pj)之間的誤差定義為：

其中

是

的切線空間的基，

定義如下[24]：

當平面

在時間ti測量時，相應的殘差是p與轉換到局部參考幀的估計平面pl之間的差：

E. 線地標因子

使用文[25]中的方法，無限直線可以由旋轉矩陣R∈SO3和兩個標量a,b∈R參數(shù)化，使得

是直線的方向，

是直線和原點之間的最近點。因此，行l(wèi)可以定義為：

?是一種運算符，代表了一種變換Tij=(Rij,pij)應用于某條線li上所有點到某條線上lj，例如：

在兩條線li,lj之間的運算符被定義為

給定上面兩式，一根被測量的線li和它的預測之間的殘差如下所示：

我們使用方程的數(shù)值導數(shù)。(11)和(15)在優(yōu)化中，使用對稱差分法。

圖4：左:視覺FAST特征m(黃色);激光雷達線l:(紅色)和平面基元(綠色)由我們的方法跟蹤。右圖:激光雷達數(shù)據(jù)(綠色)和視覺特征(黃色)投射到圖像框架中，有助于將深度與視覺特征關聯(lián)起來。

5 系統(tǒng)運行

系統(tǒng)架構如圖5所示。使用四個并行線程進行傳感器處理和優(yōu)化，系統(tǒng)以相機關鍵幀頻率(通常為15Hz)輸出因子圖估計的狀態(tài)，并以IMU頻率(通常為100Hz)輸出IMU轉發(fā)傳播狀態(tài)，分別用于導航/映射和控制。

圖5：VILENS系統(tǒng)架構概述。輸入由每個前端測量處理程序在單獨的線程中處理。后端既產生高頻前向傳播輸出，又產生低頻優(yōu)化輸出。這種并行體系結構允許根據(jù)平臺的不同進行不同的測量輸入。

使用GTSAM庫[22]，使用基于高效增量優(yōu)化解算器ISAM2的固定滯后平滑框架來求解因子圖。對于這些實驗，我們使用5到10秒的滯后時間。使用動態(tài)協(xié)方差縮放(DCS)[26]穩(wěn)健的成本函數(shù)將所有視覺和激光雷達因子添加到圖表中，以減少離群值的影響。

A. 視覺特征追蹤

我們使用快速角點檢測器檢測特征，并使用KLT特征跟蹤器跟蹤連續(xù)幀之間的特征，并使用RANSAC拒絕離群值。由于并行架構和增量優(yōu)化，每隔一幀用作關鍵幀，實現(xiàn)15Hz額定輸出。

B. 激光雷達處理和特征跟蹤

該算法的一個關鍵特點是從與攝像機幀同時表示的激光雷達點云中提取特征基元，從而可以一次對所有傳感器執(zhí)行優(yōu)化。處理流水線包括以下步驟：點云去失真與同步、濾波、基元提取與跟蹤、因子生成。

1)不失真和同步：圖6比較了我們傳感器的不同輸出頻率。當IMU和相機樣本被即時捕獲時，激光雷達在其內部繞著z軸旋轉的同時不斷地捕獲點。一旦完全旋轉完成，累積的激光回波就會轉換成點云，然后立即開始新的掃描。

由于激光回波是在移動時捕獲的，因此點云需要在唯一的任意時間戳(通常是掃描開始[27])之前的運動中保持不失真。這種方法意味著相機和激光雷達的測量具有不同的時間戳，從而分離出不同的圖形節(jié)點。

圖6：IMU、相機和激光雷達之間輸出頻率和同步的示例。IMU和相機信號被瞬間捕獲，而激光雷達點被捕獲并積累0.1s，然后作為掃描發(fā)送。當接收到掃描L2時，它與相機幀C3保持一致，并與相機添加到相同的圖節(jié)點中。

取而代之的是，我們選擇在掃描開始后將激光雷達測量結果不失真到最接近的相機時間戳。例如，在圖6中，掃描L2未失真到關鍵幀C3的時間戳。給定來自IMU模塊的前向傳播狀態(tài)，使用與云的每個點相關聯(lián)的時間戳來線性外推運動先驗(為簡單起見，我們避免了高斯過程內插[28]或具有時間偏移量的狀態(tài)擴充[29])。由于云現(xiàn)在與C3相關聯(lián)，因此激光雷達地標連接到與C3相同的節(jié)點，而不是創(chuàng)建新節(jié)點。

這一微妙的細節(jié)不僅保證了圖形優(yōu)化中添加了一致數(shù)量的新節(jié)點和因子，而且還確保了IMU、相機和激光雷達輸入之間的優(yōu)化是聯(lián)合進行的。這也確保了固定的輸出頻率，即相機幀率或激光雷達幀率(當相機不可用時)，但不是兩者的混合。

2)過濾：一旦點云沒有失真，我們從[30]開始進行分割，將點分成簇。較小的群集(小于5點)被標記為異常值并被丟棄，因為它們可能會有噪聲。

然后，利用文獻[2]的方法計算預濾波云中每個點的局部曲率。具有最低曲率和最高曲率的點分別被分配給平面候選集合Cp和線候選集合CL。

分割和基于曲率的過濾通常將點云中的點數(shù)減少90%，從而在后續(xù)的平面和線處理中提供顯著的計算節(jié)省。

3)平面和直線的提取和跟蹤：隨著時間的推移，我們跟蹤各個候選集合Cp和CL中的平面和直線。這是以類似于局部視覺特征跟蹤方法的方式來完成的，其中特征在其預測位置的局部附近被跟蹤。

首先，我們從上一次掃描中獲取被跟蹤的平面和線，pi-1和li-1，并使用IMU遞推來預測它們在當前掃描中的位置，預測值pi和li。然后，為了輔助局部跟蹤，我們使用最大點到模型距離來分割預測的特征位置周圍的Cp和CL。然后，我們進行歐幾里德聚類(對平面特征進行法向濾波)來去除離群點。然后，我們使用PROSAC[31]魯棒擬合算法將模型擬合到分割的點云。

最后，我們檢查預測的地標和檢測到的地標是否足夠相似。當兩個平面(

和

)的法線和到原點的距離之差小于閾值時，它們被認為是匹配的：

如果兩條直線的方向和中心距離小于閾值，則認為它們匹配：

在我們使用中，αp=αl=0.35rad, βp=βl=0.5m

跟蹤要素后，將從相應的候選集合中刪除該要素的內嵌對象，并對剩余的地標重復此過程。

跟蹤完成后，我們在剩余的候選云中檢測到新的地標。首先對線使用歐幾里德聚類，對平面使用基于法線的區(qū)域生長來劃分點云。然后，我們使用與地標跟蹤相同的方法在每個集群中檢測新的地標。

只有在連續(xù)掃描次數(shù)最少的情況下對點云要素進行跟蹤后，才會將其包括在優(yōu)化中。請注意，首先跟蹤最舊的要素，以確保盡可能長的要素軌跡。

C. 零速更新因子

為了限制平臺靜止時的漂移和因子圖增長，當三個模態(tài)中的兩個(相機、激光雷達、IMU)的更新報告沒有運動時，我們向圖形添加零速度約束。

6 試驗結果

我們在兩個對比鮮明的數(shù)據(jù)集中對我們的算法在各種室內和室外環(huán)境中進行了評估：較新的大學數(shù)據(jù)集[7]和DARPA SubT挑戰(zhàn)賽(城市)。這些環(huán)境的概述如圖7所示。

A. 數(shù)據(jù)集

較新的學院數(shù)據(jù)集(NC)[7]是使用配備了OS1-64 Gen1激光雷達傳感器、RealSense D435i立體紅外攝像機和Intel NUC PC的便攜式設備收集的。嵌入在激光雷達中的手機級IMU用于慣性測量。這個裝置是由一個在戶外行走的人攜帶的，周圍有建筑物、大片空地和茂密的樹葉。數(shù)據(jù)集包括具有挑戰(zhàn)性的序列，在這些序列中，設備被猛烈搖晃以測試跟蹤的極限。

SubT數(shù)據(jù)集(ST)包括SubT競賽的兩個最重要的運行(Alpha-2和Beta-2)，收集自配備了Flir BFS-U3-16S2C-CS單目攝像頭和工業(yè)級Xsens MTI-100 IMU的ANYmal B300四足機器人[6]，這兩個數(shù)據(jù)通過專用電路板進行硬件同步[32]。Velodyne VLP-16也是可用的，但通過軟件進行同步。機器人在一座未完工的核反應堆的地下內部導航。由于存在長而直的走廊和極其黑暗的環(huán)境，這一數(shù)據(jù)集具有挑戰(zhàn)性。請注意，此工作中未使用機器人的腿部運動學。

具體實驗命名如下：

NC-1：在開放的大學環(huán)境中漫步(1134米，17分鐘)。

NC-2：在強光照度變化(480m，6分鐘)下以高動態(tài)運動行走。

NC-3：以極高的角速度搖晃傳感器裝置，最高可達3.37弧度/秒(91米，2分鐘)。

ST-A：四足動物機器人在黑暗的地下反應堆設施中小跑(167米，11分鐘)。

ST-B：一個不同的動物機器人，在反應堆的一部分，包含一條長而直的走廊(490米，60分鐘)。

為了生成地面真相，ICP被用來將當前的激光雷達掃描與使用商業(yè)激光測繪系統(tǒng)收集的詳細先前地圖對準。有關地面真相生成的深入討論，讀者請參閱[7]。

B. 結果

表II總結了以下算法在10米距離上的平均相對位姿誤差(RPE)：

VILENS-LI：僅帶IMU和激光雷達的VILENS；

VILENS-LVI：具有IMU、視覺(具有激光雷達深度)和激光雷達特征的VILENS；

LOAM：在SubT比賽中使用的LOAM[1]映射模塊的輸出。

LeGO-LOAM：LeGO-LOAM [2]建圖模塊的輸出。

應該注意的是，沒有執(zhí)行任何回環(huán)，并且與LOAM和LeGO-LOAM方法相比，我們不執(zhí)行任何建圖。

對于SubT數(shù)據(jù)集，VILENS-LVI的平移/旋轉性能平均比LOAM高38%/21%，VILENS-LI的平移/旋轉性能比VILENS-LI高46%/21%。圖8顯示了全局性能的一個例子，它描述了ST-A數(shù)據(jù)集上的估計和地面真實軌跡。VILENS-L VI能夠實現(xiàn)非常慢的漂移率，即使沒有測繪系統(tǒng)或環(huán)路關閉。

對于最不動態(tài)的NC數(shù)據(jù)集NC-1，VILENS-L VI實現(xiàn)了與LeGO-LOAM相當?shù)男阅?。然而，對于更動態(tài)的數(shù)據(jù)集(高達3.37rad/s)，NC-2和NC-3，VILENS方法的性能明顯優(yōu)于LeGO-LOAM。這一性能的關鍵是激光雷達云到相機時間戳的不失真，允許精確的視覺特征與激光雷達的深度，同時最大限度地減少計算量。

總體而言，性能最好的算法是VILENS-LVI，顯示了視覺和激光雷達功能的緊密集成如何使我們能夠避免在純激光雷達慣性方法中可能出現(xiàn)的故障模式。

C. 多傳感器融合

互補傳感器模式的緊密融合帶來的一個關鍵好處是對傳感器退化具有自然的魯棒性。雖然很大比例的數(shù)據(jù)為激光雷達和視覺特征跟蹤提供了有利條件，但在許多情況下，緊密融合增強了對單個傳感器故障模式的魯棒性。

圖9顯示了NC-2的一個例子，當走出明亮的陽光進入陰影時，相機的自動曝光功能需要大約3秒的時間來調整。在此期間，視覺特征的數(shù)量從大約30個下降到不到5個(都聚集在圖像的一個角落)。這將導致估計器不穩(wěn)定。通過激光雷達的緊密融合，我們可以使用少量的視覺特征和激光雷達特征，而不會造成任何性能退化。這與[5]、[15]等方法相反，[5]、[15]等方法使用單獨的視覺-慣性子系統(tǒng)和激光雷達-慣性子系統(tǒng)意味著必須顯式地處理退化情況。

類似地，在激光雷達地標不足以完全約束估計(或接近退化)的情況下，視覺特征的緊密融合允許優(yōu)化利用激光雷達約束，同時避免退化問題。

D. 分析

在優(yōu)化中使用輕量級點云原語的一個關鍵好處是提高了效率。上述數(shù)據(jù)集的平均計算時間是用于視覺特征跟蹤的約10ms，用于點云特征跟蹤的約50ms，以及用于消費級筆記本電腦上的優(yōu)化的約20ms。這使得系統(tǒng)能夠在僅使用雷達-慣性測量時以10hz(激光雷達幀率)輸出，在融合視覺、激光雷達和慣性測量時以15hz(相機關鍵幀率)輸出。

參考文獻