摘  要： 針對目前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í)時性。

　　關(guān)鍵詞： 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)檢測的步驟如下：

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

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

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

　?。?）利用機(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所示。

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.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]。

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

　　（2）反深度參數(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（x）的雅可比矩陣，Q是fv（x）關(guān)于噪聲n的偏導(dǎo)數(shù)，Pk（1：13，1：13）表示Pk里面包含元素的第1到第13行或者第1到第13列，end指的是矩陣的最后一行或一列。

　　2.4.2 更新過程

　　計算卡爾曼增益：

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)存為4 GB，相機(jī)速度為25 f/s的速度下可以滿足實(shí)時性要求，得到圖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)的高速處理，從而提高了算法的魯棒性，獲得了更好的性能。

參考文獻(xià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 I D， MOLTON N D， et al. 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.