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

本文譯自：

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

文章來(lái)源：

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

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

關(guān)鍵詞：傳感器融合，視覺(jué)慣性SLAM，定位

1 引言

多源傳感器融合是自動(dòng)導(dǎo)航系統(tǒng)在現(xiàn)實(shí)場(chǎng)景中成功部署的關(guān)鍵能力?？赡艿膽?yīng)用范圍很廣，從自主地下探測(cè)到室外動(dòng)態(tài)測(cè)繪(見(jiàn)圖1)。激光雷達(dá)硬件的最新進(jìn)展促進(jìn)了激光雷達(dá)-慣性導(dǎo)航融合[1]-[4]的研究。激光雷達(dá)傳感器廣泛的視場(chǎng)、密度、范圍和精度使得它們適合于導(dǎo)航、定位和測(cè)繪任務(wù)。

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

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

A. 出發(fā)點(diǎn)

IMU、激光雷達(dá)和相機(jī)傳感融合的兩個(gè)主要挑戰(zhàn)是: 1)在移動(dòng)平臺(tái)有限的計(jì)算預(yù)算下實(shí)現(xiàn)實(shí)時(shí)性; 2)在不同頻率和采集方法下實(shí)現(xiàn)三種信號(hào)的適當(dāng)同步。

先前的工作通過(guò)采用松散耦合的方法[5]，[8]，[9]或運(yùn)行兩個(gè)獨(dú)立的系統(tǒng)(一個(gè)用于激光雷達(dá)慣性，另一個(gè)用于視覺(jué)慣性測(cè)程)[10]來(lái)解決這兩個(gè)問(wèn)題。

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

B. 貢獻(xiàn)

本篇論文的主要貢獻(xiàn)如下：

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

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

• 在一系列場(chǎng)景中進(jìn)行的廣泛實(shí)驗(yàn)評(píng)估表明，與在單個(gè)傳感器模式失敗時(shí)困難的更典型的方法相比，該方法具有更好的魯棒性。

我們的工作建立在我們之前的工作[11]，[12]中引入的VILENS估計(jì)系統(tǒng)的基礎(chǔ)上，增加了激光雷達(dá)特征跟蹤和激光雷達(dá)輔助視覺(jué)跟蹤。攝像頭和激光雷達(dá)的結(jié)合使便攜式設(shè)備即使在快速移動(dòng)時(shí)也能使用，因?yàn)樗梢宰匀坏靥幚韴?chǎng)景中的退化(由于缺乏激光雷達(dá)或視覺(jué)特征)。

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

2 相關(guān)工作

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

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

A. 松耦合激光雷達(dá)-慣性里程計(jì)

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

B. 松耦合視覺(jué)-慣性-激光雷達(dá)里程計(jì)

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

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

C. 緊耦合慣性雷達(dá)里程計(jì)

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

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

D. 緊耦合視覺(jué)雷達(dá)里程計(jì)

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

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

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

3 問(wèn)題描述

我們的目標(biāo)是估計(jì)一個(gè)移動(dòng)平臺(tái)(在我們的實(shí)驗(yàn)中，一個(gè)腿式機(jī)器人或一個(gè)手持傳感器負(fù)載)的位置、方向和線速度，該平臺(tái)裝備有IMU、激光雷達(dá)和一個(gè)具有低延遲和全傳感器率的單目或立體聲攝像機(jī)。

圖2：ANYmal平臺(tái)和手持設(shè)備的參考框架約定。世界坐標(biāo)系W為固定架，底座坐標(biāo)系B、光學(xué)相機(jī)坐標(biāo)系C、IMU坐標(biāo)系、I、激光雷達(dá)坐標(biāo)系L分別安裝在移動(dòng)機(jī)器人的底盤或設(shè)備上。為簡(jiǎn)單起見(jiàn)，C和B在手持設(shè)備上是一致的。

相關(guān)的參考坐標(biāo)系在圖2中指定，并且包括機(jī)器人固定的基準(zhǔn)坐標(biāo)系B、左攝像機(jī)坐標(biāo)系C、IMU坐標(biāo)系I和激光雷達(dá)坐標(biāo)系L。我們希望估計(jì)相對(duì)于固定的世界坐標(biāo)系W的基準(zhǔn)幀的位置。

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

為對(duì)應(yīng)的齊次變換)以世界坐標(biāo)表示；速度

、

以基礎(chǔ)坐標(biāo)系表示，IMU偏移bg和ba以IMU坐標(biāo)系表示。

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

在時(shí)間

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

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

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

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

B. 測(cè)量定義

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

C. 最大后驗(yàn)估計(jì)

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

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

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

4 因子圖公式

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

圖3：VILENS滑動(dòng)窗口因子圖結(jié)構(gòu)，顯示先驗(yàn)的、視覺(jué)的、平面的、線的和預(yù)集成的IMU因子。隨著時(shí)間的推移跟蹤地標(biāo)，通過(guò)允許新的測(cè)量來(lái)提高過(guò)去狀態(tài)的準(zhǔn)確性，從而提高了估計(jì)的準(zhǔn)確性。

A. IMU預(yù)積分因子

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

有關(guān)殘差的定義，請(qǐng)參見(jiàn)[23]。

B. 使用激光雷達(dá)深度的單目地標(biāo)因子

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

設(shè)

是歐氏空間中的一個(gè)視覺(jué)地標(biāo)，

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

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

 將激光雷達(dá)在時(shí)間ti和ti+1之間獲取的所有點(diǎn)

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

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

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

C. 立體地標(biāo)因子

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

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

D. 平面地標(biāo)因子

我們使用黑森范式將無(wú)限平面

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

和表示其到原點(diǎn)的距離的標(biāo)量d：

設(shè)?是對(duì)平面p上的所有點(diǎn)應(yīng)用齊次變換T的算子，并且?將兩個(gè)平面(pi,pj)之間的誤差定義為：

其中

是

的切線空間的基，

定義如下[24]：

當(dāng)平面

在時(shí)間ti測(cè)量時(shí)，相應(yīng)的殘差是p與轉(zhuǎn)換到局部參考幀的估計(jì)平面pl之間的差：

E. 線地標(biāo)因子

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

是直線的方向，

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

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

在兩條線li,lj之間的運(yùn)算符被定義為

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

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

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

5 系統(tǒng)運(yùn)行

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

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

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

A. 視覺(jué)特征追蹤

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

B. 激光雷達(dá)處理和特征跟蹤

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

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

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

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

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

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

2)過(guò)濾：一旦點(diǎn)云沒(méi)有失真，我們從[30]開(kāi)始進(jìn)行分割，將點(diǎn)分成簇。較小的群集(小于5點(diǎn))被標(biāo)記為異常值并被丟棄，因?yàn)樗鼈兛赡軙?huì)有噪聲。

然后，利用文獻(xiàn)[2]的方法計(jì)算預(yù)濾波云中每個(gè)點(diǎn)的局部曲率。具有最低曲率和最高曲率的點(diǎn)分別被分配給平面候選集合Cp和線候選集合CL。

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

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

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

最后，我們檢查預(yù)測(cè)的地標(biāo)和檢測(cè)到的地標(biāo)是否足夠相似。當(dāng)兩個(gè)平面(

和

)的法線和到原點(diǎn)的距離之差小于閾值時(shí)，它們被認(rèn)為是匹配的：

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

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

跟蹤要素后，將從相應(yīng)的候選集合中刪除該要素的內(nèi)嵌對(duì)象，并對(duì)剩余的地標(biāo)重復(fù)此過(guò)程。

跟蹤完成后，我們?cè)谑Ｓ嗟暮蜻x云中檢測(cè)到新的地標(biāo)。首先對(duì)線使用歐幾里德聚類，對(duì)平面使用基于法線的區(qū)域生長(zhǎng)來(lái)劃分點(diǎn)云。然后，我們使用與地標(biāo)跟蹤相同的方法在每個(gè)集群中檢測(cè)新的地標(biāo)。

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

C. 零速更新因子

為了限制平臺(tái)靜止時(shí)的漂移和因子圖增長(zhǎng)，當(dāng)三個(gè)模態(tài)中的兩個(gè)(相機(jī)、激光雷達(dá)、IMU)的更新報(bào)告沒(méi)有運(yùn)動(dòng)時(shí)，我們向圖形添加零速度約束。

6 試驗(yàn)結(jié)果

我們?cè)趦蓚€(gè)對(duì)比鮮明的數(shù)據(jù)集中對(duì)我們的算法在各種室內(nèi)和室外環(huán)境中進(jìn)行了評(píng)估：較新的大學(xué)數(shù)據(jù)集[7]和DARPA SubT挑戰(zhàn)賽(城市)。這些環(huán)境的概述如圖7所示。

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

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

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

具體實(shí)驗(yàn)命名如下：

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

NC-2：在強(qiáng)光照度變化(480m，6分鐘)下以高動(dòng)態(tài)運(yùn)動(dòng)行走。

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

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

ST-B：一個(gè)不同的動(dòng)物機(jī)器人，在反應(yīng)堆的一部分，包含一條長(zhǎng)而直的走廊(490米，60分鐘)。

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

B. 結(jié)果

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

VILENS-LI：僅帶IMU和激光雷達(dá)的VILENS；

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

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

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

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

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

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

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

C. 多傳感器融合

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

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

類似地，在激光雷達(dá)地標(biāo)不足以完全約束估計(jì)(或接近退化)的情況下，視覺(jué)特征的緊密融合允許優(yōu)化利用激光雷達(dá)約束，同時(shí)避免退化問(wèn)題。

D. 分析

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

參考文獻(xiàn)