基于點線特征融合的雙目慣性SLAM算法

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

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

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

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

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

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

1 系統整體框架

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

1.1 前端視覺慣性里程計

基于點線特征的前端視覺慣性里程計，采用雙目相機輸入的左右RGB圖片，在圖像中并行檢測、描述和跟蹤點線特征。其中點特征采用前后幀和左右幀2種跟蹤模式，以前幀為基礎，跟蹤當前幀，以當前左幀為基礎，跟蹤當前右幀；線特征則在左側相機圖像的前后幀進行匹配。

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

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

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

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

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

1.2 后端非線性優化

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

1.3 三維環境建圖

通過雙目相機左右目視差恢復點線特征深度，構建基于點線幾何結構的環境地圖，并在此基礎上，綜合雙目視差得到的深度圖和相機位姿，構建稠密面元地圖以及用于導航的八叉樹地圖和ESDF(Euclidean Signed Distance Functions)地圖。

2 線特征提取及參數化模型

2.1 改進FLD線特征提取算法

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

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

2.1.1 FLD線特征提取算法

現有FLD線特征提取算法，具體步驟如下：

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

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

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

(1)

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

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

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

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

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

(2)

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

2.1.2 長度抑制

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

由于長線段特征更穩定，更容易在多幀圖像中被重復檢測到，且數量越多，對定位精度的貢獻越大，故取線特征長度約束的剔除準則為

(3)

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

2.1.3 近線合并和斷線拼接

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

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

設當前最長線段為，則剩余線段集合為

圖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)

經過角度篩選得到候選線段組：

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

(5)

(6)

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

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

① 近線合并：

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

(|-|<)}

(7)

② 斷線拼接：

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

(|-|<)}

(8)

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

① 近線合并：

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

(|-|<)}

(9)

② 斷線拼接：

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

(|-|<)}

(10)

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

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

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

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

圖6(a)為原始算法檢測出的線段，紅色虛線框中表明線段存在近線和斷線情況，圖6(b)為改進后的算法檢測結果。可以看出，算法已將鄰近線段合并和斷線拼接在一起，如圖中紅色虛線框中所示，大大優化了算法線段提取的質量。

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

2.2 直線參數化

(11)

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

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

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

圖8 普呂克線坐標系Fig.8 Plücker line coordinates

(12)

(13)

正交表示與普呂克坐標之間的轉換關系有

(14)

式中：表示矩陣的第列。

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

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

(15)

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

(16)

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

(17)

式中：=[,,]。

3 基于多源信息融合的后端滑窗優化

(18)

通過最小化目標函數(包括IMU殘差、點線特征殘差和先驗殘差)來優化滑動窗口中的所有狀態變量：

(19)

(20)

4 多地圖構建

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

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

5 實驗與分析

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

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

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

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

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

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

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

5.2 公開數據集EuRoC仿真實驗

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

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

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

圖10 MH_04_difficult數據集仿真界面圖Fig.10 Simulation interface of MH_04_difficult dataset

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

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

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

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

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

表3 不同算法在EuRoC數據集下的平均計算時間

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

圖13 地圖重建結果Fig.13 Map reconstruction results

5.3 室內弱紋理長廊環境測試

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

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

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

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

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

6 結 論

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

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

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

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