基于點(diǎn)線特征融合的雙目慣性SLAM算法

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

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

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

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

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

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

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

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

1.1 前端視覺慣性里程計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1.1 FLD線特征提取算法

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

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

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

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

(1)

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

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

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

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

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

(2)

式中：len表示單幀圖像條線特征中第條線特征長(zhǎng)度；表示線段與水平方向的夾角；為線段序號(hào)。

2.1.2 長(zhǎng)度抑制

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

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

(3)

圖3 長(zhǎng)度抑制測(cè)試結(jié)果對(duì)比Fig.3 Comparison of test results before and after length suppression

2.1.3 近線合并和斷線拼接

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

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

設(shè)當(dāng)前最長(zhǎ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)

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

經(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í)驗(yàn)分析將其值設(shè)為3。

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

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

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

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

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

2.2 直線參數(shù)化

(11)

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

普呂克坐標(biāo)六參數(shù)的形式大于空間直線四自由度，依然是一種過參數(shù)化的表達(dá)方式，且存在正交約束，會(huì)增加后端優(yōu)化求解的難度，故參考Bartoli和Sturm提出的四參數(shù)最小化表示方法，在后端優(yōu)化中采用正交表示法。對(duì)普呂克坐標(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 直線的觀測(cè)模型和重投影殘差

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

(15)

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

(16)

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

(17)

式中：=[,,]。

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

(18)

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

(19)

(20)

4 多地圖構(gòu)建

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

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

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

為驗(yàn)證線特征提取的快速性和SLAM算法的有效性，對(duì)線特征提取的耗時(shí)進(jìn)行統(tǒng)計(jì)，算法有效性的驗(yàn)證分為2部分：① 在公開數(shù)據(jù)集進(jìn)行仿真并與一些優(yōu)秀的算法進(jìn)行對(duì)比；② 在真實(shí)樓道環(huán)境中進(jìn)行實(shí)驗(yàn)驗(yàn)證。PC平臺(tái)使用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 線特征提取算法性能比較

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

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

表1 3種線特征提取算法性能對(duì)比

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

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

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

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

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

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

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

圖11 弱紋理場(chǎng)景下特征提取對(duì)比Fig.11 Comparison of feature extraction in weakly textured environment

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

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

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

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

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

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

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

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

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

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

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

表4 不同算法在長(zhǎng)廊數(shù)據(jù)集下的運(yùn)動(dòng)估計(jì)誤差

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

6 結(jié) 論

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

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

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

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