基于關鍵幀的視覺慣性SLAM閉環檢測算法*

張玉龍，張國山

天津大學 電氣自動化與信息工程學院，天津 300072

1 引言

即時定位與地圖構建（simultaneous localization and mapping，SLAM）[1]，指機器人在自身位置不確定的條件下，在未知環境中創建地圖，同時利用地圖進行自主定位與導航。由于傳統視覺SLAM算法在準確性和魯棒性方面存在問題，因此視覺慣性SLAM算法在計算機視覺和增強現實領域越來越多地引起人們的關注[2]。視覺慣性SLAM算法即為在傳統視覺SLAM算法中加入慣性測量單元（inertial measurement unit，IMU），算法中的相機可以提供豐富的環境信息，用于構建3D模型；IMU提供自主運動信息，可以在短時間內提供陀螺儀及加速計的估計值。通過相機信息與IMU信息的融合，可以增強算法的準確性及魯棒性[3]。

視覺慣性SLAM算法可以分為兩類：松耦合（loosely-coupled）[4-6]和緊耦合（tightly-coupled）[7-12]。其中松耦合系統為先通過視覺算法估計位姿，將得到的位姿與IMU數據進行融合，這種融合方法可以減小算法的計算復雜度，但是算法的準確度較低；緊耦合系統為相機和IMU測量數據直接進行位姿估計。由于算法的準確性較高，緊耦合算法逐漸成為視覺慣性SLAM算法研究中的一個熱門方向。

緊耦合算法又可以分為兩類：基于濾波和基于關鍵幀。2007年Mourikis和Roumeliotis[7]提出了一種基于擴展卡爾曼濾波（extended Kalman filter，EKF）的實時單目視覺融合算法，被稱為多狀態約束卡爾曼濾波器（multi-state constraint Kalman filter，MSCKF），提出按照時間順序維護一個位姿的滑動窗口，如果特征點在滑動窗口的幾個位姿都可被觀測，就會在這幾個位姿間建立約束，從而進行EKF的更新。2015年Bl?sch等人[8]提出了基于直接EKF的方法，通過興趣點測量值直接使用EKF來估計相機位姿和IMU偏差。

2010年Strasdat等人[13]提出了在使用相同計算資源的情況下，基于優化的算法比基于濾波的算法可以得到更準確的效果，使得基于優化的算法逐漸引起人們的關注。2013年Leutenegger等人[9]驗證了在使用稀疏關鍵幀的情況下，基于非線性優化的視覺SLAM算法比基于濾波的算法更為準確。2015年Leutenegger等人[10]提出了一種基于關鍵幀的算法，在局部優化中優化相機位姿和IMU誤差，跟蹤和重建圖像中興趣點（interest-point）的3D位置。2016年Usenko等人[11]提出了一種類似于非線性優化的方法，通過在局部優化中使用直接圖像對齊而不是間接使用興趣點測量值來得到全局的連續地圖。2017年Forster等人[12]提出了一種基于預積分（preintegration）理論的視覺慣性里程計，將預積分理論應用于SO(3)模型中，將IMU測量數據與圖像中稀疏子集的直接跟蹤相結合，從而加速了優化計算。

基于緊耦合方法的視覺慣性SLAM算法大多都不具備檢測閉環和地圖重利用的能力，即使相機不斷重新訪問相同的位置，得到的軌跡也會出現累積漂移現象。2011年Jones等人[14]提出的基于濾波的視覺慣性SLAM算法可以檢測閉環，但是不能實時地建立全局地圖。2017年Mur-Artal等人[3]提出的基于關鍵幀非線性優化的方法可以檢測閉環，但是由于算法在跟蹤和局部優化過程中會固定狀態量，因此對初值的要求很高，需要一個復雜繁瑣的初始化過程。

視覺SLAM算法另一重要環節為視覺定位，即為在離線環境下構建地圖，然后在已構建地圖內進行定位的算法[15-17]。Mur-Artal等人[17]在2014年首次提出了基于關鍵幀技術的SLAM算法的重定位，可以以圖像幀速率處理包含數千個關鍵幀的地圖的重定位問題。Lynen等人[16]在2015年提出一種在移動設備上運行的視覺慣性定位算法，使用地圖壓縮和圖像匹配技術在預先建立的基于興趣點的地圖上來定位相機。但是，這些算法僅實現了設備的重定位，并不具有繼續建圖的能力。

本文算法在邊緣化圖像幀之后，通過非線性優化技術和閉環檢測技術來得到并行的全局連續軌跡，從而可以有效地檢測閉環和全局優化，建立連續的全局地圖，此外算法可以在已獲得的地圖進行重定位和具有繼續建圖的能力。

本文算法分為五部分：（1）提取特征點進行特征點匹配，運用RANSAC[18]（random sample consensus）算法排除誤匹配得到位姿，并通過匹配特征點所占的圖像區域與所有檢測到特征點的圖像區域的比率選取關鍵幀。（2）進行局部優化，最小化圖像位姿信息的二次投影誤差和IMU誤差，得到局部連續軌跡估計。（3）邊緣化圖像幀和關鍵幀，保持局部優化計算量在一個恒定的范圍。（4）在全局地圖中通過圖像檢索策略找到與當前關鍵幀匹配的關鍵幀，從而形成閉環，然后利用RANSAC算法驗證匹配并得到位姿，最后進行全局優化得到較準確的地圖。（5）重定位和繼續建圖。使用和閉環檢測類似的圖像檢索算法找到一系列連續的匹配關鍵幀對作為重定位假設，并根據新的關鍵幀與已構建的地圖關鍵幀之間的約束關系繼續進行SLAM地圖的構建。算法框圖如圖1所示。

2 狀態參數表示及IMU動力學

本文提到了三種坐標系，世界坐標系w，相機坐標系c和IMU坐標系s。在這里設位置為pws，姿態四元數為qws，速度為vws，陀螺儀誤差為bg，加速計誤差為ba，則狀態量x表示為：

Shin和El-Sheimy[19]在2004年提出了IMU的動力學和偏差的表示方法，本文是對其表示方法的改進，假設由地球自轉引起的偏差對陀螺儀測量數據的影響忽略不計，則IMU動力學方程與動態誤差表示如下：

設狀態量x的誤差向量為δx，動態誤差方程表達式為：

Fig.1 Block diagram of algorithm圖1 算法程序框圖

其中，G可以直接得到，Fc表示如下：

其中，[?]×表示向量的叉乘反對稱矩陣。

Fig.2 Graphs of state variables and measurements圖2 狀態量和測量值的圖表示

3 視覺慣性SLAM算法

3.1 特征點匹配與關鍵幀的選擇

本文視覺慣性SLAM算法采用的是ORB[20]特征。ORB特征可以自主確定描述子方向，但是通過提取沿著重力方向定向的描述子可以得到更好的匹配結果[9]，而本文算法由于融合了IMU的信息，重力方向可以直接得到。

通過當前IMU測量值和上一幀圖像幀的狀態預測當前幀的狀態，根據預測的當前幀位姿篩選局部地圖中當前幀可見的特征點，這些可見特征點與當前幀的特征點進行暴力匹配（brute-force matching）[20]。對于當前幀的每個特征點，計算和當前幀下所有可見的特征點之間的距離，本文在最近鄰點和第二近鄰點引入一個比率，因為第二近鄰點的距離肯定大于最近鄰點，因此一個適當的閾值可以有效地控制正確與錯誤匹配之間的比例。隨后排除誤匹配，排除誤匹配分為兩步：首先，根據位姿預測計算馬氏距離，選取合適的閾值判斷匹配是否有效；然后根據RANSAC算法排除誤匹配，從而得到相機位姿。

為了接下來的優化處理過程，選取某一時刻拍攝的圖像幀，這些圖像幀共享的特征點保存到局部地圖里，把這些圖像分為兩類：把當前S幀圖像作為臨時優化窗口，S幀之前選取N幀關鍵幀。關鍵幀的選取采用較簡單的方法，即如果匹配特征點所占的圖像區域與所有檢測到特征點的圖像區域的比率低于50%，就稱其為關鍵幀。

3.2 跟蹤與局部優化

在視覺SLAM算法中，最小化二次投影誤差得到相機位姿和特征點的坐標。圖2是狀態量和測量值的圖表示。本文引入IMU數據，不僅在連續位姿之間產生約束，而且對加速計和陀螺儀的速度和偏差產生影響。對于視覺慣性SLAM，最小化二次投影誤差的同時加入IMU誤差項得到的誤差函數E(x)[21]表示如下：

其中，i為立體相機檢索號；k為相機圖像幀；j為圖像中的特征點；J(i,k)表示相機i中圖像k的特征點；Wri,j,k表示特征點的信息矩陣；Wsk表示第k幀圖像的IMU誤差信息矩陣。

Furgale[22]于2011年提出了基于星體運動的投影誤差方程，本文將其應用到視覺SLAM算法之中，表示如下：

其中，hi(?)表示相機投影模型；為相機坐標的測量值。

對于在tk和tk+1兩個時刻的連續幀狀態xk和，是通過IMU的測量數據zks聯系的，IMU誤差表示如下：

3.3 邊緣化

隨著時間的推移，特征點和相機位姿越來越多，優化的計算量隨著變量的增加而增加，圖像的邊緣化就是為了限制計算量的增加，保持優化計算量在一個恒定的范圍。本文中在圖優化的同時進行圖像幀的邊緣化處理。SLAM算法中的圖優化[23]為最小二乘問題，最小二乘問題可以通過高斯牛頓迭代求解，表示如下：

其中，H為信息矩陣。設xμ為需要邊緣化的狀態量，xλ為需要保留的狀態量，則上述方程可以轉化為：

由舒爾補碼[24]（Schur complement）得到：

將邊緣化引入到基于視覺慣性的SLAM算法中，最初的邊緣化誤差項是由S個圖像幀組成的xTk，k=1,2,…,S，S幀圖像臨時優化窗口之前為N幀關鍵幀。當一個新的圖像幀xTc插入到臨時優化窗口中時，算法進行邊緣化環節，如果臨時優化窗口中最后一個圖像幀(xTc-s)為非關鍵幀，則拋棄其所有特征點測量數據并將其邊緣化；如果xTc-s為關鍵幀，直接丟棄所有特征點的測量信息會造成不利影響，導致基于兩個關鍵幀之間的相關位姿信息丟失。因此，在這種情況下，僅邊緣化N幀關鍵幀中最后關鍵幀可見而其他關鍵幀中不可見的特征點信息，而屬于不需要邊緣化的關鍵幀的特征點信息則保留。

3.4 閉環檢測

上述部分即為視覺慣性里程計，雖然可以得到地圖估計，但是由于邊緣化和局部優化，算法無法檢測閉環和地圖重利用，系統不可避免地出現誤差累積和漂移現象。采用添加并行的全局地圖來達到檢測閉環和減少漂移的目的。并行的全局地圖為Essential Graph[25]形式，Essential Graph即為關鍵幀位姿以及關鍵幀之間的轉換關系組成的無向圖。

在本文中，算法實時跟蹤相機訪問的位置，并使用圖像檢索技術檢測是否重新訪問相同的位置。當關鍵幀從視覺慣性SLAM局部優化地圖中離開時，在全局地圖中與之前臨近關鍵幀通過已估計過的位姿進行連接，通過DBOW2[26]檢索策略在全局地圖中檢索與其形成匹配的關鍵幀，然后通過2D-to-3D RANSAC[27]算法估計和驗證關鍵幀之間正確的位姿關系，并增加空間約束，最后通過圖優化來優化處理，以此來檢測閉環和減少累計漂移。下面詳細敘述閉環檢測和圖優化。

（1）閉環檢測

本文閉環檢測分為兩步：首先，當關鍵幀從局部優化地圖中邊緣化后作為查詢關鍵幀進入全局地圖，從全局地圖中的關鍵幀檢索與查詢關鍵幀相匹配的關鍵幀，這種檢索匹配是通過DBOW2完成的，形成匹配之后，在全局地圖中增加一條邊的約束；然后在相互匹配的關鍵幀之間通過2D-to-3D匹配運用RANSAC算法來得到匹配關鍵幀之間的轉換位姿，這也可以從空間角度驗證由DBOW2得到的匹配關鍵幀是否正確。

（2）圖優化

圖優化主要優化關鍵幀之間的兩種空間約束：由局部優化得到的相鄰關鍵幀之間的位姿轉換和由閉環檢測找到的全局地圖中相匹配的關鍵幀之間的位姿轉換。其中后者是由RANSAC算法2D-to-3D匹配得到的，由此可以通過優化以下的目標函數來得到狀態量：

其中，式（13）右端包含兩個誤差項，第一項為視覺慣性里程計殘差，是由視覺慣性里程計各個關鍵幀之間的相對位姿得到的；第二項為閉環殘差，是由閉環檢測各關鍵幀之間的閉環約束得到的。ξk,k+1為視覺慣性里程計得到的第k和第k+1關鍵幀之間的相對轉換位姿，ξk,k′為閉環檢測里k和k′幀之間的相對轉換位姿。ek,k+1和ek,k′表示如下:

其中，L為閉環檢測關鍵幀的匹配，即為形成的閉環約束。ek,k+1和ek,k′分別為位姿跟蹤和閉環檢測得到的位姿的殘差。運算符號⊕表示如下：

其中，ξ(T)和T(ξ)為位姿參數ξ和T之間的轉換函數，T∈SE(3)，SE(3)為三維歐氏群，物理意義為位姿。

信息矩陣Wk,k+1和Wk,k′為位姿空間約束的逆協方差矩陣。這個非線性最小二乘問題通過萊文貝格-馬夸特（Levenberg-Marquardt，LM）算法[28]進行優化求解。

通過并行全局地圖構建，可以得到形成閉環的關鍵幀，進而可以得到地圖中匹配關鍵幀之間的位姿。全局地圖中得到的位姿還可以優化和改善局部地圖中得到的位姿。同時，通過全局優化，可以減小全局誤差，從而得到更加完善的地圖。

3.5 重定位

重定位，即為機器人在自身位置未知的情況下在已有的地圖上進行定位的算法。在視覺SLAM中，因為遮擋、非紋理區域或相機快速移動所造成的跟蹤失敗非常常見，重定位可以在跟蹤失敗后進行位姿恢復。在機器人和增強現實等實際的領域內，SLAM系統要允許在之前構建的地圖中進行重定位，并且可以無縫地在新增加的環境部分中繼續構建地圖。重定位和繼續SLAM算法的難點在于檢測重定位和新地圖與已構建的地圖的結合，從而實現全局地圖框架內的位姿跟蹤，見圖3，藍色為已構建的地圖，紅色為新的SLAM地圖，綠色點和線段表示兩地圖之間關鍵幀的對應。

本文在新的軌跡ξnew和已構建軌跡ξprev之間找到重疊部分，對于新的SLAM地圖中的每一個新的關鍵幀，通過上一部分提到的閉環檢測算法檢測并驗證與已構建SLAM地圖中關鍵幀的對齊情況。建立一個閉環假設集H=(h1,h2,…,hn)，其中h表示新舊地圖關鍵幀之間的聯系。

Fig.3 Relocalization stages in previous SLAM keyframe map圖3 在已構建地圖中的重定位階段

如果新的SLAM地圖中的關鍵幀knew和之前構建的地圖中的關鍵幀kprev形成匹配，則在假設集里添加一個新的假設h=(knew,kprev,ξkprev,knew)。如果關鍵幀對(knew,kprev) 和 (k′new,k′prev) 具有相同的時間序列，即||k′new-knew||=1并且k′new-knew=k′prev-kprev，通過將匹配對組合在單個假設集合內來合并假設。

一旦某一假設擁有足夠數目的連續匹配對（本文選取4對），根據最近的匹配關鍵幀定義位姿轉換，通過將新的SLAM地圖中的所有關鍵幀與之前構建的地圖中的圖像幀建立位姿轉換關系，從而把新的SLAM地圖和已構建的SLAM地圖結合起來。優化的目標函數如下所示：

其中，Es(ξprev)為已構建地圖關鍵幀之間的誤差項；Es(ξnew)為新的地圖關鍵幀之間的誤差項；最后一項為新舊地圖之間約束的誤差項。ξ為已建地圖關鍵幀和新的地圖關鍵幀之間的位姿轉換，Cmaps為已建地圖和新的地圖之間關鍵幀的匹配，這種新舊關鍵幀之間的匹配約束類似于閉環機制，可以提供軌跡之間的約束，從而改善新舊軌跡之間的對齊情況，減小新構建地圖的誤差。

4 實驗

為了驗證本文算法的效果，選取了公用數據集EuRoC[29]。EuRoC 數據集包含“Machine Hall 01”“Vicon Room 1 01”“Vicon Room 1 02”等11個數據集序列，是由微型飛行器拍攝的視覺慣性數據集序列。該數據集在制作時包含一個視覺慣性傳感器單元，可以實時記錄陀螺儀和加速計數據，其中立體相機頻率為20 Hz，慣性傳感器頻率為200 Hz。數據集中還包括相機內參、外參以及實際運動軌跡等數據。實驗中運行算法的處理器為Intel 4核處理器i7-3700 3.4 GHz。

4.1 視覺慣性SLAM算法

本文首先在“Machine Hall 01”數據集序列上與Okvis[10]算法進行比較，數據集圖片大小為752×480，ORB特征提取數目設置為1 000個，DBOW2詞袋樹模型為6層。為了便于觀察，驗證實驗結果，采用Python工具將軌跡投影到x-y平面后進行對比，如圖4（a）（b）所示，與Okvis算法相比，本文的算法與真實軌跡更加切合，特別是在形成閉環的軌跡處，可以通過檢測閉環，增加閉環約束以減小誤差。此外全局地圖的存在使得算法可以進行全局優化，從而可以從全局方面減小算法的誤差。使算法軌跡與真實軌跡更加吻合，得到更好的定位效果。誤差統計如表1所示，可以看出，本文算法較Okvis算法軌跡的各項誤差數據均有所降低。實驗表明本文算法通過閉環檢測和全局優化可以減小誤差累積和漂移。本文算法可以得到通過稀疏點構建的場景地圖，圖5為“Vicon Room 1 02”序列的地圖估計，圖中黑色點為正確匹配的特征點，紅色點為誤匹配特征點，藍色為相機位姿，綠色線段為關鍵幀之間的連接。

Fig.4 Trajectory comparison圖4 軌跡對比圖

Table 1 Comparison of trajectory errors表1 軌跡誤差對比圖 m

Fig.5 Estimated map圖5 地圖估計

為了觀察誤差隨軌跡變化的趨勢，本文在“Vicon Room 1 01”“Vicon Room 1 02”這兩個數據集序列上分別驗證本文算法與Okvis[10]算法以及VIORB[3]算法的效果。如圖6、圖7所示，其中Translation error為算法平移誤差，即為算法估計值與真實軌跡在平移量的差值，Orientation error為方向誤差，為在旋轉量之間的差值。通過對比可以看出本文算法由于采用了閉環檢測和全局優化，使得軌跡的誤差較小，并且由于閉環約束的存在，使得誤差不會隨著路徑的增加而明顯增加，從而達到減少誤差累積和漂移的目的。

Fig.6 Vicon Room 1 01 dataset圖6 Vicon Room 1 01數據集

4.2 重定位

本節檢測算法的重定位能力。圖8中的直方圖顯示的是重定位所需要的關鍵幀的數目，這是在單一假設所含連續匹配對數為4時的實驗結果。可以看出，當使用長度為4的匹配對集合作為重定位假設時，算法從初始化開始可以在8個關鍵幀之內完成重定位。此外，根據重定位結果與真實軌跡的比較，可以得到重定位的定位誤差，圖9顯示在大約20幀圖像之后，誤差變得較小且比較穩定，即為找到了比較精確的重定位。重定位之后，本文的SLAM算法包括新軌跡和已構建的軌跡之間的閉環，從而可以改善SLAM地圖的準確性。

Fig.7 Vicon Room 1 02 dataset圖7 Vicon Room 1 02數據集

Fig.8 Number of keyframes needed to be relocalized within same sequence圖8 同一序列重定位所需的關鍵幀

Fig.9 Progression of error after relocalization圖9 重定位后誤差的變化

本文選取了不同的“Vicon Room”序列，驗證了算法。首先運行一個數據集序列得到SLAM地圖，然后在已得到的地圖基礎上再運行另一序列進行重定位與繼續建圖，由此根據聯合序列“Vicon Room”得到算法的誤差。如表2和表3顯示，由聯合序列得到的誤差和由單一序列得到的誤差非常接近，后兩組實驗誤差還有所改善。

Table 2 Error of algorithm in different sequences表2 不同序列下算法的誤差

Table 3 Error of algorithm in one sequence表3 單一序列算法的誤差

4.3 運行時間

本文算法實現了數據集上相機的實時跟蹤，表4顯示的是算法在EuRoC數據集的平均運行時間，算法運行時間包含里程計運行時間、閉環檢測時間以及圖優化時間。可以看出，盡管SLAM算法的并行處理模塊閉環檢測和圖優化以較慢的速度更新，但是視覺里程計可以以13 ms左右的時間處理圖像幀，而數據集相機的采集頻率為20 Hz，因此視覺里程計可以隨著圖像幀的更新實時地處理圖像，從而使得算法可以實現數據集上相機的實時跟蹤。

Table 4 Run-time of proposed method表4 算法運行時間

5 結束語

針對目前視覺慣性SLAM算法因缺少閉環檢測，導致算法累積誤差和漂移比較嚴重的問題，本文提出了一種基于關鍵幀技術的視覺慣性SLAM算法。在邊緣化圖像幀和關鍵幀之后引入了閉環檢測和非線性優化，建立了一個并行的全局地圖，算法對局部連續軌跡和全局連續軌跡并行進行估計，并可在已構建地圖進行重定位和在保持地圖連續性的情況下繼續建圖。實驗表明，本文提出的方法可以使得算法在減小誤差和漂移方面取得很大的改善，并可以重定位相機位置和繼續構建未知環境地圖。