張?zhí)煊睿瑓菓延睿愔经h(huán)

(武漢科技大學 機器人與智能系統(tǒng)研究院，湖北 武漢 430081)

0 引 言

視覺同時定位與地圖構(gòu)建(vision simultaneous localization and mapping，V-SLAM)是一種通過視覺傳感器來建立環(huán)境模型和估算自身位姿的系統(tǒng)。該系統(tǒng)中視覺里程計的實現(xiàn)方法主要有直接法[1]和特征點法[2-6]兩種。由于特征點法具有較高的穩(wěn)定性和較好的魯棒性，所以特征點法一直都是當前SLAM系統(tǒng)的主流方案。ORB(oriented fast and rotated brief)算法是特征點法中表現(xiàn)較突出的一種方法。

因此很多學者為了提高視覺里程計的精度，圍繞ORB算法進行研究和改進。Xin等[7]利用PROSAC算法剔除ORB特征匹配中的誤匹配提高算法精度。姚海芳等[8]綜合SURF算法的魯棒性和ORB算法實時性，提出了一種S-ORB來改善算法的實時性和準確性。李艷山等[9]在提取特征ORB點時，采用二分區(qū)域法對圖像進行預(yù)處理，以縮短特征點提取的時間，較好改善匹配速度和精度。

以上研究者所提出的方法雖能較好完成特征提取、特征匹配和誤匹配剔除，提高視覺里程計的定位精度。但在上述方法大多采用固定閾值進行特征提取，并未考慮到環(huán)境亮度變化對特征提取的影響。例如在一些實際環(huán)境中，由于窗戶和地面反光、陰雨天以及物體遮擋產(chǎn)生陰影等原因，會使得圖像亮度發(fā)生變化，導(dǎo)致特征提取和匹配的結(jié)果并不準確，從而使視覺里程計的定位精度降低。本文針對灰度變化提出一種自適應(yīng)閾值ORB算法，并引入漸進一致性算法剔除誤匹配，旨在提高視覺里程計的定位精度。

1 本文工作

1.1 視覺里程計框架介紹

視覺里程計目標是根據(jù)相機拍攝的圖像信息對相機的運動(包括相機平移T和旋轉(zhuǎn)R)進行估算，獲取實時的位姿信息。本文提出一種基于改進ORB算法的視覺里程計定位方法，具體的視覺里程計框架如圖1所示，首先對Kinect相機采集的深度圖和彩色圖并進行配準，然后在特征匹配階段根據(jù)圖像的灰度變化設(shè)置ORB特征提取的閾值并使用漸進一致性算法(PROSSAC)進行誤匹配剔除，提高特征提取和匹配的準確率，再結(jié)合Kinect相機的深度圖和彩色圖配準的結(jié)果采用ICP算法來求解相機位姿，最后用光束法平差(bundle adjustment，BA)對位姿圖進行優(yōu)化，獲取Kinect相機的實時位姿信息以及軌跡。

圖1 視覺里程計框架

1.2 三維點云生成

為了配合ICP求解位姿，需要將二維像素點轉(zhuǎn)化為三維空間點，生成三維點云。首先采用張正友標定方法[10]對相機進行標定和配準，獲取相機的內(nèi)參矩陣K，如式(1)所示

(1)

其中，fx，fy分別是x，y軸上的焦距；cx，cy為像素坐標系和成像平面的平移量。

結(jié)合相機內(nèi)參矩陣，利用針孔模型將二維像素點轉(zhuǎn)化為對應(yīng)的三維坐標點，設(shè)一個圖像特征點的像素坐標 (u，v) 和相機的測量深度d，可以求得特征點在三維空間的位置坐標 (x，y，z)， 計算如式(2)～式(4)所示

x=(u-cx)·z/fx

(2)

y=(v-cx)·z/fy

(3)

z=d/s

(4)

其中，s為Kinect相機的縮放因子。

1.3 ORB特征提取和匹配

ORB算子是一種定向二進制描述符，由FAST關(guān)鍵子和BRIEF描述子組成，該算子與SURF算子、SIFT算子相比，具有更快的特征提取速度，因此采用速度更快的ORB算子更符合里程計定位實時性要求。

1.3.1 ORB特征提取及其改進

ORB算法的特征提取部分由FAST算法來完成，該方法主要檢測局部像素灰度變化明顯的位置，考慮中心像素點p，設(shè)中心像素點的灰度為Ip，如圖2所示。

圖2 FAST特征點

假設(shè)中心點附近的圓形區(qū)域上的16個像素，有12個連續(xù)點的灰度滿足條件如式(5)，若滿足，則為特征點，反正，不是特征點。其中T為閾值，Ix代表圓形區(qū)域上的16個點的像素

∑x∈circle(p)|Ix-Ip|>T

(5)

但是在特征提取時，傳統(tǒng)ORB提取算法所使用的閾值T是人為設(shè)定的固定值，當環(huán)境亮度發(fā)生變化時，相機采集的圖片亮度也會變化，特征提取的數(shù)量會劇烈減少或者增加，這會導(dǎo)致特征提取錯誤和特征匹配錯誤，故本文采用一種自適應(yīng)閾值的方法，利用每個候選點周圍矩形區(qū)域的亮度來求取閾值，使得算法能在環(huán)境亮度變化的情況能提取到穩(wěn)定的特征點。

具體的自適應(yīng)閾值計算方法如下：設(shè)圖像上的p點(x0，y0)為候選特征點，取以(x0，y0)為中心且邊長為L方形區(qū)域，如圖2中矩形方框所示，定義特征閾值為T，計算公式如下

(6)

其中，Imax和Imin分別代表方形區(qū)域中最大的n個灰度值和最小的n個灰度值，Iarer為方形區(qū)域灰度的平均值，a為比例系數(shù)。文中a=3。

改進后的ORB特征提取流程如下：

(1)首先計算圓形區(qū)域中心點p的像素灰度Ip，再根據(jù)式(6)計算特征提取的閾值T。

(2)對p像素點附近的圓形區(qū)域上的16個像素進行采樣，每間隔3個像素點采樣一次，總共選取4個點，本文選取像素點分別為1，5，9，13(也可以取像素點2，6，10，14)，設(shè)這些點的像素灰度分別是I1、I5、I9、I13，判斷這些點的灰度是否滿足式(5)，若滿足，則p點為候選特征點，反之舍去。

(3)計算圓形區(qū)域上剩下的12個像素，設(shè)它們的像素灰度為Ix，判斷是否有9個點像素滿足式(5)，若存在，則p點為特征點，否則不是特征點。

為了實現(xiàn)特征點的旋轉(zhuǎn)不變性，通過灰度質(zhì)心法來確定特征點的方向。在以p點為圓心，半徑為3的圓形圖像塊內(nèi)，定義(x，y)為圖像塊內(nèi)的點相對于圓心的坐標，質(zhì)心矩mab、質(zhì)心C和特征方向θ如式(7)～式(9)所示

(7)

C=(m10/m00,m01/m00)

(8)

θ=arctan(m01/m10)

(9)

其中，m00為0階質(zhì)心矩；m10和m01分別代表x軸和y軸方向的1階質(zhì)心矩。

1.3.2 ORB特征匹配與誤匹配剔除

特征匹配的作用主要是通過兩張圖片的空間位置關(guān)系求解，以獲取相機的位姿。通常采用FLANN方法來進行粗匹配，然后繼續(xù)使用RANSAC(random sample consensus)對錯誤匹配進行剔除，提高匹配的準確率。傳統(tǒng)RANSAC算法雖然能剔除誤匹配，但是該剔除誤匹配的過程具有隨機性，這會導(dǎo)致計算的迭代次數(shù)不穩(wěn)定，因此本文采用PROSAC(progressive sampling consensus)算法對誤匹配進行剔除，相比RANSAC方法均勻地從整個集合中采樣，PROSAC則是先將數(shù)據(jù)排序，再按照質(zhì)量和的高低來選取數(shù)據(jù)，所以這種方法具有更高的效率和準確性。該方法通常是將兩幅圖片的匹配結(jié)果作為一個采樣集合un，再根據(jù)匹配對的質(zhì)量方程進行降序排序[11]，最后從質(zhì)量較高的匹配對中隨機抽取一部分匹配對計算參數(shù)模型和內(nèi)點數(shù)目，匹配質(zhì)量的判斷如式(10)

(10)

其中，βi和βj分別代表ui和uj樣本點的最小歐式距離和次小歐氏距離的比值，γ為質(zhì)量方程。

具體的PROSAC剔除誤匹配的流程如下：

(1)首先設(shè)置初始迭代次數(shù)為0，同時根據(jù)需求設(shè)置最大迭代次數(shù)和內(nèi)點數(shù)目閾值。

(2)將當前的迭代次數(shù)和最大迭代次數(shù)進行比較，若當前迭代次數(shù)大于最大迭代次數(shù)，找不到對應(yīng)的模型參數(shù)，反之則繼續(xù)執(zhí)行步驟(3)。

(3)按照式(10)依次比較匹配對的質(zhì)量大小并進行質(zhì)量降序排序，同時選取質(zhì)量最高的m個數(shù)據(jù)。

(4)從m個數(shù)據(jù)中隨機選取K個采樣點計算單應(yīng)矩陣H和內(nèi)點數(shù)目。同時判斷內(nèi)點數(shù)目是否大于設(shè)定的內(nèi)點數(shù)目閾值，若大于，則返回單應(yīng)矩陣以及內(nèi)點數(shù)目，反之，重復(fù)步驟(2)、步驟(3)更新內(nèi)點數(shù)目并且重新計算單應(yīng)矩陣H。

1.4 ICP位姿求解

剔除誤匹配之后得到較準確的匹配對，需要對相機的位姿進行求解。常用的位姿求解方法有PNP、ICP和對極幾何方法，由于使用的深度相機采集圖片信息深度是已知的，選擇ICP方法進行位姿求解效率會更高。ICP方法是一種3D-3D的位姿估算方法，已知兩幀圖片分別為F1和F2，其中P={p1，…，p2}∈F1和Q={q1，…，q2}∈F2為一組匹配好的3D點，主要利用SVD[12]分解對相機位姿進行求解，即求取旋轉(zhuǎn)矩陣R和平移矩陣T，使其滿足式(11)

?i,pi=Rqi+T

(11)

1.5 基于光速法平差的位姿圖優(yōu)化

因為相機初始估算的位姿存在未知噪聲和誤差，會導(dǎo)致定位不準確。通常采用光速法平差將相機位姿最優(yōu)化處理，將路標點和位姿作為代優(yōu)化的變量，運動模型和觀測模型作為約束條件?；诠馑俜ㄆ讲畹膱D優(yōu)化模型可以表示為一個最小二乘問題，模型主要對位姿xk進行優(yōu)化，那么目標函數(shù)F(X)可以表示為

F(X)=∑(i,j)∈ce(xi,xj,zij)TΩije(xi,xj,zij)

(12)

X*=argminF(X)

(13)

其中，F(xiàn)(X) 為目標函數(shù)，X是全部位姿變量的集合，xi，xj是i、j時刻估計出的機器人位姿，zij表示i、j時刻的觀測約束條件，e為實際的觀測值和計算出來的觀測值的誤差。Ωij代表約束信息矩陣，是對ei、ej誤差項相關(guān)性的預(yù)估。

由于文中的觀測方程是非線性函數(shù)，因此需要使用g2o[13]工具求解上述代價函數(shù)，該方法采用列文伯格-馬夸爾特方法作為非線性優(yōu)化的求解器，向求解器中添加當前位姿和圖像特征點(路標點)，并將位姿和路標點作為g2o圖的頂點，以位姿和路標點的連線作為g2o圖的邊，對g2o圖的兩個頂點和一條邊進行優(yōu)化，得到最優(yōu)位姿的集合和路標點的空間位置集合。

2 實驗結(jié)果與分析

為了驗證本文算法的可行性和準確性，使用德國慕尼黑工業(yè)大學公開的TUM標準數(shù)據(jù)集的3個不同場景進行測試。筆記本電腦的配置：內(nèi)存4 G、CPU為intel i5-4210U，2.5 GHZ，電腦操作系統(tǒng)為Ubuntu16.04。軟件平臺采用OpenCV3.2.0、PCL1.8.0和Eigen庫。本文首先測試改進ORB特征提取和特征匹配算法的適應(yīng)性和精度，最后使用改進后的ORB算法進行跟蹤定位，并且與RGB-D SLAMv2算法和ORB-SLAM2算法的實驗結(jié)果進行對比，最后使用JS-R移動機器人驗證改進方法的可行性。

2.1 改進ORB算法實驗驗證

2.1.1 特征提取和匹配實驗

為了驗證改進ORB提取算法的對圖像整體亮度變化的適應(yīng)性，實驗分為A、B、C(亮度50%、亮度100%和亮度150%)3組進行特征提取，并將傳統(tǒng)ORB算法與改進的ORB算法進行對比，特征提取結(jié)果如圖3所示。由于圖像在亮度增強時提取到的特征點會變多而亮度減弱時提取到的特征點會變少，傳統(tǒng)ORB算法在亮度變化時并不能提取穩(wěn)定的特征點數(shù)量，為了驗證改進的算法的特征提取效果，采用特征點數(shù)量作為判斷亮度適應(yīng)性好壞的標準。

圖3 不同亮度下特征提取結(jié)果

從表1可以看出改進的ORB特征提取算法在光線變暗或者變亮的情況下，檢測到的特征點數(shù)量變化較小。而傳統(tǒng)的ORB特征提取算法在圖像亮度增強時，特征點重疊較為嚴重；在亮度減弱時，特征點的數(shù)量會劇烈減少，亮度的變化對其影響較大。實驗結(jié)果表明，改進ORB特征提取算法的在整體亮度的變化時能提取到更穩(wěn)定的特征點，具有更好的適應(yīng)性。

進一步測試改進的ORB算法在亮度變化時的匹配情況，并且與傳統(tǒng)ORB特征匹配算法對比。在強光、弱光以及強弱光變化時，傳統(tǒng)ORB特征匹配會產(chǎn)生較多的誤匹配，因此實驗主要在強光環(huán)境、弱光環(huán)境和強弱光變化3種環(huán)境中來驗證改進ORB特征提取的效果。測試結(jié)果如圖4所示。

表1 特征提取結(jié)果

圖4 改進的ORB特征匹配結(jié)果對比

圖4是改進的ORB特征匹配算法和傳統(tǒng)ORB特征匹配算法的實驗結(jié)果，從圖4中可以看出傳統(tǒng)ORB算法匹配時，會出現(xiàn)明顯的交叉錯誤匹配對，特別的在強弱光變化的情況下交叉誤匹配對更多，而改進后的ORB算法在3種光照環(huán)境中均未出現(xiàn)明顯錯誤的匹配對，說明改進后ORB算法的特征匹配結(jié)果更為精確。

2.1.2 PROSAC誤匹配剔除實驗

為了驗證PROSAC算法剔除誤匹配的高效性，將ORB+PROSAC方法、傳統(tǒng)ORB+RANSAC方法以及未進行誤匹配剔除的ORB進行對比，測試結(jié)果如圖5，圖5中RANSAC算法和PROSAC算法都成功剔除了誤差，沒有出現(xiàn)圖5(a)中明顯的交叉匹配情況。為了更好評估PROSAC算法剔除誤匹配的性能，進一步比較PROSAC算法和RANSAC算法的剔除情況，本文使用匹配總時間、剔除誤匹配時間以及內(nèi)點數(shù)作為評估指標，實驗結(jié)果見表2。

圖5 匹配效果對比

表2 RANSAC和PROSAC算法比較

從表2中可以看出來，PROSAC算法剔除誤匹配與RANSAC算法剔除誤匹配相比獲得的內(nèi)點數(shù)要更多，說明PROSAC算法獲得正確模型的成功率也較高，因此匹配的正確率也會更高，另一方面該算法剔除誤匹配時間和匹配總時間都較RANSAC算法要少，表明利用PROSAC算法剔除誤匹配會更加高效和準確，進一步提高了相機定位的實時性和精度。

2.2 定位實驗結(jié)果和誤差分析

由于室內(nèi)環(huán)境中容易出現(xiàn)陰影、桌子遮擋和地面反光等情況，導(dǎo)致相機采集的圖片亮度發(fā)生變化，因此選擇TUM標準數(shù)據(jù)集中室內(nèi)場景fr1/desk、fr1/floor和fr1/room這3個序列來測試，并且與RGB-D SLAMv2算法和ORB_SLAM2算法進行對比，最后使用evo工具畫出3種算法的軌跡圖，并且對3種算法進行評估和分析。

為了對測試結(jié)果進行量化評估，通常會采用絕對軌跡誤差(ATE)的均方根(root mean square error，RMSE)誤差作為相機定位效果的量化指標。假設(shè)相機估算軌跡序列是X={X1，…，Xn}， 相機的實際運動軌跡序列Y={Y1，…，Yn}， 那么相機的絕對軌跡誤差的均方根誤差可表示為

(14)

圖6是在3種算法在不同場景下相機運動軌跡和真實軌跡對比結(jié)果，從圖6中可以看出改進后的算法軌跡圖沒有明顯的偏離真實軌跡，與真實軌跡基本一致。通過絕對軌跡誤差的均方根誤差對實驗結(jié)果進行量化評估，結(jié)果見表3，其中RMSE值越小，說明相機偏離真實軌跡的距離越大，相應(yīng)相機的定位精度越低，反之說明相機偏離真實軌跡距離越小，相機的定位精度越高。

表3 不同場景下的RMSE對比

從表3中可以看出，在fr1/desk和fr1/floor場景下，改進算法的絕對軌跡均方根誤差僅為0.0161 m和0.0154 m，且均低于ORB_SLAM2和RGB-D SLAMva。特別的在fr1/floor場景其絕對軌跡誤差的均方根誤差較其它兩種算法分別降低了26.6%和15.2%。而在fr1/room場景下其絕對軌跡的均方根誤差為0.0583 m，雖略大于ORB_SLAM2算法絕對軌跡均方根誤差，但是誤差小于RGB-D SLAMva算法的絕對軌跡均方根誤差。實驗結(jié)果表明，在大部分情況下本文提出的方法的絕對軌跡均方根誤差都要小于另外兩種方法，說明改進后的視覺里程計的定位精度更高。

2.3 實際場景測試

為驗證改進后的算法在實際場景中的可行性，實驗借助JS-R移動機器人來測試算法性能，JS-R移動機器人搭載Kinectv1相機，如圖7(b)所示。圖7(a)是實驗室測試場地，控制機器人沿著標記線方向運動，用evo工具畫出JS-R移動機器人的運動軌跡，測試的結(jié)果如圖8所示，軌跡圖的兩端點分別是JS-R移動機器人的起點和終點，用START和END來表示。

圖6 不同場景下各種算法軌跡

圖7 實驗場景和實驗平臺

圖8 相機軌跡

由于實際groundtruth數(shù)據(jù)需要專業(yè)設(shè)備獲取，本文無法獲取移動機器人的真實軌跡，故選擇圖7(a)黑色線作為參考軌跡，圖8的START和END分別對應(yīng)圖7(a)中的START和END，通過對比相機軌跡和紅色參考軌跡發(fā)現(xiàn)相機運動較為穩(wěn)定，其軌跡較為平滑并未出現(xiàn)異常的偏離現(xiàn)象，表明改進后的視覺里程計定位較為準確，滿足實際工程中的需求。

3 結(jié)束語

本文提出一種基于改進ORB算法的視覺里程計定位方法，主要針對光照變化對特征提取和匹配的影響，提出一種基于自適應(yīng)閾值的ORB算法，提高特征提取的穩(wěn)定性和特征匹配的精度，同時為解決傳統(tǒng)誤匹配剔除方法(RANSAC)迭代次數(shù)不穩(wěn)定的問題，使用PROSAC來剔除誤匹配，實驗結(jié)果表明，改進后的ORB算法具有較強的光照魯棒性，并且在誤匹配剔除階段效率得到大幅提升。最后通過改進的ORB算法進行跟蹤和定位，實驗數(shù)據(jù)表明，在大部分場景本文提出的方法絕對軌跡誤差的均方根誤差(RMSE)比其它算法更小，最后通過JS-R移動機器人在實際場景中進行測試，實驗結(jié)果表明，改進后的算法較為穩(wěn)定滿足實際需求。綜上所述，本文所提出的方法能夠較準確的進行定位，具有良好的實際應(yīng)用價值。另外，本文只對定位結(jié)果進行測試和比較，并未測試基于改進ORB算法的地圖構(gòu)建效果，為了進一步測試改進的視覺里程計的性能，下一步主要工作將針對地圖構(gòu)建效果進行測試和分析。