周書(shū)杰，吳功平，何文山

(武漢大學(xué)動(dòng)力與機(jī)械學(xué)院，湖北武漢 430072)

0 前言

同時(shí)定位與建圖(SLAM)可以解決移動(dòng)機(jī)器人在GPS信號(hào)較弱的環(huán)境中的定位問(wèn)題，同時(shí)通過(guò)其自身的傳感器獲得周?chē)h(huán)境的三維結(jié)構(gòu)及自身的姿態(tài)，進(jìn)而完成相關(guān)任務(wù)。視覺(jué)SLAM相比于激光SLAM，其相機(jī)成本低廉、功耗低、體積小，可以獲取豐富的環(huán)境信息，受到了工業(yè)界與學(xué)術(shù)界的廣泛關(guān)注。

通常，V-SLAM/VIO算法利用圖像特征點(diǎn)來(lái)跟蹤定位。例如，QIN等[1]提出的VINS-Mono以FAST角點(diǎn)[2]為特征點(diǎn)，用KLT光流法[3]追蹤完成特征匹配。MUR-ARTAL等[4]提出的ORB-SLAM，基于改進(jìn)的ORB算法[5]來(lái)提取特征點(diǎn)，使得特征點(diǎn)在圖像中分布均勻。CAMPOS等[6]提出的ORB_SLAM3，在ORB-SLAM中融入慣性測(cè)量單元(Inertial Measurement Unit，IMU)，顯著提升了算法的魯棒性和精度，在各個(gè)數(shù)據(jù)集上表現(xiàn)優(yōu)異。然而基于特征點(diǎn)的SLAM系統(tǒng)在弱紋理場(chǎng)景中，提取到的點(diǎn)特征較少，往往無(wú)法滿(mǎn)足跟蹤需求，導(dǎo)致算法性能下降。

為解決以上問(wèn)題，研究人員將圖像中的線特征引入到V-SLAM/VIO中，相比于點(diǎn)特征，線特征可以提供更多的結(jié)構(gòu)信息，尤其是在人造場(chǎng)景中，如辦公樓和城市建筑。因此，線特征可以彌補(bǔ)點(diǎn)特征的不足，提高基于特征點(diǎn)SLAM系統(tǒng)的魯棒性和穩(wěn)定性。PUMAROLA等[7]在ORB-SLAM的基礎(chǔ)上融入線特征，構(gòu)建一個(gè)單目點(diǎn)線SLAM系統(tǒng)，用兩個(gè)端點(diǎn)來(lái)表示空間直線。HE等[8]提出的PL-VIO，在VINS-Mono框架中加入線特征，用普呂克坐標(biāo)和正交表示法[9]來(lái)表示空間直線，減少待優(yōu)化變量個(gè)數(shù)，提高線特征優(yōu)化精度。ZUO等[10]提出的雙目點(diǎn)線SLAM系統(tǒng)PL-SLAM，將線段誤差函數(shù)定義為線段投影端點(diǎn)到線段距離。

王丹[11]改進(jìn)了SVO中提取特征點(diǎn)的方式并加入線特征，使輪式機(jī)器人在室內(nèi)環(huán)境獲得了更精確的定位。魏琛琛[12]利用點(diǎn)線特征的共線性，增加了SLAM系統(tǒng)中的測(cè)量約束，使得移動(dòng)機(jī)器人在低紋理的走廊環(huán)境中定位精度遠(yuǎn)高于其他算法。曾繼超[13]用梯度過(guò)濾機(jī)制改進(jìn)傳統(tǒng)LSD算法，提升了線段提取的穩(wěn)定性。

以上研究在線特征檢測(cè)時(shí)均采用LSD算法[14]，線段匹配用LBD描述子[15]。然而，LSD線段檢測(cè)和提取LBD描述子速度較慢，對(duì)算力弱的計(jì)算平臺(tái)不友好，限制了點(diǎn)線特征SLAM的跟蹤速度。且LSD算法容易受圖像質(zhì)量影響，檢測(cè)到的線段被分割為多個(gè)短線，易產(chǎn)生線段誤匹配，降低SLAM系統(tǒng)定位精度。許多基于線特征的SLAM算法都對(duì)LSD進(jìn)行改進(jìn)，提升了系統(tǒng)精度，但同時(shí)也增加了SLAM前端耗時(shí)[10，13，16]。

針對(duì)以上點(diǎn)線特征SLAM系統(tǒng)存在的問(wèn)題，本文作者提出一種融合點(diǎn)線特征的快速單目視覺(jué)慣性SLAM算法，使用ELSED(Enhanced Line Segment Drawing)線段檢測(cè)算法[17]，快速提取場(chǎng)景中的長(zhǎng)線段，剔除較短的線段，減少短線誤匹配對(duì)系統(tǒng)的干擾。在追蹤普通幀時(shí)，基于連續(xù)幀間微小運(yùn)動(dòng)假設(shè)，并利用幾何約束實(shí)現(xiàn)線段快速匹配，顯著提升了線特征匹配速度與匹配準(zhǔn)確率。并在地面機(jī)器人OpenLORIS數(shù)據(jù)集上對(duì)此算法進(jìn)行測(cè)試，結(jié)果表明：此算法在定位精度和實(shí)時(shí)性上都表現(xiàn)良好。

1 系統(tǒng)架構(gòu)

文中SLAM系統(tǒng)包括跟蹤、局部建圖和回環(huán)檢測(cè)3個(gè)線程，系統(tǒng)框架如圖1所示，該系統(tǒng)在ORB-SLAM3系統(tǒng)的基礎(chǔ)上加入線特征，其中涉及點(diǎn)特征的操作與ORB-SLAM3基本一致。

圖1 系統(tǒng)框架設(shè)計(jì)

主要針對(duì)傳統(tǒng)線特征提取和匹配計(jì)算耗時(shí)久的問(wèn)題，引入新的線段提取算法ELSED，提高線段提取的質(zhì)量，并利用連續(xù)幀之間的微小運(yùn)動(dòng)假設(shè)，提出一種相鄰圖像之間的無(wú)線段描述子的線特征匹配算法，避免了不必要的線特征描述子的計(jì)算。在關(guān)鍵幀上提取線段描述子，完成關(guān)鍵幀之間線段匹配，創(chuàng)造新的地圖線。

2 線特征提取與匹配

2.1 線特征提取

點(diǎn)線特征SLAM算法領(lǐng)域普遍使用LSD算法提取線特征，但LSD算法消耗大量計(jì)算資源，實(shí)時(shí)性差，并且其對(duì)圖像遮擋、局部模糊等情況魯棒性差，容易將一條長(zhǎng)線檢測(cè)為多條短線，加大匹配難度。故本文作者使用ELSED算法，其線段檢測(cè)大致為以下6步：

(1)高斯濾波去除噪聲；

(2)利用Sobel算子計(jì)算像素梯度與方向；

(3)選取局部梯度最大的像素作為錨點(diǎn)像素；

(4)依托錨點(diǎn)同時(shí)進(jìn)行邊緣檢測(cè)與直線擬合；

(5)長(zhǎng)線段斷口檢測(cè)合并；

(6)剔除錯(cuò)誤檢測(cè)線段。

ELSED有普通檢測(cè)和跳躍檢測(cè)2種模式，二者區(qū)別在于跳躍檢測(cè)模式執(zhí)行其算法的第5步，而普通檢測(cè)跳過(guò)該步。由于圖像模糊，圖像像素梯度方向變化或者梯度值轉(zhuǎn)為零，進(jìn)而出現(xiàn)線段邊緣不連續(xù)，導(dǎo)致一條長(zhǎng)線段被檢測(cè)為多條短線。為盡可能檢測(cè)出完整長(zhǎng)線段，算法一旦檢測(cè)到不連續(xù)邊緣，會(huì)跳過(guò)不連續(xù)邊緣并沿線段方向繼續(xù)檢測(cè)線段，在一定像素區(qū)間內(nèi)，若仍可檢測(cè)出線段，則擬合該方向所有線段形成長(zhǎng)線段。

ELSED中大多數(shù)參數(shù)已根據(jù)經(jīng)驗(yàn)設(shè)置，無(wú)需進(jìn)行更改。其檢測(cè)的最短線段長(zhǎng)度設(shè)置為15，較短的線段容易導(dǎo)致誤匹配，連續(xù)跟蹤效果差，過(guò)多的短線往往會(huì)降低點(diǎn)線融合的SLAM系統(tǒng)精度，故對(duì)線段提取的最短長(zhǎng)度Lmin做一些調(diào)整：

Lmin=[α·min(W，H)]

(1)

其中：W和H分別為圖像寬度和高度，取其中較小值為閾值基準(zhǔn)，以適應(yīng)不同尺寸圖像；α是尺度因子，根據(jù)經(jīng)驗(yàn)設(shè)置為0.083；[·]定義為向下取整函數(shù)，如848像素×480像素的圖片，min(W，H)=480像素，Lmin=40像素。

2.2 連續(xù)幀之間的快速線段匹配

現(xiàn)在大多數(shù)基于線特征的視覺(jué)SLAM系統(tǒng)采用LBD線段描述子進(jìn)行線段匹配跟蹤，這種方式在大多數(shù)場(chǎng)景下表現(xiàn)良好，但計(jì)算量相對(duì)較大。通常，相機(jī)在相鄰幀之間運(yùn)動(dòng)量較小，線特征匹配可以不需要描述子完成，本文作者提出一種連續(xù)幀間快速線段匹配算法，算法示意如圖2所示。

圖2 線段匹配算法示意

對(duì)于連續(xù)兩幀圖像I1和I2，將I1中提取的n條線特征存入集合L1={l1，l2，…，li，…，ln}，li表示圖像I1中第i條線段；將I2中提取的m條線特征存入集合L2={l1，l2，…，lj，…，lm}，lj表示圖像I2中第j條線段。R和t分別代表兩圖像之間的相對(duì)旋轉(zhuǎn)與位移。

λ2x2=λ1Rx1+t

(2)

從而有式(3)：

λ2K-1p2=λ1RK-1p1+t

(3)

一般情況下，相機(jī)若不是極其劇烈的抖動(dòng)，其連續(xù)兩幀的相對(duì)位移t基本趨于0，匹配像素對(duì)應(yīng)的深度λ1≈λ2，因此，可將公式(3)簡(jiǎn)化為

p2=KRK-1p1

(4)

其中，文中算法中旋轉(zhuǎn)矩陣R是由ORB-SLAM3跟蹤線程中恒速運(yùn)動(dòng)模型得到的當(dāng)前幀初始位姿，在IMU未初始化時(shí)，通過(guò)上一幀的位姿以及上一幀的速度獲得當(dāng)前幀的位姿初始值，在IMU完成初始化后，當(dāng)前幀的位姿初始值為IMU預(yù)積分得到。

本文作者提出的快速線段匹配算法基于以上假設(shè)完成，具體步驟如表1所示。

表1 快速線段匹配算法

其中，相比于傳統(tǒng)的KLT算法，式(4)給予KLT迭代優(yōu)化一個(gè)更可靠的初值，從而提高關(guān)鍵點(diǎn)跟蹤精度。線段間的匹配距離γij為

(5)

(6)

為防止誤匹配，設(shè)置匹配因子β：

隨著教學(xué)改革的步伐不斷邁進(jìn)，老師在課堂中不但要傳授學(xué)生知識(shí)，還需要把學(xué)生的探索精神與創(chuàng)造能力培養(yǎng)起來(lái)，但是由于受到根深蒂固的傳統(tǒng)教育觀念所影響，很多的老師在教學(xué)過(guò)程中并沒(méi)有把提升學(xué)生的綜合素質(zhì)重視起來(lái)。在教學(xué)的過(guò)程中老師必須改變這種傳統(tǒng)的教學(xué)觀念，充分應(yīng)用新課改背景下的現(xiàn)代化教學(xué)理念，尊重學(xué)生學(xué)習(xí)過(guò)程中的主觀能動(dòng)性，引導(dǎo)每個(gè)學(xué)生都能夠主動(dòng)深入思考，并能夠展現(xiàn)自己的個(gè)性。

(7)

其中：β為最小匹配距離γmin與次小匹配距離γmin2的比值。匹配因子β越小，匹配質(zhì)量越好，若β大于某一閾值，則認(rèn)為該匹配是誤匹配。

3 直線重投影誤差及后端優(yōu)化

3.1 直線重投影誤差

文中線特征的重投影誤差為3D-2D的方式，為非線性誤差，如圖3所示。其中，誤差表示為空間直線投影到圖像平面的直線與匹配線段兩端點(diǎn)s=[us，vs，1]和e=[ue，ve，1]的距離誤差ds和de：

(8)

圖3 線特征重投影誤差

其中：rl為線段重投影誤差；l=[l1，l2，l3]為投影直線方程。

3.2 后端優(yōu)化

(9)

由IMU預(yù)積分可以獲得i、i+1時(shí)刻之間的相對(duì)旋轉(zhuǎn)ΔRi，i+1、相對(duì)速度Δvi，i+1、相對(duì)位置Δpi，i+1和信息矩陣∑Ii，i+1，故慣性殘差可定義為

rIi，i+1=[ΔRi，i+1，Δvi，i+1，Δpi，i+1]

(10)

(11)

其中：Π(?)為相機(jī)重投影函數(shù)；uik表示地圖點(diǎn)在像素平面的觀測(cè)值；TCB∈SE(3)是從body坐標(biāo)系(慣性坐標(biāo)系)的轉(zhuǎn)移矩陣，一般通過(guò)傳感器標(biāo)定獲得；運(yùn)算符⊕表示李群SE(3)在R3上轉(zhuǎn)化操作。

ORB-SLAM3在局部或者全局優(yōu)化中，將視覺(jué)和慣性的聯(lián)合優(yōu)化視作基于關(guān)鍵幀的最小化問(wèn)題，本文作者在其中加入線特征，故視覺(jué)慣性聯(lián)合優(yōu)化問(wèn)題可以寫(xiě)為

(12)

圖4 后端優(yōu)化因子圖

4 實(shí)驗(yàn)及結(jié)果分析

采用地面機(jī)器人OpenLORIS室內(nèi)數(shù)據(jù)集對(duì)提出的線段檢測(cè)與匹配算法效果和SLAM系統(tǒng)運(yùn)行軌跡精度進(jìn)行評(píng)估。

地面機(jī)器人的移動(dòng)軌跡真值由其頭頂攜帶的激光雷達(dá)與動(dòng)態(tài)捕捉系統(tǒng)提供，前置Intel的RealSense D435i攝像頭拍攝單目及深度圖像，并提供IMU信息，如圖5所示。實(shí)驗(yàn)使用CPU為Intel Core i5-11400H@2.7 GHz，內(nèi)存為16 GB，系統(tǒng)版本為Ubuntu 18.04LTS64位。

圖5 地面機(jī)器人

4.1 線特征提取與匹配實(shí)驗(yàn)

為驗(yàn)證ELSED算法的性能，在OpenLORIS數(shù)據(jù)集走廊場(chǎng)景和咖啡館場(chǎng)景上分別與LSD算法進(jìn)行對(duì)比測(cè)試，其中走廊場(chǎng)景(corridor)包括5個(gè)子序列(corridor1-5)，咖啡館場(chǎng)景(cafe)包括2個(gè)子序列(cafe1-2)。統(tǒng)計(jì)線特征提取算法ELSED和LSD平均每幀耗時(shí)、線特征提取數(shù)量及線特征平均長(zhǎng)度，其中ELSED采用跳躍檢測(cè)模式，兩者均剔除了長(zhǎng)度小于Lmin的線段，結(jié)果如表2所示。

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

由表2可以看出：ELSED和LSD兩種算法提取的線段數(shù)量基本一樣，在corridor序列上提取的線段數(shù)量均在140條左右，在cafe系列上提取的線段數(shù)量在130條左右；而ELSED算法耗時(shí)為2.4 ms，僅為L(zhǎng)SD算法的13%左右，大大減少SLAM前端提取線特征時(shí)間；同時(shí)ELSED提取的平均線段長(zhǎng)度為80像素，是LSD提取線段長(zhǎng)度的1.3倍。

線段提取效果如圖6所示，其中圖6(a)原始圖像來(lái)自corridor4系列，文中算法提取效果如圖6(b)所示，LSD算法提取線段效果如圖6(c)所示?？梢暂^為明顯看出，LSD提取某些長(zhǎng)線段時(shí)，易受圖片質(zhì)量的干擾，使得本來(lái)的長(zhǎng)線段被分割為多條短線，而ELSED提取出高質(zhì)量的長(zhǎng)線段，提取時(shí)間短，更有利于系統(tǒng)定位精度提升，使系統(tǒng)更具魯棒性。

圖6 線段提取效果

為定量評(píng)估文中提出的無(wú)描述符的線段匹配算法，使用ELSED檢測(cè)線段，用正確匹配對(duì)、正確匹配率和匹配時(shí)間作為評(píng)價(jià)指標(biāo)。其中正確匹配率定義為正確匹配對(duì)占總匹配對(duì)的比值，將線段重投影誤差小于5像素的定義為正確匹配[18]，其余的定義為誤匹配，分別在OpenLORIS數(shù)據(jù)集的走廊場(chǎng)景和咖啡館場(chǎng)景做測(cè)試，結(jié)果如表3所示。

表3 文中線段匹配算法和LBD線段匹配方法對(duì)比

由表3可以看出：相比于文中算法，使用LBD獲得的正確匹配對(duì)更多，但正確匹配率相對(duì)較低，僅為80%左右，而文中算法的線段正確匹配率均在96%以上。這是因?yàn)槭俏闹械钠ヅ渌惴ê休^為嚴(yán)格的幾何約束，對(duì)于匹配線段外形差別較大的容忍度低，減少線段無(wú)效匹配，提高SLAM系統(tǒng)的精度。其匹配效果如圖7所示。

圖7 線段匹配算法對(duì)比

同時(shí)文中算法的復(fù)雜度為O(n2)，與檢測(cè)線段數(shù)量密切相關(guān)，在相關(guān)數(shù)據(jù)集上完成130條線段的匹配耗時(shí)大致在2.6 ms左右，相比于LBD描述子，時(shí)間效率提高了83%，大大縮短了連續(xù)幀之間的線段匹配時(shí)間，提高SLAM系統(tǒng)的跟蹤速度。

4.2 定位精度實(shí)驗(yàn)

根據(jù)絕對(duì)軌跡誤差(Absolute Trajectory Error，ATE)，即計(jì)算算法得到的相機(jī)位姿與真值之間的均方根誤差(Root Mean Square Error，RMSE)來(lái)評(píng)估算法的定位精度。將ORB-SLAM3和文中算法在OpenLORIS數(shù)據(jù)集上進(jìn)行對(duì)比，結(jié)果如表4所示。可以看出：相比于ORB-SLAM3，除了在cafe2序列上，文中算法定位精度略微下降，但在場(chǎng)景中紋理較少的走廊環(huán)境下，文中算法表現(xiàn)亮眼，尤其是在corridor3序列上，文中算法精度提高87%，在走廊場(chǎng)景下其他序列上，精度均有提高。

表4 多種算法的絕對(duì)軌跡誤差對(duì)比 單位：m Tab.4 Comparison of APE of various algorithms Unit：m

圖8為在corridor3和cafe1序列上，各算法軌跡在xy平面上與軌跡真值的對(duì)比，也可以發(fā)現(xiàn)文中算法軌跡與真值最為貼近，精度最高。圖9(a)為corridor3序列上，各算法實(shí)時(shí)絕對(duì)位姿誤差(Absolute Pose Error，APE)，可以看出文中算法APE波動(dòng)較小，誤差值在較低水平，算法效果穩(wěn)定。圖9(b)為誤差箱形圖，從中位數(shù)和平均數(shù)來(lái)看，文中算法優(yōu)于ORB-SLAM3。圖10分別表示了ORB-SLAM3算法和文中點(diǎn)線特征融合算法的在corridor3場(chǎng)景下地圖構(gòu)建效果。

圖8 算法軌跡與軌跡真值對(duì)比

圖9 實(shí)時(shí)軌跡誤差(a)及誤差箱形圖(b)

圖10 地圖構(gòu)建

4.3 系統(tǒng)耗時(shí)

ORB-SLAM3和文中算法在OpenLORIS數(shù)據(jù)集下各模塊的平均耗時(shí)如表5所示。文中算法在ORB-SLAM3的框架中加入線特征，相比于ORB-SLAM3在特征提取階段，用改進(jìn)的ORB算法和ELSED算法并行提取點(diǎn)特征和線特征，故文中算法在特征提取時(shí)并未增加系統(tǒng)耗時(shí)。在連續(xù)幀跟蹤時(shí)，利用本文作者提出的無(wú)描述符的線段匹配算法跟蹤線特征，耗時(shí)增加2.8 ms。在創(chuàng)作關(guān)鍵幀時(shí)，需要提取LBD線段描述符，為局部建圖線程三角化新地圖線提供關(guān)鍵幀之間的線段匹配，使得耗時(shí)增加。文中算法的跟蹤線程平均耗時(shí)29.82 ms，幀率為33 幀/s，滿(mǎn)足系統(tǒng)的實(shí)時(shí)性要求。

表5 PC平臺(tái)算法耗時(shí)對(duì)比 單位：ms

5 結(jié)束語(yǔ)

提出一種在ORB-SLAM3基礎(chǔ)上加入線特征的點(diǎn)線特征融合的視覺(jué)慣性SLAM算法。為解決傳統(tǒng)線特征提取算法耗時(shí)久和提取線段質(zhì)量低的問(wèn)題，引入ELSED線段提取算法，并進(jìn)行參數(shù)調(diào)整，在短時(shí)間內(nèi)可提取出高質(zhì)量的長(zhǎng)線段。傳統(tǒng)線段匹配方式采用LBD線段描述子，對(duì)大多數(shù)場(chǎng)景魯棒，但同樣存在耗時(shí)過(guò)長(zhǎng)的問(wèn)題。本文作者提出的連續(xù)幀間的快速線段匹配算法，減少線段匹配時(shí)間和線段的誤匹配。在公共的數(shù)據(jù)集上實(shí)驗(yàn)結(jié)果表明：與其他開(kāi)源算法相比，文中算法具有更高的定位精度，且保證了系統(tǒng)的實(shí)時(shí)性。