李少波，張 偉，孫采鷹，任 彥

(內(nèi)蒙古科技大學 信息工程學院，內(nèi)蒙古 包頭 014010)

1 引 言

特征點的提取與匹配是同步定位與建圖(Simultaneous Localization and Mapping)領(lǐng)域中一項重要的研究內(nèi)容[1]，是實現(xiàn)SLAM關(guān)鍵步驟之一，特征點匹配結(jié)果直接影響到機器人運動軌跡的計算精度.因此提高匹配正確率的關(guān)鍵是篩選出正確的匹配點對[2].目前，主流的特征點提取方法有文獻[3]提出的尺度不變特征轉(zhuǎn)換(Scale Invariant Feature Transform，SIFT)算法[4]，該算法在保持尺度、旋轉(zhuǎn)縮放不變性方面具有優(yōu)勢，但是算法復雜度較高，計算量較大.文獻[5]提出的加速穩(wěn)健性特征(Speed-up Robust Feature Transform，SURF)算法解決了SIFT存在的問題，但在光照變化的場景中魯棒性較差.文獻[6]中Ethan Rublee等人首次提出快速特征點提取和描述(Oriented FAST and Rotated BRIEF，ORB)算法，該算法由FAST[7]特征點和BRIEF[8]描述符組成，F(xiàn)AST特征點又稱為“Oriented FAST”；ORB算法不僅具有良好的實時性而且能夠保持特征點具有旋轉(zhuǎn)，尺度不變的特點.特征匹配是SLAM中數(shù)據(jù)關(guān)聯(lián)的關(guān)鍵部分，精確的特征匹配為SLAM的位姿估計和后端優(yōu)化提供精確的初值.目前，最簡單的匹配方法是暴力匹配，但是當特征點數(shù)量很大時，運算量會增加.文獻[9]提出的快速最近鄰(Fast Library For Approximate Nearest Neighbors，F(xiàn)LANN)算法更加適合匹配點數(shù)很多的情況[10]，但是不可避免的存在誤匹配.文獻[11]、文獻[12]中Fischler等人提出了隨機采樣一致性(Random Sample Consensus，RANSAC)算法，該算法是目前主流的匹配點對優(yōu)化方法.然而RANSAC算法的計算量較大，效率不高.Ondrej Chum和 Jiri Matas[13]提出的PROSAC方法，該方法首先要對樣本點與模型的相關(guān)性排序，然后再從相關(guān)性大的樣本點對中進行抽樣.雖然減少了抽樣次數(shù)，但容易造成局部抽樣，導致算法的魯棒性下降.張永祥[14]等人提出的改進RANSAC基礎(chǔ)矩陣方法能夠減少誤匹配，但是算法的運行時間較長.

視覺里程計是SLAM算法的關(guān)鍵環(huán)節(jié)，主要用于估計幀間的相機運動.本文以RGB-D視覺SLAM為背景，提出一種輪式水平地面運動機器人誤匹配特征點對篩除方法.該方法用于SLAM的視覺里程計部分，對粗匹配的特征點對進行了二次處理.首先，本文利用ORB法對機器人采集的圖像進行了特征點的提取，并用暴力匹配法對幀間圖像進行了特征點粗匹配.然后，本文利用機器人的運動約束條件建立了幀間圖像特征匹配點對的約束條件，并用該條件濾除了暴力匹配法產(chǎn)生的誤匹配特征點對.最后，本文將優(yōu)選出來的特征匹配點對用于RANSAC算法，估計幀間相機的運動.實驗證明，基于等高約束條件和直線約束條件的誤匹配特征點對篩除算法可有效的篩除幀間誤匹配特征點對，同時能夠提高RANSAC算法的執(zhí)行速度，并且能夠提高RANSAC算法對機器人相機的運動估計精度.

2 匹配特征點對約束條件

2.1 相機運動約束條件

平面運動輪式機器人相機運動約束條件如下：

1)機器人相機只能繞與地面垂直的軸旋轉(zhuǎn);

2)機器人相機垂直方向無平移.

相機坐標系以圖1方式建立，規(guī)定z軸為相機光軸方向.機器人在水平地面的運動時，其相機的運動可以分解為旋轉(zhuǎn)部分和平移部分.相機旋轉(zhuǎn)運動只能以y軸為軸，而相機在y軸方向的平移分量為零.因此機器人相機的運動是在有約束條件下進行的，這種約束關(guān)系最終會體現(xiàn)在所采集的RGB-D圖像上.

2.2 等高約束條件下的匹配特征點對約束

根據(jù)機器人的運動約束條件，當相機隨機器人從當前位置運動到下一個位置時，相機坐標系運動特點如圖2所示.

規(guī)定圖2中o1x1y1z1相機坐標系為參考坐標系，其位姿矩陣為4×4單位陣[15]，o2x2y2z2為相機運動后的坐標系，由相機運動約束條件可知，相機光心o1與o2等高.o2x2y2z2坐標系是由o1x1y1z1繞y1軸旋轉(zhuǎn)并且沿著x1和z1方向平移得到，所以，o2x2y2z2坐標系到o1x1y1z1坐標系的傳輸矩陣為：

(1)

式(1)中，θ為繞y1軸的旋轉(zhuǎn)角度，tx為x1軸方向的平移量，tz為z1軸方向的平移量，因為機器人相機高度不變，所以y軸方向的平移量為0.

設(shè)空間一點P在o1x1y1z1坐標系中的齊次坐標為P1=[x1，y1，z1，1]T，在o2x2y2z2坐標系中的齊次坐標為P2=[x2，y2，z2，1]T，由式(1)可得：

(2)

由相機運動約束可得：

y1=y2

(3)

即，相機坐標系運動前后空間一點P的y軸坐標不變.式(3)為等高約束條件.

當相機坐標系為o1x1y1z1時，設(shè)P點的像素齊次坐標為A1=[u1，v1，1]T，當相機坐標系為o2x2y2z2，設(shè)P點像素齊次坐標為A2=[u2，v2，1]T.如圖3所示.

圖3 成像平面投影示意圖Fig.3 Schematic diagram of imaging plane projection

相機內(nèi)參矩陣為：

(4)

其逆矩陣為：

(5)

由相機成像模型可得：

A1=KP1

(6)

坐標A1與P1坐標的關(guān)系為：

P1=K-1A1

(7)

式(7)展開式為：

(8)

由式(8)可得：

(9)

同理可得：

(10)

式(9)與式(10)中的z1及z2可由深度圖獲得，v1及v2可由彩色圖獲得，相機內(nèi)參可通過相機內(nèi)參標定獲得.由于相機徑向畸變、切向畸變、深度值測量誤差及機器人運動時相機輕微抖動等因素，式(9)式(10)計算出的y1和y2坐標往往不相等，同時，為了避免分母為零的情況出現(xiàn)，定義等高約束條件下的匹配特征點對約束為式(9)與式(10)的差值.為方便敘述，將等高約束條件下的匹配特征點對約束命名為等高約束.

(11)

式(11)中的Δy可以根據(jù)統(tǒng)計動態(tài)調(diào)節(jié)范圍或簡單的設(shè)定一個固定的值.

等高約束可篩除大部分誤匹配點對，其中包括彩色圖匹配正確但深度值誤差較大的匹配點對，但無法篩除y軸高度相同的誤匹配點對.

由于機器人運動速度較慢且圖像采集頻率較高，相鄰兩幀圖像相機運動范圍很小，因此像素坐標v1和v2非常接近.若v1和v2的值非常接近cy時，式(11)不能有效的篩除深度值誤差較大的匹配點對，因此需要增加誤匹配約束條件.

2.3 直線約束條件下的匹配特征點對約束

由式(2)可得：

x2=x1cosθ+z1sinθ+tx
z2=-x1sinθ+z1cosθ+tz

(12)

由于機器人運動速度較慢且圖像采集頻率較高，相鄰兩幀圖像相機運動范圍很小，所以θ值較小，式(12)可近似表示為：

(13)

整理式(13)得：

z1θ+tx=x2-x1

(14)

-x1θ+tz=z2-z1

(15)

對于RGB-D相機，當特征匹配點對的像素坐標已知時，式(14)、式(15)中的深度值z1、z2可直接由深度圖得到，同時可由式(8)計算出x1、x2、y1、y2的空間坐標值.因此，式(14)、式(15)是直線方程，變量分別為平移量tx、tz及旋轉(zhuǎn)量θ.

由于等高約束已經(jīng)篩除了大部分的誤匹配點，剩余少量的誤匹配點對直線擬合效果影響很小，因此可使用所有經(jīng)過等高約束過濾后的匹配特征點對，分別對式(14)、式(15)兩條直線做擬合，并計算出直線方程中tx、tz及旋轉(zhuǎn)量θ.距離式(14)、式(15)表示兩條直線遠的匹配點為誤匹配點，該條件為直線約束條件下的匹配特征點對約束條件.為方便敘述，將直線約束條件下的匹配特征點對約束條件命名為直線約束.

等高約束式(11)與直線約束式(14) 、式(15)可通過較小的運算量有效的篩除誤匹配的特征點對，其中包括像素匹配正確但深度值誤差較大的匹配點對，為視覺里程計提供優(yōu)質(zhì)的特征匹配點對.

3 等高約束和直線約束與RANSAC算法的結(jié)合

隨機采樣一致性算法是目前主流的匹配點對優(yōu)化方法，該算法的擬合模型參數(shù)由觀測數(shù)據(jù)子集計算得到，從而找到足夠多的有效樣本點[16].

RANSAC算法原理包括3個階段：抽樣建模階段、模型驗證階段、得出最優(yōu)模型階段.抽樣建模階段通過隨機抽取樣本生成模型；模型驗證階段將樣本所有數(shù)據(jù)代入模型，根據(jù)內(nèi)點誤差閾值統(tǒng)計內(nèi)點個數(shù)；得出最優(yōu)模型階段經(jīng)過有限次迭代，選出內(nèi)點數(shù)最多的模型.該模型為相機傳輸矩陣.RANSAC算法中的迭代次數(shù)由下式計算：

(16)

式(16)中，k表示迭代次數(shù)，z表示置信度，一般取z=0.99.n代表計算模型需要的數(shù)據(jù)點對數(shù)，取n=4，因為RANSAC使用P3P算法計算單應(yīng)矩陣，P3P最少需要4對匹配點.ω表示內(nèi)點在數(shù)據(jù)集中的比例.由于暴力匹配法產(chǎn)生的誤匹配特征點對較多，導致ω較小，由式(16)可知，迭代次數(shù)k增大.即誤匹配點對越多，迭代次數(shù)越多.本文采用等高約束條件和直線約束條件與RANSAC算法結(jié)合，在暴力匹配法產(chǎn)生的匹配點對的基礎(chǔ)上進一步篩除誤匹配，增加了正確匹配點對比例，因此內(nèi)點的概率ω增大，迭代次數(shù)k隨之減小，進而減少了算法的計算時間，提高了算法的效率.

因此，先使用本文提出的約束條件對誤匹配特征點對進行篩除，再使用RANSAC算法計算相機運動.算法步驟如下：

Step1.獲取第一組圖中特征點的像素值及深度值.

Step2.計算該特征點對應(yīng)的空間3D點在相機坐標系下的坐標.

Step3.獲取第二組圖中對應(yīng)匹配特征點的像素值及深度值.

Step4.計算該匹配特征點的空間3D點在相機坐標系下的坐標.

Step5.計算式(11)中等高約束條件的閾值，篩除大于閾值的匹配點對.

Step6.使用step 5優(yōu)選的匹配點對，根據(jù)式(14)、式(15)做兩條直線擬合.

Step7.篩除到直線距離超過閾值的匹配點對.

Step8.保存經(jīng)過兩次篩選后的匹配點對.

Step9.將保留匹配點對作為RANSAC算法的輸入計算相機運動.

4 實驗結(jié)果和分析

4.1 基于MORO機器人算法驗證

MORO是北京一維弦教育科技有限公司研發(fā)的一款輪式機器人，底盤配備有差動輪，并配置有RGB-D相機.如圖4所示.

圖4 MORO機器人Fig.4 MORO robot

本實驗首先通過機器人采集實驗室的相鄰兩幀圖像，然后使用ORB算法進行了特征點提取，最后使用暴力匹配法對兩幀圖像的特征點進行了匹配.其中匹配結(jié)果如圖5所示.圖5(a)中存在大量誤匹配結(jié)果，不能直接用于計算相機運動的計算，需要進一步篩選優(yōu)化.參照式(11)與式(14)、(15)，在圖5(a)匹配實驗結(jié)果的基礎(chǔ)上增加等高約束條件和直線約束條件.本實驗等高條件閾值為20，直線約束條件閾值為0.15.

經(jīng)等高約束條件和直線約束條件篩選后，圖5(b)中保留的匹配點對全部正確.圖5(c)顯示結(jié)果為篩除的匹配點對，其中包括彩色圖匹配錯誤點對和彩色圖匹配正確但深度值誤差較大的匹配點對.

圖5 特征匹配點對篩選結(jié)果Fig.5 Feature matching point pair screening results

4.2 基于TUM數(shù)據(jù)集的算法驗證

本文采用TUM數(shù)據(jù)集(rgbd_dataset_freiburg2_pioneer_slam3)對基于運動約束的匹配特征點對篩選方法進行了進一步的分析與驗證.該數(shù)據(jù)集采用搭載RGB-D相機的輪式機器人采集圖像，同時提供了采集每幀圖像時相機的位姿信息，可以用于匹配點對誤差分析.TUM數(shù)據(jù)集是機器人在水泥地面上采集的，且機器人運動過程中相機有輕微的抖動，因此本實驗也可檢驗本文算法是否具有良好的適應(yīng)性.

圖6 特征匹配點對篩選結(jié)果Fig.6 Feature matching point pair screening results

本文首先采用RANSAC算法對所有暴力匹配特征點對進行運算，同時顯示保留的匹配特征點對，實驗結(jié)果如圖6.圖6(a)是兩幅圖像暴力匹配結(jié)果.圖6(b)是RANSAC算法保留的匹配點對，可見匹配結(jié)果中仍有明顯的誤匹配.

圖7 等高約束條件特征匹配點對篩選結(jié)果Fig.7 Contour constraint feature matching point pair filter results

圖7(a)是經(jīng)等高約束條件過濾后保留的匹配點對，可見等高約束條件可濾除圖6(a)中大部分誤匹配，但仍有少量的誤匹配點對無法濾除.根據(jù)式(14)，對等高約束條件篩選后匹配點對進行直線擬合，其效果如圖7(b)所示.可以根據(jù)式(15)做另一條直線的擬合，方法類似，不再累述.本實驗等高約束條件閾值為30.

圖8 直線約束結(jié)合等高約束特征匹配點對篩選結(jié)果Fig.8 Line constraint combined with contour Constraint feature matching point pair filter results

去掉離兩條直線較遠的點，其效果如圖8(a)所示.可見保留的匹配點對全部正確.圖8(b)是圖7(a)去掉離線較遠的點得到的.本實驗等高約束條件閾值為30，兩條直線約束條件閾值均為0.03.可以看出直線約束條件對匹配點對篩選效果進一步改善，濾除掉更多誤匹配.

圖9(a)是RANSAC算法篩除的誤匹配點對，圖中顯示被篩選出來的誤匹配較少.圖9(b)是等高約束條件結(jié)合直線約束條件篩除的誤匹配點對.可見加入等高約束條件和直線約束條件后對誤匹配點對篩除效果更好.

4.3 匹配誤差分析

為進一步驗證算法的性能，本文采用兩幅圖像匹配特征點對應(yīng)世界坐標的誤差來評價配準精度．本文對圖6(a)、圖7(a)、圖8(a)中的實驗結(jié)果進行誤差分析.在o1x1y1z1參考坐標系下，通過像素坐標A1計算空間坐標P1;同理，在o2x2y2z2坐標系下，通過像素坐標A2計算得到空間坐標P2，然后通過傳輸矩陣T將P2變換到o1x1y1z1坐標系下.理論上這兩個點應(yīng)完全重合，但由于實際的參數(shù)誤差及采樣誤差，兩個點之間會存在一定的誤差.誤差計算公式如下：

e[i]=norm(z1[i]k-1A1[i]-z2[i]Tk-1A2[i])

(17)

式(17)中，e[i]為第i組匹配點誤差的模，A1[i]為第一幀圖像中第i組匹配點像素坐標，z1[i]是A1[i]對應(yīng)深度值，A2[i]為第二幀圖像中第i組匹配點像素坐標，z2[i]是A2[i]對應(yīng)深度值，k為相機內(nèi)參矩陣，T為o1x1y1z1坐標系到o2x2y2z2坐標系的傳輸矩陣，T可以通過數(shù)據(jù)集中提供的相機位姿計算.

圖10(a)是圖6(a)中兩幅圖像暴力匹配特征點對誤差統(tǒng)計圖，包含正確匹配誤差和錯誤匹配誤差.圖10(b)是圖7(a)中經(jīng)等高約束條件過濾后保留的匹配點對誤差統(tǒng)計圖，可以看出等高約束能夠有效的篩除大部分的誤匹配，并提高了匹配精度.圖10(c)是圖8(b)中直線約束條件在等高約束條件優(yōu)化的基礎(chǔ)上保留的匹配點對誤差統(tǒng)計圖，加入直線約束條件后誤匹配篩除能力進一步提升，保留特征點的匹配誤差都下降到0.5以下.因此，本文提出的等高約束條件和直線約束條件能有效篩除誤匹配，為RANSAC算法計算相機運動提供更加準確的匹配特征點對.

本文對傳統(tǒng)RANSAC算法與結(jié)合了等高約束條件、直線約束條件的RANSAC算法性能進行了比較.首先進行了兩種算法的誤差比較，然后進行了運算時間比較.誤差比較如圖11所示.

圖11(a)為平移誤差，菱形點為RANSAC算法平移誤差，五角形點是結(jié)合了等高約束條件的RANSAC算法平移誤差，圓形點是結(jié)合了等高約束條件與直線約束條件的RANSAC算法平移誤差.由圖可知僅使用RANSAC算法計算的相機平移量誤差相對較大，結(jié)合等高約束條件后，平移誤差有明顯的下降，但也存在少量匹配點誤差較大的情況.結(jié)合了等高約束與直線約束條件后，RANSAC算法計算的平移誤差進一步減小.

圖10 特征點對應(yīng)坐標誤差分析結(jié)果Fig.10 Coordinate corresponding to characteristicpoints error analysis results

圖11 平移誤差旋轉(zhuǎn)誤差評估結(jié)果Fig.11 Translation error rotation errorevaluation results

圖11(b)為相機旋轉(zhuǎn)誤差，評估量為垂直方向轉(zhuǎn)角.菱形點為RANSAC算法旋轉(zhuǎn)誤差，五角形點是結(jié)合了等高約束條件的RANSAC算法旋轉(zhuǎn)誤差，圓形點是結(jié)合了等高約束條件與直線約束條件的RANSAC算法旋轉(zhuǎn)誤差.由圖可知僅使用RANSAC算法時存在較大旋轉(zhuǎn)誤差，結(jié)合等高約束條件的RANSAC算法可有效降低旋轉(zhuǎn)誤差，但仍存在少量誤差較大的點.在此基礎(chǔ)上加入直線約束條件，旋轉(zhuǎn)誤差誤差更小，達到進一步優(yōu)化的效果.

表1 算法時間比較Table 1 Time comparison of algorithms

表1為3種算法運算時間統(tǒng)計.結(jié)合等高約束的RANSAC算法與傳統(tǒng)RANSAC相比，計算時間減少了29.55%.結(jié)合等高約束與直線約束的RANSAC算法與傳統(tǒng)RANSAC相比，計算時間減少了17.27%.

5 結(jié)束語

視覺里程計是視覺SLAM的重要環(huán)節(jié)，而正確的幀間特征點提取與匹配是視覺里程計的基礎(chǔ).本文根據(jù)裝配RGB-D相機的平面運動輪式機器人的運動約束，提出了幀間特征點對匹配約束條件，分別為等高約束條件與直線約束條件.這兩個約束條件可以有效的去除幀間誤匹配特征點對，從而到了理想的特征點匹配點對.等高約束與直線約束對各種室內(nèi)平面場景有很強的抗干擾能力，對非嚴格水平地面也有較好的適應(yīng)性.同時，本文約束條件可與RANSAC算法相結(jié)合使用來估計幀間運動.實驗結(jié)果表明，添加了本文約束條件的RANSAC算法估計相機幀間運動耗時更少，誤差更小.

本文提出約束條件具有以下的優(yōu)點：

1)對錯誤匹配點對的識別能力強;

2)可剔除匹配正確但深度值誤差大的匹配點對;

3)用較小的計算量有效的排除誤匹配點對;

4)可與視覺里程計常規(guī)算法結(jié)合，提高算法性能.

該方法也存在一些不足：

對于某些非嚴格滿足約束條件的場景(如平整度不高的水泥地面，或攝像頭固定水平度差等情況)會刪除少量的正確匹配點，以犧牲少量正確匹配點的代價換取更快的計算速度和更高的計算精度.