基于多尺度光流融合特征點視覺-慣性SLAM方法

王通典, 劉潔瑜, 吳宗收, 沈 強, 姚二亮

(火箭軍工程大學導彈工程學院, 陜西 西安 710025)

0 引 言

同時定位與地圖(simultaneous localization and mapping, SLAM)構建中核心的問題是通過視覺里程計(visual odemetry, VO)完成移動載體的位姿估計。目前VO的實現方案可以分很多種,其中將相機結合慣性測量單元(inertial measurement unit, IMU)的實現方案稱為視覺慣性里程計(visual-inertial odemetry, VIO)。依靠純視覺的VO雖然可以自主的實現位姿解算,但是仍受制于相機本身的特性,易受外界環境干擾。IMU測量頻率高于相機且受外界干擾較小,但是長時間導航存在漂移的問題。考慮兩者之間的互補性,融合視覺和IMU實現的VIO可以發揮相互之間的優勢,彌補單一傳感器的不足,從而實現更高的測量精度。

融合IMU和圖像信息的視覺-慣性SLAM(visual-inertial-SLAM, VI-SLAM)分類方式有很多種,根據數據耦合方式可分為:基于松耦合和基于緊耦合。松耦合通過獨立步驟融合VO估計的位姿和IMU測量的狀態,緊耦合則將VO和IMU的狀態量統一進行估計。按照后端優化方法可以分為:基于濾波的方法和基于非線性優化的方法。早期的SLAM主要基于擴展卡爾曼濾波(extented Kalman filter, EKF)實現,文獻[7]基于EKF搭建SLAM系統,實現狀態在線估計。隨后文獻[8]采用無跡卡爾曼濾波(unscented Kalman filer, UKF)構建VSLAM系統,該方法實現了位姿的在線更新。文獻[9]提出了一種多狀態約束卡爾曼濾波(multi-state constraint Kalman filter, MSCKF)實現IMU和視覺的融合,將視覺信息作為狀態量,使用IMU進行狀態預測,在卡爾曼濾波的框架下實現緊耦合的SLAM。文獻[10]提出了一種基于EKF進行狀態估計的魯棒VIO(robut VIO, ROVIO),將光度誤差作為觀測量進行更新,但該算法對特征點的數量依賴較大。文獻[11]提出了一種基于關鍵幀的視覺慣性SLAM算法,將視覺重投影誤差結合慣性誤差在滑動窗口中進行非線性優化,雖取得不錯的效果,但系統整體效率較低。

按照VO前端還可分為:基于圖像特征點的間接法(特征點法)和基于圖像灰度變化的直接法。文獻[13]在ORB-SLAM(oriented FAST and rotated BRIEF SLAM)的基礎上融合慣性信息提出了緊耦合的VI-SLAM算法,實現了較高的定位精度并且具有地圖重用和回環檢測等功能,但基于特征點法的SLAM算法相比直接法較為耗時,占用大量的系統資源。半直接法VO(semi-direct VO, SVO)采用半直接法實現同時定位與建圖,僅在關鍵幀進行特征點提取從而具有較高的實時性。文獻[17]在SVO的基礎上,提出了一種半直接視覺慣性里程計,通過IMU信息彌補視覺里程計的缺陷,有效提高跟蹤精度與系統魯棒性,同時較準確地恢復了環境尺度信息,但是基于半直接法的視覺前端,在相機快速運動情況下魯棒性較差。為了解決直接法依靠梯度搜索計算位姿容易陷入局部最優的問題,文獻[18]將IMU數據緊密關聯到圖像跟蹤過程中,提出了一種結合直接法和IMU的視覺里程計,并取得了較小的累計誤差,但是考慮IMU自身漂移的特性,仍不能較好地解決直接法視覺前端陷入局部最優的問題。

文獻[19]基于滑動窗口非線性優化的后端提出了結合Shi-Tomasi光流法和IMU的單目視覺慣性系統(monocular visual-inertial system, VINS-Mono)算法,在非線性優化之前創新性的采用松耦合方式進行視覺慣性聯合初始化,使得VINS-Mono的效果優于其他基于非線性的VI-SLAM算法,但基于光流法的SLAM算法易受光照變化影響,容易丟失特征,因此環境魯棒性較差。

與使用人工設置的特征點不同,文獻[20]基于卷積神經網絡進行位姿估計提出了一種端到端的卷積神經網絡SLAM(convolutional neural networks SLAM, CNN-SLAM)算法,由于通過網絡模型提取圖像更深層次的特征信息,該SLAM算法對光照變化更加魯棒。但網絡訓練的效果依賴大量的數據樣本,實時性較差。

基于光流法的SLAM算法具有較高計算速度,但需滿足很強的灰度不變假設,實際中難以滿足。在光照條件較差、相機運動過快導致圖像模糊以及弱紋理的情況下魯棒性較差,容易出現累計誤差較大甚至位姿估計失效的問題。

為提高視覺慣性導航系統實時定位精度和魯棒性,針對基于光流法的視覺前端魯棒性較差容易產生累計誤差的問題,借鑒開源算法VINS-Mono的系統框架,本文結合LK(Lucas and Kanade)光流法和ORB特征點法融合IMU信息,提出了一種多尺度光流融合特征點法的視覺-慣性SLAM方法。主要改進如下。

(1) 通過多尺度四叉樹均勻化的方法改進ORB特征提取過程,提高特征提取的分布性和魯棒性,從而提高位姿解算精度,減少累積誤差。

(2) 提出了一種光流融合特征點法的VIO,根據是否是關鍵幀進行自適應特征匹配和光流追蹤,為解決幀間位姿估計失效的問題,融合慣性信息利用幀間IMU預積分位姿估計結果預測,減少累積誤差對初始位姿的影響。

(3) 整合ORB特征匹配重投影誤差、IMU預積分誤差和雙向滑動窗口先驗誤差構建目標優化函數,采用滑動窗口非線性優化進行位姿求解,保證實時性的同時提高定位精度。

1 系統框架

本文設計的改進算法如圖1所示。主要由5個部分組成:數據預處理、視覺慣性初始化、后端非線性優化、閉環檢測以及全局位姿圖優化。本文的主要改進在數據預處理、視覺慣性初始化以及后端非線性優化3部分。

圖1 算法框圖Fig.1 Algorithmic framework

(1) 數據預處理:首先采用改進的算法進行特征點的提取,關鍵幀采用ORB特征點法建立數據關聯,普通幀采用LK光流追蹤建立數據關聯保證實時性的同時提高魯棒性。

(2) 初始化:基于LK光流建立的數據關聯進行初始位姿估計為后端非線性優化提供初值,并聯合慣性信息進行視覺慣性初始化,估計IMU偏置、尺度、重力等初始狀態;針對在相機運動過快、特征缺少等情況下丟失跟蹤從而導致位姿估計失效的問題,本文通過融合慣性信息,利用幀間IMU預積分結果對失敗的位姿估計進行預測,從而為非線性后端優化提供初始位姿估計。

(3) 后端非線性優化:聯合ORB特征點重投影誤差、IMU預積分誤差、先驗構建后端優化目標函數,采用滑動窗口非線性優化進行位姿求解。

2 多尺度光流融合特征點法和慣性信息融合方法

2.1 改進ORB特征提取算法

光流融合特征點法前端算法首先需要對ORB特征點進行提取,以便后續的LK光流追蹤和特征匹配。傳統的ORB算法為了獲得穩定的特征點,提取結果往往分布不均勻、冗余特征較多。這會導致幀間位姿解算誤差較大,從而使得定位精度降低。本文對ORB特征提取過程進行改進,整個流程如圖2所示。

圖2 改進ORB特征提取流程Fig.2 Improved ORB feature extraction process

為了使ORB特征具有尺度不變性首先構建高斯圖像金字塔,以1.2尺度因子縮放原圖像,設置圖像金字塔的層數為6層;其次對金字塔的每層圖像進行網格劃分,設置網格大小為30;在每個網格內,進行自適應FAST角點的提取;對每層圖像提取的FAST角點進行四叉樹均勻化,四叉樹劃分如圖3所示，劃分節點并篩選出節點內響應值最大的特征點;最后對得到的所有特征點在原圖像層再次進行四叉樹劃分,以實現特征點均勻化和減少特征冗余性。

圖3 四叉樹節點劃分Fig.3 Quadtree node division

采用自適應的FAST角點提取,根據每個網格內提取特征點的數量自適應選擇提取閾值。降低閾值提取的特征點存在冗余性,四叉樹分配特征點可以均勻的篩選出響應值最大的特征點。

四叉樹的思想是對數據區域進行遞歸,劃分為4個節點的樹結構。首先按照圖像的寬高比設置初始節點為1,隨后劃分為4個子節點,判斷每個節點中特征點的數量,大于1的節點繼續劃分為4個子節點,依次不斷劃分。因為每層圖像金字塔需要提取的特征點數量不同,所以每層金字塔中四叉樹節點深度也會自適應的發生變化,當節點的總數大于當前層設置的特征點數或者子節點內的特征點數等于1時,將不再繼續劃分節點。當進行完每一層的四叉樹分配后,對所有提取的特征點在原尺度圖像進行四叉樹分配,進一步提高特征分布性,整個改進ORB特征提取法流程由算法1所示。

算法1 改進ORB特征提取輸入:圖像img-金字塔層數levels-當前層提取特征點nums-閾值threshold1、threshold2輸出:提取的特征點mFor j=0;j

2.2 光流融合特征點視覺慣性里程計算法

光流法相比特征點法,雖然具有較高的速度,但是該方法基于的3個假設實際中難以滿足,易受光照變化影響魯棒性較差,跟蹤結果不穩定從而導致位姿估計精度低甚至位姿估計失效,而特征點法較為準確且魯棒性高。

結合LK光流速度快和ORB特征點法精度高的特點同時考慮位姿估計失效問題,本文提出了一種光流融合特征點法視覺慣性里程計。在采用改進特征提取算法得到特征點后,幀間進行光流追蹤,關鍵幀進行BRIEF描述子的提取以及特征匹配。同時考慮幀間的光流追蹤在光照較差環境下容易丟失特征,導致位姿估計失敗,本文采用IMU預積分信息對幀間位姿進行預測。最后構建特征匹配點的重投影誤差并加入到滑動窗口,連同IMU殘差以及先驗進行非線性優化,算法流程示意圖如圖4所示。

圖4 光流融合特征點視覺慣性里程計Fig.4 Optical flow fusion feature point visual inertial odometer

(1) 采用改進的特征提取算法進行特征點的提取。

(2) 得到提取的特征點后,利用LK光流法進行幀間追蹤,從而建立圖像幀之間的數據關聯,并根據對極幾何、PNP等方法恢復幀間位姿、為后端非線性優化提供初值,同時利用三角測量的方法得到特征點逆深度的估計值;針對位姿估計失效問題,利用IMU預計分結果進行預測。

(3) 聯合慣性信息進行視覺慣性初始化,恢復尺度、IMU零偏、重力加速度、速度等視覺慣性系統的初始狀態。

(4) 根據圖像幀之間的視差以及幀間共視特征點的個數,進行關鍵幀的選擇。

(5) 在關鍵幀對提取的特征點計算描述子,并進行特征匹配,建立關鍵幀間的數據關聯;構建重投影誤差聯合IMU預積分誤差、先驗加入到滑動窗口中進行非線性優化。

2.3 IMU預積分結果預測位姿估計

第2.2節的幀間初始位姿恢復依靠光流追蹤的特征點采用對極幾何、PNP(perspective-N-point)方法進行求解。因光流法易受光照變化的影響,在圖像模糊、相機運動過快的情況下易發生特征缺失從而導致初始位姿估計失效。針對該問題,考慮短期內IMU位姿估計結果較為準確,本文在初始位姿估計過程中引入慣性信息,利用IMU的預積分以及上一圖像幀位姿預測當前幀的位姿。設位姿失效發生在+1幀,圖像幀從世界坐標系到相機坐標系的位置wc、速度wc以及姿態wc,和從世界坐標系到IMU坐標系的位置wb、速度wb和姿態wb已知,根據IMU預積分公式可推導+1幀從世界坐標系到IMU坐標系的位置wb+1、速度wb+1和姿態wb+1,可由

wc+1=wb+1

(1)

wc+1=wb+1?

(2)

得到+1圖像幀從世界坐標系到相機坐標系的位置wc、速度wc和姿態wc,其中和為IMU坐標系和相機坐標系之間的平移、旋轉量可通過標定得到。

2.4 基于關鍵幀的滑動窗口非線性優化

緊耦合的非線性優化是使所有測量殘差馬氏范數的和最小,從而得到最大狀態后驗估計。本文對關鍵幀進行特征描述與匹配,并將由關鍵幀間匹配的特征點構建的重投影誤差,聯合IMU預積分殘差、邊緣化先驗構成最小化目標函數,加入到滑動窗口進行后端非線性優化如圖5所示,滑動窗口中目標函數()形式為

(3)

圖5 VI-SLAM后端算法示意圖Fig.5 Schematic diagram of VI-SLAM back-end algorithm

滑動窗口中待優化的系統狀態向量在時刻定義為

(4)

式中:為IMU狀態向量,包含IMU在{}系中的位置w b、速度w b、姿態w b以及加速度零偏值和角速度零偏值;為特征點逆深度。

對目標函數()采用誤差狀態進行線性化,并利用Gauss-Newton方法迭代求解和Huber魯棒核函數剔除異值,則系統狀態向量可表示為

(5)

δ=

(6)

式中:是滑動窗口先驗、IMU預積分、視覺測量的信息矩陣,=()();=-()();()為目標函數()對狀態量的雅可比。

3 實驗與分析

為驗證本文算法的有效性,分別從ORB特征提取效果,以及光流特征點視覺慣性里程計的定位精度和魯棒性進行驗證。采用公共數據集EuRoC對算法進行評估,該數據集包含11個數據序列,分為簡單、中等和困難3個等級,綜合考慮光照、運動快慢等飛行條件,可以全面評估算法的性能。實驗硬件平臺參數如表1所示。

表1 硬件平臺參數

3.1 改進ORB特征提取實驗驗證

算法的驗證實驗采用數據集中MH-05序列圖像,綜合考慮選取相機運動過快、光照較弱以及光照發生變化3個場景進行特征點的提取與光流追蹤,對比算法采用Shi-Tomasi光流法和原始ORB算法進行主觀視覺分析如圖6所示。

圖6 特征提取結果對比Fig.6 Comparison of feature extraction results

設置提取特征點數量為500個,從圖6中可以看出原始ORB算法提取特征點主要集中在物體邊緣,離散性差,因此采用該方法提取的特征點無法反應圖像的整體變化;基于Shi-Tomasi光流跟蹤的特征點易受光照變化影響,在弱光場景下跟蹤的特征點最少,光照變化場景下特征點分布在圖像下方,分布不均勻。而相比之下本文提出的算法提取特征點具有較好的分布性,受光照變化和快速運動影響較小。

為進一步客觀量化本文所提ORB特征提取算法具有良好的分布性和魯棒性,設置特征點提取閾值為500,對以上3種算法在不同的場景下進行多次實驗并計算平均值,從特征提取誤匹配率、算法耗時以及分布均勻度進行對比。分布均勻度采用統計的方法得到,利用30×30大小的網格對提取到特征點圖像進行劃分,共可劃分為個網格。分別統計每個網格中特征點的數量,記提取特征點數量不為0的網格數為,則分布均勻度的評價指標可表示為

(7)

該值越接近1則分布均勻性越好。多次實驗平均結果如表2所示。

表2 多場景算法數據指標對比

從表2中對比數據可知,ORB算法在3種場景下提取特征點最多且匹配率較為穩定,因增加了特征描述的過程,算法平均耗時較多,且均勻度較低這與圖6中結果一致。Shi-Tomasi光流法耗時最低且均勻度適中,但是在光照變化場景和弱光照場景特征點最少,這是因為光流法易受光照變化影響。本文的改進方法整體耗時介于ORB算法和Shi-Tomasi光流法之間,在弱光照場景和快速運動場景下特征點提取數量適中,因增加了特征點均勻化的過程,在3個場景下均勻度均高于其他兩個算法,且匹配率有所提升。

綜上,本文改進的特征提取算法在保證實時性的同時,提高特征提取離散性且有較高的匹配率,在多場景表現均優于對比算法,具有較強的魯棒性。較好的特征離散性能反應整幅圖像的變化提高數據關聯的準確性,從而降低累計誤差。

3.2 光流融合特征點的視覺慣性SLAM算法

針對本文提出的光流融合特征點法的視覺慣性SLAM算法,從導航定位精度和魯棒性進行實驗分析與評估。綜合考慮實驗采用EuRoC數據集中V2_01_medium序列,同等硬件條件下,采用VINS-Mono算法與本文算法進行對比。

圖7為本文SLAM算法和VINS-mono在MH_05_difficult序列上的絕對位姿誤差(APE),圖中顏色條顯示了誤差整體分布情況。從圖7中可知,VINS的平均誤差在44.8 cm,而本文的平均誤差為35 cm,本文算法的誤差極值比VINS的更小,因此本文的算法在該序列定位精度更高。將三維絕對誤差軌跡投影到-平面分析可得,相比VINS本文算法整體位姿誤差更小(表現為藍色線段占比更大),且在相機快速運動時(紅色線段)最大軌跡誤差有所降低,在該序列本文算法定位精度更高。

圖7 MH-05序列絕對位姿誤差對比Fig.7 Comparison of absolute pose error of MH-05 sequence

為了說明本文算法定位具有更好的魯棒性。選取不同的定位難度,分別在V1_02_medium和V2_01_easy序列將本文算法和VINS-Mono進行對比如圖8所示。由圖8可知,本文算法在以上兩個序列的平均位姿誤差均有所降低,在不同難度的數據集上運行均有更好的表現,表現出了更強的魯棒性。

圖8 兩種序列絕對位姿誤差對比Fig.8 Comparison of absolute pose errors in two sequences

為了進一步說明本文算法具有更好的魯棒性和定位精度,對以上3個序列(MH_05_difficult、V1_02_medium、V2_01_easy)的定位誤差進行量化,主要從均方根誤差(Rmse)、誤差平方和(Std)、誤差極值(Max、Min)以及誤差中值(Median)進行分析,本文SLAM算法和VINS的定位誤差統計特性對比如表3所示。

表3 本文算法和VINS-Mono的誤差量化指標對比

分析表3中數據可得,在3個序列的誤差均值較VINS平均降低12.4%,誤差中值平均降低12.3%,且誤差極值的表現也優于VINS-Mono,進一步對軌跡誤差的均方誤差進行對比如表4所示,相比之下本文算法的軌跡誤差更小,絕對軌跡誤差的均方誤差平均降低16.7%。

表4 絕對軌跡誤差的RMSE對比

綜上可以說明本文算法定位精度和魯棒性均優于VINS-Mono,這是因為本文提出的光流融合特征點的SLAM算法降低了位姿的累積誤差,特征提取的均勻化過程提高了位姿估計精度,且在光照較差或相機快速運動等場景下具有較強的魯棒性,其中MH序列絕對軌跡誤差降低更加明顯,這是因為MH序列存在較多處光照條件較差的飛行軌跡,而本文面對較差的光照條件更加魯棒,且在利用IMU預積分結果預測了估計失效的位姿。

4 結 論

結合光流法速度快和ORB特征點法精度高的特點,本文提出了一種光流融合特征點法的視覺-慣性SLAM方法。改進ORB特征提取過程,采用多尺度網格化的方法提取特征點,并利用四叉樹均勻分配特征點從而提高特征點的分布離散性和位姿估計的精度。基于改進特征提取過程融合光流和特征點法實現了視覺-慣性SLAM算法,基于光流法進行幀間的數據關聯從而進行位姿估計為后端優化提供初值,針對幀間位姿估計失效的問題,采用IMU預積分結果預測幀間的位姿估計,基于ORB特征點算法對關鍵幀進行數據關聯,聯合ORB特征點的重投影誤差、預積分誤差、邊緣化先驗構建目標函數進行非線性優化。實驗表明,改進的ORB特征提取算法在保證實時性的同時具有較高的分布離散性和魯棒性,從而降低累計誤差。光流融合特征點的VI-SLAM算法定位精度和魯棒性均優于VINS-Mono。