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

（浙江農(nóng)林大學光機電工程學院，浙江 杭州 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 年，F(xiàn)orster 等[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)化位姿，該方法的精度和實時性都得到了提高，但建圖部分沒有較好的地圖模塊。研究團隊經(jīng)過改進，在ORB-SLAM 的基礎上推出了ORBSLAM2[10]，在單目的基礎上增加了雙目和RGB-D 深度相機模式，使ORB-SLAM2有了更強的應用性。

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

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

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

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

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

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

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

2 SuperPoint算法

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

圖2 SuperPoint網(wǎng)絡結構圖

2.1 共享編碼器

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

2.2 特征點解碼器

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

2.3 描述子解碼器

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

2.4 損失函數(shù)

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

3 實驗結果及其分析

3.1 數(shù)據(jù)集與評價指標

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

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

3.2 實驗結果分析

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

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

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

表1 基于數(shù)據(jù)集各項指標比較

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

本實驗以Kinect1.0 和移動機器人為數(shù)據(jù)采集平臺，采用英特爾六核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)于現(xiàn)有ORB-SLAM2 算法模型，相比于傳統(tǒng)ORB 算法，其在復雜條件下具有更好的性能和更優(yōu)秀的魯棒性，在特征點提取數(shù)量上提升近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，進一步進行稠密地圖構建，實現(xiàn)機器人導航。