基于IMU預積分封閉解的單目視覺慣性里程計算法

徐曉蘇，吳 賢

（1.微慣性儀表與先進導航技術教育部重點實驗室,南京 210096；2.東南大學儀器科學與工程學院,南京 210096）

使用低成本的傳感器在移動設備和機器人上實現高精度的3D 導航，在增強現實和自動駕駛等實際應用中具有巨大意義。為此，利用慣性導航定位方案實現室內三維場景定位，該方案利用慣性測量單元(IMU)測量傳感器平臺三軸角速度和線性加速度，并將其剛性附著在傳感器平臺上。通常，IMU 工作頻率很高(如100～1000 Hz)，使它能夠測量高速運動。然而，由于傳感器噪聲和偏置的影響，單純使用IMU 測量很容易導致失敗的運動估計。因此，研究人員開發了視覺慣性組合導航定位系統（VINS）[1]，該系統融合至少一個攝像機的輔助信息來限制累積的慣性導航漂移，從而實現高精度的運動估計。再過去的十幾年中，視覺慣性導航系統取得了巨大的進步，研究人員開發出了許多應對不同環境和功能的視覺慣性融合算法[2-4]。

融合相機和IMU 數據進行運動估計的算法統稱為視覺慣性里程計（VIO），根據其實現途徑，可以分為基于濾波器的VIO[5]和基于優化的VIO[6]。傳統上，基于濾波的算法主要是通過擴展卡爾曼濾波器(EKF)來實現，其中IMU 測量載體的運動數據和相機測量的地圖特征點分別被用來運動傳播和更新狀態估計。然而這些濾波器方法不更新被邊緣化的歷史狀態，從而導致它們容易出現漂移。文獻[7]提出了一種基于雙目多狀態約束下的卡爾曼濾波器（MSCKF）并結合Lucas-Kanade（LK）光流法的視覺慣性里程計算法，該算法能夠在相機抖動和紋理缺失等情況下的精準位姿估計。相比之下，基于優化的算法同時處理在一個軌跡上的所有測量，估計平滑的載體狀態，這些方法由于能夠實現非線性測量方程并修正歷史的狀態估計，從而能夠獲得較高的運動估計精度。然而基于優化的方法需要處理所有的IMU 高頻數據，具有較大的計算復雜度；相比之下基于擴展卡爾曼濾波的方式能夠有效地處理IMU 數據并更新，速度快并且能夠實時運行。

基于濾波的視覺慣性里程計算法主要是基于緊耦合的方式，即使用相機和IMU 傳感器的數據共同進行運動估計[8]。該類型算法中的EKF-SLAM[9]將IMU 狀態和相機觀測到的地圖點共同放入系統狀態量，從而實現運動估計，但是由于狀態向量維度非常大，使得運算效率很低并且精度不高。研究人員對EKF-SLAM算法進行了一系列的改進，包括ACK-MSCKF[10]、MSCKF[1]、Trifo-VIO[11]等算法。MSCKF 算法同樣以EKF 濾波器為框架，采用一個滑動窗口來維護多個時刻的相機位姿，而不是三維地圖點，從而有效解決了EKF-SLAM 系統狀態向量維度過大的缺點，并且能實現較高的精度和實時性，但是MSCKF 對IMU 數據處理采用龍格庫塔數值積分法，通過多次迭代計算出兩幀圖像間的IMU 速度和位置量，這種算法導致較大的計算量，加重了CPU 的計算負擔，同時MSCKF 算法觀測方程中的特征點逆深度化方法存在當z 軸方向趨近于零時會出現奇點的情況，使得這種算法存在一定的缺陷。文獻[12]實現了雙目版本的MSCKF 算法，并且將IMU 和相機之間的標定外參數加入到系統狀態量中，實現在線標定，有效增加了系統的可觀測性和一致性，并且實現了小型飛行器的定位功能。文獻[13]對MSCKF 進行改進，將主參考系設置成機器人本體坐標系，而不是以世界坐標系為參考系，并且對IMU數據處理采用IMU 預積分算法，有效增加了系統的計算效率和定位精度。文獻[14]提出了一種基于優化框架下的VIO 算法，同時采用IMU 預積分方法處理IMU數據，然而這種IMU 預計分算法通過離散的解來簡化所需的預積分數值，而不計算出積分方程的閉式解。

上述MSCKF 框架下算法在處理相鄰圖像幀之間的IMU 數據時通常使用數值積分，從而遞推得到下一個圖像幀到來時的IMU 狀態量初始值估計，這種數值積分的方法增加了系統的運算量，針對此問題，本文對MSCKF 中IMU 處理算法進行改進，提出了一種基于閉式解的IMU 預積分方法來替代數值積分。同時針對MSCKF 算法觀測方程中的特征點逆深度化方法存在當z 軸方向趨近于零時會出現奇點的情況，本文提出了一種新的逆深度參數化方法，該方法提升了系統的魯棒性和數值穩定性。通過EuRoc 數據集實驗表明，本文的改進方法能提高MSCKF 框架下基于擴展卡爾曼濾波器的視覺慣性里程計算法精度。

1 基于封閉解的IMU 預積分算法

IMU預積分技術被廣泛地用于視覺慣性融合算法中，傳統的基于優化框架的VIO 算法通常在分段常數加速近似下采用離散解來簡化所需的預積分，使得VIO 系統定位結果不高，為了提升VIO 系統的定位精度，本文提出一種基于封閉解的IMU 預積分方法，并應用于MSCKF 框架下基于擴展卡爾曼濾波器的VIO系統。

IMU 通常測量載體的角速度w和加速度a，其測量值表示為:

式中，wm和am分別表示IMU 陀螺儀和加速度計的測量值；w和a分別表示陀螺儀和加速度計的真實值；表示世界坐標系G到IMU 系I的旋轉矩陣；b g和ba分別為陀螺儀和加速度計的隨機游走偏置；n g和na分別表示陀螺儀和加速度計的零均值高斯白噪聲。Gg表示重力加速度矢量。

類似于MSCKF 算法，k時刻系統IMU 狀態向量定義為XI：

式中，GIq表示世界坐標系G到IMU 系I的單位旋轉四元數；Gpk和Gvk表示k時刻IMU 在世界坐標系G中的位置和速度。

圖1 在圖像幀時刻到之間進行IMU 采樣，采樣間隔為ΔtFig.1 IMU sampling between image frame time tkandtk+ 1,sampling interval is Δt

由文獻[15]，第k+1刻的IMU 位置、速度和旋轉矩陣可以由第k時刻遞推得到，方程如下：

式中，ΔT=tk+1-tk，表示相鄰圖像幀時間間隔，如圖1所示；式(3)預積分項記為kαk+1，kβk+1，且：

連續時間旋轉四元數方程為：

對式(5)進行零階四元數積分求解出不同時刻的旋轉四元數[16]?；谑?4)的定義，預積分項動態方程如下：

由式(6)動態方程，得到預積分估計值的線性系統方程如下，

對式(7)積分，得到預積分項閉式解：

式中，Δt表示IMU采樣間隔，Δt=tτ+1-tτ，表示加速度估計值，且。根據式(5)(8)得到相鄰圖像幀時刻的IMU 預積分項，再代入式(3)中得k+1時刻的位置、速度和旋轉值。

2 基于封閉解預積分的IMU 模型傳播

本文提出的基于擴展卡爾曼濾波器的視覺慣性里程計融合定位方法，其系統狀態向量包括兩個部分，分別為IMU 狀態向量和基于滑動窗口的N 個歷史相機位姿，系統狀態向量為：

式中，XI表示k時刻IMU 的系統狀態；XC表示N個歷史相機位姿。

2.1 連續時間IMU 狀態誤差模型

類似于MSCKF 算法，連續時間系統中，IMU 運動模型為：根據式(1)(9)(10)，IMU 觀測向量的傳遞模型可以表示為：

定義四元數誤差向量為，則：

式中，表示旋轉誤差項；表示位置誤差項；表示速度誤差項；和分別表示陀螺儀偏置誤差項和加速度偏置誤差項。此時，系統誤差表達式可以定義為：

式中，表示相機狀態誤差，可以定義為：

根據式(10)～(15)，連續時間IMU狀態誤差方程為：

2.2 離散時間IMU 狀態增廣

傳統的MSCKF 算法對離散時間IMU 狀態增廣，對式(10)采用數值積分法，使得每次積分都得引入當前位置到世界坐標系的旋轉量，如圖1所示，當對tk時刻相機位姿求導時，將會對圖像幀之間所有IMU 狀態求導，這將帶來極大的計算量，因此本文采用1 節中論述的封閉解的IMU 預積分算法，替代數值積分法。

對離散時間IMU 狀態估計值增廣，從時間tk到tτ+1，通過零階四元數積分得到旋轉四元數；通過1 節中論述的IMU 預積分算法得到tτ時刻的IMU 位置和速度，方程如下：

式中，Δp和Δv表示預積分項，如第1 節所示，通過當前IMU 測量值循環遞歸得到預積分項；且假設在［tτ,tτ+1］時間內，IMU 偏置的估計值保持不變，即：

2.3 離散時間系統協方差傳播

離散式(16)系統離散時間狀態轉移矩陣和噪聲協方差矩陣方程如下所示：

式中，Q=E[nI nIT]，表示連續時間系統噪聲協方差矩陣；系統IMU 協方差矩陣傳播方程如下：

因此整個系統的協方差矩陣表達式定義為：

式中，PIIk|k為15× 15的IMU狀態協方差矩陣；PCCk|k為6N×6N的相機狀態協方差矩陣；PICk|k為IMU和相機相關矩陣。系統協方差矩陣傳播方程為：

當系統接收到新的圖像幀時，將其添加到系統狀態向量中，此時相機的狀態可以由IMU 狀態和相機與IMU 之間的外參數計算得到，如式(24)：

式中，C（·）表示單位四元數到旋轉矩陣的轉換；表示IMU 和相機之間的旋轉四元數，表示IMU 坐標系下相機的位置，可以由線下標定獲得。此時系統的協方差矩陣增廣形式如下式：

式中，J為相機狀態方程對應的雅克比矩陣。

3 逆深度參數化觀測模型

假設單個空間地圖特征點fj，能夠在連續的Mj幀中被單目相機觀測到，相機的位姿向量分別為；則每個相機對此地圖點的觀測方程可以定義為：

式中，nj,i表示相機觀測噪聲；Cipj表示地圖點fj在相機Ci中的位置，其方程如下：

式中，Gpj表示地圖點fj在世界坐標系中的位置坐標。

傳統的MSCKF 算法使用式(27)作為相機的觀測方程，當z 軸方向趨近于零時使得觀測會出現奇點的情況影響系統的數值穩定性，本文對Gpj采用逆深度參數化方法，將有效避免MSCKF 算法的缺陷。其方程為：

式中，λ=［θφ ρ］T；

系統的觀測殘差方程為：

式(30)線性化可得：

式中，HXj,i表示殘差相對于系統誤差的雅克比矩陣；表示逆深度參數λ的誤差，可由測量值通過最小二乘法解出[17]；Hλj,i表示殘差相對于參數的雅可比矩陣。

將式(31)擴展到對該地圖點的所有的相機觀測，即可得到該地圖點的觀測殘差方程為：

式中，HXj和Hλj為擴展后的協方差矩陣；nj為擴展后的噪聲協方差矩陣；HXj、Hλj和nj都是由HXj,i、Hλj,i和nj,i組成的塊矩陣或向量；由于在不同的相機觀測中地圖點特征是相互獨立的，因此相機觀測噪聲向量nj的協方差矩陣可以定義為Rj=σ2im I2Mj。為了使觀測方程能執行觀測更新，將式(32)左乘Hλj的左零空間矩陣VT，定義殘差，則：

式中，VT為Hλj的左零空間矩陣，且：

由于矩陣Hλj的秩為2Mj× 3，因此其左零空間矩陣VT為秩2Mj- 3的酉矩陣，且觀測噪聲nj的協方差矩陣為：

式(33)定義了對Mj個相機觀測到的單個地圖特征點的線性約束，充分利用了Mj個相機狀態的觀測信息，并且可以直接用于EKF 觀測更新。

4 EKF 更新

假設時間k+1時，相機觀測到M個地圖特征點，則此時整個狀態估計系統的觀測殘差可以由(j=1……M)擴增得到，定義系統殘差為，則殘差方程為：

式中，、和分別是由、和組成的塊向量或者矩陣；系統殘差r的維度為：

的噪聲協方差矩陣如式(38)：

在實際應用中，即使M很小，其對應的殘差維度d依然會是一個很大的值，這將導致非常大的系統計算量，為了減少計算量，讓算法能夠實時運行，在EKF更新之前對式(36)進行QR 分解；注意到為非滿秩矩陣，對其QR 分解如下：

式中，Q1矩陣和Q2矩陣為形式唯一的酉矩陣，TH為上三角矩陣，將式(39)代入式(36)中，得：

式中，由于項只包含測量噪聲，可以忽略。所以式(40)可以簡化為：

式中，r為矩陣Q1的列數。此時，系統卡爾曼增益矩陣為：

狀態方程修正量為：

協方差矩陣更新方程為：

5 試驗與結果分析

采用公開的EuRoc 數據集對本文提出的算法（后文記作proposed）進行性能分析。EuRoc 數據集包括從微型飛行器上收集到的11 個飛行序列數據。該數據集圖像采集頻率為20 Hz，IMU 采集頻率為200 Hz，并且數據提供真實軌跡（后文記作Groundtruth）。本文試驗使用內存為8 GB 的英特爾Core i5-8300H@2.30 GHz 四核筆記本電腦，將本文算法與MSCKF[1]和RVIO[13]兩種算法比較。

為了驗證本文提出的逆深度參數化方法相對于MSCKF 算法的穩定性，本文以EuRoc 數據集中的Vicon Room 中V2_01_easy 序列為參考標準，設計一組對比試驗。試驗中只改變MSCKF 算法更新步驟中的觀測方程，分別采用標準MSCKF 算法和基于本文提出的逆深度參數化的MSCKF 算法，試驗結果如圖2所示。從圖示的軌跡曲線可知，標準MSCKF 算法和逆深度參數化的MSCKF 算法整體上較為接近，但是在某些序列段中，逆深度參數化的MSCKF 算法跟真實的軌跡更為接近。通過查看數據集軌跡序列可知，該部分檢測到的特征點Z 軸分量接近于零，使用逆深度參數化的MSCKF 方法具有更高的定位精度。

圖2 V2_01_easy 序列軌跡Fig.2 V2_01_easy sequence trajectory

為了進一步分析逆深度參數化方法的性能，圖3展示了試驗軌跡在XYZ 三軸上的軌跡分量，由圖3可知，標準的MSCKF 算法和逆深度參數化的MSCKF算法在X 軸和Y 軸上的軌跡較為一致，但是Z 軸方向上，逆深度參數化的MSCKF 算法跟真實參考值更為接近，性能提升較為明顯。

為了驗證本文提出的兩點改進算法的有效性，將本文算法與MSCKF[1]和RVIO[13]兩種算法比較。采用EuRoc 數據集中的Vicon Room 中的6 個飛行序列，并圖示了V1_01_easy和V2_02_medium兩個數據序列軌跡，分別如圖4和圖5所示。

圖3 X、Y、Z 三個方向運動軌跡與實際軌跡Fig.3 X、Y、Z movement trajectories and actual trajectories

由3 圖中軌跡可知，MSCKF 算法軌跡與真實值軌跡偏離較大；RVIO 算法和本文提出的算法與真實值軌跡基本重合，但本文提出的算法軌跡更連續穩定，且偏離更小。

圖4 V1_01_easy 序列軌跡Fig.4 V1_01_easy sequence trajectory

圖5 V2_02_medium 序列軌跡Fig.5 V2_02_medium sequence trajectory

為了進一步分析算法的性能特性，本文以“V2_02_medium”數據集為例，對定位誤差進行分析?！癡2_02_medium”數據集是難度系數適中的數據集序列，收集數據的場景為一個房間，該序列運動速度適中，場景中有豐富的視覺特征，光度條件較好，可以保證準確地特征跟蹤。圖6為本文算法在X、Y、Z三個方向上軌跡漂移誤差曲線，圖7為本文算法中載體的俯仰、滾動和航向三個姿態角的誤差曲線，圖6為三個算法在X、Y、Z 三個方向上的運動軌跡與實際軌跡對比曲線。從圖6和圖7可以看出本文算法的位置漂移不超過0.15 m；俯仰角和滾動角漂移幅度較小，航向角誤差相對較大。從圖8可以看出MSCKF 算法在X、Y、Z 方向的漂移程度較大，而RVIO 算法和本文算法漂移程度較小，基本與真實軌跡重合。

為定量地分析本文算法的定位精度，表1對數據集6 個飛行序列上的估計結果的均方根誤差（RMSE）進行了統計計算。表1將均方根誤差指標最小值用加黑體字進行了標注。由標注結果可示，相對于其他兩種算法，本文算法在其中4 個數據集上表現出更好的精度，且整體的均方根誤差平均值表現出較好的精度，精度提升比較明顯。由此可以說明本文算法相對于傳統的MSCKF 算法或者RVIO 算法提高了定位精度。相較于傳統的MSCKF 算法，其均方根誤差減少了約36.5%。

圖6 X、Y、Z 三個方向上軌跡漂移誤差Fig.6 X、Y、Z trajectory drift error

圖7 俯仰角、滾動角和航向角漂移誤差Fig.7 Pitch,roll and yaw drift error

圖8 X、Y、Z 三個方向運動軌跡與實際軌跡Fig.8 X、Y、Z movement trajectories and actual trajectories

表1 EuRoc 數據集軌跡誤差統計（單位：米）Tab.1 Statistical of trajectory error in EuRoc dataset (unit: m)

6 結 論

為了提高MSCKF 框架下的視覺慣性里程計系統定位精度和系統魯棒性，本文對傳統的MSCKF 算法做出了兩點改進措施，并通過試驗驗證了算法的有效性。針對如何快速精確地處理兩幀圖像之間的IMU 數據的問題，提出了一種基于IMU 預積分封閉解的算法，相較于傳統基于優化的視覺慣性里程計算法在分段常數加速近似下采用離散四元數積分來簡化所需的預積分值，IMU 預積分封閉解算法在IMU 時間周期內求解解析解，并應用于MSCKF 視覺慣性里程計框架下，試驗證明可以有效提高系統定位的精度。針對MSCKF算法觀測方程參數化方法存在的數值穩定性的問題，提出了一種逆深度的參數化方法，該方法克服了MSCKF 算法在空間點坐標z 軸深度值趨近于零時，系統觀測值會出現奇點的情況，試驗結果表明，該算法相較于傳統的MSCKF 算法，能有效增加系統的魯棒性。

通過在公開的EuRoc數據集上試驗驗證了該算法的有效性，同時將本文建立的視覺慣性里程計算法與已有開源的視覺慣性里程計系統對比，在EuRoc 數據集六個飛行序列上的試驗結果表明，改進的算法在定位精度和穩定性上均有提升，相較于傳統的MSCKF視覺慣性里程計算法漂移較小，均方根誤差（RMSE）減小約36.5%，定位精度得到有效提升。