胡 衡梁嵐珍

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

0 引言

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

1 基于單目視覺(jué)的EKF-SLAM框架

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

圖1 系統(tǒng)框架

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

2 基于EKF的單目SLAM算法

2.1 相機(jī)的運(yùn)動(dòng)模型

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

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

2.2 相機(jī)的觀測(cè)模型

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

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

根據(jù)三角關(guān)系可知，此時(shí)相機(jī)到該特征點(diǎn)之間距離為

根據(jù)各坐標(biāo)系間的旋轉(zhuǎn)變換關(guān)系，將該特征點(diǎn)由世界坐標(biāo)系轉(zhuǎn)化到相機(jī)坐標(biāo)系中，得其對(duì)應(yīng)的投影坐標(biāo)為

圖2 相機(jī)的觀測(cè)模型

因此，在沒(méi)有畸變的情況下特征點(diǎn)在圖像上的投影像素坐標(biāo)可以通過(guò)相機(jī)的針孔模型得出

實(shí)際上，相機(jī)在運(yùn)動(dòng)過(guò)程中一般都會(huì)受到多種畸變影響，本文僅考慮相機(jī)鏡頭的徑向畸變，對(duì)公式(6)的結(jié)果采用二階徑向畸變校正，得到經(jīng)過(guò)補(bǔ)償后的特征點(diǎn)位置如下

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

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

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

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

因?yàn)橐鸭僭O(shè)所處場(chǎng)景是靜止的，所以在預(yù)測(cè)時(shí)各路標(biāo)點(diǎn)的方差與均值都將保持不變.

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

然后根據(jù)測(cè)量值進(jìn)行更新

其中，zk+1是圖像特征點(diǎn)實(shí)際觀測(cè)坐標(biāo)，I是單位陣.

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

3 仿真結(jié)果與分析

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

圖3 不同時(shí)刻路標(biāo)特征點(diǎn)的分布及SLAM結(jié)果

圖4 相機(jī)三維軌跡圖

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

圖5 路標(biāo)點(diǎn)及相機(jī)的x、z方向上距離誤差

4 結(jié)論

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