復(fù)雜交通環(huán)境下基于關(guān)鍵目標(biāo)的視覺SLAM

連 靜，皮家豪，李琳輝

(1.大連理工大學(xué) 汽車工程學(xué)院， 遼寧 大連 116024； 2.大連理工大學(xué) 工業(yè)裝備結(jié)構(gòu)分析國家重點(diǎn)實驗室， 遼寧 大連 116024)

0 引言

實時定位是實現(xiàn)智能汽車自主導(dǎo)航的重要前提，而在GPS距止環(huán)境下，SLAM(simultaneous localization and mapping，同時定位及地圖構(gòu)建)技術(shù)作為實現(xiàn)實時定位的重要技術(shù)[1]，是智能駕駛行業(yè)的研究熱點(diǎn)。根據(jù)傳感器的不同，SLAM主要包括視覺SLAM[2]和激光SLAM[3]，而視覺傳感器由于具有價格低、獲得信息豐富等優(yōu)點(diǎn)，成為學(xué)者們關(guān)注的重點(diǎn)。

根據(jù)前端的不同，視覺SLAM可以分為以SVO[4]、DSO[5]為代表的直接法和以O(shè)RB-SLAM3[6]為代表的特征法。特征法利用點(diǎn)特征的重投影誤差進(jìn)行位姿計算。相比基于光度誤差的直接法，特征法在光照變化、相機(jī)劇烈運(yùn)動等場景下的魯棒性更好。特征點(diǎn)法依賴于場景中特征點(diǎn)的提取，在動態(tài)特征過多和近處紋理匱乏等復(fù)雜場景下會出現(xiàn)定位精度下降甚至定位失效的情況。

為提高動態(tài)場景中的定位算法的性能，面向動態(tài)環(huán)境的視覺SLAM算法相繼被提出，如Berta[7]提出的DynaSLAM在ORB-SLAM2[8]基礎(chǔ)上融合了由Mask-RCNN[9]構(gòu)成的語義模塊，對潛在的運(yùn)動物體區(qū)域進(jìn)行分割，并結(jié)合區(qū)域增長算法對動態(tài)特征點(diǎn)進(jìn)行剔除。Yu等[10]提出的DS-SLAM則是通過SegNet[11]語義分割網(wǎng)絡(luò)進(jìn)行動態(tài)物體的識別，在去除環(huán)境中運(yùn)動物體的影響的同時，構(gòu)建八叉樹語義地圖。Han等[12]提出的MLC-SLAM在視覺詞匯表中引入語義信息進(jìn)行訓(xùn)練，而后通過定義靜態(tài)速率的方式處理動態(tài)物體。盡管上述算法在大多數(shù)室外交通場景中表現(xiàn)優(yōu)異，但是它們在一些近處紋理缺失和動態(tài)車輛同時存在的場景容易出現(xiàn)定位失效的問題。

針對復(fù)雜場景的另一種解決方法則是利用更高級的語義信息，在參數(shù)化語義目標(biāo)后，通過語義目標(biāo)之間的關(guān)聯(lián)進(jìn)行位姿求解，如Yang等[13]提出的CubeSLAM，利用目標(biāo)檢測出來的2D檢測框，通過消失點(diǎn)采樣生成3D立方體，繼而基于立方體之間的關(guān)聯(lián)進(jìn)行位姿的求解和優(yōu)化；Nicholson等[14]提出的QuadricSLAM通過目標(biāo)檢測后的邊界框?qū)⑽矬w表達(dá)成橢球體，通過橢球體的重投影誤差求解位姿，Tian等[15]進(jìn)一步完善了橢球體的初始化和橢球體之間的數(shù)據(jù)關(guān)聯(lián)問題，提高了物體級SLAM系統(tǒng)的魯棒性。但是它們在交通場景中主要是對靜態(tài)和動態(tài)車輛進(jìn)行參數(shù)化，在只有動態(tài)車輛或者沒有車輛的場景下效果并不顯著。Li等[16]提出的TextSLAM，通過文本檢測提取出文本信息，通過融合文本信息之間的光度誤差和文本內(nèi)特征點(diǎn)的幾何誤差計算位姿，提高了定位算法在商場和室內(nèi)環(huán)境下的定位精度和穩(wěn)定性。還有學(xué)者通過結(jié)合道路結(jié)構(gòu)化特征[17]，提高有效特征數(shù)量進(jìn)而提高算法在近景特征不足場景的定位精度。但是該方法在路面特征被運(yùn)動目標(biāo)遮擋嚴(yán)重的場景下效果并不顯著。

針對上述問題，提出一種基于關(guān)鍵目標(biāo)的視覺SLAM算法。通過提取交通場景中普遍存在的交通標(biāo)志和路牌等靜止目標(biāo)并在內(nèi)部進(jìn)一步完成特征的提取，經(jīng)過篩選后構(gòu)建出初始的關(guān)鍵目標(biāo)。然后，在相連幀之間通過幾何關(guān)系和特征之間的匹配完成關(guān)鍵目標(biāo)的匹配，并在此基礎(chǔ)上完成系統(tǒng)的初始化和關(guān)鍵目標(biāo)的跟蹤。最后，基于關(guān)鍵目標(biāo)求解相機(jī)位姿，并在局部地圖中對二者聯(lián)合優(yōu)化，以提高算法在復(fù)雜環(huán)境下的定位穩(wěn)定性。

1 基于關(guān)鍵目標(biāo)的SLAM算法框架

本文算法的整體流程如圖1所示，主要包括跟蹤、關(guān)鍵目標(biāo)管理和局部建圖3部分。跟蹤部分的流程是在圖像輸入后，對圖像進(jìn)行靜止目標(biāo)檢測，繼而在靜止目標(biāo)范圍內(nèi)提取特征點(diǎn)。如果SLAM系統(tǒng)還沒有初始化或者初始化失敗，則嘗試進(jìn)行地圖的初始化，在初始化成功后則進(jìn)行基于關(guān)鍵目標(biāo)的跟蹤和位姿估計；關(guān)鍵目標(biāo)管理部分主要是控制關(guān)鍵目標(biāo)的生成以及關(guān)鍵目標(biāo)內(nèi)部特征點(diǎn)的生成和更新；局部建圖部分則是控制關(guān)鍵目標(biāo)在局部地圖中的插入、更新和剔除，以及對相機(jī)位姿和關(guān)鍵目標(biāo)特征的整體優(yōu)化。

圖1 算法整體流程框圖

1.1 關(guān)鍵目標(biāo)提取

在交通環(huán)境中分布著豐富的靜止目標(biāo)，如交通指示燈、交通標(biāo)志等。它們不僅能提供車輛周圍的環(huán)境信息，為智能車輛的行駛提供方向上的指引，還能給車輛的定位系統(tǒng)提供穩(wěn)定的參考。因為它們的安裝位置較高，遍布在道路中央和兩旁，不容易被前方車輛遮擋，容易識別，具有鮮明的標(biāo)識作用。同時它們紋理豐富，適合特征的提取。

得益于深度學(xué)習(xí)在近年來的迅速發(fā)展，對交通場景中靜止目標(biāo)的檢測方案也不斷被提出[18-20]。如圖2所示，交通場景中的交通燈、交通標(biāo)志和路牌分別被目標(biāo)檢測網(wǎng)絡(luò)檢出。

圖2 靜止目標(biāo)檢測結(jié)果

當(dāng)靜止目標(biāo)檢測完成后，在目標(biāo)區(qū)域內(nèi)提取ORB[21]特征，特征點(diǎn)提取的數(shù)量根據(jù)靜止目標(biāo)區(qū)域在圖像上的占比確定。為了避免特征點(diǎn)出現(xiàn)過度扎堆的現(xiàn)象，采用ORB-SLAM3[6]中的四叉樹方法均勻化特征點(diǎn)，特征提取結(jié)果如圖3所示。廣告牌在圖像中占比較大，因此廣告牌上特征點(diǎn)分布更多。接著，通過靜止目標(biāo)中特征點(diǎn)數(shù)目篩選關(guān)鍵目標(biāo)，確保篩選出的關(guān)鍵目標(biāo)有足夠的特征用于后續(xù)基于特征的匹配和基于關(guān)鍵物之間的跟蹤。至此，完成關(guān)鍵目標(biāo)的初步構(gòu)建，并根據(jù)其檢測類別和檢測順序?qū)ζ浞诸惡途幪枴?/p>

圖3 靜止目標(biāo)特征提取

1.2 系統(tǒng)初始化

在完成關(guān)鍵目標(biāo)的構(gòu)建且關(guān)鍵目標(biāo)的數(shù)量大于初始化閾值后，則將當(dāng)前幀設(shè)置為待初始化幀，開始嘗試初始化過程。當(dāng)連續(xù)兩幀都滿足初始化的基本條件時，對兩幀之間的關(guān)鍵目標(biāo)進(jìn)行匹配并實施初始化，具體的過程如下：

1) 關(guān)鍵目標(biāo)匹配。以關(guān)鍵目標(biāo)區(qū)域的中心為圓心，設(shè)置關(guān)鍵目標(biāo)的匹配半徑，根據(jù)關(guān)鍵目標(biāo)類別在搜索范圍內(nèi)確定候選關(guān)鍵目標(biāo)，通過關(guān)鍵目標(biāo)的幾何信息篩選候選關(guān)鍵目標(biāo)。篩選條件如式(1)和式(2)所示。

(1)

(2)

篩選條件中對于關(guān)鍵目標(biāo)高度的變化要求更嚴(yán)格。因為智能車輛在道路上的行駛可以近似為平面運(yùn)動，只存在直行或轉(zhuǎn)彎的情況，相較于偏航角度的變化，俯仰角度并不會出現(xiàn)較大幅度的變化。

當(dāng)多個關(guān)鍵目標(biāo)區(qū)域滿足幾何篩選條件時，通過內(nèi)部特征關(guān)聯(lián)完成最后的篩選。篩選方式是對待匹配目標(biāo)區(qū)域與篩選目標(biāo)區(qū)域的特征點(diǎn)依次進(jìn)行匹配，并記錄匹配成功的特征點(diǎn)數(shù)目，特征點(diǎn)匹配數(shù)目最多且大于閾值條件的即視為匹配成功的關(guān)鍵目標(biāo)區(qū)域，否則匹配失敗，并將該關(guān)鍵目標(biāo)從當(dāng)前跟蹤的局部地圖中剔除。關(guān)鍵目標(biāo)的匹配結(jié)果如圖4所示，圖中連線兩端對應(yīng)著關(guān)鍵目標(biāo)內(nèi)匹配成功的特征點(diǎn)。

圖4 關(guān)鍵目標(biāo)匹配

2) 初始化條件判定。在關(guān)鍵目標(biāo)匹配步驟完成后，根據(jù)匹配成功的關(guān)鍵目標(biāo)數(shù)量判定是否滿足初始化條件，滿足閾值條件則開始初始化位姿計算，否則初始化失敗。

3) 初始化特征點(diǎn)對選擇。單目初始化過程中使用RANSAC[22]算法隨機(jī)選擇特征點(diǎn)，篩選過程與ORB-SLAM3[6]不同，分兩步進(jìn)行。首先確定關(guān)鍵目標(biāo)內(nèi)待選擇特征點(diǎn)數(shù)量，當(dāng)關(guān)鍵目標(biāo)數(shù)量不滿足8個時，則根據(jù)關(guān)鍵目標(biāo)內(nèi)匹配成功的特征點(diǎn)個數(shù)進(jìn)行排序，排序靠前的選擇更多的特征點(diǎn)參與初始化位姿計算，同時也保證每個關(guān)鍵目標(biāo)都有特征點(diǎn)參與初始化計算。然后在特定關(guān)鍵目標(biāo)內(nèi)按照指定特征點(diǎn)數(shù)目隨機(jī)選擇特征點(diǎn)。

4) 關(guān)鍵目標(biāo)和特征點(diǎn)三角化。根據(jù)特征點(diǎn)三角化的比例判斷初始化是否成功以及關(guān)鍵目標(biāo)區(qū)域三角化是否成功，如果初始化成功但是關(guān)鍵目標(biāo)區(qū)域三角化失敗，則剔除對應(yīng)的關(guān)鍵目標(biāo)區(qū)域并刪除其包含的所有三角化路標(biāo)點(diǎn)。

5) 初始地圖創(chuàng)建。當(dāng)關(guān)鍵目標(biāo)區(qū)域三角化成功后，將關(guān)鍵目標(biāo)中心的三維坐標(biāo)和對應(yīng)的路標(biāo)點(diǎn)坐標(biāo)插入到地圖中，完成初始的地圖構(gòu)建，同時為后續(xù)關(guān)鍵目標(biāo)的跟蹤和相機(jī)位姿計算建立基礎(chǔ)。

1.3 關(guān)鍵目標(biāo)跟蹤

借鑒ORB-SLAM3[6]的方式，采用恒速模型計算出關(guān)鍵目標(biāo)中心在當(dāng)前幀的像素坐標(biāo)，并基于此進(jìn)行跟蹤。

(3)

計算得到關(guān)鍵目標(biāo)中心點(diǎn)位置后，設(shè)置搜索半徑，在目標(biāo)區(qū)域內(nèi)搜索對應(yīng)的候選關(guān)鍵目標(biāo)區(qū)域，通過目標(biāo)類別和目標(biāo)區(qū)域的幾何信息進(jìn)行篩選，篩選方式與1.2節(jié)方法相似。

在關(guān)鍵目標(biāo)和關(guān)鍵目標(biāo)的匹配過程中，對其內(nèi)部特征點(diǎn)匹配前，先確定待匹配點(diǎn)在當(dāng)前幀中的搜索中心，該中心通過特征點(diǎn)在關(guān)鍵目標(biāo)區(qū)域內(nèi)的相對位置確定。在指定半徑內(nèi)搜索特征點(diǎn)，然后依據(jù)BRIEF描述子之間的漢明距離篩選相似度最高的特征點(diǎn)。接著根據(jù)旋轉(zhuǎn)一致性檢驗，剔除在方向角度上不符合角度分布規(guī)律的特征點(diǎn)，完成關(guān)鍵目標(biāo)內(nèi)特征點(diǎn)的最終匹配，并依次記錄各候選區(qū)域匹配成功的特征點(diǎn)數(shù)量。特征數(shù)量最多且大于匹配閾值的候選關(guān)鍵目標(biāo)即為匹配成功的關(guān)鍵目標(biāo)。否則，匹配失敗并剔除局部地圖中對應(yīng)的關(guān)鍵目標(biāo)。

1.4 位姿求解

完成關(guān)鍵目標(biāo)之間的數(shù)據(jù)關(guān)聯(lián)后，基于關(guān)鍵目標(biāo)內(nèi)的特征點(diǎn)和關(guān)鍵目標(biāo)中心坐標(biāo)估計相機(jī)位姿。該估計通過最小化下列的代價函數(shù)實現(xiàn)。

(4)

(5)

式中：ρ代表Huber魯棒核函數(shù)；Pij代表第i個關(guān)鍵目標(biāo)內(nèi)第j個特征對應(yīng)的三維坐標(biāo)。

1.5 局部BA

通過關(guān)鍵目標(biāo)的數(shù)量和新關(guān)鍵目標(biāo)的比例篩選當(dāng)前圖像幀，并完成關(guān)鍵幀的創(chuàng)建，然后在局部建圖線程中，對關(guān)鍵幀位姿及關(guān)鍵目標(biāo)進(jìn)行聯(lián)合優(yōu)化，優(yōu)化方法選擇魯棒的LM(Levenberg-Marquardt，列文伯格-馬夸爾特)方法。

(6)

式中：x為局部優(yōu)化窗口中所有關(guān)鍵幀位姿，關(guān)鍵目標(biāo)中心坐標(biāo)和關(guān)鍵目標(biāo)內(nèi)特征點(diǎn)的三維坐標(biāo)。

完成局部BA后，對相應(yīng)的關(guān)鍵幀位姿、關(guān)鍵目標(biāo)中心坐標(biāo)和關(guān)鍵目標(biāo)內(nèi)地圖點(diǎn)進(jìn)行更新。更新結(jié)果用于最新的關(guān)鍵目標(biāo)跟蹤和位姿計算，并等待下次局部BA的開始。

2 實驗

2.1 數(shù)據(jù)集

選擇ApolloScape[23]的自定位數(shù)據(jù)集作為算法測試的數(shù)據(jù)集。該數(shù)據(jù)集記錄了北京、廣州和成都等地的城市交通場景，并且每一個區(qū)域都在不同的天氣以及光照條件下進(jìn)行重復(fù)掃描，因此數(shù)據(jù)集的場景復(fù)雜多樣，更具有挑戰(zhàn)性。自定位數(shù)據(jù)集中對于記錄的每個片段都提供了相應(yīng)的軌跡真值，便于實驗部分的定位誤差的定量比較。

2.2 實驗結(jié)果

為了驗證算法的有效性，選擇Road11中具有挑戰(zhàn)性的Record001序列場景和Road12中動態(tài)物體遮擋嚴(yán)重的Record001序列場景進(jìn)行驗證。場景如圖5所示。場景a中，車輛剛從高樓和樹木環(huán)立的陰影中駛出，由于外部強(qiáng)烈的光照和地面的反光，近處地面上的紋理幾乎沒有。近處的樹葉由于角度的變化其輪廓也會跟隨改變，沒有區(qū)分度，不適合特征的提取和匹配。稍遠(yuǎn)處則是正在橫向行駛的車輛，馬路對面則是商鋪以及建筑等。遠(yuǎn)處可以看見清晰的交通信號燈，以及廣告牌等標(biāo)志物。場景b中，車輛正處于轉(zhuǎn)彎路口，前方都是動態(tài)的車輛，路面遮擋嚴(yán)重，不適合路面特征的提取。對面灰色建筑上分布著重復(fù)紋理，不利于特征點(diǎn)之間準(zhǔn)確的匹配。

圖5 Apollo數(shù)據(jù)集中的復(fù)雜場景

在圖5所示的典型場景中，當(dāng)前主流的視覺SLAM算法——ORB-SLAM3[6]和DynaSLAM[7]都出現(xiàn)了定位失效的問題，而本文算法則成功完成了場景的跟蹤，算法軌跡和真實軌跡的對比如圖6所示。

圖6 平面軌跡

從圖6(a)和圖6(b)可以看出，本文算法的整體軌跡與真實軌跡基本一致。其中圖6(a)中的軌跡只在中間部分和真實軌跡有些許偏差，圖6(b)中的軌跡則是在彎道中央處的弧度要稍大于真實軌跡，但并不妨礙整體的定位精度。因此，所提算法通過關(guān)鍵目標(biāo)能有效地完成復(fù)雜環(huán)境下的定位任務(wù)。使用絕對軌跡誤差作為SLAM算法定位精確性的評價指標(biāo)。從誤差最大值、誤差均值和均方根誤差等方面進(jìn)行評價，結(jié)果如表1所示。

表1 絕對軌跡誤差 m

表1中的ORB-SLAM3是當(dāng)前最先進(jìn)的特征點(diǎn)法SLAM，DynaSLAM則是面向動態(tài)環(huán)境的先進(jìn)SLAM算法，“x”表示對應(yīng)算法在該場景中定位失效，無法獲得軌跡誤差。本文算法在避免定位失效的同時，在場景a中軌跡的平均誤差在0.228 m，場景b中軌跡的平均誤差在0.119 m，滿足智能車輛車道級定位精度的要求。在上述典型場景中的實驗證明了本文算法在復(fù)雜場景下仍能保持較高的定位精度，具有較高的定位魯棒性。

3 結(jié)論

提出了一種基于關(guān)鍵目標(biāo)的視覺SLAM算法，以解決因近處紋理匱乏、動態(tài)車輛干擾等復(fù)雜環(huán)境下的定位問題。首先，利用目標(biāo)檢測算法提取出靜止目標(biāo)，然后提取靜止目標(biāo)上的特征，根據(jù)特征數(shù)目判斷能否生成關(guān)鍵目標(biāo)。在單目初始化階段，根據(jù)關(guān)鍵目標(biāo)的數(shù)目以及求解結(jié)果判斷初始化結(jié)果。在跟蹤階段，利用關(guān)鍵目標(biāo)中心和恒速模型進(jìn)行跟蹤，完成關(guān)鍵目標(biāo)之間的數(shù)據(jù)關(guān)聯(lián)，通過最小化關(guān)鍵目標(biāo)中心和關(guān)鍵目標(biāo)特征的重投影誤差求解位姿，并對關(guān)鍵目標(biāo)的狀態(tài)進(jìn)行更新。經(jīng)實驗驗證，所提算法能在當(dāng)前視覺SLAM出現(xiàn)定位失效的復(fù)雜交通場景保持定位的穩(wěn)定性，且能滿足智能車輛在道路中行駛時的定位精度，對智能車輛的定位系統(tǒng)具有重要意義。