基于改進FAST算法的移動機器人導航

蒲興成，譚少峰，張毅

(1.重慶郵電大學 數理學院，重慶 400065； 2. 重慶郵電大學 自動化學院，重慶 400065)

移動機器人在完全未知的環境中利用自身傳感器獲得周圍環境信息進行實時定位和地圖創建(simultaneous localization and mapping, SLAM)是移動機器人研究領域的一個熱點問題[1]。所謂SLAM，是指機器人在一個未知的環境中，從一個未知的位置開始，通過對環境的觀測，遞增地構建環境地圖，并同時運用環境地圖實現機器人定位的一個過程[2]。

近年來，基于單目視覺的SLAM問題獲得了非常廣泛的研究?，F有的研究表明，在簡單的結構化環境以及地圖信息已知(具有人工路標)的環境中進行視覺導航已經有了很大進展，但在完全未知的環境中進行定位與導航還存在諸多問題。造成這一現象的根本原因在于如何在未知環境中獲取自然特征。所謂自然特征，是指環境中已有的、非人工設置的、能夠用以標識不同環境場景的特征對象[3]。通過提取一些具備局部不變性的角點作為環境的自然特征，該方法選取的自然路標有非常好的穩定性，因此角點檢測也是機器人導航的熱點問題之一[4]，也出現了一些較好的角點檢測方法。FAST(features from accelerated segment test)是一種運算簡單直觀的角點檢測方法，相關測試表明，FAST在計算速度上優于SIFT、SURF和Harris等角點檢測方法[5]。為此，筆者一方面采用改進的FAST算法[6]來提取角點，剔除了大部分邊緣點和局部非極大值點；另一方面，針對誤匹配剔除，采用結合擴展卡爾曼濾波(extended Kalman filter, EKF)的一點RANSAC算法[7]。RANSAC算法是一種應用廣泛的誤匹配剔除算法，但該算法隨著匹配點個數增加，算法迭代次數會迅速上升，降低了算法的計算速度。采用一點RANSAC算法可以充分利用EKF預測階段得到的先驗信息，在確保計算精度的同時，有效降低了算法迭代次數。在整個實驗過程中，機器人通過自身攝像頭，采用改進的FAST角點提取算法采集周圍環境信息，并結合EKF的一點RANSAC算法[7]對誤匹配點進一步剔除，獲得魯棒性更強的特征點，從而構建環境的地圖信息，最后用得到的匹配點進行三維環境重建和機器人定位。

1 FAST角點提取算法

FAST角點檢測算子是Rosten和Drummond在SUSAN角點檢測方法基礎上利用機器學習原理提出的，其運算速度不僅大大高于SIFT、SURF算子，更優于以速度見長的Harris算子。FAST角點檢測的原理是檢測待檢測點與周圍點的灰度是否相近，如圖1(c)所示，即對待檢測特征點p，最多只需檢測16個候選特征點，大大減輕了計算量。圖中每個方格均代表一個像素，位于中心的p點為待檢測特征點，周圍16個方格為檢測過程中需檢測的像素點。角點的判斷過程為：設Ip為待檢測點p的灰度值，Ip→x為圓周上像素點的灰度值，x∈{1,2,…,16}，對半徑為3的圓上的像素進行分割測試， 判定條件如式(1)：

|Ip→x-Ip|≥t

(1)

如果滿足判定條件的像素點數滿足設定數量n，則判定p點為角點，其中，t為設定的閾值，n一般為12或9，分別對應FAST-12與FAST-9檢測算法。

在實際操作中，為了獲得更快的結果，還可以采用一種加速辦法。該方法首先測試候選點周圍每隔90°角的4個點，即圖1(c)中位于水平與垂直位置的1、5、9和13四點，如果至少有3個和候選點的灰度值差足夠大，則繼續計算其他12個點；否則，不用再計算其他點，直接認為該候選點不是特征點。

圖1 FAST角點提取流程Fig.1 Process of FAST corner extraction

經過上述步驟后，會初步提取到一些角點，但這些點中存在較多邊緣點和局部非極大值點，采用文獻中的方法[6]，可以剔除邊緣點，如式(2)所示：

(2)

式中：q為圓周上任意一點，Iq+1與Iq-1為關于q對稱的2個點，從圓周任一點開始計算，若有大于10個點滿足式(2)，則認為該點屬于邊緣點，予以剔除。

在剔除邊緣點后，還要剔除局部非極大值點。通過計算角點候選點周圍很小鄰域內各像素點的拉普拉斯值，進一步剔除一些局部非極大值點。此處局部極大值采用拉普拉斯極值計算公式為

(3)

在Intel Pentium處理器、2 G內存的硬件配置下，從視頻中任取一幀640像素×480像素的圖像利用FAST角點檢測算子進行檢測，實驗證明，同一幀圖像在FAST算法改進前后的提取結果有較大改變，如圖1所示，改進前圖1(a)一幀圖像中的特征點個數為4 704，改進后圖1(d)特征點個數為1 526，這是因為改進后的算法濾掉了部分偽角點，留下了角點特征更加明顯的特征點。而且提取時間上也有了提高，改進前的FAST角點提取時間為33 ms，改進后為16 ms。

2 相關歸一化的特征匹配算法

通過FAST角點檢測算子可以對視頻圖像中的每一幀圖像進行角點檢測，為了建立環境中三維特征點地圖，必須對相鄰2幀圖像進行特征匹配。筆者采用相關歸一化匹配算法，其相關歸一化函數為

ρ(f0,f1)=

(4)

3 基于擴展卡爾曼濾波的一點RANSAC算法

用相關歸一化算法對相鄰2幀圖像進行特征匹配之后，會產生很多誤匹配點，為了提高匹配精度，要對匹配點進行過濾。標準RANSAC算法是一種應用廣泛的誤匹配剔除算法，但該算法隨著匹配點個數增加，算法迭代次數會迅速上升，降低了算法的計算速度。筆者結合擴展卡爾曼濾波的一點RANSAC算法，在確保計算精度的同時，有效降低了算法迭代次數，使機器人定位與導航的時效性得到了保證[7]。

3.1 擴展卡爾曼濾波

卡爾曼濾波是一種高效率的遞歸濾波器，它能從一組有限的、包含噪聲的、對物體位置的觀察序列預測出物體位置的坐標和速度，并能夠實現對系統狀態的最優估計，使系統的真實值與觀測值之間的均方差最小[8]。它是對一個線性隨機系統的狀態進行估計，但在實際應用中，大部分系統都是非線性的，處理這些系統時，需采用EKF。其算法原理如下。

1)預測：

(5)

式中：uk表示第k步系統的輸入，Fk表示在第k步fk關于狀態向量xk|k-1的雅可比矩陣，Qk表示動態模型的零均值高斯噪聲的協方差，Gk表示該噪聲關于狀態向量xk|k-1的雅可比矩陣。

2)濾波：數據更新是用傳統的擴展卡爾曼濾波方程執行的。

Pk|k=(I-KkHk)Pk|k-1

(6)

3.2 RANSAC算法

RANSAC算法是通過一組包含異常數據的樣本數據點集計算出數據的數學模型，最終得到有效樣本數據的算法[9]。一般情況下，需要考慮的參數有構建模型所需的最少數據個數、算法迭代次數k、模型驗證條件t、判斷模型是否合理的數據點個數d。算法流程為：首先，用隨機選擇的數據擬合一個假設模型，其次，用得到的模型去測試所有其他數據，如果有足夠多的點被歸類為該假設模型的內點，則認為該模型是合理的，并用得到的內點重新估計模型；上述過程被重復執行一定的次數，產生的模型要么越來越接近真實模型，要么因為內點數目過少而被舍棄。

用w來表示每次迭代時從數據集中選取一個點并且該點是正確數據點的概率，那么，每次迭代選取的m個數據均為正確數據的概率為wm，則1-wm表示m個數據中至少有一點是異常數據的概率，而包含這些異常點則可能產生不合理的模型。因此，n次迭代過程中，每次選取的m個數據均包含至少一個錯誤數據的概率必然等于1-p,也就是

1-p=(1-wm)n

通常情況下，p事先給定，如果w也給定，n會隨著m的增加不斷增加，那么算法的計算復雜度就會相應地增加。m增大，RANSAC算法的迭代次數n將會迅速地上升。對于這里采用的一點RANSAC，攝像機運動的額外信息來自EKF概率分布函數。舉一個簡單的例子，如果w=0.5，概率p是0.99，n=1，則隨機假設的數據將會從145(m=5，沒有先驗信息)降到7(m=1,一點RANSAC采用先驗信息)。

3.3 一點RANSAC算法

一點RANSAC與普通RANSAC相比，前者采用先驗信息，降低了數據集的大小，減少了RANSAC迭代次數，從而降低了計算成本。一點RANSAC充分利用了EKF預測階段得到的地圖狀態預測值的先驗信息，選出匹配效果最好，滿足全局模型的地圖特征，從性能和計算復雜度方面對傳統的RANSAC算法做了改進，提高了SLAM系統的魯棒性，其主要步驟如下：

1)產生穩定的內點集zli_inliers，并用zli_inliers進行EKF部分更新。

假設經過匹配所得的數據點集為zi，用zli_inliers(low-innovation inliers)來表示匹配性最好的數據點集，用這些點集來擬合一個數據模型。其余的點用zhi_inliers(high-innovation inliers)來表示。

在這舉一個簡單的例子來說明“補救”不穩定點的重要性。因為，遠距離點可用來估計相機角度變換，而近距離點用來估計相機尺度變換。在RANSAC假設中，一個遠距離點可以產生一個對角度變換較敏感的點，而對尺度變換不是很精確。在這種情況下，其他遠距離點也會支持這個擬合模型。但是由于尺度變化的精度不高，附近的點有可能表現出較高的不確定性，即使他們是內點。所以，在確定了擬合的模型之后，還是要從不穩定的點集中“補救”一些內點。在僅采用可靠內點進行局部狀態和協方差更新后，這些不穩定的內點會被挽救。下面公式是用zli_inliers進行EKF部分更新的過程。

Pk|k=(I-KkHk)Pk|k-1

2)對不穩定內點進行補救，并用zhi_inliers進行EKF部分更新。

用zli_inliers進行EKF部分更新后，在EKF預測階段的大部分相關誤差得到了糾正，協方差也大大降低，這些都可以用來補救不穩定的內點，這是由于數據關聯性有所減弱，沒有必要計算點集的一致性。對“補救點”的部分EKF更新過程為

4 定位與地圖構建

4.1 地圖三維重建

機器人基于FAST角點重建運行環境的三維地圖，根據攝像機內外參模型與標定理論[10]，設焦距參數為fi，內參矩陣表示如下：

式中：(cx,cy)是視頻序列中第i幀圖像的中心。每個特征點代表了一個三維點Xj=[X1X2X31]，在每幀圖像上的投影表達式為：uij=PiXj=Ki(Ri|ti)Xj，這里uij代表了Xj在第i幀圖像中的齊次位置，Pi為射影矩陣。根據文獻中的方法[11]，即可重建出歐式空間下的所有特征點的三維坐標。

4.2 定位與導航實現

移動機器人要實現定位，需要計算當前視圖中特征點與三維地圖中特征點的幾何關系。用u、v來表示像素點在圖像中的像素坐標，dx、dy表示每個像素點在世界坐標系上的物理尺寸，圖像像素坐標與世界坐標系之間的變換關系如下：

(9)

由式9可知，只需知道自然特征點坐標、當前視圖中同名點的坐標和對應的攝像機內參，便可求解當前視圖的攝像機外參，也即機器人的位置。又因同名關系需要匹配才可以確定，所以需要尋找最近鄰自然特征才能保證定位精度。

在建立了環境信息的三維地圖信息之后，移動機器人就可通過特征點匹配得到自身在環境中的位置信息設定路徑實現機器人的導航。

5 實驗結果與分析

機器人在室內實驗過程中，一邊控制機器人移動一邊利用攝像頭采集視頻，把視頻中的每一幀圖像采集出來，共3 788幀(30幀/s,共2 min 15 s)。從所有幀中選取一些關鍵幀，可以作為匹配模板。在Intel Pentium處理器、2 GB內存的硬件配置下，計算機對每個關鍵幀進行角點提取、誤匹配剔除并完成地圖更新的處理時間為300 ms，把機器人移動速度v設定為10 cm/s、轉彎角速度w設定為0.52 rad/s(30°/s)可保證較高的魯棒性。

圖2(a)為圖像的角點匹配情況，其中粗線圓圈表示匹配性最好的點，細線圓圈表示匹配失敗的點；圖2(b)所示為重建的SLAM地圖，曲線表示機器人運動軌跡。從圖2中可清晰地看出機器人在行進過程中的90°轉角；圖3所示為畫面中存在大范圍移動目標時，會對特征點匹配造成一定影響，從而對地圖構建造成一定誤差。表1為不同時刻前后各50幀軌跡與里程計軌跡的誤差對比。

圖2 第1 472幀圖像的角點匹配與地圖構建Fig.2 The corner matching and map building of 1 472th frame

圖3 第2 093幀圖像的角點匹配與地圖構建Fig.3 The corner matching and map building of 2 093th frame

圖4 機器人室內軌跡圖Fig.4 The indoor path of robot

幀數范圍平均誤差/cm最大誤差/cm平均誤差/%475-5741.13.50.51 422-1 5211.75.10.92 043-2 1428.216.44.13 689-3 7882.05.71.0

由表1可見，機器人在直線運動中精度較高，平均誤差在1%以內。在2 043～2 142幀之間，攝像頭畫面中存在大范圍的移動目標，對特征點匹配造成了一定的影響，但誤差也只有4%左右，證明系統的魯棒性較高。該方法同樣適合室外環境的定位與導問航，在室外采集了1 min 24 s的視頻，共2 529幀圖像。下圖5分別為第1 700幀和第2 500幀時的角點匹配與地圖構建。

(a) 機器人運動過程中特征匹配情況

(b) 機器人運行軌跡

(c) 機器人運動過程中特征匹配情況

(d) 機器人運行軌跡圖5 機器人室外運動的角點匹配與地圖構建Fig.5 The outdoor corner matching and map building of robot

圖6為SLAM重建圖與機器人里程計軌跡的對比，兩者的平均誤差為11.3 cm，平均誤差百分比為0.7%。

圖6 機器人室外軌跡圖Fig.6 The outdoor path of robot

6 結束語

針對移動機器人在單目視覺導航方面實時性與魯棒性較差的問題，提出一種基于改進的FAST角點提取算法與一點RANSAC剔除誤匹配算法相結合來實現移動機器定位與導航的新方法。在角點提取方面，剔除了部分邊緣點和局部非極大值點，提高了角點提取的速度與質量；在角點匹配與誤匹配剔除方面，充分利用了濾波階段得到的先驗信息，在保證算法魯棒性的同時有效降低了算法迭代次數，提高了算法的實時性。實驗結果表明，該方法能夠實時高效地重建出機器人的運行軌跡，實現機器人的定位與導航；在周圍環境發生變化時，能夠保持較高的匹配精度，魯棒性較高；該方法同樣可以適用于室外環境。后續工作將在如何進一步提高其實時性做深入研究。

參考文獻:

[1]王耀南, 余洪山. 未知環境下移動機器人同步地圖創建與定位研究進展[J].控制理論與應用, 2008(1): 57-65.

WANG Yaonan, YU Hongshan. Mobile robots simultaneous localization and mapping under unknown environments[J]. Control Theory and Applications, 2008(1): 57-65..

[2]陳衛東, 張飛. 移動機器人的同步自定位與地圖創建研究[J]. 控制理論與應用, 2005(3): 455-460.

CHEN Weidong, ZHANG Fei. Study on mobile robots simultaneous localization and mapping[J]. Control Theory and Applications, 2005(3): 455-460.

[3]ALCANTARILLA P F, SANG M O, MARIOTTINI G L, et al. Learning visibility of landmarks for vision-based localization[OL/EB]. [2013-10-21]. https://smartech.gatech.edu/jspui/bitstream/1853/38323/1/Alcantarilla10icra2.pdf.

[4]梁艷菊, 李慶, 陳大鵬, 等. 一種快速魯棒的LOG-FAST角點算法[J]. 計算機科學， 2012(6): 251-254.

LIANG Yanju, LI Qing, CHEN Dapeng, et al. A fast robust LOG-FAST algorithm[J]. Computer Science, 2012(6): 251-254.

[5]ROSTEN E, DRUMMOND T. Machine learning for high-speed corner detection[C]//Computer Vision-ECCV 2006. [S.l.]， 2006: 430-443.

[6]燕鵬, 安如. 基于 FAST 改進的快速角點探測算法[J]． 紅外與激光工程， 2009( 6): 1104-1108.

YAN Peng, AN Ru. An improved FAST corner detector algorithm[J]. Infrared and Laser Engineering, 2009( 6): 1104-1108.

[7]STRASDA H, MONTIEL J M M, DAVISON A J. Real-time monocular SLAM: why filter[C]//Proceedings of the IEEE International Conference on Robotics and Automation. [S.l.]， 2010: 2657-2664.

[8]CIVERA J. Real-time EKF-based structure from motion[D]. System Engineering and Computer Science of University Press, 2003： 21-23.

[9]CIVERA J, GRASA O G, DAVISON A J, et al. 1-point RANSAC for EKF-based structure from motion[C]//IEEE Intelligent Robots and Systems, [S.l.]， 2009： 3498-3504.

[10]ZHANG Z, DERICHE R, FAUGERAS O, et al. A robust technique for matching two uncalibrated images through the recovery of the unknown epipolar geometry[J]. Artificial Intelligence, 1995, 78(1/2): 87-119.

[11]POLLEFEYS M, Van GOOL L, VERGAUWEN M, et al. Visual modeling with a hand-held camera[J]. International Journal of Computer Vision, 2004, 59(3): 207-232.