改進(jìn)誤匹配剔除的SLAM 算法

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

（昆明理工大學(xué)機(jī)電工程學(xué)院，云南昆明 650500）

0 引言

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

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

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

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

1 特征點(diǎn)匹配算法

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

1.1 FAST 特征點(diǎn)檢測(cè)

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

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

Fig.1 FAST feature points圖1 FAST 特征點(diǎn)

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

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

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

1.2 BRIEF 特征描述子

BRIEF 是一種二進(jìn)制描述子，即每個(gè)關(guān)鍵點(diǎn)是由0 和1組成的二進(jìn)制字符串。定義S×S 大小的圖像領(lǐng)域P 的二值化τ 為：

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

那么，nd個(gè)測(cè)試點(diǎn)對(duì)比形成描述子的計(jì)算公式為：

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

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

1.3 特征點(diǎn)匹配

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

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

2.1 粗匹配

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

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

然后從μN(yùn)數(shù)據(jù)集中抽取大小為m 的TN個(gè)樣本作為采樣點(diǎn)集合M，表示為的評(píng)價(jià)函數(shù)：

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

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

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

（3）當(dāng)?shù)螖?shù)達(dá)到設(shè)置的閾值時(shí)，獲得描述與匹配點(diǎn)集相對(duì)應(yīng)的圖像變換信息的最佳擬合模型［15］。

2.2 運(yùn)動(dòng)估計(jì)

對(duì)相鄰兩幀RGB-D 圖像進(jìn)行特征點(diǎn)提取與匹配，獲得多對(duì)匹配的3D 點(diǎn)：

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

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

首先定義第i 對(duì)點(diǎn)的誤差項(xiàng)為：

然后構(gòu)建最小二乘問(wèn)題求解R、t：

兩組點(diǎn)的質(zhì)心可定義為：

目標(biāo)函數(shù)可簡(jiǎn)化為：

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

（1）計(jì)算匹配點(diǎn)的質(zhì)心位置p、p'，計(jì)算去質(zhì)心坐標(biāo)。

（2）計(jì)算旋轉(zhuǎn)矩陣R*。

（3）計(jì)算平移量t*。

只要求出特征點(diǎn)對(duì)之間的旋轉(zhuǎn)量R，平移量t 便可通過(guò)簡(jiǎn)單計(jì)算得到。展開(kāi)關(guān)于R 的誤差項(xiàng)，表示為：

展開(kāi)項(xiàng)中，第一項(xiàng)與R 無(wú)關(guān)，第二項(xiàng)中RT R=I。因此，目標(biāo)函數(shù)優(yōu)化為：

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

式中，W 為3×3 矩陣，對(duì)W 進(jìn)行SVD 分解，得出：

式中，U、V 均為對(duì)角陣，∑為奇異值組成的對(duì)角陣，其中元素從大到小排列。當(dāng)W 滿秩時(shí)，R 為：

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

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

3.1 ORB 特征點(diǎn)直接匹配、RANSAC 算法與改進(jìn)誤匹配剔除算法實(shí)驗(yàn)對(duì)比

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

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

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

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

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

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

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

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

絕對(duì)位姿誤差A(yù)PE 指SLAM 系統(tǒng)估計(jì)位姿與真實(shí)位姿的直接差值，可直觀反映算法精度與軌跡全局一致性。對(duì)于運(yùn)動(dòng)估計(jì)軌跡序列，真實(shí)軌跡序列定義為：

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

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

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

Table 2 APE experiment results表2 絕對(duì)位姿誤差實(shí)驗(yàn)結(jié)果 單位：m

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

3.3 實(shí)際場(chǎng)景實(shí)驗(yàn)

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

Fig.7 Keyframe trajectory and 3D sparse point cloud map圖7 關(guān)鍵幀軌跡與三維稀疏點(diǎn)云地圖

4 結(jié)語(yǔ)

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