摘 要： 針對目前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í)性,。
關(guān)鍵詞： 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è)顏色固定的場景中。在以后的幾年中,，研究人員還提出采用占據(jù)柵格的地圖構(gòu)建框架和特征位置檢測算法,，即通過單個(gè)攝像頭在線處理RGB圖像序列，該系統(tǒng)并不采用傳統(tǒng)的匹配方法,，而是計(jì)算每個(gè)位置處找到物體的概率,。在立體視覺中，機(jī)器人可通過兩個(gè)或多個(gè)攝像頭來測量距離信息,，然而,，使用多個(gè)攝像頭會(huì)增加處理成本，實(shí)時(shí)性會(huì)變得非常差,。DAVISON A J等人在2007年實(shí)現(xiàn)了僅用一個(gè)火線接口攝像頭作為傳感器的monoSLAM[2]算法,。但是該單目視覺SLAM算法利用擴(kuò)展卡爾曼濾波（Compressed Extend Kalman Filtering，CEKF）,，對于運(yùn)行時(shí)間較長的任務(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)檢測和CEKF對目前的monoSLAM算法進(jìn)行改進(jìn),，并運(yùn)用特征點(diǎn)的逆深度非延時(shí)初始化技術(shù)，加入了攝像機(jī)的經(jīng)典二參數(shù)徑向畸變模型,，在降低計(jì)算復(fù)雜度的同時(shí)提出一種魯棒性更高的實(shí)時(shí)單目視覺SLAM算法,。
1 圖像特征的檢測與提取
視覺SLAM算法是基于環(huán)境的特征和紋理的，所以特征點(diǎn)檢測和提取作為計(jì)算機(jī)視覺中一個(gè)很重要的技術(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)檢測計(jì)算速度非常快,，可以應(yīng)用到實(shí)時(shí)場景中,。FAST角點(diǎn)檢測算法提出后,，機(jī)器視覺領(lǐng)域中特征提取實(shí)時(shí)性能才有了顯著的提升，目前它以其高計(jì)算效率,、高重復(fù)性成為機(jī)器視覺領(lǐng)域最流行的角點(diǎn)檢測算法,。
FAST特征點(diǎn)提取算法來自角點(diǎn)的定義，角點(diǎn)就是極值點(diǎn),，可以設(shè)定特定的臨界值進(jìn)行角點(diǎn)檢測,。特征點(diǎn)檢測就是基于候選角點(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)檢測的步驟如下：
（1）從圖片中選取一個(gè)候選點(diǎn)p,，其中,，把它的像素值設(shè)為I（p）；
?。?）設(shè)定一個(gè)合適的灰度差臨界值t,；
（3）檢測候選點(diǎn)周圍像素塊的16個(gè)像素點(diǎn),。如果在這16個(gè)像素點(diǎn)中有12個(gè)像素點(diǎn)的灰度值與候選點(diǎn)差值的絕對值大于t,，那么它就是一個(gè)環(huán)境特征點(diǎn)；
?。?）利用機(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è)灰度差的絕對值要大于t，如果都不滿足上述條件,，那么p不是一個(gè)特征點(diǎn),。
在實(shí)驗(yàn)室內(nèi)用FAST角點(diǎn)檢測算法提取的特征點(diǎn),，如圖1所示。
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ù)對環(huán)境特征的觀測,，同時(shí)估計(jì)自身和環(huán)境特征的位置。
單目視覺SLAM中根據(jù)攜帶的內(nèi)部傳感器對當(dāng)前所在位置進(jìn)行預(yù)估計(jì),，也就是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)動(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ì)受到很多噪聲干擾，為了降低干擾,，對相機(jī)的動(dòng)力學(xué)模型假設(shè)成速度不變高斯模型,，即任何速度噪聲滿足零均值高斯分布。
2.3 單目視覺SLAM觀測模型
由于采用單個(gè)攝像頭只能獲取前向信息,，而不能獲取圖像中特征的深度也就是距離信息,，進(jìn)而不能獲得地圖中特征點(diǎn)的三維信息,，因此必須對提取特征點(diǎn)作一定的處理，這就需要特征點(diǎn)逆深度參數(shù)化,。環(huán)境特征點(diǎn)參數(shù)化表示方法有歐氏參數(shù)化和反深度參數(shù)化[5],。
（1）歐氏參數(shù)化及其觀測模型
?。?）反深度參數(shù)化及其觀測模型
其中,，yi的前3個(gè)元素表示相機(jī)首次觀測到該路標(biāo)點(diǎn)那一時(shí)刻光心的三維世界坐標(biāo)，第4個(gè)元素是指在笛卡爾坐標(biāo)系下從相機(jī)的光心觀測路標(biāo)點(diǎn)的極角,，同樣第5個(gè)元素為仰角,，第6個(gè)元素表示深度的倒數(shù)，h是觀測方程,，向量m為該特征點(diǎn)坐標(biāo)在笛卡爾坐標(biāo)系下的投影,。
2.4 單目視覺SLAM預(yù)測更新模型
在這個(gè)過程中就需要利用CEKF對整個(gè)系統(tǒng)進(jìn)行預(yù)測和更新，CEKF算法存儲(chǔ)和保持一個(gè)局部區(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 更新過程
計(jì)算卡爾曼增益：
3 實(shí)驗(yàn)結(jié)果
本實(shí)驗(yàn)圖像采集場景為實(shí)驗(yàn)室環(huán)境，用一個(gè)分辨率為640×480普通手機(jī)攝像頭采集圖像,。在MATLAB 2013a下對該單目視覺SLAM算法進(jìn)行仿真實(shí)驗(yàn),，程序運(yùn)行的系統(tǒng)為Win8.1 64位環(huán)境，計(jì)算機(jī)的CPU為Intel酷睿i5-3230M,，內(nèi)存為4 GB,，相機(jī)速度為25 f/s的速度下可以滿足實(shí)時(shí)性要求，得到圖3和圖4所示的結(jié)果,。
圖3是初始圖像幀,，灰色點(diǎn)是提取的特征點(diǎn)，由于只有一個(gè)攝像頭,，在第一幅圖像中無法對特征點(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算法,，對單目SLAM算法的實(shí)時(shí)性和魯棒性做了深入研究，通過引入CEKF,，降低了擴(kuò)展卡爾曼濾波隨著時(shí)間的推移引發(fā)的維數(shù)災(zāi)難,，在不影響結(jié)果準(zhǔn)確度的前提下顯著減少計(jì)算需求，實(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.