林 林，黃宜慶*，李 宏

(1.安徽工程大學 高端裝備感知與智能控制教育部重點實驗室，安徽 蕪湖 2410002.安徽工程大學 安徽省電氣傳動與控制重點實驗室，安徽 蕪湖 241000)

在自動駕駛和智能機器人領(lǐng)域中，同時定位與建圖得到了廣泛的應用。視覺慣性系統(tǒng)亦稱VIO(Visual-inertial Odometry)系統(tǒng)，是融合相機和IMU(Inertial Measurement Unit)數(shù)據(jù)實現(xiàn)SLAM(Simultaneous Localization and Mapping)的算法，也是目前SLAM研究領(lǐng)域的研究熱點。相機在快速運動時會出現(xiàn)圖片模糊的問題，而IMU正好可以解決該類問題，且其價格便宜，IMU的缺點是它經(jīng)過積分所得的加速度和角速度存在漂移問題，而相機沒有這個缺點，所以兩者可以很好地互補。

Tong等[1]提出并開源了VINS-Mono，這是一個具有魯棒性的初始化過程，采用了一種基于緊耦合、非線性優(yōu)化的方法，其效果在當時的VIO系統(tǒng)中是非常優(yōu)秀的。但是基于點特征的VIO系統(tǒng)，其視覺包含的信息總是有限的，在光照強度較弱和快速運動的場景中，點特征的提取就變得尤為困難，使得SLAM系統(tǒng)難以保持較強的魯棒性和較高的精度。線特征廣泛存在于人造環(huán)境中，它可以提供額外的視覺約束，并為自動導航建立一個信息更豐富的地圖，因此將線特征信息加入到視覺前端很有意義。文獻[2]在VINS-Mono的框架基礎(chǔ)上將線特征集成到VIO系統(tǒng)中。文獻[3]使用梯度密度濾波器剔除圖像中線特征的密集區(qū)域，使得線特征的誤匹配大大降低，但加入濾波器后,密集區(qū)域的內(nèi)部將不進行線段檢測,而僅在其周圍輪廓區(qū)域提取部分線段，這會導致很多的線段沒有被檢測到，從而失去很多線段信息。文獻[4]則提出引入線段分組的方法，將可能是同一條線上的線段合并，生成匹配和提取效率高的長線段。但生成的長線段存在一些誤匹配，影響了系統(tǒng)的精度。

文獻[5]在ORB-SLAM2[6]的系統(tǒng)上進行創(chuàng)新，采用幾何約束進行線特征匹配，利用線特征的深度信息構(gòu)造虛擬線段，提出不一樣的重投影誤差計算方法，但其重投影誤差的計算僅采用圖像的像素觀測，卻忽略了深度觀測。文獻[7]將直線分為結(jié)構(gòu)化直線和非結(jié)構(gòu)化直線，并對這兩類直線建立不同的數(shù)學模型，構(gòu)建殘差，加入到目標函數(shù)中進行優(yōu)化。

基于上述研究，在視覺前端加入線特征，提出一種新的直線匹配方法，將幀到幀和幀到平面的兩種直線匹配方法結(jié)合，并在線特征提取階段，設(shè)置動態(tài)的長度閾值過濾掉短線特征，用以提高系統(tǒng)在弱紋理和快速運動的場景中的魯棒性和精度；在視覺后端，將構(gòu)建好的線特征重投影誤差模型加入到基于滑動窗口內(nèi)的目標函數(shù)進行求解，最后在數(shù)據(jù)集上驗證所提算法。

1 算法框架

本文算法以VINS-Mono為系統(tǒng)框架，采用優(yōu)化方法將點線特征的視覺觀測數(shù)據(jù)與IMU觀測數(shù)據(jù)緊密耦合。算法的系統(tǒng)框架如圖1所示。由圖1可知，系統(tǒng)框架主要包含3個線程：數(shù)據(jù)預處理、回環(huán)檢測與全局位姿圖優(yōu)化、滑動窗口優(yōu)化。其中，算法改進的地方已經(jīng)用底紋標出。

數(shù)據(jù)預處理：分別對IMU和相機得到的點線特征進行提取、匹配并對齊。在點特征方面，采用Harris角點[8]，LK(Lucas-Kanade)光流以及雙向跟蹤[9]相結(jié)合的方法提高跟蹤的精度。在正向光流跟蹤中，如果上一幀有特征點，就直接進行LK跟蹤并進行預測，然后統(tǒng)計和上一幀成功匹配的數(shù)目；如果小于10，就擴大搜索，輸入的最大金字塔層數(shù)為3；如果沒有進行預測的話，就直接輸入最大金字塔層數(shù)為3。接下來進行反向跟蹤，如果正向反向都能找到特征點，則認為這些特征點是符合要求的。

在線特征方面，為更好地解決位姿估計問題，在LSD(Line Segment Detector)算法[10]上進行改進，設(shè)置提取線特征的長度閾值[11]來去除掉短線特征，提高提取效率。并在直線匹配中加入基于點特征跟蹤的平面到平面(2D-2D)和平面到立體(2D-3D)的直線匹配算法，從而提高匹配模塊的速度和精度。

滑動窗口優(yōu)化，回環(huán)檢測和全局位姿圖優(yōu)化與VINS-Mono的方法大致相同，不同之處將在下文介紹，這里不再贅述。

2 線特征提取及匹配

2.1 線特征提取

對于線特征的處理方法，現(xiàn)階段基本上都是采用LSD算法。LSD算法是將梯度方向相近的像素點合并，然而由于噪聲等一些因素，一條長的直線可能被分割成很多條短的線段，這些短的線段所包含的圖像信息不足，用于后面的匹配則效率過低，為此設(shè)置長度抑制策略。

長度抑制策略：使用最小長度閾值Lmin來去除短線特征：

Lmin=[γ×min(WL,HL)],

(1)

(2)

式中,min(WL,HL)為輸入圖像的寬度與高度二者之間較小的那個值;γ為長度比例因子;i為提取到線特征的數(shù)量。在文獻[11]中，作者是根據(jù)經(jīng)驗將γ設(shè)為一個固定的值，但這樣設(shè)置的γ不能使提取數(shù)量始終維持在一個合理的值，因此根據(jù)提取線特征的數(shù)量來動態(tài)地改變其值。當線段數(shù)量較少時，增大長度閾值,反之減小。為減少計算資源，該算法依然會設(shè)置最小長度的下限。通過該方法，線特征提取精度得到提升。

2.2 2D-2D的直線匹配

針對直線匹配的精度和穩(wěn)定性，采用2D-2D和2D-3D的直線匹配方法。一般情況下，使用2D-2D的跟蹤方法來匹配新的檢測到的線，如果匹配到的線數(shù)量太少，將啟用2D-3D的方法來提升匹配到的線的數(shù)量。

此方法是對前一幀中的所有線進行采樣，得到采樣點的集合P1{Ω1}，使用極線搜索和塊匹配[12]的方法在新圖像中檢測到對應的候選匹配點。使用零均值差平方和(Zero-mean Sum of Squared Difference，ZMSSD)[13]計算兩點的匹配分數(shù)，選取匹配分數(shù)最高的點作為跟蹤點，最后可以獲取跟蹤點的集合P2{Ω2}。如果被跟蹤的點到當前幀的直線距離小于我們設(shè)定的距離閾值，則認為該被跟蹤的點是有效的。如果這些有效的點的數(shù)量大于前一幀線上采樣點的0.8倍，則這條線被認為是匹配最佳的線。

(3)

它可以被近似為

(4)

式中,N是修補的寬度乘以修補的高度。

(5)

計算兩張圖像之間的匹配點的ZMSSD值，從而判斷匹配點的相似程度。

2.3 2D-3D的直線匹配

在2D-3D的直線匹配中，采用零均值歸一化互相關(guān)(Zero-normailized Cross-correlation,ZNCC)[14]的匹配方法，相比于歸一化互相關(guān)(Normailized Cross-correlation，NCC)[15]，它在公式中去除了窗口內(nèi)的均值，對于光照的變化能表現(xiàn)出更強的魯棒性。兩張圖像的亮度可能由于照明和曝光條件而發(fā)生變化，第一張圖像可以首先被歸一化。這通常是通過減去平均值和除以標準差的方式在每一步完成。也就是說，兩張圖像之間的相似程度可以為

(6)

式中,n是f1和f2的像素數(shù)；μ是f的平均值；σ是f的標準差。

當相機快速運動和有場景遮擋時，需要匹配的線段長度會相差很大，從而影響匹配的精度。所以采用極線幾何約束來限制線的采樣范圍，配合ZNCC進行匹配。即計算了基本矩陣和當前幀之間的本質(zhì)矩陣之后，在基本矩陣中線的端點根據(jù)極線幾何投影到當前幀，并且通過兩條極線與當前幀的匹配線來確定對應點的采樣范圍，這樣成功地提高了ZNCC匹配方法。

3 線重投影誤差模型及其優(yōu)化

3.1 線重投影誤差模型

線重投影殘差以點-線距離為模型。首先，定義線幾何變換，給定一個從世界坐標系平面πw到相機坐標系平面πc的變換矩陣Tcw=[Rcw,tcw]，其中Rcw∈SO(3)和tcw∈R3定義為旋轉(zhuǎn)和平移。利用這個矩陣，將Lw從πw變換到πc為

(7)

然后將Lc進行投影得到投影直線：

I=[l1,l2,l3]T=KLnc,

(8)

式中，KL表示直線投影矩陣。

最后假設(shè)Lw為第i個相機幀觀測到的第j條空間線，線重投影誤差可以定義為

(9)

3.2 基于滑動窗口的優(yōu)化

在滑動窗口中定義與IMU、點和線測量信息相關(guān)的所有狀態(tài)向量：

(10)

oT=(ΦT,φ),

(11)

式中,Φ為一個三維向量;φ定義為一個標量。

通過最小化所有測量殘差項之和來優(yōu)化滑動窗口內(nèi)的所有狀態(tài)變量：

(12)

4 實驗驗證與結(jié)果分析

本文采用公共數(shù)據(jù)集EuRoc對此算法進行評估，并將評估的結(jié)果與經(jīng)典SLAM算法VINS-Mono的結(jié)果進行比較。本實驗在臺式電腦上展開，電腦的基本配置為：Intel(R)Core(TM)i7-9700 CPU@3.00GHz 8G內(nèi)存，Ubuntu 16.04。

本文算法在MH_05_difficult序列上的可視化如圖2、3所示。由圖2、3可以看出，在光照較暗的場景中線特征的提取效果依然良好。在V2_03_difficult序列上的兩種算法軌跡與真實軌跡的對比如圖4所示。由圖4可以明顯看出，改進后的算法軌跡更接近真實的軌跡。

為驗證本文改進系統(tǒng)相比于VINS-Mono具有更高的精確度，采用場景復雜程度較高的3個不同序列進行實驗，系統(tǒng)所得結(jié)果與實際結(jié)果的誤差如圖5～7所示。本文算法在EuRoc數(shù)據(jù)集運行的絕對位姿誤差的結(jié)果如表1所示，絕對位姿誤差與VINS-Mono的對比結(jié)果如表2所示。

表2 絕對位姿誤差與VINS-Mono的對比 (單位：m)

根據(jù)表1、2可得，在公共數(shù)據(jù)集EuRoc的7個不同難度、不同場景的序列中，本文算法相比于VINS-Mono都有提升，在MH_01_easy和MH_02_easy兩個場景比較簡單的序列中，提升效果不明顯，究其原因是在簡單場景中，點特征所包含的視覺信息足以使系統(tǒng)達到了一定的精度，即使加入線特征并改進了算法，也不能將精度提高很多，相反線特征的加入會增加計算量，降低系統(tǒng)的實時性。但在復雜的場景中，該算法的優(yōu)越性就得到了體現(xiàn)，其中在快速運動的場景中，精度提高得更明顯。這得益于在線特征提取階段使用長度抑制策略，匹配階段使用了兩種直線匹配的方法。

表1 本文算法在EuRoc數(shù)據(jù)集運行的絕對位姿誤差的結(jié)果(單位：m)

5 結(jié)束語

該算法在VINS-Mono的基礎(chǔ)上，通過將線特征融合到系統(tǒng)中，解決點特征包含視覺信息不足的問題，并在原有的直線特征提取算法上通過長度抑制過濾掉短線特征，用來改善提取效果。并且將2D-2D與2D-3D的直線匹配方法結(jié)合使用，用于提高匹配的精度。而后建立線重投影誤差模型，整合各部分殘差，構(gòu)建目標優(yōu)化函數(shù)。通過在不同難度場景下的實驗仿真，得到本系統(tǒng)的軌跡和誤差，并與VINS-Mono運行結(jié)果進行對比，通過分析可知該算法在快速運動和弱紋理的情況下表現(xiàn)出更強的魯棒性和更高的精度。