本文為作者在從事Slam相關工作中對這幾年遇到以及改進過相關VIO算法內容總結。

1.背景介紹

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

近十年來，SLAM 及其相關技術的研究取得了快速進展，研究的重點從開始的激光雷達到相機和IMU，與此同時芯片和MEMS器件的也取得快速發展，算力得到的極大提高，相機和IMU 等傳感器實現了高精度化、小型化和低成本化。這使得SLAM技術在移動端能夠實時的運用。而視覺傳感器因其體積小、 成本低和易于進行硬件設置而獲得了廣泛的關注，大量基于視覺傳感器的 SLAM 方法被提出，但純視覺 SLAM 方法存在無法在圖像紋理少的區域工作、快速運動時圖像模糊等問題；而IMU可測量角速度和加速度，其功能可以與相機進行互補，并且在融合之后能夠得到更加完善的SLAM 系統。

采用相機和 IMU 的 SLAM 方法被稱為視覺慣性 SLAM（visual-inertial SLAM，VI-SLAM），只含有很小的漂移。本文主要介紹VI-SLAM，對激光和純視覺SLAM不重點介紹。目前VI-SLAM數據融合的的方法分為兩類：緊耦合和松耦合，其中，緊耦合是指把 IMU 的狀態與相機的狀態合并在一起進行位姿估計。松耦合是指相機和 IMU 分別進行自身的位姿估計，然后對它們的估計結果進行融合。

而VI-SLAM根據后端優化方法的不同，分為基于濾波和基于優化 2 類方法，在濾波方法中，傳統的 EKF（擴展卡爾曼濾波器） 、UKF（無跡卡爾曼濾波器）、改進的 MSCKF（多狀態約束卡爾曼濾波器） 和OpenVins都取得了一定的成果。現階段基于優化的方法則慢慢占據了主流。

2. 基于優化的方案

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

基于優化的方法有兩種類型，一種是含有局部優化（LBA）和全局優化（GBA），一種是只有全局優化（GBA）

LBA通過設定固定的窗口，將在這個窗口內的系統狀態進行估計，同時邊緣化舊的狀態維持實時優化窗口。相比于濾波方法，基于優化的方法精度更高，因為它們可以對過去的測量結果重新進行線性化處理，同時在優化時可以設置魯棒核函數來剔除一些異常值。從而提高精度；但是缺點是，邊緣化舊的一幀時帶來了稠密的先驗信息，這在一定程度上降低了效率，針對這個問題，一些文獻 中提出了為了稀疏性而放棄某些測量的方法。此外，由于采用了邊緣化的方法，LBA方法具有與部分濾波方法相似的問題，如一致性，線性化誤差的累積等。

蘇 黎 世 聯 邦 理 工 學 院 ASL 實 驗 室 Leutenegger 等 提 出 了 OKVIS（open keyframe-based visual-inertial SLAM）；利用基于關鍵幀的滑動窗口進行批量非線性優化，先于滑動窗口的關鍵幀被邊緣化，不用來進行估計。系統前端使用多尺度 Harris 特征檢測器 來提取特征，然后在其基礎上計算 BRISK（binary robust invariant scalable keypoint）描述子，以便在幀與幀之間進行數據關聯。。如圖 3 所示

香港科技大學飛行機器人實驗室提出的 VINS-Mono（monocular visual-inertial system）方法類似于 OKVIS，是一種基于非線性優化的 VI-SLAM方法。VINS-Mono 方法為這類框架引入了幾個全新的功能，其完整系統包括觀測值預處理、 初始化、 局部視覺慣性聯合優化、全局圖優化和回環檢測 5 個部分， 前端提取 Harris 特征點，并采用 LK（Lucas-Kanade）光流（opticalflow）法跟蹤相鄰幀。光流法是一種描述像素隨時間在圖像之間運動的方法。LK 光流法計算部分像素，在 SLAM 中用于跟蹤特征點的位置。

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

香港科技大學飛行機器人實驗室隨后又推出了 VINS-Mobile 和 VINS-Fusion。VINS-Mobile 在 iOS 設備上運行，并為增強現實（AR）應用程序提供本地化服務。VINS-Mobile 對 VINS-Mono 框架作了許多輕量化處理，并采用了一種基于小滑動窗口的聯合優化方案。VINS-Fusion 在完善 VINS 框架的同時，提供了 4 個不同硬件的版本，包括單目 IMU、 雙目 IMU、 純雙目和雙目 IMU 與GPS 松耦合。

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

因為即使重新計算，精度也不會有太大的改善。對于LBA 來說，由于幾乎每個點都被滑動窗口里面所有幀看到，因此即使只重新線性化一部分點，也會帶來較大的改動。因此在LBA這個部分，系統中采用了一個更高效的針對局部 BA 的后端設計。這個方法把長期跟蹤的特征點分成很多短期跟蹤段，這樣每次重新計算特征點的時候，重新線性化的區域減小，從而提高了效率。見圖5；對于滑窗系統，往往都有邊緣化這一步，把有些本應該出了滑動窗口的信息以另一種形式存儲下來。但是當有GBA 存在時，邊緣化的效果未必會有很大的提升，反而可能會下降。ICE-BA 中改進了邊緣化方法，保持了邊緣化和全局 BA 的一致性，見圖6。但在測試的時候，去掉相對邊緣化有些數據集效果會更好。

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

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

在2020年，ORB-SLAM3開源，其第一個能夠執行視覺、視覺慣性和多地圖重用的系統，相比于ORB-SLAM2，ORB-SLAM3新增元素有以下面這幾個：

1. 單、雙目的vo/vio slam系統，并支持魚眼相機。

2. 地圖復用，擁有合并地圖的功能。

3. 高召回的地點識別（占用較低的計算資源換得高召回與精度）

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

其主要的創新點分為：

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

2.多地圖系統，當定位丟失即lost時，ORB-SLAM3會自動建立一個新的小地圖，并在revisit兩張地圖上的同一地點的時候進行地圖的seamlessly merge，因此，這一算法能夠使用不僅僅幾幀之前的信息，而是運用了全局的信息，能夠在bundle adjustment中利用視差較大的幀來增加BA求解的準確性（因為當視察較小時，求解不準確，且優化容易進入局部極值）。這種方法里對同一特征的觀測可能在時間上的間隔較大，因為我們對其上一次的觀測甚至可能出現在之前一張小地圖中（中間過程里lost了至少一次）。

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

基 于 優 化 的 方 法 是 目 前 VI-SLAM 領 域 以 及SLAM 相關領域的熱點之一，非線性優化方法實現了相較于濾波方法更精確的狀態估計效果。但由于計算資源有限，目前的方法往往以犧牲一部分精度和路標點為代價換取系統的實時性。新型的優化器以及并行計算方法的出現有望進一步減少對計算資源的占用，但大范圍在線系統的實時性仍然是一個有待解決的問題。

3. 基于濾波的方案

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

一個完整的 EKF 框架包括預測步驟和更新步驟。對于基于濾波的 VI-SLAM 方法，慣性傳感器能夠提供 3 軸的加速度和角速度，用于計算 3 維剛體運動的動態模型并在預測步驟中進行預測。相機能夠提供在特征和移動機器人之間的角度和距離測量結果，并在更新步驟中更新預測結果。

早期的 SLAM 工作主要是基于擴展卡爾曼濾波器，由 Smith 等基于早期的工作實現。Jones 等將 EKF 框架引入 VI-SLAM 系統。該系統實現了將狀態和參數作為在線程序的一部分進行估計，并用 EKF 框架進行了有效實現.Kelly 等將 UKF 框架引入 VI-SLAM 系統。該方法能夠在線、隨時校準更新位姿，比如正在進行的導航或建圖任務。

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

蘇黎世聯邦理工學院無人系統實驗室（Autono-mous Systems Lab，ASL）的 Bloesch 等提出了一種基于迭代擴展卡爾曼濾波器（IEKF）的直接法單目視覺慣性里程計，簡稱 ROVIO（robust visual inertialodometry） 。如圖 11所示，該算法將視覺信息和IMU信息進行緊耦合的一種視覺慣性測量單元。數據融合的方法主要是通過迭代卡爾曼濾波來進行的。對于視覺方面的信息，作者主要是通過將路標點在圖像中對應的點周圍的圖像塊做為路標點的描述子，從而得到光度誤差。然后將光度誤差進行變換得到IEKF中的innovation term，進而進行濾波狀態的更新。

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

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

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

Open VINS是黃國權老師團隊在2019年8月份開源的一套基于MSCKF的VINS算法。主要提供了流形滑動窗口卡爾曼濾波器、在線攝像機內、外標定、攝像機-慣性傳感器時間偏移標定、具有不同表示和一致第一估計的SLAM地標（FEJ）處理、用于狀態管理的模塊化系統、可擴展視覺慣性系統模擬器、用于算法評估的廣泛工具箱等功能。其在前端圖像信息處理中，結合了EKF-SLAM、MSCKF和Hybird SLAM等算法處理方式，提高了系統的穩定性，在一些開源數據集上其精度可以媲美基于優化的SLAM。該開源代碼的主要貢獻

1. 提供了一個可拓展、開源的代碼庫

2. 提供了許多視覺視覺慣性的基礎，包括多相機，多IMU，視覺慣性運動對象跟蹤，Schmidt-based visual-inertial SLAM，點平面和點線視覺慣性導航等。

開源代碼大致流程如圖12

圖12 openvins算法流程示意圖

表1 代表性 VI-SLAM 框架對比

4.濾波方法與優化方法的聯系與對比

利用貝葉斯推斷可以建立基于濾波和基于優化這 2 類方法之間的聯系。基于濾波的方法，位姿的先驗分布由內感受型傳感器的測量構建，似然分布由外感受型傳感器的測量建立，故可以視為最大后驗估計（maximum a posteriori，MAP）問題。基于優化的方法，通過迭代找到測量總概率最高的狀態，故可視為最大似然估計（maximum likelihood，ML）問題。基于優化的方法，可以通過向傳感器的測量值中添加正則化項或先驗項，使其由 ML 問題轉化為 MAP 問題。如果把 EKF 框架看作是非線性高斯-牛頓法或高斯法的近似，那么它的表現無疑是達不到要求的。

主要原因是，EKF 沒有迭代至收斂的過程，其雅可比矩陣只計算 1 次，可能會遠離所期望的最優估計。但由于 EKF 沒有一次性計算所有的雅可比矩陣，EKF 的結果比單次的高斯-牛頓法迭代更精確，它的缺陷只在于沒有迭代這個步驟。從優化的角度來看，這是一個顯而易見的問題，因為優化是需要最后迭代至收斂的。EKF 使用了馬爾可夫假設來實現其遞歸形式，由于使用了馬爾可夫假設，一旦濾波器建立在該假設上就無法擺脫它。包括IEKF 在內，雖然 IEKF 一次迭代了一個時間步長，但它仍然依賴于馬爾可夫假設，而且僅在一個時刻上進行了迭代，并非在整個軌跡上。

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

5. 展望

5.1 與深度學習結合

構建語義地圖、在幀間匹配和回環檢測中采用 深 度 學 習 的 方 法 是 目 前 SLAM 的 研 究 熱 點 之一 。深度學習方法的引入使移動機器人可以理解周圍環境的語義信息。完成一些更復雜的任務。但深度學習方法對于計算資源的需求巨大，在硬件條件較差的嵌入式場景難以使用，需要研究人員進一步地改進網絡。

5.2 輕量化

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

5.3 與更多傳感器融合

視覺與 IMU 的融合已經在不同的領域取得了較理想的效果，但是當場景特別大時，視覺和 IMU傳感器的效果不太理想，這時就需要用到其他傳感器。在自動駕駛領域，將激光雷達、 GPS/GNSS（全球衛星導航系統）、相機以及 IMU 等多傳感器進行融合已經成為一種趨勢，引入激光雷達等傳感器會為系統提供更準確的深度信息。但是更多的傳感器會帶來更復雜的標定與融合方面的問題，有待研究人員加以解決。

5.4 嵌入式系統

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

責任編輯：haq