一種實(shí)時的單目視覺SLAM改進(jìn)算法

李全科，曾連蓀（上海海事大學(xué)　信息工程學(xué)院，上海　201306）

一種實(shí)時的單目視覺SLAM改進(jìn)算法

李全科，曾連蓀
（上海海事大學(xué)信息工程學(xué)院，上海201306）

針對目前SLAM算法實(shí)時性和魯棒性的問題，提出了一種改進(jìn)的實(shí)時單目視覺SLAM算法。該算法采用一個攝像頭作為外部傳感器來提取機(jī)器人行進(jìn)過程中周圍環(huán)境的特征信息，用實(shí)時性良好的FAST提取環(huán)境特征點(diǎn)，結(jié)合逆深度參數(shù)化進(jìn)行特征點(diǎn)非延時初始化，用壓縮擴(kuò)展卡爾曼濾波更新地圖。實(shí)驗(yàn)研究表明，該方法提高了算法的魯棒性和實(shí)時性。

SLAM；單目視覺；FAST；壓縮擴(kuò)展卡爾曼濾波

0　引言

移動機(jī)器人自主導(dǎo)航是自治機(jī)器人領(lǐng)域研究的熱點(diǎn)。而同時定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）［1］被認(rèn)為是自主導(dǎo)航的關(guān)鍵，因此研究SLAM算法有著深遠(yuǎn)意義。一個視覺傳感器可看作一種被動傳感器，與看作主動傳感器的激光、紅外和聲吶等傳感器相比，價格低廉，抗干擾性強(qiáng)，獲取信息豐富。因?yàn)橐曈X傳感器通過發(fā)射光或波來獲取數(shù)據(jù)而不會改變環(huán)境，并且所獲得的圖像包含更多的信息，所以，研究基于機(jī)器視覺感知機(jī)制的SLAM更具有價值和應(yīng)用前景。

世界范圍內(nèi)開展了大量該領(lǐng)域的廣泛研究，并且已得出一些非常有價值和應(yīng)用前景的研究成果。20世紀(jì)90年代，Horswill研制了POLLY機(jī)器人，該機(jī)器人采用單目視覺進(jìn)行導(dǎo)航，但是只能工作在一個顏色固定的場景中。在以后的幾年中，研究人員還提出采用占據(jù)柵格的地圖構(gòu)建框架和特征位置檢測算法，即通過單個攝像頭在線處理RGB圖像序列，該系統(tǒng)并不采用傳統(tǒng)的匹配方法，而是計算每個位置處找到物體的概率。在立體視覺中，機(jī)器人可通過兩個或多個攝像頭來測量距離信息，然而，使用多個攝像頭會增加處理成本，實(shí)時性會變得非常差。DAVISON A J等人在2007年實(shí)現(xiàn)了僅用一個火線接口攝像頭作為傳感器的monoSLAM［2］算法。但是該單目視覺SLAM算法利用擴(kuò)展卡爾曼濾波（Compressed Extend Kalman Filtering，CEKF），對于運(yùn)行時間較長的任務(wù)，路標(biāo)數(shù)會不斷增大，最終導(dǎo)致計算資源不足以實(shí)時更新地圖，該問題的產(chǎn)生是由于每個路標(biāo)都與其他路標(biāo)相關(guān)聯(lián)。而CEKF算法可在不影響結(jié)果準(zhǔn)確度的條件下顯著減少計算需求，正好能夠改進(jìn)monoSLAM算法存在的問題，所以本文利用實(shí)時性良好的FAST角點(diǎn)檢測和CEKF對目前的monoSLAM算法進(jìn)行改進(jìn)，并運(yùn)用特征點(diǎn)的逆深度非延時初始化技術(shù)，加入了攝像機(jī)的經(jīng)典二參數(shù)徑向畸變模型，在降低計算復(fù)雜度的同時提出一種魯棒性更高的實(shí)時單目視覺SLAM算法。

1　圖像特征的檢測與提取

視覺SLAM算法是基于環(huán)境的特征和紋理的，所以特征點(diǎn)檢測和提取作為計算機(jī)視覺中一個很重要的技術(shù)應(yīng)用到解決視覺SLAM中。加速分割測試特征（Features from Accelerated Segment Test，F(xiàn)AST）算法是由ROSTEN E和DRUMMOND T［3］在2006年提出來的。它僅僅利用中心點(diǎn)周圍像素同其比較的信息就能夠提取到特征點(diǎn)。相對于許多其他的特征點(diǎn)檢測算法，F(xiàn)AST角點(diǎn)檢測計算速度非常快，可以應(yīng)用到實(shí)時場景中。FAST角點(diǎn)檢測算法提出后，機(jī)器視覺領(lǐng)域中特征提取實(shí)時性能才有了顯著的提升，目前它以其高計算效率、高重復(fù)性成為機(jī)器視覺領(lǐng)域最流行的角點(diǎn)檢測算法。

FAST特征點(diǎn)提取算法來自角點(diǎn)的定義，角點(diǎn)就是極值點(diǎn)，可以設(shè)定特定的臨界值進(jìn)行角點(diǎn)檢測。特征點(diǎn)檢測就是基于候選角點(diǎn)周圍的一個像素塊，利用像素塊上面各個像素點(diǎn)與候選角點(diǎn)灰度值的差值點(diǎn)的個數(shù)來判斷是否是一個角點(diǎn)，一般如果這些點(diǎn)的個數(shù)大于總數(shù)的3/4，就判別該點(diǎn)為一個環(huán)境特征點(diǎn)。

其中，I（x）代表了像素塊上任意一點(diǎn)的灰度值，I（p）為候選中心角點(diǎn)的灰度值，灰度差臨界值設(shè)為t，N為達(dá)到灰度差臨界值的像素點(diǎn)的個數(shù)。如果N像素點(diǎn)的個數(shù)大于總數(shù)的3/4，則p是一個特征點(diǎn)。

FAST角點(diǎn)檢測的步驟如下：

（1）從圖片中選取一個候選點(diǎn)p，其中，把它的像素值設(shè)為I（p）；

（2）設(shè)定一個合適的灰度差臨界值t；

（3）檢測候選點(diǎn)周圍像素塊的16個像素點(diǎn)。如果在這16個像素點(diǎn)中有12個像素點(diǎn)的灰度值與候選點(diǎn)差值的絕對值大于t，那么它就是一個環(huán)境特征點(diǎn)；

（4）利用機(jī)器學(xué)習(xí)算法快速排除那些偽角點(diǎn)。該機(jī)器學(xué)習(xí)算法只檢查圓上具有代表性的4個點(diǎn)像素，首先檢查上下兩點(diǎn)，看它們是否高于或低于閾值，如果是，再檢查左右兩點(diǎn)。如果p是一個特征點(diǎn)，則上述4個點(diǎn)中至少有3個灰度差的絕對值要大于t，如果都不滿足上述條件，那么p不是一個特征點(diǎn)。

在實(shí)驗(yàn)室內(nèi)用FAST角點(diǎn)檢測算法提取的特征點(diǎn)，如圖1所示。

圖1　FAST檢測結(jié)果

2　單目視覺同時定位與構(gòu)圖

SLAM的實(shí)質(zhì)是移動機(jī)器人能夠建立環(huán)境地圖并同時根據(jù)該地圖來推導(dǎo)自身位置，初始時地圖和移動機(jī)器人位置均未知，已知機(jī)器人的運(yùn)動學(xué)模型，并在一個存在大量人工路標(biāo)或自然路標(biāo)的位置環(huán)境中運(yùn)動，根據(jù)對環(huán)境特征的觀測，同時估計自身和環(huán)境特征的位置。

單目視覺SLAM中根據(jù)攜帶的內(nèi)部傳感器對當(dāng)前所在位置進(jìn)行預(yù)估計，也就是SLAM算法中的預(yù)測過程，然后通過觀測已經(jīng)創(chuàng)建好的地圖特征點(diǎn)逐步來更新自己的位置和地圖特征點(diǎn)信息，最后通過單目視覺傳感器觀測提取環(huán)境特征點(diǎn)，創(chuàng)建并更新環(huán)境地圖。單目視覺SLAM算法框圖如圖2所示。

圖2　單目視覺SLAM算法框圖

2.1單目視覺SLAM全狀態(tài)模型

單目視覺SLAM的全狀態(tài)模型由目前已經(jīng)創(chuàng)建的地圖中所有路標(biāo)的狀態(tài)和相機(jī)運(yùn)動狀態(tài)組成，可表示為：

其中，xc是相機(jī)的狀態(tài)，yi是當(dāng)前地圖中所有環(huán)境特征點(diǎn)的狀態(tài)。

協(xié)方差矩陣表示為：

2.2單目視覺SLAM運(yùn)動模型

在實(shí)驗(yàn)中通過實(shí)驗(yàn)人員手持一個攝像頭移動來模擬真實(shí)的移動機(jī)器人探索未知環(huán)境，這樣相機(jī)的運(yùn)動模型就是機(jī)器人的運(yùn)動模型。在這里使用了CIVERA J等人提出來的恒定速度模型［4］，相機(jī)的運(yùn)動模型使用13個變量進(jìn)行建模。用相機(jī)的光心運(yùn)動狀態(tài)等效成移動機(jī)器人的運(yùn)動狀態(tài)，相機(jī)的光心用rWC來表示，相機(jī)坐標(biāo)系到世界坐標(biāo)系之間的轉(zhuǎn)換關(guān)系用四元數(shù)qWC表示，用vW表示相機(jī)線速度，相機(jī)角速度用ωC表示。因此，得到相機(jī)的運(yùn)動狀態(tài)表示為：

其中，下標(biāo)C表示camera，上標(biāo)WC指定由相機(jī)坐標(biāo)系到世界坐標(biāo)系，上標(biāo)W指定以笛卡爾坐標(biāo)系作為參考系，上標(biāo)C指定以相機(jī)坐標(biāo)系作為參考系，因此，可得到單目視覺SLAM的運(yùn)動模型：

此外相機(jī)運(yùn)動過程中會受到很多噪聲干擾，為了降低干擾，對相機(jī)的動力學(xué)模型假設(shè)成速度不變高斯模型，即任何速度噪聲滿足零均值高斯分布。

2.3單目視覺SLAM觀測模型

由于采用單個攝像頭只能獲取前向信息，而不能獲取圖像中特征的深度也就是距離信息，進(jìn)而不能獲得地圖中特征點(diǎn)的三維信息，因此必須對提取特征點(diǎn)作一定的處理，這就需要特征點(diǎn)逆深度參數(shù)化。環(huán)境特征點(diǎn)參數(shù)化表示方法有歐氏參數(shù)化和反深度參數(shù)化［5］。

（1）歐氏參數(shù)化及其觀測模型

其中，yi的前3個元素表示相機(jī)首次觀測到該路標(biāo)點(diǎn)那一時刻光心的三維世界坐標(biāo)，第4個元素是指在笛卡爾坐標(biāo)系下從相機(jī)的光心觀測路標(biāo)點(diǎn)的極角，同樣第5個元素為仰角，第6個元素表示深度的倒數(shù)，h是觀測方程，向量m為該特征點(diǎn)坐標(biāo)在笛卡爾坐標(biāo)系下的投影。

2.4單目視覺SLAM預(yù)測更新模型

在這個過程中就需要利用CEKF對整個系統(tǒng)進(jìn)行預(yù)測和更新，CEKF算法存儲和保持一個局部區(qū)域所采集的所有信息，以及與該區(qū)域中路標(biāo)數(shù)二次方成正比的成本，然后以類似于完全SLAM的成本將這些信息轉(zhuǎn)移到全局地圖中的其余部分，但是只需一次迭代運(yùn)行。

2.4.1預(yù)測過程

預(yù)測狀態(tài)變量：

預(yù)測誤差協(xié)方差：

其中，F(xiàn)是fv（xCk+1）的雅可比矩陣，Q是fv（xCk+1）關(guān)于噪聲n的偏導(dǎo)數(shù)，Pk（1：13，1：13）表示Pk里面包含元素的第1到第13行或者第1到第13列，end指的是矩陣的最后一行或一列。

2.4.2更新過程

計算卡爾曼增益：

通過觀測更新狀態(tài)：

更新誤差協(xié)方差：

3　實(shí)驗(yàn)結(jié)果

本實(shí)驗(yàn)圖像采集場景為實(shí)驗(yàn)室環(huán)境，用一個分辨率為640×480普通手機(jī)攝像頭采集圖像。在MATLAB 2013a下對該單目視覺SLAM算法進(jìn)行仿真實(shí)驗(yàn)，程序運(yùn)行的系統(tǒng)為Win8.1 64位環(huán)境，計算機(jī)的CPU為Intel酷睿i5-3230M，內(nèi)存為4GB，相機(jī)速度為25f/s的速度下可以滿足實(shí)時性要求，得到圖3和圖4所示的結(jié)果。

圖3　初始幀

圖4　結(jié)束幀

圖3是初始圖像幀，灰色點(diǎn)是提取的特征點(diǎn)，由于只有一個攝像頭，在第一幅圖像中無法對特征點(diǎn)進(jìn)行定位，可以看到圖3中右半部分環(huán)境特征點(diǎn)是發(fā)散的，而隨著圖像序列的推移，特征點(diǎn)匹配的成功率提升，到最后可以看到圖4的實(shí)驗(yàn)結(jié)果，可以看到圖4右半部分的特征點(diǎn)已經(jīng)收斂了，也就是已經(jīng)定位，其灰色收斂橢圓代表特征點(diǎn)定位情況，曲線代表攝像機(jī)的運(yùn)動軌跡，至此實(shí)現(xiàn)了該算法。

4　結(jié)論

本文提出一種改進(jìn)的單目視覺SLAM算法，對單目SLAM算法的實(shí)時性和魯棒性做了深入研究，通過引入CEKF，降低了擴(kuò)展卡爾曼濾波隨著時間的推移引發(fā)的維數(shù)災(zāi)難，在不影響結(jié)果準(zhǔn)確度的前提下顯著減少計算需求，實(shí)現(xiàn)視覺特征點(diǎn)的高速處理，從而提高了算法的魯棒性，獲得了更好的性能。

［1］SMITH R，SELF M，CHEESEMAN P.Estimating uncertain spatial relationships in robotics［A］.Autonomous robot vehicles［C］.Springer New York，1990：167-193.

［2］DAVISON A J，REID ID，MOLTON N D，etal.MonoSLAM：real-time single camera SLAM［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.

［3］ROSTEN E，DRUMMOND T.Machine learning for high speed corner detection［A］.Computer Vision-ECCV 2006［C］.Springer Berlin Heidelberg，2006：430-443.

［4］CIVERA J，GRASA O G，DAVISON A J，et al.1-point RANSAC for EKF-based structure from motion［C］.IEEE/RSJ International Conference on Intelligent Robots and Systems，IROS 2009，IEEE，2009：3498-3504.

［5］CIVERA J，DAVISON A J，MONTIEL J.Inverse depth parametrization for monocular SLAM［J］.IEEE Transactions on Robotics，2008，24（5）：932-945.

An improved real-time monocular visual SLAM algorithm

Li Quanke，Zeng Liansun
（School of Information Engineering，Shanghai Maritime University，Shanghai 201306，China）

Aiming at the problem of robust SLAM algorithm，an improved real-time monocular visual SLAM algorithm is proposed.Taking a camera as its external sensor，this algorithm can obtain the feature information of the surrounding.It obtains the feature points of environment by using FAST which hasgood real-time capability.With the combination of inverse depth parametrization，the feature points begin to initialize without delay.The map is updated by the CEKF.The experimental study shows that this method improves the robustness and real-time of the algorithm.

Simultaneous Localization and Mapping（SLAM）；monocular vision；Features from Accelerated Segment Test（FAST）；Compressed Extend Kalman Filtering（CEKF）

TP242

A

1674-7720（2015）16-0015-03

李全科，曾連蓀.一種實(shí)時的單目視覺SLAM改進(jìn)算法［J］.微型機(jī)與應(yīng)用，2015，34（16）：15-17，20.

2015-03-20）

李全科（1988-），男，碩士，主要研究方向：移動機(jī)器人導(dǎo)航、計算機(jī)視覺。

曾連蓀（1962-），男，碩士，教授，主要研究方向：無線接入技術(shù)、定位測控技術(shù)。