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

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

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

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

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

0　引言

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

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

1　圖像特征的檢測(cè)與提取

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

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

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

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

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

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

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

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

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

圖1　FAST檢測(cè)結(jié)果

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

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

單目視覺SLAM中根據(jù)攜帶的內(nèi)部傳感器對(duì)當(dāng)前所在位置進(jìn)行預(yù)估計(jì)，也就是SLAM算法中的預(yù)測(cè)過程，然后通過觀測(cè)已經(jīng)創(chuàng)建好的地圖特征點(diǎn)逐步來更新自己的位置和地圖特征點(diǎn)信息，最后通過單目視覺傳感器觀測(cè)提取環(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)動(dòng)狀態(tài)組成，可表示為：

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

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

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

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

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

2.3單目視覺SLAM觀測(cè)模型

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

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

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

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

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

2.4.1預(yù)測(cè)過程

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

預(yù)測(cè)誤差協(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更新過程

計(jì)算卡爾曼增益：

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

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

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

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

圖3　初始幀

圖4　結(jié)束幀

圖3是初始圖像幀，灰色點(diǎn)是提取的特征點(diǎn)，由于只有一個(gè)攝像頭，在第一幅圖像中無法對(duì)特征點(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)動(dòng)軌跡，至此實(shí)現(xiàn)了該算法。

4　結(jié)論

本文提出一種改進(jìn)的單目視覺SLAM算法，對(duì)單目SLAM算法的實(shí)時(shí)性和魯棒性做了深入研究，通過引入CEKF，降低了擴(kuò)展卡爾曼濾波隨著時(shí)間的推移引發(fā)的維數(shù)災(zāi)難，在不影響結(jié)果準(zhǔn)確度的前提下顯著減少計(jì)算需求，實(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í)時(shí)的單目視覺SLAM改進(jìn)算法［J］.微型機(jī)與應(yīng)用，2015，34（16）：15-17，20.

2015-03-20）

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

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