復雜交通環境下基于關鍵目標的視覺SLAM

連 靜，皮家豪，李琳輝

(1.大連理工大學 汽車工程學院， 遼寧 大連 116024； 2.大連理工大學 工業裝備結構分析國家重點實驗室， 遼寧 大連 116024)

0 引言

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

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

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

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

針對上述問題，提出一種基于關鍵目標的視覺SLAM算法。通過提取交通場景中普遍存在的交通標志和路牌等靜止目標并在內部進一步完成特征的提取，經過篩選后構建出初始的關鍵目標。然后，在相連幀之間通過幾何關系和特征之間的匹配完成關鍵目標的匹配，并在此基礎上完成系統的初始化和關鍵目標的跟蹤。最后，基于關鍵目標求解相機位姿，并在局部地圖中對二者聯合優化，以提高算法在復雜環境下的定位穩定性。

1 基于關鍵目標的SLAM算法框架

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

圖1 算法整體流程框圖

1.1 關鍵目標提取

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

得益于深度學習在近年來的迅速發展，對交通場景中靜止目標的檢測方案也不斷被提出[18-20]。如圖2所示，交通場景中的交通燈、交通標志和路牌分別被目標檢測網絡檢出。

圖2 靜止目標檢測結果

當靜止目標檢測完成后，在目標區域內提取ORB[21]特征，特征點提取的數量根據靜止目標區域在圖像上的占比確定。為了避免特征點出現過度扎堆的現象，采用ORB-SLAM3[6]中的四叉樹方法均勻化特征點，特征提取結果如圖3所示。廣告牌在圖像中占比較大，因此廣告牌上特征點分布更多。接著，通過靜止目標中特征點數目篩選關鍵目標，確保篩選出的關鍵目標有足夠的特征用于后續基于特征的匹配和基于關鍵物之間的跟蹤。至此，完成關鍵目標的初步構建，并根據其檢測類別和檢測順序對其分類和編號。

圖3 靜止目標特征提取

1.2 系統初始化

在完成關鍵目標的構建且關鍵目標的數量大于初始化閾值后，則將當前幀設置為待初始化幀，開始嘗試初始化過程。當連續兩幀都滿足初始化的基本條件時，對兩幀之間的關鍵目標進行匹配并實施初始化，具體的過程如下：

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

(1)

(2)

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

當多個關鍵目標區域滿足幾何篩選條件時，通過內部特征關聯完成最后的篩選。篩選方式是對待匹配目標區域與篩選目標區域的特征點依次進行匹配，并記錄匹配成功的特征點數目，特征點匹配數目最多且大于閾值條件的即視為匹配成功的關鍵目標區域，否則匹配失敗，并將該關鍵目標從當前跟蹤的局部地圖中剔除。關鍵目標的匹配結果如圖4所示，圖中連線兩端對應著關鍵目標內匹配成功的特征點。

圖4 關鍵目標匹配

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

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

4) 關鍵目標和特征點三角化。根據特征點三角化的比例判斷初始化是否成功以及關鍵目標區域三角化是否成功，如果初始化成功但是關鍵目標區域三角化失敗，則剔除對應的關鍵目標區域并刪除其包含的所有三角化路標點。

5) 初始地圖創建。當關鍵目標區域三角化成功后，將關鍵目標中心的三維坐標和對應的路標點坐標插入到地圖中，完成初始的地圖構建，同時為后續關鍵目標的跟蹤和相機位姿計算建立基礎。

1.3 關鍵目標跟蹤

借鑒ORB-SLAM3[6]的方式，采用恒速模型計算出關鍵目標中心在當前幀的像素坐標，并基于此進行跟蹤。

(3)

計算得到關鍵目標中心點位置后，設置搜索半徑，在目標區域內搜索對應的候選關鍵目標區域，通過目標類別和目標區域的幾何信息進行篩選，篩選方式與1.2節方法相似。

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

1.4 位姿求解

完成關鍵目標之間的數據關聯后，基于關鍵目標內的特征點和關鍵目標中心坐標估計相機位姿。該估計通過最小化下列的代價函數實現。

(4)

(5)

式中：ρ代表Huber魯棒核函數；Pij代表第i個關鍵目標內第j個特征對應的三維坐標。

1.5 局部BA

通過關鍵目標的數量和新關鍵目標的比例篩選當前圖像幀，并完成關鍵幀的創建，然后在局部建圖線程中，對關鍵幀位姿及關鍵目標進行聯合優化，優化方法選擇魯棒的LM(Levenberg-Marquardt，列文伯格-馬夸爾特)方法。

(6)

式中：x為局部優化窗口中所有關鍵幀位姿，關鍵目標中心坐標和關鍵目標內特征點的三維坐標。

完成局部BA后，對相應的關鍵幀位姿、關鍵目標中心坐標和關鍵目標內地圖點進行更新。更新結果用于最新的關鍵目標跟蹤和位姿計算，并等待下次局部BA的開始。

2 實驗

2.1 數據集

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

2.2 實驗結果

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

圖5 Apollo數據集中的復雜場景

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

圖6 平面軌跡

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

表1 絕對軌跡誤差 m

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

3 結論

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