基于改進ORB-SLAM2 的單目視覺算法

于 航 ，楊維鑫 ，李光雷 ，樊夢成 ，楊科成 ，余 鋮

（浙江農林大學光機電工程學院，浙江 杭州 311300）

同時定位與建圖（Simultaneous Localization and Mapping, SLAM）[1]是指機器人在構建當前環(huán)境地圖時，同時定位自身位置和姿態(tài)的過程，其被廣泛應用于機器人、無人機等領域。最早應用在SLAM 技術上的是基于概率統(tǒng)計的擴展卡爾曼濾波器（EKF）[2]。2002 年，Montemerlo 等針對EKF 計算量大的缺點進行改進，提出Fast SLAM1.0 和Fast SLAM2.0[3]。近年來，隨著計算機圖像處理技術的進步以及視覺傳感器的高速發(fā)展，學者們逐漸將目光聚焦在視覺SLAM 技術上[4]。2010 年，Henry 等提出了一種結合RANSAC 和ICP 來計算機器人位姿的SLAM 方法[5]。2012 年，Sturm 等提出一種RGBDSLAM 算法評判標準[6]。2014 年，Forster 等[7]提出了SVO 算法。2014 年，Engel 等[8]提出了LSD-SLAM算法，LSD-SLAM 算法是一種基于直接法的單目SLAM 算法。2015 年，Mur-Artal 等[9]提出了一種ORB-SLAM 方法。ORB-SLAM 系統(tǒng)包含了跟蹤、建圖、重定位、閉環(huán)檢測等模塊，使用ORB 方法提取出每個圖像中的特征，使用DBoW2 庫進行回環(huán)檢測，最后使用g2o 優(yōu)化位姿，該方法的精度和實時性都得到了提高，但建圖部分沒有較好的地圖模塊。研究團隊經過改進，在ORB-SLAM 的基礎上推出了ORBSLAM2[10]，在單目的基礎上增加了雙目和RGB-D 深度相機模式，使ORB-SLAM2有了更強的應用性。

1 改進SLAM系統(tǒng)框架結構

本文提出基于改進ORB-SLAM2 的SuperPoint-SLAM 算法模型，使用SuperPoint 網絡提取圖片特征點以及計算描述子來取代原有ORB 特征提取。改進后的模型具有特征跟蹤、局部建圖、回環(huán)檢測這三個線程，并通過System 總線程完成對三個線程的調度，可實現地圖重用、回環(huán)檢測以及重新定位等功能，實現移動機器人的定位與建圖。SuperPoint 網絡的特征提取與匹配取代ORB 特征提取與匹配的單目SLAM系統(tǒng)框圖，如圖1所示。

圖1 SuperPoint-SLAM系統(tǒng)框架

該系統(tǒng)框架研究方法如下：

1）特征跟蹤線程。主要功能為對地圖關鍵幀進行匹配篩選，從而提升地圖建圖精度。使用SuperPoint 網絡從當前幀圖像中提取特征點，并在當前幀特征點中尋找與上一幀特征點相匹配的特征點，引入RANSAC 算法對特征匹配點進行誤匹配剔除處理，計算單應性矩陣H 并估計相機位，然后對局部地圖進行跟蹤，將局部地圖的地圖點和當前幀投影得到更多匹配，優(yōu)化當前幀的位置姿態(tài)，最后判斷是否需要插入新的關鍵幀。

2）局部建圖線程。主要功能為繼續(xù)優(yōu)化地圖點，降低回環(huán)檢測運算壓力。在插入新的關鍵幀之后，局部建圖線程首先更新共視圖和生長樹，計算詞袋Bow，插入到地圖中，接下來剔除冗余的地圖點，避免誤匹配和錯誤的三角化，保留高質量的地圖點。同時，對新插入幀中沒有匹配到的特征點與局部地圖點進行匹配，使其滿足極線約束，并進行正深度檢驗、視差檢驗、重投影誤差檢驗和尺度連續(xù)性檢驗，創(chuàng)建一個新的地圖點。然后，對地圖點和位姿進行局部BA 優(yōu)化。最后，剔除冗余關鍵幀，減輕回環(huán)檢測線程計算壓力。

3）閉環(huán)檢測線程。主要功能為檢測地圖點閉環(huán)程度，并進行閉環(huán)優(yōu)化。首先，計算當前關鍵幀與回環(huán)候選幀之間的相似度，約束Bow 向量相似度、共有單詞數量及連續(xù)性。如果判斷存在回環(huán)，則將當前幀的共視圖和匹配幀的共視圖的地圖點進行匹配，建立共事關系，以融合重復的地圖點。然后，使用本征圖優(yōu)化軌跡位置。最后，對所有關鍵幀和地圖點進行優(yōu)化。

2 SuperPoint算法

SuperPoint 是一種基于全卷積網絡的自監(jiān)督網絡模型，區(qū)別于傳統(tǒng)的SIFT 算法，SuperPoint 神經網絡同時輸出特征點與特征點的描述子。將帶有描述子的SuperPoint 神經網絡引入SLAM 算法中可以大幅提高特征點匹配精度。SuperPoint 神經網絡主要分為三部分：共享編碼器、特征點解碼器以及描述子解碼器，各部分結構如圖2所示。

圖2 SuperPoint網絡結構圖

2.1 共享編碼器

編碼器結構如圖2 所示，一共有8 個3×3 的卷積層，padding 值均為1，步長值均為l，卷積核的數量依次為64、64、64、64、128、128、128、128，每個卷積層后面都有激活函數ReLU，卷積核的步長均為1，在經過第2、4、6 次的激活函數后還會用2×2 的最大池化進行降采樣。最后輸出W/8×H/8×128，減少了輸入圖像的維度。

2.2 特征點解碼器

檢測關鍵點的解碼器輸入為W/8×H/8×128，在經過通道數為256 的CR 模塊后，進入通道數為65 的卷積層，輸出W/8×H/8×128 的張量信息。因為經過三次最大池化后，一個像素點的信息對應原始圖像中不重疊的8×8 區(qū)域大小的像素信息，再加一個通道用于沒有關鍵點的情況，所以最后的卷積層通道數是65個123。將關鍵點提取問題轉換為分類問題，即8×8像素大小的區(qū)域中是否有關鍵點，并找到關鍵點的位置。通過Softmax 函數去掉沒有關鍵點存在的通道，變?yōu)閃/8×H/8×64，最后將圖像大小W/8×H/8 恢復W×H，同時通道數從64 變成1，即可在輸入圖像中找到對應的關鍵點。

2.3 描述子解碼器

計算描述子的解碼器輸入為W/8×H/8×128，在經過通道數為256 的CR 模塊后，進入通道數為256的卷積層，輸出了W/8×H/8×256 的張量信息。然后，用雙三次插值把W/8×H/8×256 變成W×H×256，最后，用L2 范數將描述子歸一化即可得到描述子。SuperPoint 在編碼器部分實現了關鍵點和描述子的參數共享，減少了計算量。

2.4 損失函數

損失函數由兩部分組成，如式（1）所示。式中，Lp是提取關鍵點的解碼器損失函數；Ld是生成描述子的解碼器的損失函數；X和D是圖像經過網絡輸出后的特征點特征圖以及描述子特征圖；Y為圖像特征點的標簽值；X'，D'，Y'為圖像經過單應性變換后再輸入網絡后輸出的特征點特征圖、描述子特征圖和圖像特征點的標簽值；λ是超參數，用來平衡特征點檢測損失和描述子損失。

3 實驗結果及其分析

3.1 數據集與評價指標

實驗采用TUM 標準數據集dynamic objects 中的freiburg3_sitting_xyz 靜態(tài)和freiburg3_walking_xyz_validation 動態(tài)數據集來進行定位實驗驗證，使用EVO 工具對關鍵幀軌跡和實際軌跡進行對比得到軌跡偏差。

實驗采用絕對軌跡誤差的均方根誤差與特征點選取質量來評價估計的軌跡偏差。絕對軌跡誤差均方根公式如式（2）所示：

3.2 實驗結果分析

將改進后SLAM 算法與ORB-SLAM2 算法分別在freiburg3_sitting_xyz 與freiburg3_walking_xyz_validation 兩個數據集進行訓練驗證，對實驗結果進行絕對軌跡誤差圖繪制，結果如圖3 所示。圖3 中，虛線標識代表真實軌跡，實線標識代表算法軌跡。由圖3（a）和（b）可知，兩種算法在對靜態(tài)環(huán)境的特征跟蹤過程中，與真實軌跡對比，均表現出了良好的定位跟蹤效果，但SuperPoint-SLAM 在y方向的追蹤效果更優(yōu)。由圖3（c）和（d）可知，在相同環(huán)境引入動態(tài)干擾時，SuperPoint-SLAM 對真實軌跡的定位跟蹤效果明顯優(yōu)于ORB-SLAM2算法。

圖3 靜態(tài)動態(tài)絕對軌跡誤差對比

基于不同數據集，兩種方法的各項指標比較如表1 所示。對于sitting_xyz 靜態(tài)數據集，SuperPoint-SLAM 與ORB-SLAM2 均根方誤差相差不大，僅相差0.000 9 m，關鍵幀數也相差不大，定位精度差別很小。但在walking_xyz_validation 動態(tài)數據集中，SuperPoint-SLAM 的均根方誤差小于ORB-SLAM2，相差0.008 m，且關鍵幀數量提高50%。實驗結果表明，SuperPoint-SLAM 算法在具有動態(tài)干擾時的采定位效果及精度均遠優(yōu)于ORB-SLAM2。

表1 基于數據集各項指標比較

3.3 基于光照強弱環(huán)境的特征點提取實驗

本實驗以Kinect1.0 和移動機器人為數據采集平臺，采用英特爾六核i7 CPU，NVIDIA GeForce GTX 1650 的筆記本，系統(tǒng)版本為64 位的Ubuntun18.04 TS，在真實環(huán)境下比較了SuperPoint-SLAM 和ORBSLAM2在特征點識別和跟蹤上的差異。

無論是強光還是昏暗環(huán)境ORB-SLAM2 系統(tǒng)由于采集的特征點過少，一直處于初始化失敗的狀態(tài)，如圖4 所示。相反SuperPoint-SLAM 算法在陽光直射下可以采集到足夠的特征點。由實驗結果可知，ORB-SLAM2 開始到結束一直未提取到特征點，從而初始化失敗，無法估計相機位姿；SuperPoint-SLAM則在強光下可持續(xù)提取到特征點，為系統(tǒng)估計相機位姿提供了條件。

圖4 ORB-SLAM2與SuperPoint-SLAM特征點提取比較

4 結論

本文提出基于改進ORB-SLAM2 的SuperPoint-SLAM 算法模型，在動態(tài)環(huán)境檢測與復雜環(huán)境條件下明顯優(yōu)于現有ORB-SLAM2 算法模型，相比于傳統(tǒng)ORB 算法，其在復雜條件下具有更好的性能和更優(yōu)秀的魯棒性，在特征點提取數量上提升近50%，軌跡誤差減少0.008 m。

實驗結果表明，SuperPoint-SLAM 算法在特征提取性能上優(yōu)于ORB-SLAM2 算法，SuperPoint-SLAM算法在光照強弱環(huán)境中對圖片的特征提取效果明顯好于ORB-SLAM2 算法，并且該算法能夠獲得優(yōu)于傳統(tǒng)ORB-SLAM2 算法的定位精度。由于ORBSLAM2 是一種基于稀疏特征的輕量級定位方案，無法利用其稀疏地圖進行機器人導航。未來研究團隊將利用Kinect2.0，基于SuperPoint 與ORB-SLAM2，進一步進行稠密地圖構建，實現機器人導航。