視覺慣性SLAM 研究進展*

郭金輝，陳秀萬，王 媛

（1.北京大學遙感與地理信息系統研究所，北京 100871；2.中國電子科學研究院，北京 100041）

0 引言

同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）是一種起源于上個世紀80 年代的機器人領域技術，其目標是在未知環境中進行建圖并確定機器人自身位置［1］。SLAM 具有多種分類方式，常見的SLAM 系統根據傳感器類別區分可劃分為單目SLAM、雙目SLAM、RGBD-SLAM、激光SLAM、微波SLAM、WiFi-SLAM、視覺慣性SLAM等。在SLAM 發展早期，激光SLAM 占據機器人領域相關研究的主要地位。但隨著傳感器不斷發展，低成本、體積小、信息豐富的相機和慣性測量傳感器（Inertial Measurement Unit，IMU）逐漸成為研究熱點［2］。

在實際應用過程中，單傳感器定位往往存在問題。例如相機獲取的圖像雖然信息豐富但易受環境干擾（遮擋、運動物體、無紋理場景、光照等），在運動較快時圖像會出現模糊。而IMU 屬于內部傳感器，其不受成像質量影響且采樣頻率高，但長時間積分后位姿漂移較大。因此，相比單個傳感器，相機和IMU 融合定位方式可取得更高的精度，提高整個系統的魯棒性［3］。近年來，視覺慣性SLAM（Visual Inertial SLAM，VISLAM）發展迅速，在攝影測量、三維重建、深空探測、室內導航、增強現實、無人駕駛等領域已得到廣泛應用。

當前已有許多視覺SLAM 的綜述研究［4-6］，但VISLAM 的研究綜述相對較少。本文主要專注于對VISLAM 的方法進行分析和討論，系統地介紹并分析幾種經典算法框架及其優缺點。另外，本文從硬件和算法層面討論了近年研究熱點與發展趨勢，并在最后作出總結。

1 VISLAM 關鍵技術

與視覺SLAM 一樣，VISLAM 也可分為前端、后端、回環檢測和建圖4 個部分［7］。前端部分通過對傳感器數據進行處理解算，獲得相鄰觀測間的運動及局部地圖。后端部分將前端和回環檢測的信息作為輸入，通過濾波或非線性優化的方式對整個系統進行狀態估計，從而獲得全局一致的軌跡和地圖。回環檢測用于判斷機器人是否到達過先前的位置，可有效減小累積誤差。而建圖是指根據自身定位結果，構建與任務要求對應的地圖的過程。

1.1 前端

1.1.1 視覺前端

視覺前端是指利用序列圖像估計圖像間運動，可分為特征點法和直接法。特征點法首先需要對圖像提取特征點，然后通過特征匹配實現數據關聯。良好的特征點具有可重復性與獨特性，并具有一定程度上的平移、旋轉和尺度不變性，常用的特征點包括SIFT［8］、SURF［9］、ORB［10］等。SIFT 和SURF 特征點計算較為復雜，因此，常用于運動恢復結構（Structure From Motion，SFM）問題的離線解算，而使用ORB 特征點可以保證SLAM 求解的實時性［11］。如果相鄰圖像基線較小，可使用光流跟蹤的方法代替特征匹配［12］。光流跟蹤一般只使用簡單角點如Harris［13］和FAST［14］角點，因此，跟蹤速度較快。但光流跟蹤基于灰度不變假設，因此，光照條件變化較大時會導致跟蹤丟失。在實現數據關聯后，使用P3P［15］、EPnP［16］、UPnP［17］等方法求得位姿初值，再使用光束法平差（Bundle Adjustment，BA）最小化重投影誤差，從而解得精確相機位姿。

特征點法將數據關聯和位姿估計分為兩個部分，而直接法通過最小化光度誤差，以更整體的方式直接計算得到位姿［18］。由于不需要進行特征提取和跟蹤，直接法可實時進行半稠密甚至稠密建圖［19］。直接法在缺乏角點、重復紋理的環境下仍可正常使用，但直接法與光流跟蹤同樣基于灰度不變假設，因此，也存在對環境光照變化敏感的問題。

1.1.2 IMU 預積分

IMU 可以獲得自身坐標系下的加速度和角速度觀測值，直接對IMU 測量值按時間進行積分可以由一個時刻的狀態量遞推得另一個時刻的狀態量。但每次狀態量更新后，均需要重新進行積分，因此，計算量較大。而使用預積分可轉換為相對積分量，表示對兩個時刻狀態量的約束，從而避免新狀態量依賴先前狀態的問題［20］。由于IMU 的采樣頻率一般比圖像頻率高，通常將兩個關鍵幀時刻之間的所有IMU 觀測積分起來。常用的積分形式包括歐拉積分、中值積分和龍格-庫塔法積分，其中中值積分計算量小且精度較高，在實際應用中經常使用。

1.2 后端

早期視覺SLAM 中使用基于濾波的后端［21］，自2007 年PTAM［22］提出后，基于非線性優化的方法成為視覺SLAM 所使用的主流方法。但在VISLAM 中，待估計狀態量維數比視覺SLAM 更多而計算量更大。因此，基于濾波的后端和基于非線性優化的后端均十分常見，且效果相當。濾波與非線性優化在貝葉斯框架下相統一，簡要推導如下。首先機器人定位模型中運動方程和觀測方程可分別表示為：

其中，xk表示k 時刻的待估計變量，即位姿和路標點等，uk為運動傳感器輸入項，即IMU 測量信息，zk表示k 時刻所有視覺觀測，wk和vk為噪聲項，通常假設滿足零均值高斯分布。后端的任務即根據過去0到k 時刻的數據估計第k 時刻的狀態分布：

其中，設定上標k 表達1∶k 所有時刻，而下標k 表達當前時刻。為描述方便，忽略初始值x0得：

利用貝葉斯準則，可得：

其中，η 等于P（zk|uk，zk-1），為與變量無關的歸一化常量。假設各觀測值相互獨立，則化簡為：

似然部分由觀測方程給定，先驗部分表明當前狀態xk基于過去狀態估計得，按照xk-1時刻為條件概率展開得：

上式表示貝葉斯估計過程，其后續處理分為兩種方法：

1）第一種方法基于馬爾可夫性假設。一階馬氏性認為，k 時刻狀態只與k-1 時刻狀態有關，而與之前無關。故上式先驗展開部分可簡化為：

上式最后一項為k-1 時刻后驗概率，因此，將k時刻后驗概率寫作遞推的形式得：

上式表示由之前時刻狀態遞推得當前時刻狀態需要不斷迭代（即預測）和更新。在VISLAM 中對應即使用IMU 進行狀態預測，并使用視覺信息進行狀態更新。假設狀態量服從高斯分布且整個系統非線性，則可得到以擴展卡爾曼濾波（Extended Kalman Filter，EKF）為代表的濾波器方法。

2）另外一種方法依然考慮k 時刻狀態與之前所有狀態的關系。為求解狀態的最優估計，需最大化后驗概率（Maximize a Posterior，MAP）。當狀態量先驗信息未知時，則轉換為求解狀態量的最大似然估計（Maximize Likelihood Estimation，MLE）。假設狀態量噪聲項滿足零均值高斯分布，則最大似然估計可轉換為非線性最小二乘問題。在VISLAM中，則有：

其中，第1 項表示視覺殘差，可以為重投影誤差，也可以為光度誤差。第2 項表示IMU 殘差約束，而P和R 分別為兩個殘差的協方差陣。為減小異常值的影響，通常還對殘差項使用核函數進行魯棒估計，常用的魯棒核函數有Huber 核、Cauchy 核和Tukey核［23］。

VISLAM 中待估計參數量較多，因此，為減少計算量，通常在非線性優化時只維護優化一個滑動窗口（Sliding Window）內的各狀態量。每當有新的幀被觀測到時，需進行窗口滑動。為避免約束信息丟失，需對滑出窗口的幀進行邊緣化操作，從而將約束信息保留為下一次求解的先驗約束［24-25］。邊緣化操作會造成填充（Fill-in）現象，使得原先相互獨立的變量產生依賴，使滑動窗口內的信息矩陣變得稠密，因此，實際應用中還需盡可能維持信息矩陣的稀疏性。例如文獻［25］提出保留臨近的非關鍵幀和關鍵幀，根據對當前幀的影響選擇直接丟棄或邊緣化滑出窗口的幀。

滑動窗口算法優化時，信息矩陣由新的測量信息和之前的測量信息兩部分構成。而這兩部分計算雅克比矩陣時線性化點不同，這會導致信息矩陣的零空間發生變化，從而在求解時引入錯誤信息，破壞整個求解系統。還將導致不可觀變量的可觀性（Observability）發生變化（如航偏角），導致誤差累積。因此，必須使用FEJ 算法（First Estimated Jacobian，FEJ），使不同殘差對同一個狀態求雅克比時，保持線性化點一致，避免求解零空間退化［26］。

1.3 回環檢測

回環檢測是一個SLAM 系統關鍵部分之一，正確檢測回環事件是得到全局一致地圖和軌跡的前提，目前常用詞袋法（Bag of Words，BoW）和基于深度學習的方法。詞袋法最先起源于文本檢索，通過統計文檔詞頻，構建直方圖，用于表示該文檔特有特征。其基本思想后來用于回環檢測，即首先對圖像提取合適特征，然后為減少字典冗余進行K-means 等算法聚類壓縮，最終訓練得詞典，并據此進行匹配檢索［27］。深度學習近年來取得巨大成功，沖擊著各個學科和領域［28］。深度學習可自主學習復雜圖像特征，從而避免傳統詞袋法需人工設計特征的缺點，研究表明基于深度學習的方法可取得同樣精度且更具普適性［29-30］。相比召回率，回環檢測對準確率要求更高。因此，實際應用中，除外觀相似性約束外，還需時間和空間上一致性結合進行驗證，以確保將假陽性情況降至最少［31-32］。

1.4 建圖

建圖即根據所估計位姿，建立與任務要求對應的地圖。稀疏路標點地圖常用于手機、機器人自身定位，計算量通常較小，但易受光照和場景影響；稠密點云地圖或八叉樹地圖可用于導航和避障，但一般需要足夠算力才能保證實時性［33］；面片地圖和TSDF 地圖（Truncated Signed Distance Function，TSDF）能比點云更精細表示三維重建結果［34-35］；而語義地圖使得與地圖間可以交互，提供更高層次語義信息［36-38］。

2 已有研究工作

視覺與IMU 信息的融合方式分松耦合和緊耦合兩種，松耦合表示將IMU 定位與視覺定位位姿直接融合得到結果，而緊耦合將視覺約束信息與IMU約束聯合解算，最終得到待求位姿。因此，根據融合方式和后端形式，VISLAM 可大致分為以下4 類。

2.1 基于濾波的松耦合方法

松耦合方法比較直接，如MSF 算法（Multi-Sensor Fusion，MSF）使用擴展卡爾曼濾波作為后端，在預測階段使用IMU 數據進行系統狀態傳遞，而將視覺前端當作黑箱進行更新［39］。由于松耦合法結構簡單，因此，具有計算量小且運算速度較快的優勢。此外這種模塊化架構使得系統可方便與其他傳感器結合，如與GPS、聲吶等。模塊化架構使得系統脫離具體模型限制，即使模型不夠準確，仍可通過其他觀測彌補。

2.2 基于濾波的緊耦合方法

Mourikis 等于2007 年提出MSCKF 算法（Multi-State Constraint Kalman Filter）［40］，該算法同樣使用擴展卡爾曼濾波作為后端，在預測階段使用IMU數據進行系統狀態傳遞，在更新階段維護一個滑動窗口。MSCKF 通過對路標點邊緣化來給共視幀添加約束，從而實現既不損失信息，又大大降低計算復雜度。

傳統EKF 法只在固定點做線性化近似，無法保證全局最優，因此，更容易造成誤差累積。在同等計算量下，非線性優化法效果一般優于濾波方法［41］。

2.3 基于優化的松耦合方法

文獻［42］提出一種基于優化的松耦合VISLAM方法［42］，融合結構如圖1 所示。首先通過直接法計算得幀間相對位姿，然后將位姿變換到IMU 坐標系下，并直接以約束項的形式加入到IMU 優化框架中，從而聯合估計得位姿。這類方法計算量大且精度不夠理想，因此，研究者把目光投向基于優化的緊耦合方案。

圖1 基于優化的松耦合方法框架

2.4 基于優化的緊耦合方法

OKVIS［25］是基于關鍵幀優化的緊耦合開源VISLAM 系統，算法具體流程是通過IMU 測量值對當前狀態做預測，根據預測進行特征提取和特征匹配，從而計算重投影殘差，同時IMU 狀態量預測值和優化參數之間構成IMU 測量誤差，最后將視覺殘差和IMU 殘差放在滑動窗口中進行聯合優化。另外，為保證系統一致性，OKVIS 采用FEJ 策略，理論上更加準確。

VIORB［43］同樣是一個著名的緊耦合VISLAM系統，其基于開源框架ORBSLAM［11］，且與ORBSLAM 同樣包含圖優化后端、回環檢測及重定位功能。與一般VISLAM 方案不同，VIORB 沒有采用經典的滑動窗口結合邊緣化的方式，而每次使用局部光束法平差進行優化求解，如圖2 所示。另外，VIORB 提出一種初始化方式，即首先只進行單目初始化，然后再對尺度、重力、速度向量、IMU 零偏進行估計［43］。

圖2 ORBSLAM 局部BA 與VIORB 局部BA 對比

Vins-mono［44］是香港科技大學團隊于2017 年開源的一個成熟的VISLAM 系統，其主要包括前端、初始化、后端優化、閉環檢測和全局位姿圖優化5個部分，如下頁圖3 所示。Vins-mono 前端使用Harris 角點光流跟蹤，而IMU 部分使用預積分得到觀測量。初始化部分與VIORB 類似，采用松耦合方式進行。即首先通過SFM 初始化，然后以此為運動參考估計其他參數，再將視覺坐標系與世界坐標系對齊。后端部分進行一個滑動窗口優化，并采用一種邊緣化策略來盡可能防止信息丟失或冗余［44］，如圖4 所示：1）當滑動窗口中第二新的圖像幀為關鍵幀，則邊緣化最老的幀，以及上面的路標點；2）當滑動窗口中第二新的圖像幀不是關鍵幀，則丟棄這一幀上的視覺測量信息，并將IMU 預積分傳給下一幀。此外，為提供全局一致地圖，Vins-mono 還加入閉環檢測、重定位等功能，使整個系統更加完整、魯棒。

3 研究熱點與發展趨勢

隨著技術的發展和越來越多開源系統的出現，VISLAM 技術正逐漸趨于成熟。然而盡管在理想條件下已經可以達到很高的精度和計算效率，如需真正投入實際應用，算法的魯棒性、實時性和準確性仍需進一步提高，硬件和算法的許多問題尚待進一步解決。

圖3 Vins-mono 系統框架圖

圖4 Vins-mono 邊緣化策略

3.1 硬件發展

VISLAM 不僅是一種算法，也是一種系統工程。其對硬件質量依賴較強，實際效果對相機IMU 外參、相機IMU 時間戳延遲、IMU 零偏等參數均十分敏感。盡管有研究已經可以通過算法減弱卷簾相機、相機IMU 時間戳未對齊等問題的影響，但VISLAM 系統精度提升仍需硬件質量的提升［45-46］。

此外，相機傳感器本身具有天然缺陷。例如圖像受光照變化影響很大，且存在運動模糊，基本無法滿足無人機、無人駕駛等高速運動下的實際應用。近年來，事件相機（Event Camera）［47］引起許多研究人員關注。事件相機僅響應每個像素亮度變化，并以一組異步事件的形式表示，其中每個事件包括亮度變化的時空坐標及其正負號，如圖5 所示。事件相機具有高動態響應范圍（140 dB，而普通相機約60 dB）和低功耗的優勢，且響應時間小于1 ms，因此，幾乎不存在運動模糊。

圖5 普通相機與事件相機對比

基于事件相機的上述優勢，蘇黎世大學研究團隊提出基于事件相機的VISLAM 系統［48］。算法首先將一段時間內事件進行運動補償，然后合成得到事件圖像［49］，如圖6 所示。前端對每一幀事件圖像進行光流跟蹤，后端使用緊耦合的方式對視覺殘差和IMU 殘差進行非線性優化。后來該團隊對此系統進行改進，提出終極SLAM（UltimateSLAM）［49］。UltimateSLAM 在之前基礎上，引入普通相機作為第3 種傳感器，將普通圖像的視覺殘差作為額外約束。從而共同發揮3 種傳感器優勢，在極具挑戰的數據集上取得了很好的效果。

圖6 合成事件圖像示意圖

3.2 后端加速

VISLAM 等多傳感器融合方案可充分發揮不同傳感器優勢，實現精確魯棒的定位與建圖，但也同時造成待估計參數維數增加。盡管如今設備計算能力不斷提高，仍需從算法上加速后端優化過程。

后段加速有許多不同的方案，其中增量優化是近年來的一個研究熱點。增量平滑和建圖（Incremental Smoothing and Mapping，iSAM）算法使用QR 分解，每次迭代只更新增量方程的一小部分［50］。在此基礎上，iSAM2 還考慮消元順序對計算量的影響，其在計算中動態維護一個貝葉斯樹，以此確定消元順序，從而減少不必要計算，加快后端求解速度［51］。

iSAM 主要針對信息矩陣稀疏情況，比如大尺度SFM 問題。而VISLAM 中經常會用到滑窗優化法，滑窗中信息矩陣結構稠密，此時iSAM 算法失效。ICE-BA 算 法（Incremental，Consistent and Efficient Bundle Adjustment，ICE-BA）［52］提出三點解決該問題：1）在全局BA 優化時，使用增量方式進行，即每次迭代時只對變化的相機或路標點進行更新［53］；2）在局部滑窗BA 優化時，采用分段BA 方式來維持滑窗內稀疏性結構；3）在滑窗邊緣化時，采用相對邊緣化方式，以保證局部BA 優化和全局BA 優化對精度貢獻一致。ICE-BA 通過采用這些技巧，保障了VISLAM 求解的實時性。

3.3 語義SLAM

隨著深度學習的廣泛應用，SLAM 與深度學習結合更加緊密。一些研究利用深度學習對圖像進行識別、檢測和分割，從而得到語義地圖［54-57］。語義建圖雖然在增強現實和三維重建中比較重要，但其對SLAM 本身促進有限。另一些研究將深度學習引入SLAM 本身的位姿估計中，但這些方法效果一般，目前還未成為主流［58-59］。還有一些研究利用深度學習檢測或分割出物體，然后使用物體實例地圖替代傳統路標點地圖，從而為SLAM 系統提供語義觀測，如圖7 所示［60-62］。這類物體級SLAM（Object SLAM）通過將圖像的高層語義信息納入定位流程中，從而能一定程度上克服大時間跨度下光照、天氣、季節、場景結構變化等影響。隨著訓練數據集的增多和深度學習技術的發展，語義SLAM 定將大大提高SLAM 系統的準確性和魯棒性。

圖7 物體實例語義地圖

4 結論

近年來，隨著移動終端、無人機、機器人的快速發展，SLAM 技術也發展迅速。VISLAM 作為成熟的多傳感器融合方案之一，取得了不錯的精度和魯棒性。盡管近年來VISLAM 在困難條件下定位與建圖取得了很大的進步，但其魯棒性、實時性和準確性仍有待提高，這勢必會與硬件提升、后端加速、深度學習等內容進行結合，從而真正滿足實際應用要求。