傘紅軍，王汪林，陳久朋，王 晨

（昆明理工大學機電工程學院，云南昆明 650500）

0 引言

同步定位與地圖構建（Simultaneous Localization and Mapping，SLAM）是移動機器人在未知環(huán)境下實現(xiàn)自主定位導航的核心技術［1］。相較于激光、雷達，視覺同步定位與地圖構建（Visual SLAM，VSLAM）因采用視覺傳感器而具有一定成本優(yōu)勢，成為SLAM 領域的熱門研究方向之一［2］。

按照視覺里程計的計算方法不同可將VSLAM 分為直接法［3］和特征點法［4］。直接法基于灰度不變假設直接根據(jù)像素亮度信息估計相機位姿，完全不用計算關鍵點和描述子，但在高光環(huán)境中存在退化或失效等現(xiàn)象。Engel 等［5］結合地圖梯度與直接法提出LSD-SLAM，其可在CPU 上實時構建半稠密地圖，缺點是在快速運動和曝光環(huán)境中相機位姿會丟失；Forster 等［6］提出SVO（Semidirect Visual Odome?try）算法，使用半直接法利用稀疏特征點定位相機位姿，運行速度快，但跟蹤丟失現(xiàn)象嚴重，且不適用于高光環(huán)境；En?gel 等［7］提出的DSO（Direct Sparse Odometry）是純直接法的SLAM 系統(tǒng)，能夠快速實現(xiàn)稀疏點云地圖的構建，但對光照敏感，魯棒性不高。

特征點法使用特征點的匹配計算相機位姿，對關鍵點的提取與匹配精度要求高，如果存在太多誤匹配，相機位姿估計會有很大偏差。因此，如何有效剔除誤匹配并提高位姿估計精度是SLAM 研究者們一直關注的問題。傳統(tǒng)SLAM 系統(tǒng)使用隨機抽樣一致（Random Sample Consensus，RANSAC）算法［8］剔除誤匹配特征點，該算法通過隨機抽取樣本數(shù)據(jù)計算模型參數(shù)，能快速找到最優(yōu)估計［9］。Mur-Ar?tal 等［10］提出一個基于特征點的實時SLAM 系統(tǒng)ORBSLAM2，無論規(guī)模大小、室內室外都可以運行。該系統(tǒng)對劇烈運動也很魯棒，能實時計算相機軌線，并生成場景的稀疏三維重建，但耗時長且存在一定的誤匹配。

本文基于ORB-SLAM2，在關鍵幀圖像匹配的過程中引入一種新的誤匹配剔除算法替換RANSAC 算法。其選用kinect v1 深度相機，先對TUM 數(shù)據(jù)集進行實驗，獲得相機軌跡，進而驗證改進SLAM 算法的可行性。同時，在實驗室環(huán)境中對改進SLAM 算法進行三維稀疏點云地圖構建。結果表明，相較于傳統(tǒng)的RANSAC 算法，改進誤匹配剔除的SLAM 算法增加了正確匹配點對數(shù)量，提高了相機位姿估計精度。

1 特征點匹配算法

在實時性方面，特征點匹配算法ORB（Oriented FAST and Rotated BRIEF）［11］是VSLAM 系統(tǒng)中最具代表性的特征提取算法之一。ORB 改進了FAST（Features from Accelerat?ed Segment Test）算法中檢測子不具有方向性的問題，采用計算速度極快且具有旋轉不變性的二進制描述子（Binary Robust Independent Elementary Feature，BRIEF）［12］，使圖像特征提取速度大大加快。

1.1 FAST 特征點檢測

FAST 是一種高效的特征點（角點）檢測算法，可基本滿足實時檢測需求，是計算機視覺領域最主流的角點檢測算法之一。FAST 檢測角點的基本思路如圖1 所示，以某一像素點為圓心，半徑值固定的圓周上其余像素點與圓心像素點灰度值差別較大即可被認為是角點。由于只需要比較一維灰度值的大小，該算法計算時間大大縮短。對于灰度圖像，F(xiàn)AST 算子考察的屬性是像素與其鄰域的灰度差異，檢測過程如下：

（1）在圖像中選取一像素點p，即可確定出以p 點為圓心、半徑為3 的16 個像素點，將其灰度值與p 點進行比較。若有連續(xù)的n（通常取12）個像素點比p 點灰度值大或者小，則p 點即為特征點。為提高檢測效率，通?？蓹z測圖1中1、5、9、13 四個位置中是否有連續(xù)3 個及以上像素點大于或小于p 點的灰度值，若滿足則需進一步比較，若不滿足可直接排除。

Fig.1 FAST feature points圖1 FAST 特征點

（2）篩選最優(yōu)特征點。

（3）去除局部較密集特征點，刪除p 點與周圍特征點偏差絕對值之和較小的特征點。

（4）實現(xiàn)特征點的多尺度不變性。通過構建圖像金字塔，在金字塔的每一層檢測角點，實現(xiàn)尺度不變性，特征的旋轉通過灰度質心實現(xiàn)［13］。設置比例因子和金字塔層數(shù)，對原圖像進行不同層次的降采樣，將不同比例圖像中提取的特征點總和作為FAST 特征點。

1.2 BRIEF 特征描述子

BRIEF 是一種二進制描述子，即每個關鍵點是由0 和1組成的二進制字符串。定義S×S 大小的圖像領域P 的二值化τ 為：

式中，p（x）為圖像鄰域P 在點x 的灰度值。

那么，nd個測試點對比形成描述子的計算公式為：

式中，nd可為128、256 或512 等。

上述描述子不具有旋轉不變性，ORB 算法利用角點方向使描述子包含方向信息。

1.3 特征點匹配

在完成特征點提取與描述子計算后，還需對圖像中的特征點進行匹配計算以精確判斷兩幅圖像之間的相似性。特征匹配主要分為暴力匹配法（Brute Force，BF）和快速最鄰近搜索庫法（Fast Library for Approximate Nearest Neigh?bors，F(xiàn)LANN）。BF 即采用圖像特征點暴力匹配找到點集一中每個descriptor 在點集二中距離最近的descriptor；而FLANN 一般將點集一作為訓練集對應模板圖像，點集二作為查詢集，對應查找模板圖的目標圖像，通過直接計算兩個特征點之間的漢明距離［14］是否大于參考值以判別圖像相似性。

2 基于ORB-SLAM2 系統(tǒng)框架的改進誤匹配剔除算法

2.1 粗匹配

通過對ORB 特征點進行粗匹配排序相關函數(shù)，根據(jù)排序結果構造一個假設的生成集并隨機采樣，以獲得描述與該匹配點集相對應的圖像變換信息的最佳擬合模型。

將含有N 個點的數(shù)據(jù)集表示為μN，按照相關性q 對μN中的兩個采樣點ui、uj進行降序排列，表示為：

然后從μN數(shù)據(jù)集中抽取大小為m 的TN個樣本作為采樣點集合M，表示為的評價函數(shù)：

在采樣數(shù)據(jù)集中提出假設，即內部點比外部點更多，具體算法為：

（1）輸入一個包含誤匹配對的粗匹配點對集M，并根據(jù)相關性函數(shù)對M 中的匹配點進行排序。在排序結果中，從大到小選取m 個數(shù)據(jù)子集Sm構建初始擬合模型。

（2）將粗匹配點對集合M 中的數(shù)據(jù)點代入初始擬合模型，根據(jù)生成集大小之間的相關性計算誤差，測試模型性能，并保留評估值較高的ORB 特征點集合。

（3）當?shù)螖?shù)達到設置的閾值時，獲得描述與匹配點集相對應的圖像變換信息的最佳擬合模型［15］。

2.2 運動估計

對相鄰兩幀RGB-D 圖像進行特征點提取與匹配，獲得多對匹配的3D 點：

求解最優(yōu)的R、t，使得：

根據(jù)上式可以看出，兩組3D 點的變換可采用迭代最近點算法（Iterative Closest Point，ICP）求解，分為兩種方式，分別為利用線性代數(shù)求解，如奇異值分解（Singular Value De?composition，SVD），以及非線性優(yōu)化方式求解。

首先定義第i 對點的誤差項為：

然后構建最小二乘問題求解R、t：

兩組點的質心可定義為：

目標函數(shù)可簡化為：

由上式可看出，采用SVD 法求解ICP 主要分為3 個步驟：

（1）計算匹配點的質心位置p、p'，計算去質心坐標。

（2）計算旋轉矩陣R*。

（3）計算平移量t*。

只要求出特征點對之間的旋轉量R，平移量t 便可通過簡單計算得到。展開關于R 的誤差項，表示為：

展開項中，第一項與R 無關，第二項中RT R=I。因此，目標函數(shù)優(yōu)化為：

通過SVD 法求解出最優(yōu)R，定義矩陣：

式中，W 為3×3 矩陣，對W 進行SVD 分解，得出：

式中，U、V 均為對角陣，∑為奇異值組成的對角陣，其中元素從大到小排列。當W 滿秩時，R 為：

求解R 后，代入式t*=p-Rp'可求解出平移量t。當R的行列式為負值時，則取-R 為最優(yōu)解。

3 SLAM 系統(tǒng)總體實現(xiàn)與實驗

3.1 ORB 特征點直接匹配、RANSAC 算法與改進誤匹配剔除算法實驗對比

實驗環(huán)境為Ubuntu18.04 系統(tǒng)、Intel i7-8750H CPU、NVIDIA GTX1060 GPU、8GB 顯存，采用OpenCV3.2.0 開源庫。采集像素大小為640×480 的真實環(huán)境圖像進行實驗，對比ORB 特征點直接匹配、RANSAC 算法和改進誤匹配剔除算法的匹配效果。實驗結果如圖2 所示，圖中的原點代表圖像之間匹配的特征點，原點之間的直線代表特征點的對應匹配關系。

Fig.2 Comparison of mismatch elimination of three algorithms圖2 3 種算法誤匹配剔除對比

從圖2（a）中可以看出，ORB 特征點直接匹配會因各種原因造成錯誤匹配；圖2（b）的RANSAC 算法雖然在一定程度上降低了錯誤匹配數(shù)量，但匹配點數(shù)偏少；圖2（c）的改進誤匹配剔除算法匹配點數(shù)適中。3 種算法的誤匹配剔除時間與匹配點對數(shù)如表1 所示，其中改進誤匹配剔除算法的運行速度比RANSAC 算法提升了38%，匹配點對數(shù)量提高了7%，符合SLAM 系統(tǒng)實時性與匹配準確性的要求。

Table 1 Experimental data of mismatched feature points eliminated by the three algorithms表1 3 種算法剔除誤匹配特征點實驗數(shù)據(jù)

3.2 基于數(shù)據(jù)集的SLAM 算法仿真

在仿真環(huán)境下，采用德國慕尼黑工業(yè)大學的TUM 公開數(shù)據(jù)集［16］驗證基于改進誤匹配剔除的SLAM 算法的有效性。該數(shù)據(jù)集中包含各類環(huán)境序列，且采用專業(yè)設備采集真實運動軌跡和絕對位姿，因此“3.1”項下選取fr1_desk 這個具有豐富紋理的辦公環(huán)境圖像序列對比算法的匹配效果。通過EVO 工具庫分別繪制SLAM 計算出的位姿與數(shù)據(jù)集提供的真實位姿，采用絕對軌跡誤差（Abosulte Trajectory Error，ATE）和均方根誤差（Root Mean Square Error，RMSE）等量化指標［17］評價ORB SLAM2 算法與基于改進誤匹配剔除的SLAM 算法的優(yōu)劣性，實驗結果如圖3、圖4 所示。從三維軌跡圖可以看出，基于改進誤匹配剔除的SLAM 算法的估計軌跡比與ORB SLAM2 算法更連貫，更接近于真實軌跡。

Fig.3 ORB SLAM2 trajectory圖3 ORB SLAM2 軌跡

Fig.4 SLAM algorithm based on improved mismatch elimination trajectory圖4 基于改進誤匹配剔除的SLAM 算法軌跡

絕對位姿誤差APE 指SLAM 系統(tǒng)估計位姿與真實位姿的直接差值，可直觀反映算法精度與軌跡全局一致性。對于運動估計軌跡序列，真實軌跡序列定義為：

式中，trans 表示姿態(tài)的平移向量。

根據(jù)位姿時間戳將真實值與估計值對齊，計算對齊后的APE，繪制其折線圖和數(shù)值分布軌跡圖，具體如圖5 所示。詳細比較誤差值APE 的最大值Max、平均數(shù)Mean、最小值Min 和RMSE，如表2 所示。實驗數(shù)據(jù)表明，基于改進誤匹配剔除的SLAM 算法的RMSE 比改進前的ORB SLAM2優(yōu)化了26%，均值優(yōu)化了20%，最大值與最小值也進一步說明了改進算法的可靠性。圖6（彩圖掃OSID 碼可見）直觀地采用顏色深淺描述兩種算法的誤差，藍色越深代表誤差越小，紅色越深代表誤差越大。顯然，ORB SLAM2 的偏紅色區(qū)域多于基于改進誤匹配剔除的SLAM 算法，說明后者絕對軌跡誤差較小。

Fig.5 APE line chart of the two algorithms圖5 兩種算法的APE 折線圖

Table 2 APE experiment results表2 絕對位姿誤差實驗結果 單位：m

Fig.6 APE map of the two algorithms圖6 兩種算法的APE map 圖

3.3 實際場景實驗

實驗環(huán)境為室內辦公桌，工控機配置為Ubuntu18.04 系統(tǒng)，Intel i7-8750H CPU，NVIDIA GTX1060 GPU，8GB 顯存，視覺傳感器為kinect v1 深度相機，連續(xù)采集圖像序列進行實時計算。從實驗結果來看，基于改進誤匹配剔除的SLAM 算法能實時運行，跟蹤穩(wěn)定，無丟幀和失幀現(xiàn)象，能建立三維稀疏點云地圖，具有一定可行性，具體如圖7 所示。

Fig.7 Keyframe trajectory and 3D sparse point cloud map圖7 關鍵幀軌跡與三維稀疏點云地圖

4 結語

本文提出一種基于改進誤匹配剔除的SLAM 算法，并對其實時性、魯棒性、位姿估計精度和建圖性能進行了測試。實驗結果表明，該算法在特征檢測匹配和去除誤匹配環(huán)節(jié)不僅能保留更多正確匹配點，運行速度也有所提高。相較于ORB-SLAM2，基于改進誤匹配剔除的SLAM 算法獲得的真實軌跡與計算軌跡重合度更高，均方根誤差也更小，具有一定可行性。然而，該算法未充分利用圖像中的語義信息，未來可考慮將環(huán)境中的語義信息加入SLAM 算法中，幫助智能移動機器人更精確地獲取自身位姿，實現(xiàn)在未知環(huán)境中的導航與路徑規(guī)劃。