基于SURF特征的單目視覺SLAM方法研究?

胡 衡梁嵐珍

(1.新疆大學電氣工程學院，新疆烏魯木齊830046；2.北京聯(lián)合大學自動化學院，北京100101)

0 引言

移動機器人在未知場景中進行同步定位與地圖創(chuàng)建(Simultaneous Localization and Mapping，SLAM)一直都是移動機器人研究領域的熱點[1].視覺SLAM中一般采用SIFT（Scale Invariant Feature Transform）特征點[2]作為場景地圖中的路標，因為它對圖像縮放、旋轉、仿射變換、光照等具有不變性且對噪聲不太敏感.但SIFT算法的計算量大、時間復雜度高等缺點，導致圖像匹配的速度較慢，因而難以滿足實時性高的場合[3].Bay等人[4]在SIFT檢測算子的基礎上提出了基于積分圖像和方形濾波器的SURF(Speeded Up Robust Features)算法，其檢測速度快，魯棒性也較好.針對移動機器人單目SLAM問題，文章采用SURF特征點，利用反向深度法進行特征初始化，提出一種基于擴展卡爾曼濾波器(Extended Kalman Filter，EKF)的單目SLAM方法.

1 基于單目視覺的EKF-SLAM框架

文章使用內參數(shù)已知的單目相機完成室內結構化場景的視覺SLAM任務，應用擴展卡爾曼濾波器融合SURF特征信息與機器人位姿信息并實現(xiàn)地圖的更新.系統(tǒng)框架如圖1所示.

圖1 系統(tǒng)框架

首先，使用單目視覺傳感器對未知環(huán)境進行觀測，采集的圖像經過畸變校正后進行SURF特征提取，將當前幀與前一幀所提取出的特征點進行匹配，把匹配成功的特征點確定為環(huán)境路標并加入到特征地圖中.在首次檢測出圖像SURF特征之后，采用逆深度參數(shù)化方法對它們進行非延時初始化，創(chuàng)建出初始地圖.之后機器人在行進過程中通過觀測和識別場景視覺特征不斷創(chuàng)建環(huán)境地圖.當觀測結果與特征地圖庫中的某個特征路標點相匹配時，用該特征點來更新地圖，并依據(jù)EKF算法更新機器人的位姿信息，否則將該特征點作為新的環(huán)境路標添加到所創(chuàng)建的特征地圖庫中.

2 基于EKF的單目SLAM算法

2.1 相機的運動模型

一般情況下，相機的狀態(tài)向量xV由它在世界坐標系中的3維坐標、方位角、線速度、角速度等矢量組成，共13個元素[5]，可表示如下

其中，rw是相機光心坐標、qW C是方位角的四元素表示，vW、ωW分別表示相機運動的線速度和角速度.由此可得出單目SLAM的相機狀態(tài)模型

2.2 相機的觀測模型

相機提供對環(huán)境特征的觀測信息，測量過程處于特征點的提取階段.根據(jù)Montiel所提出的反向深度法[6]，可將場景中路標點表示成如下6維狀態(tài)向量

其中(xi,yi,zi)T是首次觀測到該特征點時相機光心的3維坐標，θi、?i分別表示該特征點沿光軸方向在世界坐標系中的極角與仰角，ρi用來表示反向深度大小.假設所觀測特征點的狀態(tài)為yi，m(θi,?i)為該特征點光線投影向量在世界坐標系中的表示，則基于逆深度參數(shù)化方法的觀測模型[7]如下圖2所示.

根據(jù)三角關系可知，此時相機到該特征點之間距離為

根據(jù)各坐標系間的旋轉變換關系，將該特征點由世界坐標系轉化到相機坐標系中，得其對應的投影坐標為

圖2 相機的觀測模型

因此，在沒有畸變的情況下特征點在圖像上的投影像素坐標可以通過相機的針孔模型得出

實際上，相機在運動過程中一般都會受到多種畸變影響，本文僅考慮相機鏡頭的徑向畸變，對公式(6)的結果采用二階徑向畸變校正，得到經過補償后的特征點位置如下

2.3 單目SLAM系統(tǒng)的擴展卡爾曼濾波

在SLAM過程中，相機狀態(tài)xV和環(huán)境地圖中所有路標點信息yi需要同時不斷進行估計和更新，故系統(tǒng)的全狀態(tài)向量如下

EKF過程一般分為兩步，首先根據(jù)k時刻的狀態(tài)對k+1時刻的狀態(tài)進行預測，得到k+1|k；然后再利用所得到的觀測值對狀態(tài)進行不斷更新，得到接近真實值的估計k+1|k+1.

SLAM系統(tǒng)的全狀態(tài)EKF預測階段可描述如下

因為已假設所處場景是靜止的，所以在預測時各路標點的方差與均值都將保持不變.

在更新階段中，首先求出卡爾曼增益

然后根據(jù)測量值進行更新

其中，zk+1是圖像特征點實際觀測坐標，I是單位陣.

如果有新特征點添加，則根據(jù)目前相機狀態(tài)和圖像觀測坐標按圖2所示的方法獲取新特征點的狀態(tài)和方差，然后將其增廣到系統(tǒng)的全狀態(tài)估計向量k+1|k+1和協(xié)方差矩陣Pk+1|k+1中.

3 仿真結果與分析

選取實驗室某處環(huán)境為實驗場景，根據(jù)SURF特征點提取與匹配方法進行單目EKF-SLAM仿真實驗.如圖3(c)所示，經過8幀的EKF濾波，路標特征點的位置可收斂到一定范圍內.隨著EKF迭代次數(shù)的不斷增加，地圖特征點信息的不確定性將逐漸減小而最終能夠收斂在估計值附近，如圖3(d)第60幀的SLAM結果所示.同時，隨著地圖特征點的位置越來越精確，相機的定位準確性也隨之不斷提高，相機的三維軌跡如圖4所示.

圖3 不同時刻路標特征點的分布及SLAM結果

圖4 相機三維軌跡圖

路標點和相機的距離估計誤差分別如圖5所示.顯然，誤差值隨著擴展卡爾曼濾波迭代次數(shù)的增加而減小，最終逐步趨于穩(wěn)定范圍內.結果顯示，路標點在x、z方向上的最大誤差不超過0.04 m；相機在x、z方向上的最大誤差不超過0.01 m.產生誤差的原因主要是因為復雜的實驗場景和實驗器材測量精度有限.需要說明的是，為了顯示直觀，除圖4外都僅顯示了x、z方向上2維結果.

圖5 路標點及相機的x、z方向上距離誤差

4 結論

文章提出一種基于SURF特征的擴展卡爾曼濾波單目視覺SLAM方法并通過了仿真實現(xiàn).在傳統(tǒng)EKFSLAM基礎上，采用SURF算子提取場景圖像中的路標點，結合反向深度估計法初始化后，將包含機器人位姿和場景路標點信息的全狀態(tài)向量及其協(xié)方差矩陣應用擴展卡爾曼濾波器進行迭代遞推，從而完成系統(tǒng)狀態(tài)的估計更新和場景地圖的創(chuàng)建.實驗仿真效果比較理想，表明了該方法能夠在僅有單個攝像頭的情況下很好地完成室內結構化場景的地圖創(chuàng)建任務.