周 彥，曠鴻章，牟金震，王冬麗，劉宗明

1.湘潭大學(xué) 自動(dòng)化與電子信息學(xué)院，湖南 湘潭411105

2.上海航天控制技術(shù)研究所，上海201109

目前同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)是機(jī)器人、自動(dòng)駕駛、增強(qiáng)現(xiàn)實(shí)等領(lǐng)域的關(guān)鍵技術(shù)之一。定位與地圖構(gòu)建是自主移動(dòng)平臺(tái)導(dǎo)航與控制領(lǐng)域的兩個(gè)基本問(wèn)題，SLAM技術(shù)恰好是同時(shí)解決這兩個(gè)問(wèn)題的最有效解決方案。人們?cè)谏a(chǎn)生活中對(duì)自主移動(dòng)產(chǎn)品與日俱增的需求也使得SLAM技術(shù)成為自主移動(dòng)平臺(tái)領(lǐng)域研究的熱點(diǎn)問(wèn)題。

回溯SLAM技術(shù)的發(fā)展歷程，早期主要研究基于概率估計(jì)的SLAM，例如拓展卡爾曼濾波[1]、粒子濾波[2]和組合濾波[3]。文獻(xiàn)[4]中以擴(kuò)展卡爾曼濾波構(gòu)圖與定位（Extended Kalman Filter-SLAM，EKF-SLAM）為基礎(chǔ)，對(duì)該拓展卡爾曼濾波SLAM 算法進(jìn)行了改進(jìn)以實(shí)現(xiàn)機(jī)器人的定位與地圖創(chuàng)建。之后基于關(guān)鍵幀的單目視覺(jué)SLAM逐漸發(fā)展起來(lái)，自從Klein和Murray在2007年提出了具有開(kāi)創(chuàng)性的并行跟蹤與制圖（Parallel Tracking And Mapping，PTAM）方法[5]以來(lái)，現(xiàn)已經(jīng)有許多研究者提出其他實(shí)時(shí)視覺(jué)SLAM（visual SLAM，vSLAM）方法，包括基于特征點(diǎn)的ORB-SLAM[6]（Oriented FAST and Rotated BRIEF-SLAM）方法，以及基于直接法的大范圍單目定位與制圖[7]（Large-Scale Direct Monocular SLAM，LSD-SLAM）方法和使用RGB-D攝像頭來(lái)同時(shí)定位與制圖的RGBD-SLAM[8]方法。國(guó)內(nèi)研究者在文獻(xiàn)[9]中對(duì)vSLAM在無(wú)人平臺(tái)上的研究進(jìn)展做了深刻地論述。

一個(gè)好的SLAM 算法應(yīng)該具有在復(fù)雜環(huán)境下進(jìn)行準(zhǔn)確處理、算法計(jì)算復(fù)雜度要求盡可能低、實(shí)時(shí)性好、能產(chǎn)生精確的結(jié)果等優(yōu)點(diǎn)，而且要在運(yùn)算條件和傳感器價(jià)格低的條件下盡可能產(chǎn)生稠密的、細(xì)節(jié)豐富的地圖。Mur-Artal 等于2015 年提出的ORB-SLAM 方法是目前最先進(jìn)的算法之一。ORB-SLAM具有遠(yuǎn)超其他方案的穩(wěn)健性，當(dāng)然也存在一些不足之處，如構(gòu)建的稀疏特征點(diǎn)地圖只能滿足對(duì)定位的需求，而無(wú)法提供機(jī)器人導(dǎo)航、避障和交互等諸多功能。為了盡可能實(shí)現(xiàn)這些優(yōu)點(diǎn)，本文將詳細(xì)論述在多個(gè)算法基礎(chǔ)上重新設(shè)計(jì)的一個(gè)新算法，此算法擁有上述多種優(yōu)點(diǎn)，在多個(gè)數(shù)據(jù)集上取得了很不錯(cuò)的實(shí)驗(yàn)效果。

圖1 改進(jìn)ORB-SLAM方法的框架

1 回顧單目ORB-SLAM方法

單目ORB-SLAM方法能從由單目攝像頭獲取到的連續(xù)視頻中估計(jì)關(guān)鍵幀的位姿以及由這些關(guān)鍵幀計(jì)算得到大量的地圖點(diǎn)。該基于特征的SLAM 方法選擇了ORB 特征進(jìn)行精確跟蹤與構(gòu)建精簡(jiǎn)的地圖，同時(shí)使用基于DBoW2[10]的詞袋向量模型去服務(wù)于全局重定位和閉環(huán)檢測(cè)。

ORB-SLAM方法框架由跟蹤線程（Tracking）、局部建圖線程（Local Mapping）和回環(huán)檢測(cè)線程（Loop Closing）組成。跟蹤線程負(fù)責(zé)對(duì)每幀圖像的相機(jī)位姿進(jìn)行定位，并決定何時(shí)插入新的關(guān)鍵幀。局部建圖線程負(fù)責(zé)處理新的關(guān)鍵幀，并對(duì)新插入的關(guān)鍵幀周?chē)年P(guān)鍵幀對(duì)應(yīng)的相機(jī)位姿進(jìn)行局部捆集約束（BA）[11]，同時(shí)也負(fù)責(zé)剔除冗余關(guān)鍵幀?；丨h(huán)檢測(cè)線程判斷新插入的關(guān)鍵幀是否使得所有關(guān)鍵幀形成閉環(huán)，如果閉環(huán)被檢測(cè)到則進(jìn)一步計(jì)算累積誤差并減小累積漂移量。

2 改進(jìn)ORB-SLAM方法的框架

本文在ORB-SLAM 算法上進(jìn)行改進(jìn)，使其能對(duì)場(chǎng)景進(jìn)行半稠密地圖重建。如圖1中各個(gè)階段所示，圖中紅色與綠色虛線框出的區(qū)域?yàn)樵瓉?lái)的單目ORB-SLAM方法部分，圖中藍(lán)色與綠色虛線框出的區(qū)域?yàn)樵贠RBSLAM 方法的基礎(chǔ)上新增的三個(gè)部分。本文方法由如下四部分構(gòu)成。

（1）原來(lái)的單目ORB-SLAM方法：獲得關(guān)鍵幀信息與一系列地圖點(diǎn)。

（2）關(guān)鍵幀再剔除算法（KRC）：將上一階段得到的關(guān)鍵幀再剔除一遍。

（3）直線段提取算法：提取剔除冗余關(guān)鍵幀后的關(guān)鍵幀中的直線段。

（4）三維直線段重建算法：通過(guò)匹配各個(gè)關(guān)鍵幀中提取到的直線段來(lái)重建場(chǎng)景的三維模型。

3 基于ORB-SLAM方法三維重建

3.1 關(guān)鍵幀再剔除

首先由ORB-SLAM方法實(shí)時(shí)提供關(guān)鍵幀信息和計(jì)算得到的大量地圖點(diǎn)。通過(guò)分析與實(shí)驗(yàn)，如果直接使用這些關(guān)鍵幀將導(dǎo)致計(jì)算復(fù)雜度大、不確定性高和信息冗余等問(wèn)題。本文在文獻(xiàn)[12]中的關(guān)鍵幀選擇算法的基礎(chǔ)上設(shè)計(jì)了一種使用幀間平移向量和旋轉(zhuǎn)向量的范數(shù)和（運(yùn)動(dòng)度）作為參考值來(lái)再次裁剪關(guān)鍵幀的算法，將其稱(chēng)為關(guān)鍵幀再剔除算法。通過(guò)計(jì)算幀間運(yùn)動(dòng)度來(lái)考慮是否將該關(guān)鍵幀用于后續(xù)三維重建線程。

相機(jī)運(yùn)動(dòng)是一個(gè)剛體運(yùn)動(dòng)，在三維空間坐標(biāo)系下可用旋轉(zhuǎn)矩陣R和平移向量t描述相機(jī)的旋轉(zhuǎn)與平移。旋轉(zhuǎn)矩陣R可由歐式空間中的一個(gè)四元數(shù)q表示，式（1）中θ代表相機(jī)繞單位向量n旋轉(zhuǎn)的角度。

如圖2 所示，單位四元數(shù)q1將向量p0旋轉(zhuǎn)到p1，q2將向量p0旋轉(zhuǎn)到p2。p0對(duì)應(yīng)的c0是相機(jī)的初始幀光心，p1和p2對(duì)應(yīng)的c1和c2是兩個(gè)旋轉(zhuǎn)后的相機(jī)的光心。c0到c1的平移向量是t1,c0到c2的平移向量是t2,c1到c2的平移向量是Δt=t2-t1，Δt即為兩關(guān)鍵幀之間的平移向量。由公式推導(dǎo)單位四元數(shù)把p1旋轉(zhuǎn)到p2,q2q1-1定義如式（2），式中‖nx,ny,nz‖=1，角度θ=2×arccosω∈(0,2)，表示為兩個(gè)關(guān)鍵幀之間的旋轉(zhuǎn)角度。

圖2 單目相機(jī)運(yùn)動(dòng)關(guān)系示意圖

KRC 算法的思想是使用當(dāng)前幀相對(duì)于參考幀的平移量Δt和旋轉(zhuǎn)角度θ的范數(shù)和（運(yùn)動(dòng)度）來(lái)度量攝像機(jī)的運(yùn)動(dòng)程度。如公式（3）定義，max_norm為相機(jī)最小運(yùn)動(dòng)閾值，max_norm為相機(jī)的最大運(yùn)動(dòng)度閾值，只有在當(dāng)前幀相對(duì)參考幀的運(yùn)動(dòng)度處于上面兩閾值之間才把該幀視為關(guān)鍵幀，否則直接剔除該幀。在本文中根據(jù)實(shí)際經(jīng)驗(yàn)，min_norm和max_norm分別取值0.4和5。

3.2 直線段提取

本文使用Topal等人提出的實(shí)時(shí)直線段檢測(cè)（EDLines）方法[13]來(lái)檢測(cè)KRC 算法剔除掉冗余幀后的所有關(guān)鍵幀的直線段。EDLines由邊緣描繪（ED）算法[14]、直線段提取以及直線段驗(yàn)證三個(gè)步驟構(gòu)成。接下來(lái)簡(jiǎn)單描述這幾個(gè)步驟。

整個(gè)LS 提取算法包括三個(gè)階段：首先輸入灰度圖像，使用邊緣檢測(cè)算法產(chǎn)生一組干凈的、連續(xù)的邊緣圖。第二步，利用直線段擬合方法從邊緣圖中提取直線段。最后，遵循赫姆霍茲原理來(lái)驗(yàn)證直線段。

3.3 三維直線段場(chǎng)景重建

ORB-SLAM方法能持續(xù)地給后續(xù)進(jìn)程輸入關(guān)鍵幀信息，記做St幀關(guān)鍵幀K。同時(shí)獲取了當(dāng)前地圖中的地圖點(diǎn)Mt和關(guān)鍵幀的位姿信息Tt。在經(jīng)過(guò)3.1 節(jié)的KRC算法后，被剔除去冗余幀，保留下來(lái)的關(guān)鍵幀為Kt。

整個(gè)三維重建方法流程是一個(gè)大的循環(huán)過(guò)程，假設(shè)在t1、t2時(shí)刻獲得的關(guān)鍵幀Kt1與Kt2。由關(guān)鍵幀的性質(zhì)可知Kt1與Kt2有很大一部分是相同的，所以3.2 節(jié)的直線段提取算法只需對(duì)新增關(guān)鍵幀Knew={Kn1,Kn2,…,Kni} (Knew?Kt2但Knew?Kt1) 進(jìn) 行 直 線 段 提取。將Knew中每一幀輸入到直線段提取算法進(jìn)行直線段檢測(cè)。算法運(yùn)行至此處時(shí)就擁有Kt2幀內(nèi)的所有直線段、地圖點(diǎn)Mt和位姿信息Tt。將以上信息輸入到由Hofer 等人提出的基于直線段提取有效三維場(chǎng)景（Line3D++）算法[15]，就能重建得到一系列精確的三維直線數(shù)據(jù)。下面簡(jiǎn)單介紹該三維重建算法的步驟，具體原理請(qǐng)參閱文獻(xiàn)[15]中算法。

（1）建立不同圖像線段之間的潛在對(duì)應(yīng)關(guān)系。

（2）基于相鄰視圖的支持度來(lái)評(píng)估這些對(duì)應(yīng)關(guān)系。

（3）為每個(gè)二維直線段選擇最合理的對(duì)應(yīng)結(jié)果作為其3D位置假設(shè)。

（4）基于三維空間鄰近關(guān)系對(duì)二維線段進(jìn)行聚類(lèi)。

（5）使用組合束調(diào)整（BA）優(yōu)化結(jié)果[可選]。

表1 KRC算法參與與否對(duì)改進(jìn)ORB-SLAM算法性能影響對(duì)比

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

為了評(píng)估新算法的性能，接下將使用TUM RGB-D數(shù)據(jù)集基準(zhǔn)[16]將本文新提出的改進(jìn)ORB-SLAM方法與當(dāng)前最先進(jìn)的幾個(gè)SLAM方法進(jìn)行比較，包括PTAM算法[5]、LSD-SLAM[7]和RGBD-SLAM[8]。該數(shù)據(jù)集與基準(zhǔn)可支持對(duì)算法的科學(xué)評(píng)價(jià)和客觀比較。通過(guò)實(shí)驗(yàn)即可記錄在統(tǒng)一基準(zhǔn)下本文方法與其他方法關(guān)鍵幀定位軌跡的絕對(duì)誤差（ATE）。在實(shí)驗(yàn)中，本文提出的方法不使用TUM RGB-D數(shù)據(jù)集的深度通道的信息。

測(cè)試條件為一臺(tái)普通筆記本電腦，配備Intel?CoreTMi7-3520 CPU @ 2.9 GHz，8 GB內(nèi)存與Ubuntu 14.04操作系統(tǒng)，本文提出的改進(jìn)算法由C++編寫(xiě)實(shí)現(xiàn)，同時(shí)盡可能多地使用了計(jì)算機(jī)的并行計(jì)算能力。如在三維直線段重構(gòu)階段，就多次使用到OpenMP這一種共享內(nèi)存并行系統(tǒng)的多處理器程序設(shè)計(jì)方案。而且原來(lái)的ORBSLAM 部分與本文新增部分也都采用了線程并行計(jì)算方式運(yùn)行。

為了證明本文提出方法性能的良好一致性，且能重建效果出色的半稠密三維場(chǎng)景，實(shí)驗(yàn)選取三個(gè)主流的測(cè)試視頻序列：TUM RGB-D 數(shù)據(jù)集中的fr2_desk、fr3_nstr_tex_near 和fr3_str_tex_near,并與LSD-SLAM運(yùn)行結(jié)果進(jìn)行比較，幾個(gè)有代表性序列的測(cè)試結(jié)果展示在圖3中，本文方法和LSD-SLAM方法都能構(gòu)建場(chǎng)景的半稠密地圖，本文的方法半稠密場(chǎng)景由直線段重建，LSD-SLAM 方法則由點(diǎn)云構(gòu)成，而單目ORB-SLAM 方法構(gòu)建的地圖為稀疏的點(diǎn)云地圖。實(shí)驗(yàn)表明該改進(jìn)的方法能產(chǎn)生由直線段重建的高質(zhì)量地圖，跟蹤建圖的穩(wěn)健性能比LSD-SLAM方法要好。

圖3 三維重建效果對(duì)比

為了分析本文提出的KRC算法對(duì)改進(jìn)ORB-SLAM算法性能影響，將KRC 算法參與與否對(duì)整個(gè)方法性能的對(duì)比數(shù)據(jù)記錄于表1中。從表中可見(jiàn)，在運(yùn)行同一個(gè)序列的情況下，不執(zhí)行KRC 算法時(shí)會(huì)減小整個(gè)方法三維重建的循環(huán)次數(shù)，但是執(zhí)行KRC算法后，關(guān)鍵幀數(shù)目會(huì)大量減小。另外，本文提出的改進(jìn)的ORB-SLAM 方法在不執(zhí)行KRC 算法時(shí)運(yùn)行一遍約需要166 s，而執(zhí)行KRC 算法時(shí)約需要173 s。同時(shí)注意到不執(zhí)行KRC 算法，會(huì)使得單次循環(huán)處理的視頻幀數(shù)量變得很大，致使單次直線段匹配過(guò)程十分耗時(shí)。KRC算法參與與否對(duì)改進(jìn)ORB-SLAM算法在同一數(shù)據(jù)集上共計(jì)運(yùn)行時(shí)長(zhǎng)相差不大，但重建的循環(huán)次數(shù)在執(zhí)行KRC算法時(shí)增大，從而能使得改進(jìn)算法的每次循環(huán)時(shí)間縮短，進(jìn)而加快了三維地圖重建的刷新頻率。

如表2所示，對(duì)本文提出的方法的關(guān)鍵幀定位精度進(jìn)行評(píng)估。由于程序進(jìn)程某些階段的隨機(jī)性，例如初始化、位置優(yōu)化和直線段重建等，所有實(shí)驗(yàn)運(yùn)行11 次，然后報(bào)告記錄實(shí)驗(yàn)結(jié)果的中位數(shù)。表中顯示了執(zhí)行5 個(gè)測(cè)試序列的關(guān)鍵幀軌跡的均方根誤差（RMSE）中位數(shù)。請(qǐng)注意，本文提出的方法在大多數(shù)序列中不斷提高了LSD-SLAM的軌跡精度。除了1個(gè)序列外，其他在所有序列中都能產(chǎn)生最佳結(jié)果，PTAM在這方面的表現(xiàn)稍好一些。然而，PTAM變得不那么可靠，因?yàn)槠渲?個(gè)測(cè)試序列失去了跟蹤。而且LSD-SLAM 也在1 個(gè)測(cè)試中丟失位姿。實(shí)驗(yàn)結(jié)果表明本文提出的方法的魯棒性要強(qiáng)于其他三種SLAM方法。

表2 關(guān)鍵幀絕對(duì)定位精度對(duì)比cm

為了評(píng)估本文所提出的方法的實(shí)時(shí)性能，表3中給出了改進(jìn)的ORB-SLAM 方法與原來(lái)的ORB-SLAM 方法實(shí)驗(yàn)條件下的運(yùn)行時(shí)長(zhǎng)對(duì)比和數(shù)據(jù)集拍攝時(shí)長(zhǎng)數(shù)據(jù)。從表中可以看出本文改進(jìn)的方法在5 個(gè)數(shù)據(jù)集上運(yùn)行時(shí)間都較原來(lái)的ORB-SLAM 方法要長(zhǎng)，這主要是改進(jìn)的方法不僅運(yùn)行了基于特征點(diǎn)的ORB-SLAM 方法，而且還在不降低定位精度的前提下構(gòu)建了半稠密的三維場(chǎng)景地圖。受限于計(jì)算條件，如普通的筆記本電腦的低計(jì)算能力、沒(méi)有使用圖形處理器（GPU）算力等條件，雖然本文方法的運(yùn)行時(shí)間對(duì)比各個(gè)序列的視頻采集時(shí)間都有一定的延長(zhǎng)，但是在較短的時(shí)間內(nèi)構(gòu)建的場(chǎng)景包含了豐富的直線段信息，而原來(lái)的單目ORB-SLAM方法只能構(gòu)建稀疏的點(diǎn)云地圖。

表3 實(shí)時(shí)運(yùn)行時(shí)長(zhǎng)對(duì)比s

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

本文提出一種用于半稠密三維重建的改進(jìn)單目ORB-SLAM 算法。實(shí)驗(yàn)表明該算法能生成精確的、定位良好的直線段，且能高效地匹配和聚類(lèi)直線段來(lái)進(jìn)行半稠密三維場(chǎng)景重建。為了方便更好地解釋該改進(jìn)的單目ORB-SLAM 方法的工作原理，讀者可以在GitHub項(xiàng)目托管平臺(tái)上下載本文提出的SLAM 方法的C++代碼。開(kāi)源項(xiàng)目網(wǎng)址為https：//github.com/BTREE-C802/3DLine-SLAM。

目前，改進(jìn)的方法只能對(duì)二維直線段進(jìn)行三維重建。未來(lái)，計(jì)劃持續(xù)改進(jìn)該方法使其能對(duì)圓弧進(jìn)行三維重建，因?yàn)槭褂弥本€段與圓弧的組合幾乎可以構(gòu)造出所有常見(jiàn)事物的三維結(jié)構(gòu)。此外，對(duì)于局部特定目標(biāo)的三維重建將是今后的一個(gè)研究方向。