徐曉蘇，吳 賢

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

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

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

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

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

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

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

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

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

類似于MSCKF 算法，k時刻系統(tǒng)IMU 狀態(tài)向量定義為XI：

式中，GIq表示世界坐標系G到IMU 系I的單位旋轉(zhuǎn)四元數(shù)；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 位置、速度和旋轉(zhuǎn)矩陣可以由第k時刻遞推得到，方程如下：

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

連續(xù)時間旋轉(zhuǎn)四元數(shù)方程為：

對式(5)進行零階四元數(shù)積分求解出不同時刻的旋轉(zhuǎn)四元數(shù)[16]。基于式(4)的定義，預積分項動態(tài)方程如下：

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

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

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

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

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

式中，XI表示k時刻IMU 的系統(tǒng)狀態(tài)；XC表示N個歷史相機位姿。

2.1 連續(xù)時間IMU 狀態(tài)誤差模型

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

定義四元數(shù)誤差向量為，則：

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

式中，表示相機狀態(tài)誤差，可以定義為：

根據(jù)式(10)～(15)，連續(xù)時間IMU狀態(tài)誤差方程為：

2.2 離散時間IMU 狀態(tài)增廣

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

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

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

2.3 離散時間系統(tǒng)協(xié)方差傳播

離散式(16)系統(tǒng)離散時間狀態(tài)轉(zhuǎn)移矩陣和噪聲協(xié)方差矩陣方程如下所示：

式中，Q=E[nI nIT]，表示連續(xù)時間系統(tǒng)噪聲協(xié)方差矩陣；系統(tǒng)IMU 協(xié)方差矩陣傳播方程如下：

因此整個系統(tǒng)的協(xié)方差矩陣表達式定義為：

式中，PIIk|k為15× 15的IMU狀態(tài)協(xié)方差矩陣；PCCk|k為6N×6N的相機狀態(tài)協(xié)方差矩陣；PICk|k為IMU和相機相關(guān)矩陣。系統(tǒng)協(xié)方差矩陣傳播方程為：

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

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

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

3 逆深度參數(shù)化觀測模型

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

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

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

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

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

系統(tǒng)的觀測殘差方程為：

式(30)線性化可得：

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

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

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

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

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

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

4 EKF 更新

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

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

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

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

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

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

式中，r為矩陣Q1的列數(shù)。此時，系統(tǒng)卡爾曼增益矩陣為：

狀態(tài)方程修正量為：

協(xié)方差矩陣更新方程為：

5 試驗與結(jié)果分析

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

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

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

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

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

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

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

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

為定量地分析本文算法的定位精度，表1對數(shù)據(jù)集6 個飛行序列上的估計結(jié)果的均方根誤差（RMSE）進行了統(tǒng)計計算。表1將均方根誤差指標最小值用加黑體字進行了標注。由標注結(jié)果可示，相對于其他兩種算法，本文算法在其中4 個數(shù)據(jù)集上表現(xiàn)出更好的精度，且整體的均方根誤差平均值表現(xiàn)出較好的精度，精度提升比較明顯。由此可以說明本文算法相對于傳統(tǒng)的MSCKF 算法或者RVIO 算法提高了定位精度。相較于傳統(tǒng)的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 數(shù)據(jù)集軌跡誤差統(tǒng)計（單位：米）Tab.1 Statistical of trajectory error in EuRoc dataset (unit: m)

6 結(jié) 論

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

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