摘 要： 針對目前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)檢測的步驟如下：
?。?）從圖片中選取一個(gè)候選點(diǎn)p，其中，把它的像素值設(shè)為I（p）；
?。?）設(shè)定一個(gè)合適的灰度差臨界值t；
?。?）檢測候選點(diǎn)周圍像素塊的16個(gè)像素點(diǎn)。如果在這16個(gè)像素點(diǎn)中有12個(gè)像素點(diǎn)的灰度值與候選點(diǎn)差值的絕對值大于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è)灰度差的絕對值要大于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]。
?。?）歐氏參數(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.