一種實時的單目視覺SLAM改進算法

該算法采用一個攝像頭作為外部傳感器來提取機器人行進過程中周圍環境的特征信息，用實時性良好的FAST提取環境特征點，結合逆深度參數化進行特征點非延時初始化，用壓縮擴展卡爾曼濾波更新地圖。實驗研究表明，該方法提高了算法的魯棒性和實時性。

0 引言

移動機器人自主導航是自治機器人領域研究的熱點。而同時定位與地圖構建（Simultaneous Localization And Mapping，SLAM）[1]被認為是自主導航的關鍵，因此研究SLAM算法有著深遠意義。一個視覺傳感器可看作一種被動傳感器，與看作主動傳感器的激光、紅外和聲吶等傳感器相比，價格低廉，抗幹擾性強，獲取信息豐富。因為視覺傳感器通過發射光或波來獲取數據而不會改變環境，並且所獲得的圖像包含更多的信息，所以，研究基於機器視覺感知機制的SLAM更具有價值和應用前景。

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

1 圖像特征的檢測與提取

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

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

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

FAST角點檢測的步驟如下：

（1）從圖片中選取一個候選點p，其中，把它的像素值設為I（p）；

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

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

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

在實驗室內用FAST角點檢測算法提取的特征點，如圖1所示。

2 單目視覺同時定位與構圖

SLAM的實質是移動機器人能夠建立環境地圖並同時根據該地圖來推導自身位置，初始時地圖和移動機器人位置均未知，已知機器人的運動學模型，並在一個存在大量人工路標或自然路標的位置環境中運動，根據對環境特征的觀測，同時估計自身和環境特征的位置。

單目視覺SLAM中根據攜帶的內部傳感器對當前所在位置進行預估計，也就是SLAM算法中的預測過程，然後通過觀測已經創建好的地圖特征點逐步來更新自己的位置和地圖特征點信息，最後通過單目視覺傳感器觀測提取環境特征點，創建並更新環境地圖。單目視覺SLAM算法框圖如圖2所示。

2.1 單目視覺SLAM全狀態模型

單目視覺SLAM的全狀態模型由目前已經創建的地圖中所有路標的狀態和相機運動狀態組成，可表示為：

其中，xc是相機的狀態，yi是當前地圖中所有環境特征點的狀態。

協方差矩陣表示為：

2.2 單目視覺SLAM運動模型

在實驗中通過實驗人員手持一個攝像頭移動來模擬真實的移動機器人探索未知環境，這樣相機的運動模型就是機器人的運動模型。在這裏使用了CIVERA J等人提出來的恒定速度模型[4]，相機的運動模型使用13個變量進行建模。用相機的光心運動狀態等效成移動機器人的運動狀態，相機的光心用rWC來表示，相機坐標系到世界坐標系之間的轉換關系用四元數qWC表示，用vW表示相機線速度，相機角速度用C表示。因此，得到相機的運動狀態表示為：

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

此外相機運動過程中會受到很多噪聲幹擾，為了降低幹擾，對相機的動力學模型假設成速度不變高斯模型，即任何速度噪聲滿足零均值高斯分布。

2.3 單目視覺SLAM觀測模型

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

（1）歐氏參數化及其觀測模型

（2）反深度參數化及其觀測模型

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

2.4 單目視覺SLAM預測更新模型

在這個過程中就需要利用CEKF對整個系統進行預測和更新，CEKF算法存儲和保持一個局部區域所采集的所有信息，以及與該區域中路標數二次方成正比的成本，然後以類似於完全SLAM的成本將這些信息轉移到全局地圖中的其余部分，但是只需一次叠代運行。

2.4.1 預測過程

預測狀態變量：

預測誤差協方差：

其中，F是fv（x）的雅可比矩陣，Q是fv（x）關於噪聲n的偏導數，Pk（1：13，1：13）表示Pk裏面包含元素的第1到第13行或者第1到第13列，end指的是矩陣的最後一行或一列。

2.4.2 更新過程

計算卡爾曼增益：

3 實驗結果

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

圖3是初始圖像幀，灰色點是提取的特征點，由於只有一個攝像頭，在第一幅圖像中無法對特征點進行定位，可以看到圖3中右半部分環境特征點是發散的，而隨著圖像序列的推移，特征點匹配的成功率提升，到最後可以看到圖4的實驗結果，可以看到圖4右半部分的特征點已經收斂了，也就是已經定位，其灰色收斂橢圓代表特征點定位情況，曲線代表攝像機的運動軌跡，至此實現了該算法。

單目視覺SLAM:采用逆深度參數化的方法求取深度信息的過程