林 林,黃宜慶*,李 宏
(1.安徽工程大學(xué) 高端裝備感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室,安徽 蕪湖 2410002.安徽工程大學(xué) 安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室,安徽 蕪湖 241000)
在自動(dòng)駕駛和智能機(jī)器人領(lǐng)域中,同時(shí)定位與建圖得到了廣泛的應(yīng)用。視覺(jué)慣性系統(tǒng)亦稱VIO(Visual-inertial Odometry)系統(tǒng),是融合相機(jī)和IMU(Inertial Measurement Unit)數(shù)據(jù)實(shí)現(xiàn)SLAM(Simultaneous Localization and Mapping)的算法,也是目前SLAM研究領(lǐng)域的研究熱點(diǎn)。相機(jī)在快速運(yùn)動(dòng)時(shí)會(huì)出現(xiàn)圖片模糊的問(wèn)題,而IMU正好可以解決該類問(wèn)題,且其價(jià)格便宜,IMU的缺點(diǎn)是它經(jīng)過(guò)積分所得的加速度和角速度存在漂移問(wèn)題,而相機(jī)沒(méi)有這個(gè)缺點(diǎn),所以兩者可以很好地互補(bǔ)。
Tong等[1]提出并開(kāi)源了VINS-Mono,這是一個(gè)具有魯棒性的初始化過(guò)程,采用了一種基于緊耦合、非線性優(yōu)化的方法,其效果在當(dāng)時(shí)的VIO系統(tǒng)中是非常優(yōu)秀的。但是基于點(diǎn)特征的VIO系統(tǒng),其視覺(jué)包含的信息總是有限的,在光照強(qiáng)度較弱和快速運(yùn)動(dòng)的場(chǎng)景中,點(diǎn)特征的提取就變得尤為困難,使得SLAM系統(tǒng)難以保持較強(qiáng)的魯棒性和較高的精度。線特征廣泛存在于人造環(huán)境中,它可以提供額外的視覺(jué)約束,并為自動(dòng)導(dǎo)航建立一個(gè)信息更豐富的地圖,因此將線特征信息加入到視覺(jué)前端很有意義。文獻(xiàn)[2]在VINS-Mono的框架基礎(chǔ)上將線特征集成到VIO系統(tǒng)中。文獻(xiàn)[3]使用梯度密度濾波器剔除圖像中線特征的密集區(qū)域,使得線特征的誤匹配大大降低,但加入濾波器后,密集區(qū)域的內(nèi)部將不進(jìn)行線段檢測(cè),而僅在其周圍輪廓區(qū)域提取部分線段,這會(huì)導(dǎo)致很多的線段沒(méi)有被檢測(cè)到,從而失去很多線段信息。文獻(xiàn)[4]則提出引入線段分組的方法,將可能是同一條線上的線段合并,生成匹配和提取效率高的長(zhǎng)線段。但生成的長(zhǎng)線段存在一些誤匹配,影響了系統(tǒng)的精度。
文獻(xiàn)[5]在ORB-SLAM2[6]的系統(tǒng)上進(jìn)行創(chuàng)新,采用幾何約束進(jìn)行線特征匹配,利用線特征的深度信息構(gòu)造虛擬線段,提出不一樣的重投影誤差計(jì)算方法,但其重投影誤差的計(jì)算僅采用圖像的像素觀測(cè),卻忽略了深度觀測(cè)。文獻(xiàn)[7]將直線分為結(jié)構(gòu)化直線和非結(jié)構(gòu)化直線,并對(duì)這兩類直線建立不同的數(shù)學(xué)模型,構(gòu)建殘差,加入到目標(biāo)函數(shù)中進(jìn)行優(yōu)化。
基于上述研究,在視覺(jué)前端加入線特征,提出一種新的直線匹配方法,將幀到幀和幀到平面的兩種直線匹配方法結(jié)合,并在線特征提取階段,設(shè)置動(dòng)態(tài)的長(zhǎng)度閾值過(guò)濾掉短線特征,用以提高系統(tǒng)在弱紋理和快速運(yùn)動(dòng)的場(chǎng)景中的魯棒性和精度;在視覺(jué)后端,將構(gòu)建好的線特征重投影誤差模型加入到基于滑動(dòng)窗口內(nèi)的目標(biāo)函數(shù)進(jìn)行求解,最后在數(shù)據(jù)集上驗(yàn)證所提算法。
本文算法以VINS-Mono為系統(tǒng)框架,采用優(yōu)化方法將點(diǎn)線特征的視覺(jué)觀測(cè)數(shù)據(jù)與IMU觀測(cè)數(shù)據(jù)緊密耦合。算法的系統(tǒng)框架如圖1所示。由圖1可知,系統(tǒng)框架主要包含3個(gè)線程:數(shù)據(jù)預(yù)處理、回環(huán)檢測(cè)與全局位姿圖優(yōu)化、滑動(dòng)窗口優(yōu)化。其中,算法改進(jìn)的地方已經(jīng)用底紋標(biāo)出。
數(shù)據(jù)預(yù)處理:分別對(duì)IMU和相機(jī)得到的點(diǎn)線特征進(jìn)行提取、匹配并對(duì)齊。在點(diǎn)特征方面,采用Harris角點(diǎn)[8],LK(Lucas-Kanade)光流以及雙向跟蹤[9]相結(jié)合的方法提高跟蹤的精度。在正向光流跟蹤中,如果上一幀有特征點(diǎn),就直接進(jìn)行LK跟蹤并進(jìn)行預(yù)測(cè),然后統(tǒng)計(jì)和上一幀成功匹配的數(shù)目;如果小于10,就擴(kuò)大搜索,輸入的最大金字塔層數(shù)為3;如果沒(méi)有進(jìn)行預(yù)測(cè)的話,就直接輸入最大金字塔層數(shù)為3。接下來(lái)進(jìn)行反向跟蹤,如果正向反向都能找到特征點(diǎn),則認(rèn)為這些特征點(diǎn)是符合要求的。
在線特征方面,為更好地解決位姿估計(jì)問(wèn)題,在LSD(Line Segment Detector)算法[10]上進(jìn)行改進(jìn),設(shè)置提取線特征的長(zhǎng)度閾值[11]來(lái)去除掉短線特征,提高提取效率。并在直線匹配中加入基于點(diǎn)特征跟蹤的平面到平面(2D-2D)和平面到立體(2D-3D)的直線匹配算法,從而提高匹配模塊的速度和精度。
滑動(dòng)窗口優(yōu)化,回環(huán)檢測(cè)和全局位姿圖優(yōu)化與VINS-Mono的方法大致相同,不同之處將在下文介紹,這里不再贅述。
對(duì)于線特征的處理方法,現(xiàn)階段基本上都是采用LSD算法。LSD算法是將梯度方向相近的像素點(diǎn)合并,然而由于噪聲等一些因素,一條長(zhǎng)的直線可能被分割成很多條短的線段,這些短的線段所包含的圖像信息不足,用于后面的匹配則效率過(guò)低,為此設(shè)置長(zhǎng)度抑制策略。
長(zhǎng)度抑制策略:使用最小長(zhǎng)度閾值Lmin來(lái)去除短線特征:
Lmin=[γ×min(WL,HL)],
(1)
(2)
式中,min(WL,HL)為輸入圖像的寬度與高度二者之間較小的那個(gè)值;γ為長(zhǎng)度比例因子;i為提取到線特征的數(shù)量。在文獻(xiàn)[11]中,作者是根據(jù)經(jīng)驗(yàn)將γ設(shè)為一個(gè)固定的值,但這樣設(shè)置的γ不能使提取數(shù)量始終維持在一個(gè)合理的值,因此根據(jù)提取線特征的數(shù)量來(lái)動(dòng)態(tài)地改變其值。當(dāng)線段數(shù)量較少時(shí),增大長(zhǎng)度閾值,反之減小。為減少計(jì)算資源,該算法依然會(huì)設(shè)置最小長(zhǎng)度的下限。通過(guò)該方法,線特征提取精度得到提升。
針對(duì)直線匹配的精度和穩(wěn)定性,采用2D-2D和2D-3D的直線匹配方法。一般情況下,使用2D-2D的跟蹤方法來(lái)匹配新的檢測(cè)到的線,如果匹配到的線數(shù)量太少,將啟用2D-3D的方法來(lái)提升匹配到的線的數(shù)量。
此方法是對(duì)前一幀中的所有線進(jìn)行采樣,得到采樣點(diǎn)的集合P1{Ω1},使用極線搜索和塊匹配[12]的方法在新圖像中檢測(cè)到對(duì)應(yīng)的候選匹配點(diǎn)。使用零均值差平方和(Zero-mean Sum of Squared Difference,ZMSSD)[13]計(jì)算兩點(diǎn)的匹配分?jǐn)?shù),選取匹配分?jǐn)?shù)最高的點(diǎn)作為跟蹤點(diǎn),最后可以獲取跟蹤點(diǎn)的集合P2{Ω2}。如果被跟蹤的點(diǎn)到當(dāng)前幀的直線距離小于我們?cè)O(shè)定的距離閾值,則認(rèn)為該被跟蹤的點(diǎn)是有效的。如果這些有效的點(diǎn)的數(shù)量大于前一幀線上采樣點(diǎn)的0.8倍,則這條線被認(rèn)為是匹配最佳的線。
(3)
它可以被近似為
(4)
式中,N是修補(bǔ)的寬度乘以修補(bǔ)的高度。
(5)
計(jì)算兩張圖像之間的匹配點(diǎn)的ZMSSD值,從而判斷匹配點(diǎn)的相似程度。
在2D-3D的直線匹配中,采用零均值歸一化互相關(guān)(Zero-normailized Cross-correlation,ZNCC)[14]的匹配方法,相比于歸一化互相關(guān)(Normailized Cross-correlation,NCC)[15],它在公式中去除了窗口內(nèi)的均值,對(duì)于光照的變化能表現(xiàn)出更強(qiáng)的魯棒性。兩張圖像的亮度可能由于照明和曝光條件而發(fā)生變化,第一張圖像可以首先被歸一化。這通常是通過(guò)減去平均值和除以標(biāo)準(zhǔn)差的方式在每一步完成。也就是說(shuō),兩張圖像之間的相似程度可以為
(6)
式中,n是f1和f2的像素?cái)?shù);μ是f的平均值;σ是f的標(biāo)準(zhǔn)差。
當(dāng)相機(jī)快速運(yùn)動(dòng)和有場(chǎng)景遮擋時(shí),需要匹配的線段長(zhǎng)度會(huì)相差很大,從而影響匹配的精度。所以采用極線幾何約束來(lái)限制線的采樣范圍,配合ZNCC進(jìn)行匹配。即計(jì)算了基本矩陣和當(dāng)前幀之間的本質(zhì)矩陣之后,在基本矩陣中線的端點(diǎn)根據(jù)極線幾何投影到當(dāng)前幀,并且通過(guò)兩條極線與當(dāng)前幀的匹配線來(lái)確定對(duì)應(yīng)點(diǎn)的采樣范圍,這樣成功地提高了ZNCC匹配方法。
線重投影殘差以點(diǎn)-線距離為模型。首先,定義線幾何變換,給定一個(gè)從世界坐標(biāo)系平面πw到相機(jī)坐標(biāo)系平面πc的變換矩陣Tcw=[Rcw,tcw],其中Rcw∈SO(3)和tcw∈R3定義為旋轉(zhuǎn)和平移。利用這個(gè)矩陣,將Lw從πw變換到πc為
(7)
然后將Lc進(jìn)行投影得到投影直線:
I=[l1,l2,l3]T=KLnc,
(8)
式中,KL表示直線投影矩陣。
最后假設(shè)Lw為第i個(gè)相機(jī)幀觀測(cè)到的第j條空間線,線重投影誤差可以定義為
(9)
在滑動(dòng)窗口中定義與IMU、點(diǎn)和線測(cè)量信息相關(guān)的所有狀態(tài)向量:
(10)
oT=(ΦT,φ),
(11)
式中,Φ為一個(gè)三維向量;φ定義為一個(gè)標(biāo)量。
通過(guò)最小化所有測(cè)量殘差項(xiàng)之和來(lái)優(yōu)化滑動(dòng)窗口內(nèi)的所有狀態(tài)變量:
(12)
本文采用公共數(shù)據(jù)集EuRoc對(duì)此算法進(jìn)行評(píng)估,并將評(píng)估的結(jié)果與經(jīng)典SLAM算法VINS-Mono的結(jié)果進(jìn)行比較。本實(shí)驗(yàn)在臺(tái)式電腦上展開(kāi),電腦的基本配置為:Intel(R)Core(TM)i7-9700 CPU@3.00GHz 8G內(nèi)存,Ubuntu 16.04。
本文算法在MH_05_difficult序列上的可視化如圖2、3所示。由圖2、3可以看出,在光照較暗的場(chǎng)景中線特征的提取效果依然良好。在V2_03_difficult序列上的兩種算法軌跡與真實(shí)軌跡的對(duì)比如圖4所示。由圖4可以明顯看出,改進(jìn)后的算法軌跡更接近真實(shí)的軌跡。
為驗(yàn)證本文改進(jìn)系統(tǒng)相比于VINS-Mono具有更高的精確度,采用場(chǎng)景復(fù)雜程度較高的3個(gè)不同序列進(jìn)行實(shí)驗(yàn),系統(tǒng)所得結(jié)果與實(shí)際結(jié)果的誤差如圖5~7所示。本文算法在EuRoc數(shù)據(jù)集運(yùn)行的絕對(duì)位姿誤差的結(jié)果如表1所示,絕對(duì)位姿誤差與VINS-Mono的對(duì)比結(jié)果如表2所示。
表2 絕對(duì)位姿誤差與VINS-Mono的對(duì)比 (單位:m)
根據(jù)表1、2可得,在公共數(shù)據(jù)集EuRoc的7個(gè)不同難度、不同場(chǎng)景的序列中,本文算法相比于VINS-Mono都有提升,在MH_01_easy和MH_02_easy兩個(gè)場(chǎng)景比較簡(jiǎn)單的序列中,提升效果不明顯,究其原因是在簡(jiǎn)單場(chǎng)景中,點(diǎn)特征所包含的視覺(jué)信息足以使系統(tǒng)達(dá)到了一定的精度,即使加入線特征并改進(jìn)了算法,也不能將精度提高很多,相反線特征的加入會(huì)增加計(jì)算量,降低系統(tǒng)的實(shí)時(shí)性。但在復(fù)雜的場(chǎng)景中,該算法的優(yōu)越性就得到了體現(xiàn),其中在快速運(yùn)動(dòng)的場(chǎng)景中,精度提高得更明顯。這得益于在線特征提取階段使用長(zhǎng)度抑制策略,匹配階段使用了兩種直線匹配的方法。
表1 本文算法在EuRoc數(shù)據(jù)集運(yùn)行的絕對(duì)位姿誤差的結(jié)果(單位:m)
該算法在VINS-Mono的基礎(chǔ)上,通過(guò)將線特征融合到系統(tǒng)中,解決點(diǎn)特征包含視覺(jué)信息不足的問(wèn)題,并在原有的直線特征提取算法上通過(guò)長(zhǎng)度抑制過(guò)濾掉短線特征,用來(lái)改善提取效果。并且將2D-2D與2D-3D的直線匹配方法結(jié)合使用,用于提高匹配的精度。而后建立線重投影誤差模型,整合各部分殘差,構(gòu)建目標(biāo)優(yōu)化函數(shù)。通過(guò)在不同難度場(chǎng)景下的實(shí)驗(yàn)仿真,得到本系統(tǒng)的軌跡和誤差,并與VINS-Mono運(yùn)行結(jié)果進(jìn)行對(duì)比,通過(guò)分析可知該算法在快速運(yùn)動(dòng)和弱紋理的情況下表現(xiàn)出更強(qiáng)的魯棒性和更高的精度。