相關(guān)VI-SLAM算法內(nèi)容總結(jié)

本文為作者在從事Slam相關(guān)工作中對(duì)這幾年遇到以及改進(jìn)過(guò)相關(guān)VIO算法內(nèi)容總結(jié)。

1.背景介紹

一個(gè)完整的 SLAM（simultaneous localization and mapping） 框架包括傳感器數(shù)據(jù)、 前端、 后端、 回環(huán)檢測(cè)與建圖，如圖1所示，其中，前端將傳感器的數(shù)據(jù)抽象成適用于估計(jì)的模型，回環(huán)檢測(cè)判斷機(jī)器人是否經(jīng)過(guò)已知的位置。而后端接受不同時(shí)刻前端測(cè)量的位姿和回環(huán)檢測(cè)的信息并對(duì)它們進(jìn)行優(yōu)化，從而得到全局一致的軌跡。建圖則是根據(jù)狀態(tài)估計(jì)得到的軌跡建立與任務(wù)要求相對(duì)應(yīng)的地圖。通常，僅含有前端和局部后端的框架被稱(chēng)為里程計(jì)， 而帶有回環(huán)檢測(cè)和全局后端的完整框架被稱(chēng)為 SLAM。

近十年來(lái)，SLAM 及其相關(guān)技術(shù)的研究取得了快速進(jìn)展，研究的重點(diǎn)從開(kāi)始的激光雷達(dá)到相機(jī)和IMU，與此同時(shí)芯片和MEMS器件的也取得快速發(fā)展，算力得到的極大提高，相機(jī)和IMU 等傳感器實(shí)現(xiàn)了高精度化、小型化和低成本化。這使得SLAM技術(shù)在移動(dòng)端能夠?qū)崟r(shí)的運(yùn)用。而視覺(jué)傳感器因其體積小、 成本低和易于進(jìn)行硬件設(shè)置而獲得了廣泛的關(guān)注，大量基于視覺(jué)傳感器的 SLAM 方法被提出，但純視覺(jué) SLAM 方法存在無(wú)法在圖像紋理少的區(qū)域工作、快速運(yùn)動(dòng)時(shí)圖像模糊等問(wèn)題；而IMU可測(cè)量角速度和加速度，其功能可以與相機(jī)進(jìn)行互補(bǔ)，并且在融合之后能夠得到更加完善的SLAM 系統(tǒng)。

采用相機(jī)和 IMU 的 SLAM 方法被稱(chēng)為視覺(jué)慣性 SLAM（visual-inertial SLAM，VI-SLAM），只含有很小的漂移。本文主要介紹VI-SLAM，對(duì)激光和純視覺(jué)SLAM不重點(diǎn)介紹。目前VI-SLAM數(shù)據(jù)融合的的方法分為兩類(lèi)：緊耦合和松耦合，其中，緊耦合是指把 IMU 的狀態(tài)與相機(jī)的狀態(tài)合并在一起進(jìn)行位姿估計(jì)。松耦合是指相機(jī)和 IMU 分別進(jìn)行自身的位姿估計(jì)，然后對(duì)它們的估計(jì)結(jié)果進(jìn)行融合。

而VI-SLAM根據(jù)后端優(yōu)化方法的不同，分為基于濾波和基于優(yōu)化 2 類(lèi)方法，在濾波方法中，傳統(tǒng)的 EKF（擴(kuò)展卡爾曼濾波器） 、UKF（無(wú)跡卡爾曼濾波器）、改進(jìn)的 MSCKF（多狀態(tài)約束卡爾曼濾波器） 和OpenVins都取得了一定的成果?，F(xiàn)階段基于優(yōu)化的方法則慢慢占據(jù)了主流。

2. 基于優(yōu)化的方案

基于優(yōu)化的方法主要依靠圖像處理技術(shù)進(jìn)行特征提取和圖像匹配，而 IMU 數(shù)據(jù)則被視為先驗(yàn)項(xiàng)或者正則化項(xiàng)。自 PTAM（parallel tracking and map-ping） 之后，批量非線性?xún)?yōu)化方法一般都分為2 個(gè)線程：跟蹤和建圖。在跟蹤線，通過(guò)各種特征檢測(cè)器從圖像中提取 3 維空間中的點(diǎn)、線或其他路標(biāo)特征。然后，針對(duì)檢測(cè)到的所有特征，在 2 個(gè)圖像之間定義重投影誤差。之后用該誤差建立優(yōu)化的代價(jià)函數(shù)，以便找到特征或地標(biāo)的坐標(biāo)，即光束平差法（bundle adjustment，BA） 。在建圖線程，地圖中特征和路標(biāo)的坐標(biāo)被用于定義 2 幅圖像之間的重投影誤差，然后再次應(yīng)用優(yōu)化算法來(lái)發(fā)現(xiàn)移動(dòng)機(jī)器人位姿的變化。將優(yōu)化問(wèn)題分成 2 個(gè)并行線程的目的是使跟蹤部分實(shí)時(shí)響應(yīng)圖像數(shù)據(jù)，以便快速獲得跟蹤結(jié)果。而地圖的優(yōu)化沒(méi)有必要實(shí)時(shí)計(jì)算，可以放在后端緩慢運(yùn)行?；趦?yōu)化的方法通常使用g 2 o 、Ceres 、GTSAM 等非線性?xún)?yōu)化庫(kù)實(shí)現(xiàn)優(yōu)化。

基于優(yōu)化的方法有兩種類(lèi)型，一種是含有局部?jī)?yōu)化（LBA）和全局優(yōu)化（GBA），一種是只有全局優(yōu)化（GBA）

LBA通過(guò)設(shè)定固定的窗口，將在這個(gè)窗口內(nèi)的系統(tǒng)狀態(tài)進(jìn)行估計(jì)，同時(shí)邊緣化舊的狀態(tài)維持實(shí)時(shí)優(yōu)化窗口。相比于濾波方法，基于優(yōu)化的方法精度更高，因?yàn)樗鼈兛梢詫?duì)過(guò)去的測(cè)量結(jié)果重新進(jìn)行線性化處理，同時(shí)在優(yōu)化時(shí)可以設(shè)置魯棒核函數(shù)來(lái)剔除一些異常值。從而提高精度；但是缺點(diǎn)是，邊緣化舊的一幀時(shí)帶來(lái)了稠密的先驗(yàn)信息，這在一定程度上降低了效率，針對(duì)這個(gè)問(wèn)題，一些文獻(xiàn) 中提出了為了稀疏性而放棄某些測(cè)量的方法。此外，由于采用了邊緣化的方法，LBA方法具有與部分濾波方法相似的問(wèn)題，如一致性，線性化誤差的累積等。

蘇 黎 世 聯(lián) 邦 理 工 學(xué) 院 ASL 實(shí) 驗(yàn) 室 Leutenegger 等 提 出 了 OKVIS（open keyframe-based visual-inertial SLAM）；利用基于關(guān)鍵幀的滑動(dòng)窗口進(jìn)行批量非線性?xún)?yōu)化，先于滑動(dòng)窗口的關(guān)鍵幀被邊緣化，不用來(lái)進(jìn)行估計(jì)。系統(tǒng)前端使用多尺度 Harris 特征檢測(cè)器 來(lái)提取特征，然后在其基礎(chǔ)上計(jì)算 BRISK（binary robust invariant scalable keypoint）描述子，以便在幀與幀之間進(jìn)行數(shù)據(jù)關(guān)聯(lián)。。如圖 3 所示

香港科技大學(xué)飛行機(jī)器人實(shí)驗(yàn)室提出的 VINS-Mono（monocular visual-inertial system）方法類(lèi)似于 OKVIS，是一種基于非線性?xún)?yōu)化的 VI-SLAM方法。VINS-Mono 方法為這類(lèi)框架引入了幾個(gè)全新的功能，其完整系統(tǒng)包括觀測(cè)值預(yù)處理、 初始化、 局部視覺(jué)慣性聯(lián)合優(yōu)化、全局圖優(yōu)化和回環(huán)檢測(cè) 5 個(gè)部分， 前端提取 Harris 特征點(diǎn)，并采用 LK（Lucas-Kanade）光流（opticalflow）法跟蹤相鄰幀。光流法是一種描述像素隨時(shí)間在圖像之間運(yùn)動(dòng)的方法。LK 光流法計(jì)算部分像素，在 SLAM 中用于跟蹤特征點(diǎn)的位置。

VINS-Mono 方法只計(jì)算特征點(diǎn)，不計(jì)算描述子，同時(shí)使用光流法跟蹤特征點(diǎn)的運(yùn)動(dòng)。這樣就減少了計(jì)算和匹配描述子的時(shí)間和資源，只需要計(jì)算光流。在初始化部分，作者使用了一種松耦合的傳感器融合初始化程序，稱(chēng)為動(dòng)態(tài)初始化，先用圖像信息構(gòu)建SFM，后面結(jié)合IMU信息，進(jìn)行在線VIO標(biāo)定。使得系統(tǒng)可以在優(yōu)化之前從任意初始狀態(tài)引入估計(jì)器。同時(shí)采用預(yù)積分的處理方法處理IMU信息，得到兩幀之間的IMU相對(duì)運(yùn)動(dòng)，而不是隨著某一時(shí)刻位姿改變而要重新處理IMU信息，減少計(jì)算量。系統(tǒng)采用與 OKVIS相似的基于滑動(dòng)窗口的緊耦合位姿估計(jì)方法，并且加入了基于DBoW2（bag of binary words 2）的回環(huán)檢測(cè)線程，使系統(tǒng)具有重定位功能，而全局優(yōu)化則采用4Dof優(yōu)化。

香港科技大學(xué)飛行機(jī)器人實(shí)驗(yàn)室隨后又推出了 VINS-Mobile 和 VINS-Fusion。VINS-Mobile 在 iOS 設(shè)備上運(yùn)行，并為增強(qiáng)現(xiàn)實(shí)（AR）應(yīng)用程序提供本地化服務(wù)。VINS-Mobile 對(duì) VINS-Mono 框架作了許多輕量化處理，并采用了一種基于小滑動(dòng)窗口的聯(lián)合優(yōu)化方案。VINS-Fusion 在完善 VINS 框架的同時(shí)，提供了 4 個(gè)不同硬件的版本，包括單目 IMU、 雙目 IMU、 純雙目和雙目 IMU 與GPS 松耦合。

百度 AR 技術(shù)小組提出的 ICE-BA（incremental，consistent and efficient bundle adjustment） 沿 用了 OKVIS 以及 VINS-Mono 這一類(lèi)框架。前端提取Harris 特征點(diǎn) 并采用 LK 光流法 跟蹤相鄰幀，與 VINS-Mono 相似。后端則是論文中提出的增量式 BA，主要分為 3 個(gè)部分：局部 BA（LBA）、全局 BA（GBA） 以及相對(duì)邊緣化（Relative-Marginalization），前兩者采用增量式方法提升了后端速度，后者保證了LBA 和GBA 的一致性。其中，G BA 在建立增量方程時(shí)，對(duì)系統(tǒng)已經(jīng)計(jì)算過(guò)且不變的狀態(tài)向量不進(jìn)行計(jì)算。

因?yàn)榧词怪匦掠?jì)算，精度也不會(huì)有太大的改善。對(duì)于LBA 來(lái)說(shuō)，由于幾乎每個(gè)點(diǎn)都被滑動(dòng)窗口里面所有幀看到，因此即使只重新線性化一部分點(diǎn)，也會(huì)帶來(lái)較大的改動(dòng)。因此在LBA這個(gè)部分，系統(tǒng)中采用了一個(gè)更高效的針對(duì)局部 BA 的后端設(shè)計(jì)。這個(gè)方法把長(zhǎng)期跟蹤的特征點(diǎn)分成很多短期跟蹤段，這樣每次重新計(jì)算特征點(diǎn)的時(shí)候，重新線性化的區(qū)域減小，從而提高了效率。見(jiàn)圖5；對(duì)于滑窗系統(tǒng)，往往都有邊緣化這一步，把有些本應(yīng)該出了滑動(dòng)窗口的信息以另一種形式存儲(chǔ)下來(lái)。但是當(dāng)有GBA 存在時(shí)，邊緣化的效果未必會(huì)有很大的提升，反而可能會(huì)下降。ICE-BA 中改進(jìn)了邊緣化方法，保持了邊緣化和全局 BA 的一致性，見(jiàn)圖6。但在測(cè)試的時(shí)候，去掉相對(duì)邊緣化有些數(shù)據(jù)集效果會(huì)更好。

由于 VI-SLAM 系統(tǒng)實(shí)現(xiàn)了優(yōu)異的效果，一些著名的視覺(jué) SLAM 系統(tǒng) 相繼推出了自己系統(tǒng)的 VI 版本，VI-ORB SLAM 就是其中最有代表性的系統(tǒng)之一。在這里首先介紹純視覺(jué)ORB-SLAM ，其首次使用 3 個(gè)線程：實(shí)時(shí)跟蹤特征點(diǎn)的線程、局部建圖的優(yōu)化線程（co-visibility graph）和全局回環(huán)檢測(cè)與優(yōu)化線程（essential graph），來(lái)完成 SLAM。跟蹤線程對(duì)每幅圖像提取 ORB 特征點(diǎn)，并與最近的關(guān)鍵幀比較，計(jì)算特征點(diǎn)位置并估計(jì)位姿。局部 BA 線程求解更精細(xì)的相機(jī)位姿和特征點(diǎn)空間位置。全局回環(huán)檢測(cè)線程采用 DBoW2 對(duì)全局的地圖與關(guān)鍵幀進(jìn)行回環(huán)檢測(cè)，消除累積誤差。ORB-SLAM 的三線程結(jié)構(gòu)取得了優(yōu)異的跟蹤和建圖效果，保證了移動(dòng)機(jī)器人軌跡與地圖的全局一致性，該算法框架在開(kāi)源數(shù)據(jù)集測(cè)試的結(jié)果是效果最好的開(kāi)源代碼。

VI-ORB SLAM 改進(jìn)自 ORB-SLAM2，作者引入 IMU 嘗試解決在快速運(yùn)動(dòng)時(shí)丟失特征點(diǎn)的問(wèn)題。VI ORB-SLAM 分別對(duì) 3 個(gè)線程作了修改，用以融合 IMU 信息。在跟蹤線程，基于重投影誤差和 IMU 預(yù)積分，建立幀與幀之間的約束關(guān)系來(lái)構(gòu)造代價(jià)函數(shù)，從而得到當(dāng)前幀位姿的最優(yōu)估計(jì)。在局部建圖線程，有了新的關(guān)鍵幀之后，將會(huì)對(duì)前 N 個(gè)關(guān)鍵幀進(jìn)行優(yōu)化，當(dāng)前的關(guān)鍵幀（第 N + 1幀）將固定不變，提供 IMU 預(yù)積分約束。如圖 7所示，P、v、b 分別為優(yōu)化的位姿、IMU 速度與偏差。在全局回環(huán)檢測(cè)線程，由于 IMU 提供了尺度信息，因此全局優(yōu)化將從 7 個(gè)自由度下降到 6 個(gè)自由度。全局位姿優(yōu)化將忽略 IMU 信息，因此不再優(yōu)化速度和偏差，當(dāng)完成全局位姿優(yōu)化后，再根據(jù)矯正后的位姿對(duì)速度進(jìn)行矯正。

在2020年，ORB-SLAM3開(kāi)源，其第一個(gè)能夠執(zhí)行視覺(jué)、視覺(jué)慣性和多地圖重用的系統(tǒng)，相比于ORB-SLAM2，ORB-SLAM3新增元素有以下面這幾個(gè)：

1. 單、雙目的vo/vio slam系統(tǒng)，并支持魚(yú)眼相機(jī)。

2. 地圖復(fù)用，擁有合并地圖的功能。

3. 高召回的地點(diǎn)識(shí)別（占用較低的計(jì)算資源換得高召回與精度）

4. 不限制相機(jī)模型，只需提供投影，反投影及Jacobian方程（程序里提供了針孔與魚(yú)眼模型）

其主要的創(chuàng)新點(diǎn)分為：

1.“完全”基于最大后驗(yàn)估計(jì)MAP的VI-SLAM ，無(wú)論在初始化階段還是運(yùn)行階段，都采用了MAP進(jìn)行狀態(tài)估計(jì)，因此ORB-SLAM3在室內(nèi)外、大小場(chǎng)景中魯棒性很好，且精確度是其他方法的2~5倍； 如文中所講，本文的IMU和視覺(jué)的組合系統(tǒng)是extremely robust的。

2.多地圖系統(tǒng)，當(dāng)定位丟失即lost時(shí)，ORB-SLAM3會(huì)自動(dòng)建立一個(gè)新的小地圖，并在revisit兩張地圖上的同一地點(diǎn)的時(shí)候進(jìn)行地圖的seamlessly merge，因此，這一算法能夠使用不僅僅幾幀之前的信息，而是運(yùn)用了全局的信息，能夠在bundle adjustment中利用視差較大的幀來(lái)增加BA求解的準(zhǔn)確性（因?yàn)楫?dāng)視察較小時(shí)，求解不準(zhǔn)確，且優(yōu)化容易進(jìn)入局部極值）。這種方法里對(duì)同一特征的觀測(cè)可能在時(shí)間上的間隔較大，因?yàn)槲覀儗?duì)其上一次的觀測(cè)甚至可能出現(xiàn)在之前一張小地圖中（中間過(guò)程里lost了至少一次）。

在這里簡(jiǎn)單介紹一下iSAM，iSAM是增量式圖優(yōu)化，該算法可簡(jiǎn)單的理解為iSAM可以自適應(yīng)的判斷當(dāng)前測(cè)量是否影響某個(gè)歷史狀態(tài)，從而判斷是否要計(jì)算更新，從而能夠減少不必要的計(jì)算；在2012年，iSAM2 用貝葉斯樹(shù)+在線啟發(fā)式排序重新解決iSAM問(wèn)題，后端部分原理相似與ICE-BA相似，都是增量式BA，該技術(shù)是GBA的一項(xiàng)突破，利用因子圖來(lái)保持稀疏性，并僅識(shí)別和更新受新度量影響的變量的一小部分。使用貝葉斯樹(shù)數(shù)據(jù)結(jié)構(gòu)來(lái)獲得有效的變量排序，從而最大程度地減少了計(jì)算時(shí)間。由于新的測(cè)量結(jié)果只對(duì)估計(jì)值的局部產(chǎn)生影響，通過(guò)分析受影響的區(qū)域，可以減少一些沒(méi)必要的計(jì)算，加速優(yōu)化流程。

基 于 優(yōu) 化 的 方 法 是 目 前 VI-SLAM 領(lǐng) 域 以 及SLAM 相關(guān)領(lǐng)域的熱點(diǎn)之一，非線性?xún)?yōu)化方法實(shí)現(xiàn)了相較于濾波方法更精確的狀態(tài)估計(jì)效果。但由于計(jì)算資源有限，目前的方法往往以犧牲一部分精度和路標(biāo)點(diǎn)為代價(jià)換取系統(tǒng)的實(shí)時(shí)性。新型的優(yōu)化器以及并行計(jì)算方法的出現(xiàn)有望進(jìn)一步減少對(duì)計(jì)算資源的占用，但大范圍在線系統(tǒng)的實(shí)時(shí)性仍然是一個(gè)有待解決的問(wèn)題。

3. 基于濾波的方案

不同濾波方法的分類(lèi)如圖 9 所示，卡爾曼濾波器作為一種解決狀態(tài)估計(jì)問(wèn)題的經(jīng)典模型，主要存在 2 個(gè)問(wèn)題。首先，它要求時(shí)序和測(cè)量方程是線性的，這個(gè)問(wèn)題可以由 EKF 和 UKF 解決。其次，卡爾曼濾波器假設(shè)后驗(yàn)分布是單峰分布的，而且需要通過(guò)均值和協(xié)方差來(lái)表達(dá)，因此，它針對(duì)物體的位姿只能有一個(gè)假設(shè)，并不能保證關(guān)于狀態(tài)的多個(gè)假設(shè)，這個(gè)問(wèn)題可以由粒子濾波器（particle filter，PF） 解決。

一個(gè)完整的 EKF 框架包括預(yù)測(cè)步驟和更新步驟。對(duì)于基于濾波的 VI-SLAM 方法，慣性傳感器能夠提供 3 軸的加速度和角速度，用于計(jì)算 3 維剛體運(yùn)動(dòng)的動(dòng)態(tài)模型并在預(yù)測(cè)步驟中進(jìn)行預(yù)測(cè)。相機(jī)能夠提供在特征和移動(dòng)機(jī)器人之間的角度和距離測(cè)量結(jié)果，并在更新步驟中更新預(yù)測(cè)結(jié)果。

早期的 SLAM 工作主要是基于擴(kuò)展卡爾曼濾波器，由 Smith 等基于早期的工作實(shí)現(xiàn)。Jones 等將 EKF 框架引入 VI-SLAM 系統(tǒng)。該系統(tǒng)實(shí)現(xiàn)了將狀態(tài)和參數(shù)作為在線程序的一部分進(jìn)行估計(jì)，并用 EKF 框架進(jìn)行了有效實(shí)現(xiàn).Kelly 等將 UKF 框架引入 VI-SLAM 系統(tǒng)。該方法能夠在線、隨時(shí)校準(zhǔn)更新位姿，比如正在進(jìn)行的導(dǎo)航或建圖任務(wù)。

MSCKF 是 Mourikis 在 2007 年 提 出 的，MSCKF 2.0 也在隨后由 Mourikis 和 Li 提出 ，是目前許多 VI-SLAM 系統(tǒng)的基礎(chǔ)。MSCKF 是一種基于擴(kuò)展卡爾曼濾波器的 VI-SLAM 框架。在傳統(tǒng)EKF 框架中，特征點(diǎn)信息會(huì)加入到狀態(tài)向量和協(xié)方差矩陣?yán)?，這會(huì)加大矩陣維度，增加計(jì)算量，如圖 10 所示。而MSCKF 則在狀態(tài)向量里只添加窗口內(nèi)的相機(jī)狀態(tài)，而忽略特征點(diǎn)，減少計(jì)算量。同時(shí)系統(tǒng)一般采用IMU靜態(tài)初始化，系統(tǒng)啟動(dòng)后需要靜止一段時(shí)間，才能初始化成功，該系統(tǒng)沒(méi)有回環(huán)檢測(cè)功能。MSCKF 工作的最主要貢獻(xiàn)在于推導(dǎo)出一種測(cè)量模型，該模型能夠表達(dá)從多個(gè)相機(jī)位姿觀察到靜態(tài)特征時(shí)出現(xiàn)的幾何約束。系統(tǒng)中維護(hù)一個(gè)位姿的滑動(dòng)窗口，如果一個(gè)特征點(diǎn)在滑動(dòng)窗口內(nèi)的幾個(gè)位姿都被觀察到的話就會(huì)在這幾個(gè)位姿間建立約束。這種方法用一個(gè)特征點(diǎn)約束多個(gè)相機(jī)位姿，從而進(jìn)行 KF 的更新，舊的特征點(diǎn)和滑動(dòng)窗口之外的相機(jī)姿態(tài)則被丟棄。近幾年，許多基于 MSCKF 的工作相繼提出，框架整體的精度和魯棒性得到了不斷的提升。

蘇黎世聯(lián)邦理工學(xué)院無(wú)人系統(tǒng)實(shí)驗(yàn)室（Autono-mous Systems Lab，ASL）的 Bloesch 等提出了一種基于迭代擴(kuò)展卡爾曼濾波器（IEKF）的直接法單目視覺(jué)慣性里程計(jì)，簡(jiǎn)稱(chēng) ROVIO（robust visual inertialodometry） 。如圖 11所示，該算法將視覺(jué)信息和IMU信息進(jìn)行緊耦合的一種視覺(jué)慣性測(cè)量單元。數(shù)據(jù)融合的方法主要是通過(guò)迭代卡爾曼濾波來(lái)進(jìn)行的。對(duì)于視覺(jué)方面的信息，作者主要是通過(guò)將路標(biāo)點(diǎn)在圖像中對(duì)應(yīng)的點(diǎn)周?chē)膱D像塊做為路標(biāo)點(diǎn)的描述子，從而得到光度誤差。然后將光度誤差進(jìn)行變換得到IEKF中的innovation term，進(jìn)而進(jìn)行濾波狀態(tài)的更新。

整體的濾波方程的構(gòu)造是以機(jī)器人為中心進(jìn)行構(gòu)造的（fully robocentric）——實(shí)際上這個(gè)robocentric就是以IMU為原點(diǎn)固連在IMU上的坐標(biāo)系——從而保證能觀狀態(tài)不受不斷增長(zhǎng)的全局協(xié)方差的影響，這樣可以減小因非線性而造成的誤差。此外，作者還將路標(biāo)點(diǎn)的空間位置信息拆開(kāi)成了兩項(xiàng)。一項(xiàng)是bearing vector（二維向量），還有一項(xiàng)是distance（這里實(shí)際上用的是逆深度）。并在 IMU 預(yù)測(cè)階段對(duì)路標(biāo)點(diǎn)進(jìn)行預(yù)測(cè)，在視覺(jué)更新時(shí)對(duì)其修正，不像其他框架一般僅在視覺(jué)階段去計(jì)算。這樣的構(gòu)造方式可以避免不能觀狀態(tài)量，并且使得初始化沒(méi)有延遲。由于使用了基于 QR 分解的測(cè)量空間縮減方法，并執(zhí)行每個(gè)路標(biāo)點(diǎn)的更新迭代，因此系統(tǒng)具有較高的效率和準(zhǔn)確性。

ARL 實(shí) 驗(yàn) 室 隨 后 又 推 出 了 Maplab 框 架，與 ROVIO 不同的是，Maplab 帶有一個(gè)完整的 VI-SLAM 系統(tǒng)，具有回環(huán)檢測(cè)和重定位功能。系統(tǒng)由兩部分組成，一部分是 ROVIOLI（ROVIO with lo-calization integration），一個(gè)在線的視覺(jué)慣性全局定位系統(tǒng)，其接收?qǐng)D像與慣性傳感器數(shù)據(jù)作為輸入，輸出全局的位姿估計(jì)，并建立地圖。另一部分是離線的 Maplab 控制臺(tái)，可以讓使用者以離線批處理方式在地圖上應(yīng)用各種算法。

在 松 耦 合 方 法 中， Faessler 等使 用 了 一種基于 SVO（semi-direct visual odometry）和 MSF（multi-sensor fusion）的方案。SVO 是一種計(jì)算量較小的稀疏直接算法。其通過(guò)跟蹤 FAST（fromaccelerated segment test） 特征并最小化周?chē)鷪D像塊的光度誤差來(lái)匹配幀間圖像，使用非線性最小二乘法來(lái)最小化特征中的重投影誤差，得到僅由相機(jī)估計(jì)的位姿。MSF是一種通用的 EKF 框架，用于在位姿估計(jì)中融合來(lái)自不同傳感器的數(shù)據(jù)。系統(tǒng)將純視覺(jué) SVO 得到的位姿作為通用位姿傳感器的輸出提供給 MSF，然后與 IMU 數(shù)據(jù)融合。由于是一種松耦合框架，位姿的尺度需要近似正確，因而時(shí)常要手動(dòng)初始化。

Open VINS是黃國(guó)權(quán)老師團(tuán)隊(duì)在2019年8月份開(kāi)源的一套基于MSCKF的VINS算法。主要提供了流形滑動(dòng)窗口卡爾曼濾波器、在線攝像機(jī)內(nèi)、外標(biāo)定、攝像機(jī)-慣性傳感器時(shí)間偏移標(biāo)定、具有不同表示和一致第一估計(jì)的SLAM地標(biāo)（FEJ）處理、用于狀態(tài)管理的模塊化系統(tǒng)、可擴(kuò)展視覺(jué)慣性系統(tǒng)模擬器、用于算法評(píng)估的廣泛工具箱等功能。其在前端圖像信息處理中，結(jié)合了EKF-SLAM、MSCKF和Hybird SLAM等算法處理方式，提高了系統(tǒng)的穩(wěn)定性，在一些開(kāi)源數(shù)據(jù)集上其精度可以媲美基于優(yōu)化的SLAM。該開(kāi)源代碼的主要貢獻(xiàn)

1. 提供了一個(gè)可拓展、開(kāi)源的代碼庫(kù)

2. 提供了許多視覺(jué)視覺(jué)慣性的基礎(chǔ)，包括多相機(jī)，多IMU，視覺(jué)慣性運(yùn)動(dòng)對(duì)象跟蹤，Schmidt-based visual-inertial SLAM，點(diǎn)平面和點(diǎn)線視覺(jué)慣性導(dǎo)航等。

開(kāi)源代碼大致流程如圖12

圖12 openvins算法流程示意圖

表1 代表性 VI-SLAM 框架對(duì)比

4.濾波方法與優(yōu)化方法的聯(lián)系與對(duì)比

利用貝葉斯推斷可以建立基于濾波和基于優(yōu)化這 2 類(lèi)方法之間的聯(lián)系?；跒V波的方法，位姿的先驗(yàn)分布由內(nèi)感受型傳感器的測(cè)量構(gòu)建，似然分布由外感受型傳感器的測(cè)量建立，故可以視為最大后驗(yàn)估計(jì)（maximum a posteriori，MAP）問(wèn)題?；趦?yōu)化的方法，通過(guò)迭代找到測(cè)量總概率最高的狀態(tài)，故可視為最大似然估計(jì)（maximum likelihood，ML）問(wèn)題?；趦?yōu)化的方法，可以通過(guò)向傳感器的測(cè)量值中添加正則化項(xiàng)或先驗(yàn)項(xiàng)，使其由 ML 問(wèn)題轉(zhuǎn)化為 MAP 問(wèn)題。如果把 EKF 框架看作是非線性高斯-牛頓法或高斯法的近似，那么它的表現(xiàn)無(wú)疑是達(dá)不到要求的。

主要原因是，EKF 沒(méi)有迭代至收斂的過(guò)程，其雅可比矩陣只計(jì)算 1 次，可能會(huì)遠(yuǎn)離所期望的最優(yōu)估計(jì)。但由于 EKF 沒(méi)有一次性計(jì)算所有的雅可比矩陣，EKF 的結(jié)果比單次的高斯-牛頓法迭代更精確，它的缺陷只在于沒(méi)有迭代這個(gè)步驟。從優(yōu)化的角度來(lái)看，這是一個(gè)顯而易見(jiàn)的問(wèn)題，因?yàn)閮?yōu)化是需要最后迭代至收斂的。EKF 使用了馬爾可夫假設(shè)來(lái)實(shí)現(xiàn)其遞歸形式，由于使用了馬爾可夫假設(shè)，一旦濾波器建立在該假設(shè)上就無(wú)法擺脫它。包括IEKF 在內(nèi)，雖然 IEKF 一次迭代了一個(gè)時(shí)間步長(zhǎng)，但它仍然依賴(lài)于馬爾可夫假設(shè)，而且僅在一個(gè)時(shí)刻上進(jìn)行了迭代，并非在整個(gè)軌跡上。

本文比較了幾種代表性的 VI-SLAM 框架，如表 1 所示。可以看出，目前主流的 VI-SLAM 實(shí)現(xiàn)方法以緊耦合的優(yōu)化方法為主。相較于松耦合的方法，把 IMU 狀態(tài)與相機(jī)狀態(tài)合并在一起進(jìn)行狀態(tài)估計(jì)的緊耦合方法具有更高的精度。而由于基于濾波的方法具有馬爾可夫性，無(wú)法考慮到某時(shí)刻狀態(tài)與之前所有時(shí)刻狀態(tài)的關(guān)系，目前普遍認(rèn)為在計(jì)算資源足夠的情況下，基于優(yōu)化的方法會(huì)得到更精確的效果。但是在計(jì)算資源受限，或者移動(dòng)機(jī)器人位姿軌跡比較簡(jiǎn)單的場(chǎng)合里，基于濾波的方法仍然是一種有效的方式。

5. 展望

5.1 與深度學(xué)習(xí)結(jié)合

構(gòu)建語(yǔ)義地圖、在幀間匹配和回環(huán)檢測(cè)中采用 深 度 學(xué) 習(xí) 的 方 法 是 目 前 SLAM 的 研 究 熱 點(diǎn) 之一 。深度學(xué)習(xí)方法的引入使移動(dòng)機(jī)器人可以理解周?chē)h(huán)境的語(yǔ)義信息。完成一些更復(fù)雜的任務(wù)。但深度學(xué)習(xí)方法對(duì)于計(jì)算資源的需求巨大，在硬件條件較差的嵌入式場(chǎng)景難以使用，需要研究人員進(jìn)一步地改進(jìn)網(wǎng)絡(luò)。

5.2 輕量化

SLAM 本身是為了給上層應(yīng)用提供自身位姿估計(jì)，在實(shí)際應(yīng)用中，研究人員并不希望算法占用太多運(yùn)算資源。使用者希望 SLAM 框架能夠輕量化，不影響移動(dòng)機(jī)器人或者手機(jī)移動(dòng)端所要完成的其他工作。相比于激光 SLAM 等成本高昂的方法，由于 VI-SLAM 系統(tǒng)只使用相機(jī)和慣性傳感器，在無(wú)人機(jī)或手持移動(dòng)設(shè)備上良好運(yùn)行 SLAM 程序成為可能。通過(guò)繼續(xù)改進(jìn)算法，現(xiàn)在很多大廠均能夠在移動(dòng)端實(shí)現(xiàn)效果良好的定位效果。

5.3 與更多傳感器融合

視覺(jué)與 IMU 的融合已經(jīng)在不同的領(lǐng)域取得了較理想的效果，但是當(dāng)場(chǎng)景特別大時(shí)，視覺(jué)和 IMU傳感器的效果不太理想，這時(shí)就需要用到其他傳感器。在自動(dòng)駕駛領(lǐng)域，將激光雷達(dá)、 GPS/GNSS（全球衛(wèi)星導(dǎo)航系統(tǒng)）、相機(jī)以及 IMU 等多傳感器進(jìn)行融合已經(jīng)成為一種趨勢(shì)，引入激光雷達(dá)等傳感器會(huì)為系統(tǒng)提供更準(zhǔn)確的深度信息。但是更多的傳感器會(huì)帶來(lái)更復(fù)雜的標(biāo)定與融合方面的問(wèn)題，有待研究人員加以解決。

5.4 嵌入式系統(tǒng)

移動(dòng)機(jī)器人通常使用嵌入式設(shè)備作為平臺(tái)的計(jì)算硬件，而隨著計(jì)算能力的大力發(fā)展，嵌入式設(shè)備的使用頻率逐漸增多。新型 GPU（圖形處理器）的出現(xiàn)使得嵌入式系統(tǒng)的性能更加接近 PC 機(jī)，但是計(jì)算能力仍然有限。一些嵌入式硬件，例如Jetson Nano、TX1/2 的出現(xiàn)使得研究人員可以在嵌入式系統(tǒng)中使用 GPU，便于移動(dòng)機(jī)器人在惡劣的條件下應(yīng)用 VI-SLAM 算法。隨著一些代表性產(chǎn)品 Google Project Tango、Microsoft Hololens、MagicLeap、Nreal等的應(yīng)用，VI-SLAM 技術(shù)會(huì)逐步融入到平常生活之中。

責(zé)任編輯：haq