基于動態(tài)邊緣化的雙目視覺慣性SLAM算法

龔 歡 何志琴

(貴州大學電氣工程學院 貴州 貴陽 550025)

0 引 言

狀態(tài)估計和3D重建是機器人應用中的關鍵任務。多年來，已有各種傳感器被用于完成以上任務，例如：GPS、輪速計、慣性測量單元(IMU)、激光雷達、相機等。近十年來，因相機和IMU的低成本、小體積、輕便和低功耗等特點，基于兩者的融合方案(VINS)受到研究者們的普遍關注[1]。

一方面，相機利用多視圖的理論可在運動中估計相機的位姿并構(gòu)建場景的三維模型；另一方面，IMU作為板載傳感器測量局部線性角速度和加速度，能夠為視覺系統(tǒng)提供運動約束，恢復單目SLAM的絕對尺度和重力方向，同時提供魯棒及精確的幀間運動估計[2-8]。相機具備圖像信息豐富、沒有運動漂移的優(yōu)點，但在低紋理區(qū)域或快速運動的情況下缺乏魯棒性；IMU具備數(shù)據(jù)更新快、外部依賴少的優(yōu)點，但易發(fā)生運動漂移；VINS系統(tǒng)利用相機和IMU的這種互補性來實現(xiàn)魯棒且精確的姿態(tài)估計。目前，視覺慣性SLAM(VI-SLAM)和視覺慣性里程計(VIO)已廣泛用于室內(nèi)、城市峽谷或其他星球等GPS信號缺乏場景的自主定位導航和三維重建[1]。

1 相關工作

近年來，關于視覺SLAM的研究非常廣泛。根據(jù)使用的視覺殘差模型可大致分為間接法和直接法。ORBSLAM2[9]是當前間接法中最先進的一個案例，通過提取和跟蹤環(huán)境中的點特征，同時使用幾何重投影約束來進行基于圖優(yōu)化的相機位姿優(yōu)化。與基于特征的間接法不同，直接法[10]使用圖像中的原始像素強度來估計相機的運動，可以得到稀疏的重建效果。其中，由于光度一致性假設，直接法需要良好的初始值和較高的幀率，而間接法提取和匹配特征時需額外的計算資源。

由于相機和IMU的互補特性，研究者通過融合兩類傳感器進行了大量研究。現(xiàn)有的大多數(shù)VIO/VI-SLAM算法主要都集中在單目系統(tǒng)，例如MSCKF[2]、ROVIO[3]、OKVIS[4]、VI-ORB[5]、VINS-Mono[6]、Maplab[7]、VI-DSO[8]。盡管可以從IMU加速度計測量值中估計出單目VIO/VI-SLAM算法的尺度，但是無法從帶噪聲的加速度測量值中準確地減去重力向量[11]且后端邊緣化時同樣會邊緣化IMU信息[8]，因此單目VIO/VI-SLAM估計的尺度通常是不精確的。然而，基于雙目視覺的VIO/VI-SLAM算法[11-17]可利用對極約束提供額外的尺度信息來獲得更高的精度。

基于雙目的VIO/VI-SLAM算法通常分為以下兩類[1]：基于過濾的方法(例如PIRVS[12]、S-MSCKF[13]和Trifo-VI[16])和基于非線性優(yōu)化的方法(例如ICE-BA[15]和 VINS-Fusion[17])。由文獻[18-19]的實驗對比結(jié)果可見，基于優(yōu)化的方法比基于過濾的方法在定位精度方面更具潛力，而基于濾波的方法在計算成本方面更具優(yōu)勢。

受文獻[18-19]對前沿的VI-SLAM方案在多種嵌入式平臺性能評測結(jié)果及VIDSO[5]中提出的動態(tài)邊緣化的啟發(fā)，本文在VINS-MONO[6]的基礎上，提出了一種基于動態(tài)邊緣化的雙目視覺SLAM(DM-SVI-SLAM)算法。該系統(tǒng)通過分而治之的方法初始化IMU，并根據(jù)IMU預積分預測當前幀的位姿；然后采用帶動態(tài)邊緣化的滑動窗口，以緊耦合的方式融合IMU預積分和視覺SLAM的位姿估計，并在構(gòu)建優(yōu)化求解器時使用并行化的Dog-Leg算法加速運算，最后根據(jù)構(gòu)建的稀疏地圖進行回環(huán)檢測和重定位。

與現(xiàn)有的VI-SLAM算法相比，本文方法的主要創(chuàng)新點如下：

(1)傳統(tǒng)單目VI-SLAM算法的尺度恢復完全依賴IMU估計值，在邊緣化IMU信息和IMU發(fā)生漂移時尺度恢復不可靠，本文算法同時包含來自雙目對極約束尺度估計值及各時刻的IMU尺度估計值，使得尺度恢復更加精確；(2)傳統(tǒng)雙目SLAM或者雙目VI-SLAM后端捆集調(diào)整成本函數(shù)僅考慮雙目匹配成功的誤差項，而本文算法同時考慮了雙目匹配和單目匹配的誤差項，充分利用滑窗內(nèi)的有效信息；(3)引入動態(tài)邊緣化策略，降低可能對系統(tǒng)造成干擾的較“舊”信息在滑窗內(nèi)的比重，既減少了后端過程的計算量，又使得算法更加魯棒。

本文算法的框架流程如圖1所示。

圖1 DM-SVI-SLAM算法的框架流程

2 IMU預積分

2.1 相對運動增量

本文使用的檢測器件為雙目相機和IMU，考慮IMU器件的偏置和隨機游走噪聲，IMU測量模型定義為：

(1)

結(jié)合IMU測量模型，可推導出IMU傳感器的運動狀態(tài)位置p、速度v和旋轉(zhuǎn)q，為便于描述，下文記為PVQ。根據(jù)運動學和微積分知識可得出從第i時刻的PVQ到第j時刻的PVQ：

(2)

從上述IMU積分公式可以發(fā)現(xiàn)，j時刻的運動狀態(tài)依賴于世界坐標系的狀態(tài)，即每次“舊”時刻的狀態(tài)變化后，都需要重新計算其后的PVQ值。處理時通常使用式(3)將上述積分模型轉(zhuǎn)為預積分模型：

qwbt=qwbi?qbibt

(3)

PVQ積分公式中的積分項則變成相對于第i時刻的姿態(tài)，而不是相對于世界坐標系的姿態(tài)：

(4)

預積分量僅僅跟IMU測量值有關，將一段時間內(nèi)IMU數(shù)據(jù)直接積分起來就得到了預積分量：

(5)

重整PVQ的積分公式，可得式(6)，這里假定短時間內(nèi)陀螺儀和加速度計的偏置不變。

(6)

由于傳感器測量值是由采樣數(shù)據(jù)得出，故需要將預積分模型轉(zhuǎn)化成離散形式，本文通過mid-point方法實現(xiàn)，即兩個相鄰(k時刻和k+1時刻)的位姿是用兩個時刻的測量值a、ω的平均值來計算：

βbibk+1=βbibk+aδt

(7)

2.2 IMU測量噪聲

如果偏差的估計值變化很小，可以使用一階線性近似表示：

(8)

否則，當偏差估計值變化較大時，可在新的偏差估計值的基礎上重新傳播。這一策略為基于優(yōu)化的后端算法節(jié)省了大量的計算資源(不需要多次重復傳播IMU的測量值)。

3 DM-SVI-SLAM

本文提出的DM-SVI_SLAM系統(tǒng)主要步驟包含視覺跟蹤和IMU預積分、系統(tǒng)初始化、基于動態(tài)邊緣化的后端優(yōu)化和回環(huán)檢測。

3.1 視覺跟蹤和IMU預積分

本系統(tǒng)前端圖像特征提取的是Shi-Tomasi角點。處理時為避免特征點分布不均，將特征點半徑為10像素區(qū)域內(nèi)的值全設置為0。使用光流法特征追蹤，由于引入IMU數(shù)據(jù)，DM-SVI-SLAM的光流預測既可運用視覺估計的運動模型，又可運用IMU的預測結(jié)果，故使得本系統(tǒng)在快速運動的圖像特征模糊的場景下依然能保持良好的魯棒性。由于視覺跟蹤技術在開源庫OpenCV中已經(jīng)有較好的實現(xiàn)，故本文不作介紹。

數(shù)據(jù)處理部分還包括IMU數(shù)據(jù)的預積分計算，其計算方法參照式(7)，為mid-point方法。

3.2 系統(tǒng)初始化

1)初始陀螺儀偏置估計。

通過傳感器標定，標定出外參數(shù)qbc和pbc，利用式(9)的旋轉(zhuǎn)約束，可根據(jù)式(10)估計陀螺儀偏置。

(9)

(10)

式中：B表示所有圖像的關鍵幀集合。結(jié)合預積分的一階泰勒近似：

(11)

對于式(10)的最小二乘問題，可由其雅可比矩陣構(gòu)建正定方程HX=b求解。

2)初始速度和重力向量估計。

需要估計的變量如下：

(12)

(13)

將世界坐標系w換成相機初始時刻坐標系co有：

(14)

將式(3)代入式(14)進行簡單整理有：

(15)

將待估計變量放到方程右側(cè)，有：

(16)

式(16)中，有：

(17)

式(16)轉(zhuǎn)化成線性最小二乘問題對狀態(tài)量進行求解：

(18)

對于優(yōu)化重力向量gco，根據(jù)事先標定可以得出模長大小(如‖gco‖=9.81)，但式(16)求解重力向量gco過程中，并沒有加入模長限制。三維向量gco實際只有兩個自由度。可通過球面坐標進行參數(shù)化：

(19)

式中：w1、w2為待優(yōu)化變量。

(20)

估計出重力方向后，將式(19)代入式(16)，整理得：

(21)

如此迭代，直至g收斂，得出初始速度和重力方向g，系統(tǒng)的初始化完成。

3.3 基于動態(tài)邊緣化的后端優(yōu)化

系統(tǒng)在初始化后得到了初始陀螺儀偏置、速度及重力方向，接著需要對位姿進行求解和優(yōu)化，簡稱為后端優(yōu)化。本系統(tǒng)使用非線性優(yōu)化方法融合視覺和IMU信息，并充分利用雙目相機的匹配測距的特性，使得求解工作更具魯棒性。如式(22)所示，誤差項包含單目點殘差項、雙目點殘差項、IMU預積分殘差項和滑動窗口先驗殘差項四部分。

{qwb,vwb,pwb,ba,bg}=

(22)

其中，單目點殘差項表示只在左右目中某一個相機檢測到三維特征點殘差，由于只有二維坐標，使用重投影誤差計算其殘差項。雙目點殘差項表示經(jīng)校正的左右目匹配成功的三維特征點殘差，由于可通過雙目測距原理求取深度，可用式(23)計算。IMU預積分殘差項即利用IMU信息約束前后兩幀的狀態(tài)量，分為預積分平移殘差、預積分速度殘差和預積分旋轉(zhuǎn)殘差。先驗殘差項即邊緣化操作消去了舊狀態(tài)的變量，并把約束關系疊加到滑窗內(nèi)而保留的先驗信息，隨著迭代推進，變量被不斷優(yōu)化，先驗殘差需跟隨變化。

(23)

傳統(tǒng)的單VI-SLAM系統(tǒng)完全依靠IMU信息來估計尺度，但邊緣化IMU信息易導致尺度漂移；雙目雖然能夠根據(jù)測距原理計算尺度，但是僅依賴雙目視覺信息在特征缺失或重復紋理下易出現(xiàn)較大誤差。為使得邊緣化更有效，本系統(tǒng)使用種動態(tài)邊緣化的策略。Mcurr包含了滑窗內(nèi)完整的邊緣化信息，但由于累積的信息越來越多，信息越來越陳舊，反而會干擾尺度估計的準確性，故當尺度超過縮放間隔后舊的參數(shù)直接使用雙目視覺Mvisual來重置邊緣化。

(1)Mcurr←Mvisual,smiddle←smiddle·di，scurr>smiddle·di。

(2)Mcurr←Mvisual,smiddle←smiddle/di，scurr

(24)

3.4 回環(huán)檢測

當相機再次到達某區(qū)域時，通過回環(huán)檢測能有效減小軌跡的累積誤差。由于VINS-Mono的回環(huán)重定位模塊能幫助取得不錯的定位精度且計算簡便，本文回環(huán)檢測采用VINS-Mono的方案，詳細介紹見文獻[6]。

4 實驗仿真

4.1 實驗條件

本文使用公開可用的EuRoC數(shù)據(jù)集[20]評估提出的DM-SVI-SLAM系統(tǒng)的有效性。該數(shù)據(jù)集通過MT9 V304型號相機提供20 Hz的立體視覺圖像，ADIS 16448同步提供200 Hz的IMU測量數(shù)據(jù)，且配備一套精準的運動捕獲系統(tǒng)提供真實的軌跡數(shù)據(jù)。EuRoC根據(jù)測試過程中光照、環(huán)境紋理、運動快慢和圖像模糊程度劃分出了簡單、中等和困難三個等級，共包含11個測試序列。本文實驗測試平臺為CPU i7-4770HQ，內(nèi)存16 GB的筆記本電腦。實驗的精度和魯棒性評估標準采用算法估計軌跡與真實軌跡的絕對軌跡誤差(ATE)的均方根誤差(RMSE)，記為ATErmse，形如：

(25)

4.2 精度和魯棒性評估

實驗中，單目VIO/VI-SLAM算法選取VI-ORB[5]和VINS-Mono[6]作為代表；雙目VIO/VI-SLAM算法選取了開源的S-MSCKF[13]、VINS-Fusion[17]和Basalt[21]作為代表，S-MSCKF是基于濾波的VIO算法，VINS-Fusion和Basalt都是基于非線性優(yōu)化的VI-SLAM算法；此外還選取ORBSLAM2[9]純雙目版本進行比較。為保證評估實驗結(jié)果的公平性，在4.1節(jié)的實驗條件下，對于開源算法VINS-Mono、ORBSLAM2、S-MSCKF、VINS-Fusion、Basalt及本文提出的DM-SVI-SLAM取獨立運行10次的實驗結(jié)果均值進行對比。而VI-ORB由于代碼未開源，比對結(jié)果取自文獻[5]中的公開數(shù)據(jù)。測試結(jié)果如表1所示，其中"×"代表跟蹤失敗。圖2給出了本文的DM-SVI-SLAM算法在MH5序列中的追蹤效果。圖3顯示了MH5和V13上估計軌跡的示例。

表1 EuRoC 數(shù)據(jù)集中的ATErmse對比 單位：m

圖2 MH5序列中本文算法追蹤效果

(a)MH5的估計軌跡與真值

由表1可以看出，與純雙目ORBSLAM2、VI-ORB和Basalt相比，本文方法在場景較大的工廠序列中的均方根誤差結(jié)果更優(yōu)，而在場景較小的室內(nèi)Vicon序列中精度略低于這些方法，這是由于本文算法的動態(tài)邊緣化策略和綜合考慮單/雙目匹配點的策略在大場景和較低速下作用更大。值得注意的是，VI-ORB算法在V13序列，ORBSLAM2和Basalt算法在V23序列這兩個快速運動抖動劇烈場景下跟蹤失敗，而VINS-Mono和VINS-Fusion及本文方法都可以正常跟蹤全部序列，說明光流法前端比ORB特征點法前端在快速運動下具有更好的魯棒性。從精度方面比較，S-MSCKF方法總體來說表現(xiàn)最差，這是由于該算法沒有回環(huán)檢測模塊且后端優(yōu)化位姿誤差時使用了局部線性化來簡化計算導致。與開源的VINS-Mono和VINS-Fusion比，DM-SVI-SLAM在所有序列的均方根誤差均有明顯降低，說明后端BA優(yōu)化構(gòu)建成本函數(shù)，考慮單目匹配點誤差和雙目匹配點誤差并引入動態(tài)邊緣化的策略，可以有效提升位姿估計的精度。

4.3 實時性評估

為評估本文算法的實時性能，使用CPU i7-4770HQ，內(nèi)存16 GB的筆記本電腦在EuRoC數(shù)據(jù)集上進行測試。值得注意的是，為說明引入動態(tài)邊緣化對計算效率的影響，本文算法運行時采用的是和VINS-Fusion相同的Levenberg-Marquardt算法進行優(yōu)化求解。表2顯示了本文算法、S-MSCKF和VINS-Fusion處理一幀的平均耗時。由于S-MSCKF后端僅基于EKF進行一次優(yōu)化，因此效率最高，但如表1所示，S-MSCKF獲得的估計精度最差。與VINS-Fusion相比，本文算法的執(zhí)行效率更高，這是因為本文方法使用動態(tài)邊緣化策略可降低較“舊”信息在滑窗內(nèi)的比重，從而節(jié)省了后端過程的計算資源。

表2 EuRoC數(shù)據(jù)集中平均處理時間對比

為進一步提升算法的實時性能，本文根據(jù)VINS-Mono的開源版本代碼，發(fā)現(xiàn)程序中有MakeHessian函數(shù)和Marginalize函數(shù)兩部分涉及Hessian矩陣構(gòu)建。VINS-Mono中只在矩陣的拼接過程進行加速,與VINS-Mono不同，本文實驗對Marginalize函數(shù)中每條邊殘差和雅可比矩陣的計算都進行四線程加速。

本節(jié)對比了文獻[22]中記錄的五種Levenberg-Marquardt算法和文獻[23]記錄的三種Dog-Leg算法。其中VINS-Mono開源程序中的后端求解器實現(xiàn)與文獻[21]中的算法1原理相同，為便于比較，本節(jié)將文獻[22]的五種Levenberg-Marquardt算法按原文順序依次編號為1-5，而文獻[23]的Dog-Leg方法依原文順序依次編號為6-8。關于各算法的詳細介紹見文獻[22-23]。

實驗硬件平臺同4.2節(jié)實驗，測試數(shù)據(jù)集均為EuRoC MH_05_diffcult。表3是前面介紹的幾種后端求解算法的solve時間、軌跡精度誤差及變化百分比(以算法1為基準)的對比結(jié)果。可見算法6(文獻[23]第一種Dog-Leg)的精度最高(較VINS-Mono原方法提升了20.104%)，耗時也最短(較VINS-Mono原方法降低13.450%)。此外，Dog-Leg系列的算法在耗時方面均有較大提升。這是由于Dog-Leg在無效迭代過程中可直接利用之前計算得出的高斯牛頓法增量和最速下降法增量，結(jié)合當前置信區(qū)間delta，獲得新的迭代增量，從而縮短計算時間。圖4更直觀地顯示了不同算法的對比效果。

表3 不同方法在MH5上的solve時間和軌跡誤差對比

圖4 各方法solve時間和軌跡誤差對比

5 結(jié) 語

本文提出了帶動態(tài)邊緣化的雙目視覺慣性SLAM算法(DM-SVI-SLAM)，相比VI-ORB采用IMU輔助單目視覺的方案，雙目VI-SLAM算法具有更多約束信息。通過EuRoC數(shù)據(jù)集驗證了雙目VI-SLAM在快速運動引起圖像模糊及特征缺失等有挑戰(zhàn)的場景下魯棒性更好，且與其他兩個兩開源的雙目VI-SLAM算法S-MSCKF和VINS-Fusion相比，本文算法的定位精度效果更高。最后，為降低雙目視覺系統(tǒng)計算量增加的影響，本文通過實驗對比了多種Levenberg-Marquardt算法、Dog-Leg算法的后端求解效果，結(jié)果表明采用Dog-Leg算法有助于降低計算耗時。本文并未對DM-SVI-SLAM使用GPU加速計算，下一步將研究更多加速方案以降低整個雙目視覺慣性融合系統(tǒng)的計算耗時。此外，將該系統(tǒng)應用于移動機器人或無人機等也是未來研究方向。