基于改進關鍵幀選擇的ORB-SLAM2 算法

張洪，于源卓，邱曉天

(1. 江南大學 機械工程學院，無錫 214122； 2. 江蘇省食品先進制造裝備技術重點實驗室，無錫 214122)

近年來，移動機器人逐漸走進社會為人們的生產生活提供便利[1-2]。在這種情況下，機器人的控制與路徑規劃顯得尤為重要[3-4]?？刂婆c路徑規劃需要定位與建圖作為前提，因此，將機器視覺與同時定位與建圖相結合(V-SLAM)[5-7]成為了機器人領域研究的熱點[8]。相比于采用雷達的二維SLAM，V-SLAM 通過機器視覺獲取三維信息，對物體空間的幾何外形進行描述，對于機器人在未知領域的導航有著重要意義[9]。隨著國內外學者的逐漸深入研究，V-SLAM 先后出現了LSD-SLAM、RGB-D SLAM、ORB-SLAM 及語義SLAM。

TUM[10-11]計算機視覺組采用了直接法的方式，通過灰度圖的強度對相鄰的2 幀進行匹配成功，實現大規模環境下的定位與建圖。Endres 等[12]使用深度相機獲取視覺信息，完成了RGB-D SLAM 算法，以提高建圖魯棒性。Mur-Artal 等[13]提出了經典的ORB-SLAM 算法，在跟蹤、建圖、重定位及閉環檢測的過程中，全程采用ORB 特征完成，使其具有較好的實時性；隨后，為解決ORB-SLAM 所用相機單一且精度低的問題，又提出了ORB-SLAM2[14]，該算法采用光速平差法進行優化使定位更加精確，并提出了一種輕量級的定位模式，使其能夠更加有效地重用地圖。Liu 等[15]對物體語義信息進行了總結，語義SLAM[16-17]也逐漸出現在人們的視野中，借用物體的語義信息，成功提高了SLAM 的人機交互能力，提高了算法精度。

相比于其他SLAM 算法，ORB-SLAM2 算法具有高效的實時性、相機使用的多樣性及良好的精度,深受國內外研究學者的喜愛。關鍵幀作為決定ORB-SLAM2 算法精度的重要因素，廣大學者針對其進行了大量創新優化，以提升算法性能。Yang 等[18]提出基于地圖元素和關鍵幀數據庫的離線地圖構建算法，通過對關鍵幀與地圖元素在線儲存離線構圖的方式提高建圖精度。Xie 等[19]提出重復運動檢測方法改進關鍵幀選擇算法，剔除冗余關鍵幀以提高整個系統的實時性能及定位精度。Cui 等[20]在ORB-SLAM2 算法的基礎上使用圖像強度而非特征點匹配關鍵幀，提出了Direct-ORB-SLAM，該方法避免了計算冗余，使系統在效率與準確度上均有提高。綜上所述，國內外針對ORB-SLAM2 關鍵幀的研究已取得大量成果，但不難發現：①在跟蹤定位的過程中，跟蹤失敗情況經常發生，嚴重影響定位效果；②雖然對算法進行了優化，但精度不高，誤差較大的問題依然存在。

基于此，本文首先采用ORB-SLAM2 算法對幀間相對位姿進行估計；其次在原有算法模型的基礎上，增加旋轉與平移量作為判定依據，決定是否插入新關鍵幀，解決了跟蹤定位易失敗的問題，并提高了定位精度；然后設計了劣質關鍵幀剔除算法，避免了因移動機器人所安裝的相機與機器人機身產生相對運動而導致劣質關鍵幀生成的問題；最后通過RGB-D 數據集與移動機器人開展了實驗驗證。

1 ORB-SLAM2 算法

經典的V-SLAM 算法框架主要通過視覺傳感器讀取數據，完成前端視覺里程計，通過后端進行非線性優化，回環檢測再次對整體地圖進行優化更新，以完成地圖的構建。而ORB-SLAM2 算法將經典的V-SLAM 算法劃分為3 個線程：Tracking、Local Mapping、Loop Closing，并通過System 總線程完成對移動機器人的定位與建圖。

Tracking 線程主要包含相機定位、關鍵幀選取與插入、ORB 特征點提取及相機位姿估計。ORBSLAM2 算法采用了ORB 特征點：由FAST 角點與角點位置的BRIEF 描述子組成，這種特征點有著較快的計算速率及較高的精度，其采用opencv 庫中的BruteForceMatcher 結合漢明距離進行匹配，采用隨機采樣一致性算法(RANSAC)減少誤匹配對，抑制噪聲，剔除外點。

同時，ORB-SLAM2 之所以能夠擁有較好的實時性，是因為在跟蹤線程運行的過程中，不對相機傳來的所有幀進行處理，而是如圖1 所示根據關鍵幀生成規則生成關鍵幀，僅對選取的關鍵幀進行處理，減少處理的幀數，節約運算資源，保證系統的實時性。

圖1 關鍵幀選取示意圖Fig. 1 Schematic of key frame selection

2 關鍵幀選擇算法

2.1 常用關鍵幀選擇算法

常用的關鍵幀選擇算法首先要保證當前幀的內點數超過設定的最小閾值，并且重疊度不高，既保證關鍵幀跟蹤的指令，同時又避免引入過多的信息冗余。上述條件符合時，線程開始對下述條件進行判定，當有一條判定成功時，插入關鍵幀。

1) 為避免通過幀數過多造成跟蹤丟失，在距離上次關鍵幀插入過去MAX 幀(預設的最大值)時，插入關鍵幀。

2) 為防止圖像重疊度過高，在距離上次插入關鍵幀至少過去MIN 幀(預設的最小值)且局部建圖線程處于空閑狀態時，插入關鍵幀。

3) 局部建圖線程處理能力有限，在該線程中需要保持關鍵幀隊列的關鍵幀不超過3 個。

但是由于此關鍵幀篩選條件只與線程中通過幀的數量有關，當機器人在轉彎或者移動速度過快的情況下，就會導致關鍵幀信息采集不足，漏掉重要信息，導致軌跡造成較大偏差，甚至跟蹤中斷。針對這一問題，本文新增幀間相對運動量作為關鍵幀選擇條件，并設計劣質關鍵幀剔除算法，以提高跟蹤定位精度。

2.2 改進關鍵幀選擇算法

2.2.1 關鍵幀的選擇

為提高定位精度，本文增加幀間相對運動量作為關鍵幀選擇條件之一。幀間相對運動量表示幀與幀之間的相對運動程度，主要由2 部分組成，分別是旋轉和平移，其中將旋轉作為主要變化量，平移作為次要變化量，計算方式如下：

通過視覺里程計得到相匹配的特征點，計算2 組點的質心位置p'和p，計算每個點的去質心坐標：

對式（3）進行化簡，去除與R無關項，實際優化函數化簡為

式中：E為奇異值組成的對角矩陣，對角線元素從大到小排列；U、V為對角矩陣。

當W為滿秩時，即可根據式（6）計算得出R：

通過計算出的R計算平移向量t：

式中：R表示當前幀與最新關鍵幀之間的幀間旋轉矩陣；t表示二者之間的平移向量。

通過R和t求出D，并在ORB-SLAM2 系統中為其設定閾值Dk：

式中： α為位移與轉角的平衡權值系數，取值大小隨轉角增大呈指數形式增加。由于本文算法在原有以幀數為判定依據的關鍵幀選擇算法基礎上增加了轉角作為主要變化量，轉角所采用的平衡權值系數，在存在角度轉變時遠大于位置移動的平衡權值系數，采用式(9)的計算公式進行計算，設ω=min(2π-‖R‖,‖R‖)，取值范圍為0°～90°（當旋轉角度大于90°時，將徹底失去視角，無法特征點匹配）：

對計算所得相對運動量與其閾值進行比較：當D≥Dk時， Framekey=Framecur， FrameRef=Framecur； 當D

當判定通過后，將當前幀設為關鍵幀，同時更新參考關鍵幀，再次計算當前幀與新關鍵幀之間的相對運動量，算法流程如圖2 所示。圖中：Tracking線程中存在成員變量指針mCurrentFrame，指向當前幀成員變量，其中包括圖像、深度信息、相對位姿等信息。

圖2 關鍵幀選擇算法流程Fig. 2 Flowchart of key frame selection algorithm

步驟 1 判斷插入關鍵幀的前置條件是否滿足：系統不處于定位模式且不在全局閉環的條件下，同時距離上一次重定位的距離較遠，內點數必須超過設定的最小閾值，保證關鍵幀跟蹤質量。

步驟 2 在原有關鍵幀篩選條件的基礎上，增加了幀間相對運動量作為篩選條件之一，System 主線程中將Tracking 線程中的第1 幀作為初始關鍵幀，根據式(8)計算出當前幀與關鍵幀的幀間相對運動量D。

步驟 3 對幀間相對運動量進行比較。如果當前幀的相對運動大于或等于一定的閾值，說明相機相對于之前跟蹤定位的位置有了較大的相對運動，相機的視角發生了較大改變，此時不生成新的關鍵幀進行定位，極有可能在跟蹤定位時產生誤差，甚至因檢測不到地圖點造成跟蹤中斷。

因此，當相對運動較大時，添加關鍵幀可以有效改善這種情況。當關鍵幀生成后，用新產生的關鍵幀作為參考關鍵幀繼續計算下一幀的相對位姿，直到整個系統循環結束。

2.2.2 劣質關鍵幀剔除

本文自主設計研發的移動機器人，在運行過程中與機械臂共用相機，因此地面輪胎的打滑及機器人移動過程中的震動，會導致相機與機器人本身發生相對運動從而改變相機的視角，產生誤拍攝，此時所得關鍵幀會對跟蹤定位產生誤導，從而使定位精度降低，本文將其定義為劣質關鍵幀。針對這一問題，本文設計了劣質關鍵幀剔除算法，剔除由于誤拍攝造成的劣質關鍵幀，提高跟蹤定位精度。

在ORB-SLAM2 跟蹤定位線程中，存在冗余關鍵幀剔除模塊，該模塊只對冗余關鍵幀進行剔除，并不對精度產生影響，其目的是為了節約資源，減輕工作線程負擔。本文基于冗余關鍵幀剔除模塊，設計了劣質關鍵幀剔除算法，剔除過程如下：

步驟 1 在關鍵幀生成時，臨時存儲彩色圖像及時間戳。

步驟 2 選取相鄰3 關鍵幀進行ORB 特征匹配，分別保存了2 次相鄰關鍵幀特征點匹配數量NUMAdj1、NUMAdj2及一次相隔關鍵幀匹配數NUMSep。

步驟 3若相鄰2 幀的特征點匹配數量小于相隔2 幀，說明3 幀中的中間幀大概率是由于誤拍攝而產生的關鍵幀。

步驟 4激活關鍵幀剔除函數對劣質關鍵幀進行剔除，繼續線程的運行，直到整個線程結束，釋放內存。

偽代碼如下：

這種方法區別于傳統SLAM 算法中只對相鄰2幀的關鍵幀進行比較，而是采用了比較相鄰3 幀的方法。由于本文所設計的移動機器人平臺不存在視角突變，算法運行過程中，如果第1 幀和第3 幀差異較小，而第2 幀與兩者之間圖像差異較大，說明第2 幀在拍攝的過程中，因震動或其他原因導致相機本身與移動機器人之間發生了相對運動，從而使視角發生了改變，產生了劣質關鍵幀。

3 實 驗

3.1 公開數據集實驗

在Ubuntul 16.04 環境下進行了公開數據集實驗，計算機所用的CPU 為2.0 GHz i7，運行內存為4 GB。實驗采用TUM RGB-D Benchmark 數據集，選取fr1中的desk、xyz 和room 進行。依次對比了常用關鍵幀選擇算法、改進關鍵幀選擇算法(包含劣質關鍵幀)、改進關鍵幀選擇算法(剔除劣質關鍵幀)的實驗效果。

3.1.1 ORB-SLAM2 實 時 性 分 析

SLAM 的實時性是評價其優劣程度重要的性能指標，表1 對各個數據集平均跟蹤時間進行了記錄。

從表1 可知，由于對關鍵幀選擇算法進行了改進，增加了關鍵幀選擇的計算量，導致運行時間稍有增加，但由于相對整個系統而言計算并不復雜，增加時間極少，對系統的實時性并沒有造成影響。

表1 平均跟蹤時間對比結果Table 1 Comparison of mean tracking time

TUM 計算機視覺組數據集中包含利用運動視覺捕捉系統記錄的攝像頭在該場景下真實且連續的軌跡數據，圖3(a)～圖3(c)分別為3 種算法所得到的估計軌跡與真實軌跡的對比軌跡曲線。可以明顯看出：①在圖3(a)的軌跡交叉處有較大視角突變，原有關鍵幀選擇算法未能檢測到此信息，導致跟蹤失敗，采用線段直接相連的方式進行軌跡估計；而改進后的算法如圖3(b)、圖3(c)所示，跟蹤失敗并未發生，說明改進算法可以將轉彎信息較好地采集，并用于關鍵幀選擇，有效解決了跟蹤失敗的問題。②圖3(a)中軌跡最右側轉彎后估計軌跡明顯偏離真實值，是由于原有關鍵幀選擇算法檢測轉彎信息出現了滯后，造成較大偏差；改進后的算法增加了相對變化量，縮減了該處的定位誤差。③由于劣質關鍵幀僅在移動機器人實驗中產生，圖3(b)與圖3(c)相差不大。

圖3 三種關鍵幀選擇算法運動軌跡對比Fig. 3 Comparison of motion trajectories of three key frame selection algorithms

對于定位精度，采用絕對軌跡誤差（absolute trajectory error，ATE）進行比較。ATE 越小，精度越高。第i幀的ATE 定義如下：

3.1.2 ORB-SLAM2 跟蹤定位精度分析

式中： Δ為真實位置與估計位置之間的誤差；Fi為第i幀的ATE；Qi為真實位姿；Pi為算法估計位姿；S為從估計位姿到真實位姿的相似旋轉矩陣。

將估計軌跡與真實軌跡進行對比，繪制出絕對軌跡誤差曲線，如圖4 所示。圖4(a)為常用關鍵幀選擇算法所得到的估計軌跡與真實曲線的誤差，圖4(b)為2 種改進關鍵幀選擇算法后所得到估計軌跡與真實軌跡的誤差，是否添加劣質關鍵幀剔除算法對誤差結果并沒有太大影響，說明劣質關鍵幀剔除算法對數據集的定位精度并沒有太大影響。由圖4可以看出，相較于常用關鍵幀選擇算法，改進后的算法有明顯提高。

圖4 定位軌跡誤差曲線Fig. 4 Curve of track positioning error

選取最大絕對軌跡誤差作為評判標準，表2 為3 種算法下的最大絕對軌跡誤差。

由表2 可知：①改進關鍵幀選擇算法相比于常用關鍵幀選擇算法，在3 個數據集中誤差均明顯降低，最優結果改進后的算法誤差僅為改進前誤差的51.9%；②剔除了劣質關鍵幀的改進算法與包含劣質關鍵幀的選擇算法軌跡誤差相差較小，僅存在合理波動，這是由于以數據集作為實驗對象時，數據集內部并不包含劣質關鍵幀。

表2 最大絕對軌跡誤差對比Table 2 Maximum absolute trajectory error

綜上，改進后的關鍵幀選擇算法在保證實時性的情況下，能夠有效地提高算法估計軌跡精度，同時還能有效避免跟蹤失敗的情況發生。

3.1.3 ORB-SLAM2 關鍵幀數量分析

圖5 比較了不同數據集下3 種關鍵幀選擇算法計算的關鍵幀數量。顯然，改進后的關鍵幀選擇算法的關鍵幀數量明顯大于改進前，說明改進后的算法選擇了更多的關鍵幀以提升估計定位精度。

圖5 數據集實驗關鍵幀數量比較Fig. 5 Comparison of key frame numbers in dataset experiments

3.2 移動機器人實驗

本文自主設計的移動機器人如圖6 所示，采用大唐工控機新創云i7mini 主機作為平臺的嵌入式主控機，搭配一款15.6 寸觸控屏作為人機交互硬件，并選用奧比中光深度相機作為視覺傳感器為機器人傳送視覺信息數據。實驗比較了3 種算法在實時運行過程中的定位精度。

圖6 移動機器人Fig. 6 Mobile robot

實驗內容主要包括機器人直線運動、轉彎運動及回環運動3 種運動方式。對機器人設計搭建、相機標定等操作后，通過對3 種運動的實際軌跡和ORB-SLAM2 跟蹤估計的定位軌跡進行測量，分別進行了6 次實驗，并取誤差平均值進行比較。圖7為3 種關鍵幀選擇算法對應的回環運動軌跡對比。由于數據為機器人實時輸出，真實軌跡存在差異，并不能在同一坐標系內進行繪制。

由圖7 可知：①圖7(b)相比于圖7(a)有一部分路程未能檢出，發生了跟蹤失敗的情況，在轉彎處，明顯因檢測信息滯后而導致估計轉角過小增大誤差；②由圖7(c)、(d)軌跡曲線可知，算法未發生大規模跟蹤失敗，且轉角處定位精度明顯提高；③圖7(e)、(f)為剔除了劣質關鍵幀的改進算法，由于剔除了劣質關鍵幀，圖7(f)的軌跡更接近真實軌跡曲線，抖動明顯減少，在一定程度上提高了定位精度。

圖7 回環運動軌跡對比Fig. 7 Comparison of loop trajectories

表3 為不同關鍵幀選擇算法所得到的軌跡與真實軌跡平均位置誤差?？芍孩侔淤|關鍵幀的改進算法相較于改進前算法，成功提高了定位精度，由于算法改進主要針對轉向，直線時誤差減小并不明顯，但轉彎與回環運動時定位精度顯著提高，誤差僅為原誤差的58.1%；②剔除劣質關鍵幀的改進算法軌跡精度與未剔除相比明顯提高，尤其在直行時，誤差為包含劣質關鍵幀算法誤差的82.1%。

表3 平均位置誤差Table 3 Average position error

圖8 比較了機器人不同運行方式下的3 種關鍵幀選擇算法計算的關鍵幀數量。顯然，機器人實時運行的過程中，改進后的算法除直線外同樣選擇更多的關鍵幀以保證算法精度，而直線過程未發生角度轉變，因此關鍵幀數量在誤差范圍內合理波動；剔除劣質關鍵幀后，關鍵幀數量相對減少，在直線過程中最為明顯，所剔除的劣質關鍵幀數量約達到關鍵幀總數的25%；回環運動的直線行駛路程為直線運動的2 倍，但多次實驗所剔除的劣質關鍵幀數量均未達到直線運動的2 倍。造成這種現象的原因有3 點：①角度的轉變會對劣質關鍵幀的判斷造成干擾；②劣質關鍵幀是否生成與機器人所處環境的優劣特別是路面的平滑度有著極大的關聯；③實時運行的過程不能嚴格保證機器人運行軌跡完全相同。

圖8 移動機器人實驗關鍵幀數量比較Fig. 8 Comparison of key frame numbers in mobile robot experiments

3.3 實驗參數分析

實驗過程中應對某些參數進行調整，參數調整不當會造成定位錯誤，系統運行資源浪費，甚至使系統崩潰。對于不同的數據集，通過計算及多次實驗調整的方式確定算法所用參數。

最大幀間相對位移量Dk是通過計算并進行多次實驗后，依據算法最小絕對估計誤差確定。Dk越大，表明選取關鍵幀時通過的相對位移越大，可能導致重要關鍵幀被忽略而導致精度降低；Dk越小，表明選取時通過的相對位移越小，選取的關鍵幀就會隨之增多，產生冗余，導致系統實時性降低，對后續的建圖工作產生較大影響。本文中Dk取轉角為45°、位移為0 時計算值，并通過實驗進行微調。此轉角下相鄰2 關鍵幀在匹配到足夠特征點進行計算的同時，有效防止過多關鍵幀的產生浪費系統內存。在不同的實驗條件下，相機的采樣頻率、機器人的運行速度及環境的規模大小均對Dk的最佳值產生影響。一般情況，Dk取30～50 為最佳。

4 結 論

1) 提出了一種改變關鍵幀選擇判定條件的ORBSLAM2 算法，在原有算法的基礎增加了相對運動量作為選擇依據，有效避免了跟蹤失敗，提高了定位精度。

2) 提出了一種劣質關鍵幀剔除算法，解決了相機與移動機器人機身之間的相對運動產生的劣質關鍵幀問題。

3) 通過數據集與移動機器人實驗驗證了算法的優異性。實驗結果表明，數據集估計定位誤差的最優結果約為改進前算法的51.9%；移動機器人實驗約為改進前的62.9%。剔除劣質關鍵幀后，直線誤差僅為原來的82.1%。