趙良玉，金瑞，朱葉青，高鳳杰

1.北京理工大學(xué) 宇航學(xué)院，北京 100081 2.海鷹航空通用裝備有限責(zé)任公司，北京 100070

同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)被認(rèn)為是實現(xiàn)移動機(jī)器人自主作業(yè)的核心技術(shù)，已被廣泛應(yīng)用于無人機(jī)、無人車和虛擬現(xiàn)實等領(lǐng)域。對于室內(nèi)環(huán)境，由于建筑物會對GPS信號進(jìn)行遮蔽，無人機(jī)定位多采用SLAM技術(shù)，為了克服單一傳感器自身精度的不足，常采用多傳感器融合策略，如視覺慣性就是一種有效的融合方法，且相機(jī)和慣性測量單元(Inertial Measurement Unit，IMU)均具有重量輕、成本低的特點，符合室內(nèi)無人機(jī)輕量化和低功耗的需求。依據(jù)視覺與IMU的耦合方式，視覺慣性SLAM系統(tǒng)可分為松耦合和緊耦合2種類型。松耦合方法是融合視覺與IMU 2個模塊分別獨立進(jìn)行運動估計后的結(jié)果，緊耦合方法則是利用相機(jī)與IMU的原始數(shù)據(jù)聯(lián)合進(jìn)行優(yōu)化，更容易獲得全局一致的估計結(jié)果，如OKVIS和VINS-mono等。從前端里程計的實現(xiàn)方法來看，SLAM系統(tǒng)又可分為特征法和直接法。與直接法相比，特征法更加魯棒且抗光照變化的能力更強，其中點特征是最常用的特征類型，如經(jīng)典的PTAM和ORB-SLAM2系統(tǒng)。但基于特征點的SLAM系統(tǒng)在遭遇場景紋理缺失，如走廊、樓道等弱紋理環(huán)境的時候，特征點提取效果較差，甚至可能追蹤不到足夠多的特征點來估計相機(jī)運動，從而導(dǎo)致運動估計失敗。

為了解決上述特征點SLAM系統(tǒng)所存在的問題，國內(nèi)外專家學(xué)者開展了大量研究工作。其中一種有效的解決方案是在前端提取更高層次的結(jié)構(gòu)特征——線特征。與點特征相比，室內(nèi)人造場景具有豐富的邊緣和線性結(jié)構(gòu)，線特征尤為明顯，如圖1所示。線特征具有較好的光照變化不變性和旋轉(zhuǎn)不變性，可以提供更多的幾何結(jié)構(gòu)信息，因此線特征可以對點特征進(jìn)行補充，提高基于單一特征點SLAM系統(tǒng)的魯棒性和準(zhǔn)確性。Gomez-Ojeda等依據(jù)SVO的框架提出基于雙目相機(jī)的PL-SVO系統(tǒng)，其基本思路是利用LSD線段檢測和匹配線段后，基于光度不變假設(shè)最小化各點與重投影點之間的光度誤差，之后又將此運動估計方法運用到SLAM前端中提出雙目PL-SLAM算法。Pumarola等參考ORB-SLAM的框架引入線特征，將誤差函數(shù)定義為線段兩端點的重投影點和測量線段之間的直線距離。賀一家等在VINS-Mono的基礎(chǔ)上加入線特征提出PL-VIO系統(tǒng)。鄒丹平等則基于曼哈頓世界假設(shè)，將結(jié)構(gòu)線特征加入到StructVIO中，提高了相機(jī)方位的可觀測性和全局精度。上述研究工作無一例外均采用LSD(Line Segment Detector)算法作為線特征檢測的工具，但是LSD算法的設(shè)計初衷是用來表征場景的結(jié)構(gòu)特征，提取速度較慢，且無參數(shù)調(diào)整的LSD算法在面對復(fù)雜背景或有噪聲的圖像時，易檢測出過多的短線段特征，不僅浪費了線段檢測、描述和匹配的計算資源，且易產(chǎn)生較多離群值，導(dǎo)致定位精度明顯下降。此外，LSD算法通常存在線段重復(fù)檢測和過分割問題，即存在過多鄰近相似線段和長線段易被分割為多個短線段的情況，增加了SLAM系統(tǒng)線段檢測和定位精度的不確定性，即同一位置不同時刻的線段檢測結(jié)果差異性較大，且過多被檢測到的鄰近相似線段和分割產(chǎn)生的短線段特征使后續(xù)的線段匹配任務(wù)變得復(fù)雜，從而降低了SLAM系統(tǒng)的定位精度。

針對室內(nèi)弱紋理環(huán)境，如樓道或走廊等，純視覺SLAM系統(tǒng)容易丟失特征點，從而導(dǎo)致運動估計失敗的問題，提出基于點線特征融合的雙目慣性SLAM系統(tǒng)，采用緊耦合的方法有效地融合了點、線和IMU數(shù)據(jù)，并在后端采用光束法平差(Bundle Adjustmant，BA)方法實現(xiàn)了高精度的位姿估計。采用雙目相機(jī)相較于RGBD相機(jī)測量范圍更大，抗光照變化能力更強，與單目相機(jī)相比具有尺度恢復(fù)能力，魯棒性更好。針對傳統(tǒng)線特征檢測算法速度慢且線段提取質(zhì)量不高的問題，提出一種基于改進(jìn)FLD(Fast Line Detector)算法的線特征檢測器，通過長度抑制、近線合并和斷線拼接策略，提升了線段提取的質(zhì)量。傳統(tǒng)特征SLAM所構(gòu)建的稀疏地圖主要應(yīng)用于提升位姿估計的精度和回環(huán)檢測以保證構(gòu)建地圖的全局一致性，即稀疏地圖僅僅表達(dá)結(jié)構(gòu)環(huán)境中特征明顯的路標(biāo)，因而無法有效判斷地圖中缺少路標(biāo)點的空間是否空置。針對特征SLAM所構(gòu)建的稀疏地圖樣式單一，主要被用于定位，不能應(yīng)用于無人機(jī)避障、路徑規(guī)劃以及不夠直觀等可視化程度與可用性能力不足的問題，利用雙目相機(jī)左右視差圖和計算得到的相機(jī)位姿構(gòu)建能滿足多種應(yīng)用需求的環(huán)境點線結(jié)構(gòu)地圖、稠密地圖和導(dǎo)航地圖。最后在公共數(shù)據(jù)集EuRoC和真實的室內(nèi)弱紋理環(huán)境中進(jìn)行性能測試，系統(tǒng)在提升準(zhǔn)確性的基礎(chǔ)上，運行速度更快。

圖1 弱紋理環(huán)境中點線特征提取效果圖Fig.1 Effect diagrams of point-line feature extraction in weakly textured environment

1 系統(tǒng)整體框架

基于點線特征融合的雙目慣性SLAM系統(tǒng)共包括3個模塊，分別為前端視覺慣性里程計、后端非線性優(yōu)化和三維環(huán)境建圖，系統(tǒng)整體框架如圖2所示。

1.1 前端視覺慣性里程計

基于點線特征的前端視覺慣性里程計，采用雙目相機(jī)輸入的左右RGB圖片，在圖像中并行檢測、描述和跟蹤點線特征。其中點特征采用前后幀和左右?guī)?種跟蹤模式，以前幀為基礎(chǔ)，跟蹤當(dāng)前幀，以當(dāng)前左幀為基礎(chǔ)，跟蹤當(dāng)前右?guī)?；線特征則在左側(cè)相機(jī)圖像的前后幀進(jìn)行匹配。

1) 點特征：每幀圖像提取Shi-Tomasi角點，利用KLT光流法實現(xiàn)特征點跟蹤，并基于反向光流法跟蹤剔除差異較大的點。

2) 線特征：左側(cè)每幀圖像采用改進(jìn)FLD算法的線特征提取策略，并使用LBD(Line Band Descriptor)描述符來匹配前一幀中檢測到的線段。

對于IMU測量得到的加速度計和陀螺儀數(shù)據(jù)，按照參考文獻(xiàn)[17]提到的IMU運動模型在2個視覺幀之間進(jìn)行預(yù)積分處理。

前端中另一項重要工作是初始化，目的是給整個系統(tǒng)提供優(yōu)化求解的初值，采用視覺和IMU的松耦合方案。首先在純視覺多幀中，利用運動結(jié)構(gòu)恢復(fù)(Structure From Motion，SFM)方法估計出相機(jī)姿態(tài)和三維點線路標(biāo)的空間位置，然后與IMU預(yù)積分?jǐn)?shù)據(jù)對齊，求解出重力方向、尺度因子、陀螺儀偏置及每一幀對應(yīng)的速度。

圖2 視覺慣性SLAM系統(tǒng)整體框架Fig.2 System framework of visual-inertial SLAM

1.2 后端非線性優(yōu)化

基于滑動窗口的緊耦合后端BA優(yōu)化系統(tǒng)，采用固定大小的滑動窗口求解包括IMU狀態(tài)(位置、速度、朝向、加速度計和陀螺儀偏置)及點線路標(biāo)在內(nèi)的最優(yōu)狀態(tài)向量，通過聯(lián)合最小化多殘差函數(shù)，包括邊緣化的先驗信息、IMU測量殘差及視覺點線重投影殘差，估計高精度的六自由度相機(jī)位姿，其中三維點線路標(biāo)分別用逆深度和普呂克坐標(biāo)參數(shù)化。相比于濾波求解，BA優(yōu)化(非線性優(yōu)化)方法更傾向于使用所有時刻的歷史數(shù)據(jù)，它不僅考慮鄰近時刻的特征與軌跡關(guān)系，更會考慮之前所有時刻的狀態(tài)量，稱為全體時間上的SLAM(Full-SLAM)，在這種意義下，BA優(yōu)化方法使用了更多信息，從理論上來說也就更加精確，但計算量會隨之提升?？紤]到無人機(jī)上板載計算機(jī)的計算能力有限，本文在BA優(yōu)化中，采用提取關(guān)鍵幀的方式嚴(yán)格控制需要進(jìn)行的優(yōu)化量，對于非關(guān)鍵幀，可用于定位檢測，但不再用于建圖優(yōu)化。隨著關(guān)鍵幀和路標(biāo)點(空間地圖點)的增多，后端BA優(yōu)化的計算效率仍會不斷下降。為了避免這種情況，本文使用滑動窗口(Sliding Window)將待優(yōu)化的關(guān)鍵幀限制在一定的數(shù)量來控制BA優(yōu)化的規(guī)模。

1.3 三維環(huán)境建圖

通過雙目相機(jī)左右目視差恢復(fù)點線特征深度，構(gòu)建基于點線幾何結(jié)構(gòu)的環(huán)境地圖，并在此基礎(chǔ)上，綜合雙目視差得到的深度圖和相機(jī)位姿，構(gòu)建稠密面元地圖以及用于導(dǎo)航的八叉樹地圖和ESDF(Euclidean Signed Distance Functions)地圖。

2 線特征提取及參數(shù)化模型

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

視覺SLAM中線特征檢測常用LSD算法，其核心在于像素合并，但是合并像素的方法通常運算量較大。如在文獻(xiàn)[20]給出的實驗中發(fā)現(xiàn)，在480像素×317像素尺寸的圖像中提取線段平均需要66 ms，且隨著圖像尺寸的增大而提高，這嚴(yán)重影響了視覺慣性里程計的實時性。

為解決LSD算法提取時間過長的問題，采用更加快速的FLD線特征提取算法，其應(yīng)用邊緣檢測代替運算量較大的像素合并方法，能在線性時間內(nèi)得到亞像素級準(zhǔn)確度的直線段。針對線段提取質(zhì)量不高的問題，利用長度抑制、近線合并和斷線拼接策略加以修正，改進(jìn)后的線特征提取算法在保留原有FLD算法快速性的同時，得到了更加有效的線段特征。最終實驗表明，本文線特征提取算法具備較好的實時性和較高的線段提取質(zhì)量。

2.1.1 FLD線特征提取算法

現(xiàn)有FLD線特征提取算法，具體步驟如下：

1) 圖像灰度化：對于傳感器輸入的彩色圖，根據(jù)圖像各通道的采樣值進(jìn)行加權(quán)平均轉(zhuǎn)化為灰度圖。

2) 降噪：使用高斯濾波器濾除噪聲并平滑圖像，以防止由噪聲引起的錯誤檢測。

3) 計算圖像中每個像素的梯度值和方向：利用常用的邊緣差分算子(如Rober、Prewitt、Sobel)計算水平和垂直方向的差分和，最終得到梯度值和方向的計算公式為

(1)

式中：、表示像素點坐標(biāo)；(,)表示該像素點的梯度值；表示該像素點方向。

4) 非極大值(Non-Maximum Suppression)抑制：對圖像進(jìn)行梯度計算后，將局部最大值之外的所有梯度值抑制為0。

5) 雙閾值檢測：為了濾除噪聲或顏色變化引起的小梯度值而保留大梯度值，應(yīng)用高、低閥值策略來區(qū)分邊緣像素。

6) 滯后邊界跟蹤：搜索所有連通的弱邊緣，如果一條連通的弱邊緣的任何一個點和強邊緣點連通，則保留這條弱邊緣，否則抑制這條弱邊緣。

7) 計算線特征參數(shù)：通過檢測到的線段首尾兩端點坐標(biāo)，計算線段的長度、角度等基本參數(shù)，設(shè)提取到的線段首尾端點坐標(biāo)分別為(,)和(,)，則線段長度和角度分別為

(2)

式中：len表示單幀圖像條線特征中第條線特征長度；表示線段與水平方向的夾角；為線段序號。

2.1.2 長度抑制

傳統(tǒng)線特征檢測算法在面對復(fù)雜的真實場景時，會檢測到大量的短線段特征，如圖3(a)所示。過多的短線段特征不僅加重了線段檢測、描述和匹配的計算成本，而且增加了線段誤匹配的概率。對于點線特征融合的SLAM系統(tǒng)，線特征提供額外約束以提高位姿估計的精度，因此線特征檢測算法只需要檢測出場景中明顯的線段特征，而不需要用過多線特征精細(xì)地構(gòu)建場景。

由于長線段特征更穩(wěn)定，更容易在多幀圖像中被重復(fù)檢測到，且數(shù)量越多，對定位精度的貢獻(xiàn)越大，故取線特征長度約束的剔除準(zhǔn)則為

(3)

圖3 長度抑制測試結(jié)果對比Fig.3 Comparison of test results before and after length suppression

2.1.3 近線合并和斷線拼接

常用線特征提取算法經(jīng)常將一段較長的線段分割成幾個較短的線段，此外在一些細(xì)長區(qū)域的邊緣處會被多次檢測，導(dǎo)致提取到的線段可能離得比較近，存在大量相似線段。這些線段提取質(zhì)量不高，往往使后續(xù)的線特征匹配任務(wù)變得復(fù)雜，從而增加了線段檢測和系統(tǒng)定位精度的不確定性。為了解決上述線特征提取中存在的問題，提出“近線合并”和“斷線拼接”方法來改進(jìn)FLD算法的提取結(jié)果，提升線段提取質(zhì)量，如圖4所示。

算法流程如下：首先按線段長度對提取出的線段進(jìn)行降序排列，得到{,,…,}，其中表示第條線段?？紤]到較長的線段特征往往來自具有連續(xù)強梯度的圖像區(qū)域，提取精度更加可靠，因而從最長的線段開始進(jìn)行線段分組。如圖5所示，按照需要合并或拼接線段出現(xiàn)的位置不同，存在首尾2種情形，設(shè)線段和為近線合并改進(jìn)措施下需要合并的2條線段，其首尾端點分別為(,)、(,)和(,)、(,)；線段和為斷線拼接改進(jìn)措施下需要拼接的2條線段，其首尾端點分別為(,)、(,)和(,)、(,)。

設(shè)當(dāng)前最長線段為，則剩余線段集合為

圖4 近線合并和斷線拼接Fig.4 Merge of near line segments into a new one, and merge of chain short line segments into a longer one

圖5 近線合并和斷線拼接算法示意圖Fig.5 Schematic diagram of algorithm for merge of near line segments and merge of chain short line segments

={,,…,}

(4)

經(jīng)過角度篩選得到候選線段組：

={?∈:(|-|<)}

(5)

(6)

式中：為線段集合中的線段，其長度為len，與水平方向的夾角為；為衡量線特征角度相近程度的角度篩選閾值，經(jīng)實驗分析將其值設(shè)為π/90；為自適應(yīng)比例系數(shù)函數(shù)，由實驗所得數(shù)據(jù)擬合而成，與線段長度len成反比，len越小，系數(shù)越大，即線段長度越短，可合并性越大。

經(jīng)過水平距離篩選分別得到近線合并候選線段組1和斷線拼接候選線段組2：

① 近線合并：

1={?,∈:(|-|<)or

(|-|<)}

(7)

② 斷線拼接：

2={?,∈:(|-|<)or

(|-|<)}

(8)

經(jīng)過垂直距離篩選分別得到近線合并候選線段組1和斷線拼接候選線段組2：

① 近線合并：

1={?,∈1:(|-|<)or

(|-|<)}

(9)

② 斷線拼接：

2={?,∈2:(|-|<)or

(|-|<)}

(10)

式中：為衡量線特征水平和垂直距離接近程度的距離篩選閾值，經(jīng)實驗分析將其值設(shè)為3。

依據(jù)上述篩選條件，將角度和空間上都與相接近的線段分為一組，最終得到候選線段集={1,2}?？紤]到需要對每一幀圖像頻繁地進(jìn)行距離閾值的篩選操作，二維歐幾里得距離會增加一定的計算量，給算法的實時性帶來挑戰(zhàn)，式(10)中選擇以水平、垂直距離篩選取代二維歐幾里得距離篩選，由于僅涉及絕對值的加減運算，大幅減少了計算代價。并且第2輪垂直距離篩選操作的線段組，是在進(jìn)行完第1輪水平距離篩選操作后，所剩下滿足閾值要求的線段組，這極大地減少了第2輪所需進(jìn)行距離篩選的線段數(shù)量，也進(jìn)一步提升了篩選效率。

將加入經(jīng)過篩選的候選線段集中，組成新的線段集合{,}。從中分別選擇首尾端點中偏離最遠(yuǎn)的兩端點作為新線段的首尾端點，合成新的線段，重新計算其角度，若滿足<，則合并成立，并以合并線段替換線段集合{,}，否則由于合并前后角度相差過大，合并結(jié)果偏離原線段，則放棄合并。

重復(fù)進(jìn)行上述步驟1)至步驟5)直到無法再合并和拼接為止。

圖6(a)為原始算法檢測出的線段，紅色虛線框中表明線段存在近線和斷線情況，圖6(b)為改進(jìn)后的算法檢測結(jié)果?？梢钥闯觯惴ㄒ褜⑧徑€段合并和斷線拼接在一起，如圖中紅色虛線框中所示，大大優(yōu)化了算法線段提取的質(zhì)量。

圖6 近線合并和斷線拼接測試結(jié)果對比Fig.6 Comparison of test results before and after merge of near line segments and of chain short line segments

2.2 直線參數(shù)化

(11)

式中：和表示齊次因子；是一個6×1向量，包含和，是直線的方向向量，是過該直線且通過原點的平面的法向量，垂直于，即=0，如圖8所示。

普呂克坐標(biāo)六參數(shù)的形式大于空間直線四自由度，依然是一種過參數(shù)化的表達(dá)方式，且存在正交約束，會增加后端優(yōu)化求解的難度，故參考Bartoli和Sturm提出的四參數(shù)最小化表示方法，在后端優(yōu)化中采用正交表示法。對普呂克坐標(biāo)進(jìn)行QR分解，其正交表示為(,)∈SO(3)×SO(2)。

圖7 線特征重投影殘差Fig.7 Re-projection error of line feature

圖8 普呂克線坐標(biāo)系Fig.8 Plücker line coordinates

(12)

(13)

正交表示與普呂克坐標(biāo)之間的轉(zhuǎn)換關(guān)系有

(14)

式中：表示矩陣的第列。

2.3 直線的觀測模型和重投影殘差

空間直線的觀測模型存在3D-3D和3D-2D等多種定義方式，通常采用精度更高的3D-2D方式將空間直線重投影到圖像上，然后計算投影直線和圖中匹配直線間的誤差。類比于空間中三維點到圖像坐標(biāo)系的轉(zhuǎn)換方式，將直線轉(zhuǎn)換到相機(jī)坐標(biāo)系，記為，并將用普呂克坐標(biāo)表示，

(15)

式中：為直線變換矩陣(由旋轉(zhuǎn)矩陣和平移向量構(gòu)成)；()^表示平移向量的反對稱矩陣。將投影到圖像平面得到，其普呂克坐標(biāo)表示為

(16)

式中：表示直線的投影矩陣，、和、分別為相機(jī)的焦距和主點。

(17)

式中：=[,,]。

3 基于多源信息融合的后端滑窗優(yōu)化

(18)

通過最小化目標(biāo)函數(shù)(包括IMU殘差、點線特征殘差和先驗殘差)來優(yōu)化滑動窗口中的所有狀態(tài)變量：

(19)

(20)

4 多地圖構(gòu)建

SLAM算法自建立之初便以定位和建圖作為其兩大核心任務(wù)，對于建圖部分，傳統(tǒng)基于特征點的視覺SLAM算法，每幀僅采用幾百個點的像素信息來計算空間路標(biāo)點，導(dǎo)致建圖稀疏，所建地圖無法有效表示周圍環(huán)境，且點云存儲規(guī)模較大，無法在有效的內(nèi)存中建模較大的環(huán)境。本文多地圖構(gòu)建模塊有助于解決傳統(tǒng)SLAM算法后端建圖樣式單一，可視化程度與可用性能力不足的缺陷，構(gòu)建多種能適應(yīng)不同應(yīng)用需求的地圖樣式，包括點線特征結(jié)構(gòu)地圖、稠密面元地圖、八叉樹地圖和ESDF地圖。加入線特征后，在提升系統(tǒng)精度的同時構(gòu)建更高層次的環(huán)境點線結(jié)構(gòu)地圖，主要用于SLAM系統(tǒng)重定位和回環(huán)檢測時的位姿校正，但所建地圖仍為稀疏特征地圖。考慮到雙目相機(jī)左右目視差可以恢復(fù)圖像深度，以及無人機(jī)后續(xù)導(dǎo)航的需要，構(gòu)建易于可視化場景外貌特征的稠密面元地圖和導(dǎo)航地圖。在導(dǎo)航地圖的表達(dá)方式中，選擇存儲容量更小的八叉樹地圖和ESDF地圖。八叉樹地圖使用分層的八叉樹結(jié)構(gòu)存儲占據(jù)的概率大小，然而多數(shù)路徑規(guī)劃方法要求直接給出到障礙物的距離而不僅僅是前方障礙物占據(jù)的概率大小，因而采用ESDF進(jìn)行地圖構(gòu)建。使用Voxblox給出的Grouped Raycast方法，可以實現(xiàn)從TSDF(Truncated Signed Distance Functions)地圖到ESDF地圖的快速構(gòu)建。TSDF是一種將結(jié)構(gòu)化點云數(shù)據(jù)映射到一個預(yù)先定義的三維立體空間中，并用截斷符號距離函數(shù)表示真實場景表面附近區(qū)域的地圖重建算法。由TSDF融合后的ESDF地圖可以快速對障礙物進(jìn)行距離和梯度信息的查詢，并解決了原生TSDF需要已知地圖大小且內(nèi)存開銷過大的問題，長期在路徑規(guī)劃中被用于碰撞檢測以及查找較大的空置區(qū)域。

考慮到建圖算法的計算量會影響SLAM系統(tǒng)的實時性，在后端開辟一個新的線程接收來自前端傳感器的左右相機(jī)圖像和后端優(yōu)化后的位姿數(shù)據(jù)，并將系統(tǒng)統(tǒng)一集成到ROS機(jī)器人操作平臺下，便于后續(xù)無人機(jī)導(dǎo)航工作的開展。

5 實驗與分析

為驗證線特征提取的快速性和SLAM算法的有效性，對線特征提取的耗時進(jìn)行統(tǒng)計，算法有效性的驗證分為2部分：① 在公開數(shù)據(jù)集進(jìn)行仿真并與一些優(yōu)秀的算法進(jìn)行對比；② 在真實樓道環(huán)境中進(jìn)行實驗驗證。PC平臺使用Intel NUC8i7BEH迷你主機(jī)，具體配置如下：CPU為IntelCorei7-8559U處理器，主頻為2.7 GHz-4.5 GHz，內(nèi)存為8 G，無GPU，系統(tǒng)版本為Ubuntu 16.04 LTS 64位，ROS Kinetic。

5.1 線特征提取算法性能比較

為驗證線特征提取算法的性能，選擇EuRoC數(shù)據(jù)集中4個場景提取難度最大的子序列MH_04_difficult、MH_05_difficult、V1_03_difficult、V2_03_difficult，分別進(jìn)行3次實驗，統(tǒng)計線特征提取算法LSD、FLD和改進(jìn)FLD平均每幀的耗時和線特征提取數(shù)量，結(jié)果如表1所示。本文算法(改進(jìn)FLD)，在參數(shù)調(diào)整的基礎(chǔ)上融合了長度抑制、近線合并和斷線拼接策略，相比使用原始FLD算法(平均每幀約21.6 ms)的線特征提取耗時有所增加，平均每幀耗時約為23.1 ms，平均增幅在7%左右，但仍處于較低水準(zhǔn)。LSD線特征提取算法耗時最長，平均每幀約49 ms，算法耗時是本文的2倍多。本實驗中EuRoC數(shù)據(jù)集輸出頻率約為20 Hz，特征點提取耗時約為15 ms，本文算法的點線特征提取線程耗時共計約為38 ms，滿足實時運行要求，基于LSD的線特征提取明顯超時。

由表1中線特征提取數(shù)量可以看出，LSD算法提取線段數(shù)量最多，F(xiàn)LD算法次之，本文算法最少，相對于FLD算法平均降幅在77.8%左右，提取效果如圖9所示，其中圖9(a)、圖9(c)、圖9(e) 為Machine Hall場景下測試結(jié)果對比，圖9(b)、圖9(d)、圖9(f)為Vicon Room場景下測試結(jié)果對比?？梢钥闯?，使用LSD和FLD等傳統(tǒng)算法提取線段時，會有大量短線段特征存在，增加線段檢測和匹配的計算成本，且一些長線段被分割成許多短線段，以及鄰近相似線段的存在使后續(xù)的線段匹配任務(wù)變得復(fù)雜，增加了線段檢測和系統(tǒng)定位精度的不確定性。改進(jìn)后的FLD算法使用長度抑制策略，大大減少了短線段特征的數(shù)量，且保留了對系統(tǒng)定位精度影響較大的長線段特征。使用近線合并和斷線拼接方法，再次改良了線段提取的質(zhì)量，有效地提升了系統(tǒng)精度。

表1 3種線特征提取算法性能對比

實驗結(jié)果表明，改進(jìn)后的FLD算法，線段提取時間少，減少了后端線特征重投影殘差的求解時間，保證了算法的實時性。且提取到的長線段特征明顯，線段質(zhì)量高，利于系統(tǒng)定位精度的提升，魯棒性更好。

圖9 3種線特征提取算法效果對比Fig.9 Comparison of results of three line feature extraction algorithms

5.2 公開數(shù)據(jù)集EuRoC仿真實驗

為驗證雙目視覺慣性SLAM算法的有效性和準(zhǔn)確度，使用公開的EuRoC數(shù)據(jù)集對算法進(jìn)行仿真實驗對比，該數(shù)據(jù)集由無人機(jī)掛載VI-Sensor記錄200 Hz的IMU數(shù)據(jù)和20 Hz的雙目相機(jī)數(shù)據(jù)。圖10為本文算法在EuRoC數(shù)據(jù)集MH_04_difficult序列運行過程中的效果圖及構(gòu)建的特征地圖與軌跡的顯示界面。

圖11為EuRoC數(shù)據(jù)集序列中室內(nèi)弱紋理場景下本文算法的特征提取對比圖，其中圖11(a)為點特征提取圖，圖11(b)為線特征提取圖?？梢钥闯?，在光線較強或較弱以及特征不豐富的弱紋理場景下，點特征提取較為稀疏。而室內(nèi)多為人造結(jié)構(gòu)化場景，具有豐富的邊緣、直角等線性結(jié)構(gòu)，利用線特征提取算法仍能獲得豐富的線特征，點線特征互補的方法相較于單一特征點提取，可為后續(xù)運動估計提供更加豐富且魯棒的特征信息。

VINS-Fusion算法是香港科技大學(xué)開源的基于優(yōu)化的多傳感器狀態(tài)估計器，前端采用光流法，支持雙目視覺慣導(dǎo)SLAM方案；PL-VIO算法是賀一家博士開源的使用點和線特征的緊耦合單目視覺慣性里程計，在VINS-mono的基礎(chǔ)上增加LSD線特征提取器。不同算法在EuRoC數(shù)據(jù)集(共11個子序列)下的運動估計誤差如表2所示，采用絕對軌跡誤差(Absolute Trajectory Error，ATE)評估算法的準(zhǔn)確性，即計算估計位姿與真實位姿之間歐氏距離的均方根誤差(Root Mean Square Error，RMSE)和最大值。在與真實軌跡作對比時使用EVO(Evaluation of Odometry and SLAM)工具進(jìn)行數(shù)據(jù)對齊和誤差計算，其中均方根誤差為同時考慮平移和旋轉(zhuǎn)兩部分得到的最終誤差，表中加粗值為當(dāng)前數(shù)據(jù)集序列下3種算法中的最小均方根誤差值。

圖10 MH_04_difficult數(shù)據(jù)集仿真界面圖Fig.10 Simulation interface of MH_04_difficult dataset

圖11 弱紋理場景下特征提取對比Fig.11 Comparison of feature extraction in weakly textured environment

從表2可以看出，加入線特征后算法準(zhǔn)確度明顯優(yōu)于純點特征視覺SLAM算法，本文采用點線VIO系統(tǒng)，因線特征提取質(zhì)量高，定位精度優(yōu)于同類型PL-VIO算法，除了MH_01_easy和MH_02_easy兩個簡單序列下的精度略輸于PL-VIO，其余場景均為最優(yōu)，在場景紋理缺失的MH_04_difficult和相機(jī)快速運動的V2_03_difficult序列下，算法均表現(xiàn)出優(yōu)異的穩(wěn)定性，沒有出現(xiàn)PL-VIO在V1_02_medium中特征丟失的情形(如表中“×”所示)。系統(tǒng)采用雙目相機(jī)也有效避免了單目相機(jī)尺度不確定的缺陷，在提升系統(tǒng)精度的同時，更加魯棒。由表2中數(shù)據(jù)和圖12(a)各算法在EuRoC數(shù)據(jù)集下的均方根誤差柱形圖可以得出，本文算法與僅使用點特征的VINS-Fusion比較，平均定位誤差下降了68%；與使用單目點線特征的PL-VIO比較，平均定位誤差下降了30%。圖12(b)為MH_04_difficult序列下，各算法平面軌跡與真實軌跡對比圖，可以看出本文算法與真值間的差距十分接近，精度較高。

不同算法在EuRoC數(shù)據(jù)集下各模塊平均每幀的計算時間如表3所示，其中點特征提取與追蹤、線特征提取和線特征追蹤模塊均屬于圖2系統(tǒng)框架的前端線程，位姿優(yōu)化屬于后端線程，多地圖構(gòu)建則屬于建圖線程。從表3可以看出，本文采用改進(jìn)FLD的線特征提取算法計算時間約為PL-VIO算法的一半，平均每幀耗時為25 ms，前端點線特征提取與追蹤線程耗時共計50 ms，而EuRoC數(shù)據(jù)集輸出頻率為20 Hz，系統(tǒng)滿足實時運行要求。多地圖構(gòu)建線程相對比較耗時，平均每幀約為60 ms，其余各模塊的計算時間，3種算法基本保持一致。

表2 不同算法在EuRoC數(shù)據(jù)集下的運動估計誤差Table 2 Motion estimation errors of different algorithms in EuRoC dataset

圖12 不同算法仿真結(jié)果對比Fig.12 Comparison of simulation results of different algorithms

表3 不同算法在EuRoC數(shù)據(jù)集下的平均計算時間

系統(tǒng)在定位的同時，后端建圖模塊同步構(gòu)建環(huán)境地圖，如圖13所示，為算法在運行EuRoC數(shù)據(jù)集MH_01_easy、V1_01_easy和V2_01_easy子序列下的建圖結(jié)果，包括稀疏的點線特征結(jié)構(gòu)地圖、稠密面元地圖、稠密八叉樹地圖和ESDF地圖。線特征的加入為稀疏的點特征地圖增加了場景的結(jié)構(gòu)化特征，如邊緣、直角等，也便于系統(tǒng)建立更高層次環(huán)境地圖。稠密面元地圖則能直觀恢復(fù)EuRoC數(shù)據(jù)集錄制地點的真實場景外貌。與多數(shù)SLAM系統(tǒng)加入深度相機(jī)后構(gòu)建的稠密點云地圖不同，稠密八叉樹地圖和ESDF地圖存儲規(guī)模更低，稠密八叉樹地圖以體素信息來表示空間中障礙物占據(jù)的概率大小，ESDF地圖則能快速對障礙物進(jìn)行距離和梯度信息的查詢，針對應(yīng)用需求的不同均可為后續(xù)無人機(jī)的自主導(dǎo)航提供先決地圖數(shù)據(jù)。對圖13中同一數(shù)據(jù)集序列下的四中建圖結(jié)果進(jìn)行橫向?qū)Ρ?，可以看出面元地圖和八叉樹地圖在障礙物表現(xiàn)上做到了一致，輪廓清晰，地面的呈現(xiàn)也較為平整，與ESDF地圖中占有和空置區(qū)域形成了對應(yīng)。

圖13 地圖重建結(jié)果Fig.13 Map reconstruction results

5.3 室內(nèi)弱紋理長廊環(huán)境測試

為驗證算法在實際弱紋理環(huán)境中的性能，使用由移動平臺TurtleBot搭載Intel NUC主機(jī)和Mynteye雙目慣性導(dǎo)航相機(jī)(相機(jī)幀率30 Hz，IMU幀率200 Hz)采集的實際場景數(shù)據(jù)對算法進(jìn)行測試，實驗采集設(shè)備如圖14(a)所示，實驗軌跡如圖14(b)所示。該數(shù)據(jù)集主要分布在北京理工大學(xué)中關(guān)村校區(qū)宇航樓長走廊環(huán)境下，走廊內(nèi)白墻特征明顯，特征點提取困難，且光照變化差異大，對算法性能具有較大挑戰(zhàn)性，如圖14(c)所示。經(jīng)測試，本文算法在白墻較多、特征點提取不足的長走廊環(huán)境下，能夠提取到豐富的線特征，與PL-VIO算法提取到的線特征相比，濾除了較短的線段和相似的鄰近線段，對于過分割產(chǎn)生的線段也進(jìn)行了拼接，如圖14(d)、圖14(e)所示，算法提取線段質(zhì)量更高，大大降低了線特征的誤匹配率，提升了系統(tǒng)定位精度。

圖14 弱紋理長廊環(huán)境下不同算法仿真結(jié)果對比Fig.14 Comparison of simulation results of different algorithms in weakly textured corridor environment

不同算法在室內(nèi)長廊數(shù)據(jù)集下的運動估計誤差如表4所示，采用與第5.2節(jié)一致的絕對軌跡誤差(ATE)來評估算法的準(zhǔn)確性。從表中可以看出，本文算法均方根誤差和誤差最大值均為3種算法中的最小值，定位精度最高。圖15為本文算法與真值間的絕對軌跡誤差顯示，從圖中可以看出本文算法全段均位于較低誤差區(qū)間內(nèi)，與真值間的差距十分接近。

表4 不同算法在長廊數(shù)據(jù)集下的運動估計誤差

圖15 絕對軌跡誤差結(jié)果Fig.15 Absolute trajectory error results

6 結(jié) 論

針對室內(nèi)弱紋理環(huán)境中，純視覺SLAM系統(tǒng)容易丟失特征點而導(dǎo)致運動估計失敗的問題，提出了一種基于點線特征融合的雙目慣性SLAM算法，在公共數(shù)據(jù)集EuRoC和室內(nèi)弱紋理環(huán)境中進(jìn)行了仿真測試，結(jié)果表明本文算法具有更高的定位準(zhǔn)確度和魯棒性，且運行速度更快，主要貢獻(xiàn)如下：

1) 在基于優(yōu)化的滑動窗口中有效融合了點、線和IMU數(shù)據(jù)信息，解決了弱紋理環(huán)境下點特征視覺SLAM算法精度不高且不穩(wěn)定的問題，實現(xiàn)了更高精度的位姿估計。

2) 提出了一種基于改進(jìn)FLD算法的線特征檢測器，利用長度抑制、近線合并和斷線拼接策略，在保證快速提取的同時，優(yōu)化了傳統(tǒng)算法線段提取質(zhì)量不高的問題，降低了系統(tǒng)線特征的誤匹配率。

3) 利用雙目相機(jī)左右目視差生成的深度圖像和SLAM后端計算得到的相機(jī)位姿，構(gòu)建多種能適應(yīng)不同應(yīng)用需求的地圖樣式，包括點線特征結(jié)構(gòu)地圖、稠密面元地圖、稠密八叉樹地圖和ESDF地圖，解決傳統(tǒng)SLAM算法后端建圖樣式單一，可視化程度與可用性能力不足的缺陷。