基于多尺度光流融合特征點(diǎn)視覺(jué)-慣性SLAM方法

王通典, 劉潔瑜, 吳宗收, 沈 強(qiáng), 姚二亮

(火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院, 陜西 西安 710025)

0 引 言

同時(shí)定位與地圖(simultaneous localization and mapping, SLAM)構(gòu)建中核心的問(wèn)題是通過(guò)視覺(jué)里程計(jì)(visual odemetry, VO)完成移動(dòng)載體的位姿估計(jì)。目前VO的實(shí)現(xiàn)方案可以分很多種,其中將相機(jī)結(jié)合慣性測(cè)量單元(inertial measurement unit, IMU)的實(shí)現(xiàn)方案稱為視覺(jué)慣性里程計(jì)(visual-inertial odemetry, VIO)。依靠純視覺(jué)的VO雖然可以自主的實(shí)現(xiàn)位姿解算,但是仍受制于相機(jī)本身的特性,易受外界環(huán)境干擾。IMU測(cè)量頻率高于相機(jī)且受外界干擾較小,但是長(zhǎng)時(shí)間導(dǎo)航存在漂移的問(wèn)題?？紤]兩者之間的互補(bǔ)性,融合視覺(jué)和IMU實(shí)現(xiàn)的VIO可以發(fā)揮相互之間的優(yōu)勢(shì),彌補(bǔ)單一傳感器的不足,從而實(shí)現(xiàn)更高的測(cè)量精度。

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

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

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

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

基于光流法的SLAM算法具有較高計(jì)算速度,但需滿足很強(qiáng)的灰度不變假設(shè),實(shí)際中難以滿足。在光照條件較差、相機(jī)運(yùn)動(dòng)過(guò)快導(dǎo)致圖像模糊以及弱紋理的情況下魯棒性較差,容易出現(xiàn)累計(jì)誤差較大甚至位姿估計(jì)失效的問(wèn)題。

為提高視覺(jué)慣性導(dǎo)航系統(tǒng)實(shí)時(shí)定位精度和魯棒性,針對(duì)基于光流法的視覺(jué)前端魯棒性較差容易產(chǎn)生累計(jì)誤差的問(wèn)題,借鑒開(kāi)源算法VINS-Mono的系統(tǒng)框架,本文結(jié)合LK(Lucas and Kanade)光流法和ORB特征點(diǎn)法融合IMU信息,提出了一種多尺度光流融合特征點(diǎn)法的視覺(jué)-慣性SLAM方法。主要改進(jìn)如下。

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

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

(3) 整合ORB特征匹配重投影誤差、IMU預(yù)積分誤差和雙向滑動(dòng)窗口先驗(yàn)誤差構(gòu)建目標(biāo)優(yōu)化函數(shù),采用滑動(dòng)窗口非線性優(yōu)化進(jìn)行位姿求解,保證實(shí)時(shí)性的同時(shí)提高定位精度。

1 系統(tǒng)框架

本文設(shè)計(jì)的改進(jìn)算法如圖1所示。主要由5個(gè)部分組成:數(shù)據(jù)預(yù)處理、視覺(jué)慣性初始化、后端非線性優(yōu)化、閉環(huán)檢測(cè)以及全局位姿圖優(yōu)化。本文的主要改進(jìn)在數(shù)據(jù)預(yù)處理、視覺(jué)慣性初始化以及后端非線性優(yōu)化3部分。

圖1 算法框圖Fig.1 Algorithmic framework

(1) 數(shù)據(jù)預(yù)處理:首先采用改進(jìn)的算法進(jìn)行特征點(diǎn)的提取,關(guān)鍵幀采用ORB特征點(diǎn)法建立數(shù)據(jù)關(guān)聯(lián),普通幀采用LK光流追蹤建立數(shù)據(jù)關(guān)聯(lián)保證實(shí)時(shí)性的同時(shí)提高魯棒性。

(2) 初始化:基于LK光流建立的數(shù)據(jù)關(guān)聯(lián)進(jìn)行初始位姿估計(jì)為后端非線性優(yōu)化提供初值,并聯(lián)合慣性信息進(jìn)行視覺(jué)慣性初始化,估計(jì)IMU偏置、尺度、重力等初始狀態(tài);針對(duì)在相機(jī)運(yùn)動(dòng)過(guò)快、特征缺少等情況下丟失跟蹤從而導(dǎo)致位姿估計(jì)失效的問(wèn)題,本文通過(guò)融合慣性信息,利用幀間IMU預(yù)積分結(jié)果對(duì)失敗的位姿估計(jì)進(jìn)行預(yù)測(cè),從而為非線性后端優(yōu)化提供初始位姿估計(jì)。

(3) 后端非線性優(yōu)化:聯(lián)合ORB特征點(diǎn)重投影誤差、IMU預(yù)積分誤差、先驗(yàn)構(gòu)建后端優(yōu)化目標(biāo)函數(shù),采用滑動(dòng)窗口非線性優(yōu)化進(jìn)行位姿求解。

2 多尺度光流融合特征點(diǎn)法和慣性信息融合方法

2.1 改進(jìn)ORB特征提取算法

光流融合特征點(diǎn)法前端算法首先需要對(duì)ORB特征點(diǎn)進(jìn)行提取,以便后續(xù)的LK光流追蹤和特征匹配。傳統(tǒng)的ORB算法為了獲得穩(wěn)定的特征點(diǎn),提取結(jié)果往往分布不均勻、冗余特征較多。這會(huì)導(dǎo)致幀間位姿解算誤差較大,從而使得定位精度降低。本文對(duì)ORB特征提取過(guò)程進(jìn)行改進(jìn),整個(gè)流程如圖2所示。

圖2 改進(jìn)ORB特征提取流程Fig.2 Improved ORB feature extraction process

為了使ORB特征具有尺度不變性首先構(gòu)建高斯圖像金字塔,以1.2尺度因子縮放原圖像,設(shè)置圖像金字塔的層數(shù)為6層;其次對(duì)金字塔的每層圖像進(jìn)行網(wǎng)格劃分,設(shè)置網(wǎng)格大小為30;在每個(gè)網(wǎng)格內(nèi),進(jìn)行自適應(yīng)FAST角點(diǎn)的提取;對(duì)每層圖像提取的FAST角點(diǎn)進(jìn)行四叉樹(shù)均勻化,四叉樹(shù)劃分如圖3所示，劃分節(jié)點(diǎn)并篩選出節(jié)點(diǎn)內(nèi)響應(yīng)值最大的特征點(diǎn);最后對(duì)得到的所有特征點(diǎn)在原圖像層再次進(jìn)行四叉樹(shù)劃分,以實(shí)現(xiàn)特征點(diǎn)均勻化和減少特征冗余性。

圖3 四叉樹(shù)節(jié)點(diǎn)劃分Fig.3 Quadtree node division

采用自適應(yīng)的FAST角點(diǎn)提取,根據(jù)每個(gè)網(wǎng)格內(nèi)提取特征點(diǎn)的數(shù)量自適應(yīng)選擇提取閾值。降低閾值提取的特征點(diǎn)存在冗余性,四叉樹(shù)分配特征點(diǎn)可以均勻的篩選出響應(yīng)值最大的特征點(diǎn)。

四叉樹(shù)的思想是對(duì)數(shù)據(jù)區(qū)域進(jìn)行遞歸,劃分為4個(gè)節(jié)點(diǎn)的樹(shù)結(jié)構(gòu)。首先按照?qǐng)D像的寬高比設(shè)置初始節(jié)點(diǎn)為1,隨后劃分為4個(gè)子節(jié)點(diǎn),判斷每個(gè)節(jié)點(diǎn)中特征點(diǎn)的數(shù)量,大于1的節(jié)點(diǎn)繼續(xù)劃分為4個(gè)子節(jié)點(diǎn),依次不斷劃分。因?yàn)槊繉訄D像金字塔需要提取的特征點(diǎn)數(shù)量不同,所以每層金字塔中四叉樹(shù)節(jié)點(diǎn)深度也會(huì)自適應(yīng)的發(fā)生變化,當(dāng)節(jié)點(diǎn)的總數(shù)大于當(dāng)前層設(shè)置的特征點(diǎn)數(shù)或者子節(jié)點(diǎn)內(nèi)的特征點(diǎn)數(shù)等于1時(shí),將不再繼續(xù)劃分節(jié)點(diǎn)。當(dāng)進(jìn)行完每一層的四叉樹(shù)分配后,對(duì)所有提取的特征點(diǎn)在原尺度圖像進(jìn)行四叉樹(shù)分配,進(jìn)一步提高特征分布性,整個(gè)改進(jìn)ORB特征提取法流程由算法1所示。

算法1 改進(jìn)ORB特征提取輸入:圖像img-金字塔層數(shù)levels-當(dāng)前層提取特征點(diǎn)nums-閾值threshold1、threshold2輸出:提取的特征點(diǎn)mFor j=0;j

2.2 光流融合特征點(diǎn)視覺(jué)慣性里程計(jì)算法

光流法相比特征點(diǎn)法,雖然具有較高的速度,但是該方法基于的3個(gè)假設(shè)實(shí)際中難以滿足,易受光照變化影響魯棒性較差,跟蹤結(jié)果不穩(wěn)定從而導(dǎo)致位姿估計(jì)精度低甚至位姿估計(jì)失效,而特征點(diǎn)法較為準(zhǔn)確且魯棒性高。

結(jié)合LK光流速度快和ORB特征點(diǎn)法精度高的特點(diǎn)同時(shí)考慮位姿估計(jì)失效問(wèn)題,本文提出了一種光流融合特征點(diǎn)法視覺(jué)慣性里程計(jì)。在采用改進(jìn)特征提取算法得到特征點(diǎn)后,幀間進(jìn)行光流追蹤,關(guān)鍵幀進(jìn)行BRIEF描述子的提取以及特征匹配。同時(shí)考慮幀間的光流追蹤在光照較差環(huán)境下容易丟失特征,導(dǎo)致位姿估計(jì)失敗,本文采用IMU預(yù)積分信息對(duì)幀間位姿進(jìn)行預(yù)測(cè)。最后構(gòu)建特征匹配點(diǎn)的重投影誤差并加入到滑動(dòng)窗口,連同IMU殘差以及先驗(yàn)進(jìn)行非線性優(yōu)化,算法流程示意圖如圖4所示。

圖4 光流融合特征點(diǎn)視覺(jué)慣性里程計(jì)Fig.4 Optical flow fusion feature point visual inertial odometer

(1) 采用改進(jìn)的特征提取算法進(jìn)行特征點(diǎn)的提取。

(2) 得到提取的特征點(diǎn)后,利用LK光流法進(jìn)行幀間追蹤,從而建立圖像幀之間的數(shù)據(jù)關(guān)聯(lián),并根據(jù)對(duì)極幾何、PNP等方法恢復(fù)幀間位姿、為后端非線性優(yōu)化提供初值,同時(shí)利用三角測(cè)量的方法得到特征點(diǎn)逆深度的估計(jì)值;針對(duì)位姿估計(jì)失效問(wèn)題,利用IMU預(yù)計(jì)分結(jié)果進(jìn)行預(yù)測(cè)。

(3) 聯(lián)合慣性信息進(jìn)行視覺(jué)慣性初始化,恢復(fù)尺度、IMU零偏、重力加速度、速度等視覺(jué)慣性系統(tǒng)的初始狀態(tài)。

(4) 根據(jù)圖像幀之間的視差以及幀間共視特征點(diǎn)的個(gè)數(shù),進(jìn)行關(guān)鍵幀的選擇。

(5) 在關(guān)鍵幀對(duì)提取的特征點(diǎn)計(jì)算描述子,并進(jìn)行特征匹配,建立關(guān)鍵幀間的數(shù)據(jù)關(guān)聯(lián);構(gòu)建重投影誤差聯(lián)合IMU預(yù)積分誤差、先驗(yàn)加入到滑動(dòng)窗口中進(jìn)行非線性優(yōu)化。

2.3 IMU預(yù)積分結(jié)果預(yù)測(cè)位姿估計(jì)

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

wc+1=wb+1

(1)

wc+1=wb+1?

(2)

得到+1圖像幀從世界坐標(biāo)系到相機(jī)坐標(biāo)系的位置wc、速度wc和姿態(tài)wc,其中和為IMU坐標(biāo)系和相機(jī)坐標(biāo)系之間的平移、旋轉(zhuǎn)量可通過(guò)標(biāo)定得到。

2.4 基于關(guān)鍵幀的滑動(dòng)窗口非線性優(yōu)化

緊耦合的非線性優(yōu)化是使所有測(cè)量殘差馬氏范數(shù)的和最小,從而得到最大狀態(tài)后驗(yàn)估計(jì)。本文對(duì)關(guān)鍵幀進(jìn)行特征描述與匹配,并將由關(guān)鍵幀間匹配的特征點(diǎn)構(gòu)建的重投影誤差,聯(lián)合IMU預(yù)積分殘差、邊緣化先驗(yàn)構(gòu)成最小化目標(biāo)函數(shù),加入到滑動(dòng)窗口進(jìn)行后端非線性優(yōu)化如圖5所示,滑動(dòng)窗口中目標(biāo)函數(shù)()形式為

(3)

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

滑動(dòng)窗口中待優(yōu)化的系統(tǒng)狀態(tài)向量在時(shí)刻定義為

(4)

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

對(duì)目標(biāo)函數(shù)()采用誤差狀態(tài)進(jìn)行線性化,并利用Gauss-Newton方法迭代求解和Huber魯棒核函數(shù)剔除異值,則系統(tǒng)狀態(tài)向量可表示為

(5)

δ=

(6)

式中:是滑動(dòng)窗口先驗(yàn)、IMU預(yù)積分、視覺(jué)測(cè)量的信息矩陣,=()();=-()();()為目標(biāo)函數(shù)()對(duì)狀態(tài)量的雅可比。

3 實(shí)驗(yàn)與分析

為驗(yàn)證本文算法的有效性,分別從ORB特征提取效果,以及光流特征點(diǎn)視覺(jué)慣性里程計(jì)的定位精度和魯棒性進(jìn)行驗(yàn)證。采用公共數(shù)據(jù)集EuRoC對(duì)算法進(jìn)行評(píng)估,該數(shù)據(jù)集包含11個(gè)數(shù)據(jù)序列,分為簡(jiǎn)單、中等和困難3個(gè)等級(jí),綜合考慮光照、運(yùn)動(dòng)快慢等飛行條件,可以全面評(píng)估算法的性能。實(shí)驗(yàn)硬件平臺(tái)參數(shù)如表1所示。

表1 硬件平臺(tái)參數(shù)

3.1 改進(jìn)ORB特征提取實(shí)驗(yàn)驗(yàn)證

算法的驗(yàn)證實(shí)驗(yàn)采用數(shù)據(jù)集中MH-05序列圖像,綜合考慮選取相機(jī)運(yùn)動(dòng)過(guò)快、光照較弱以及光照發(fā)生變化3個(gè)場(chǎng)景進(jìn)行特征點(diǎn)的提取與光流追蹤,對(duì)比算法采用Shi-Tomasi光流法和原始ORB算法進(jìn)行主觀視覺(jué)分析如圖6所示。

圖6 特征提取結(jié)果對(duì)比Fig.6 Comparison of feature extraction results

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

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

(7)

該值越接近1則分布均勻性越好。多次實(shí)驗(yàn)平均結(jié)果如表2所示。

表2 多場(chǎng)景算法數(shù)據(jù)指標(biāo)對(duì)比

從表2中對(duì)比數(shù)據(jù)可知,ORB算法在3種場(chǎng)景下提取特征點(diǎn)最多且匹配率較為穩(wěn)定,因增加了特征描述的過(guò)程,算法平均耗時(shí)較多,且均勻度較低這與圖6中結(jié)果一致。Shi-Tomasi光流法耗時(shí)最低且均勻度適中,但是在光照變化場(chǎng)景和弱光照?qǐng)鼍疤卣鼽c(diǎn)最少,這是因?yàn)楣饬鞣ㄒ资芄庹兆兓绊?。本文的改進(jìn)方法整體耗時(shí)介于ORB算法和Shi-Tomasi光流法之間,在弱光照?qǐng)鼍昂涂焖龠\(yùn)動(dòng)場(chǎng)景下特征點(diǎn)提取數(shù)量適中,因增加了特征點(diǎn)均勻化的過(guò)程,在3個(gè)場(chǎng)景下均勻度均高于其他兩個(gè)算法,且匹配率有所提升。

綜上,本文改進(jìn)的特征提取算法在保證實(shí)時(shí)性的同時(shí),提高特征提取離散性且有較高的匹配率,在多場(chǎng)景表現(xiàn)均優(yōu)于對(duì)比算法,具有較強(qiáng)的魯棒性。較好的特征離散性能反應(yīng)整幅圖像的變化提高數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性,從而降低累計(jì)誤差。

3.2 光流融合特征點(diǎn)的視覺(jué)慣性SLAM算法

針對(duì)本文提出的光流融合特征點(diǎn)法的視覺(jué)慣性SLAM算法,從導(dǎo)航定位精度和魯棒性進(jìn)行實(shí)驗(yàn)分析與評(píng)估。綜合考慮實(shí)驗(yàn)采用EuRoC數(shù)據(jù)集中V2_01_medium序列,同等硬件條件下,采用VINS-Mono算法與本文算法進(jìn)行對(duì)比。

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

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

為了說(shuō)明本文算法定位具有更好的魯棒性。選取不同的定位難度,分別在V1_02_medium和V2_01_easy序列將本文算法和VINS-Mono進(jìn)行對(duì)比如圖8所示。由圖8可知,本文算法在以上兩個(gè)序列的平均位姿誤差均有所降低,在不同難度的數(shù)據(jù)集上運(yùn)行均有更好的表現(xiàn),表現(xiàn)出了更強(qiáng)的魯棒性。

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

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

表3 本文算法和VINS-Mono的誤差量化指標(biāo)對(duì)比

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

表4 絕對(duì)軌跡誤差的RMSE對(duì)比

綜上可以說(shuō)明本文算法定位精度和魯棒性均優(yōu)于VINS-Mono,這是因?yàn)楸疚奶岢龅墓饬魅诤咸卣鼽c(diǎn)的SLAM算法降低了位姿的累積誤差,特征提取的均勻化過(guò)程提高了位姿估計(jì)精度,且在光照較差或相機(jī)快速運(yùn)動(dòng)等場(chǎng)景下具有較強(qiáng)的魯棒性,其中MH序列絕對(duì)軌跡誤差降低更加明顯,這是因?yàn)镸H序列存在較多處光照條件較差的飛行軌跡,而本文面對(duì)較差的光照條件更加魯棒,且在利用IMU預(yù)積分結(jié)果預(yù)測(cè)了估計(jì)失效的位姿。

4 結(jié) 論

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