線特征融合光流的單目SLAM算法*

賈 哲，冷建偉

(天津理工大學電氣電子工程學院，天津300384)

1 引言

隨著人工智能時代的來臨，對機器人的智能化要求也越來越高，目前同步定位與地圖構建SLAM(Simultaneous Location And Mapping)問題已成為機器人領域研究的熱點。SLAM問題可以被描述為在未知環境里，機器人從某初始位置開始移動，并且在這個移動過程中根據自身所攜帶的傳感器采集到的數據進行自主定位，同時構建增量式地圖［1］。該技術是機器人能夠完全實現自主規劃路徑和獨立移動的重要技術之一。目前在SLAM系統中，視覺傳感器因其體積較其他傳感器小，成本低廉，安裝便捷，同時又能采集到機器人運動環境中豐富的場景信息而被廣泛采用［2，3］。主流的視覺SLAM算法在制圖和定位時以提取到的圖像中的點作為特征，點特征能夠描述出圖像里的明顯特征區域，常見的特征點描述算法有尺度不變特征變換SIFT(Scale Invariant Feature Transform)、加速穩健特征SURF(Speeded Up Robust Features)、快速特征點提取和描述ORB(Oriented FAST and Rotated BRIEF)等成熟跟蹤算法。但是，基于點特征構建出的地圖為三維點云地圖，點云地圖較為稀疏，而面向機器人導航的地圖要求能夠反映場景中物體的結構信息，以便后續機器人的路徑規劃研究。與點特征相比，線特征同樣廣泛存在于各類環境場景中，線特征不易受視點和光照變化的影響，而且由于直線的空間維度比點高一維，對于一些結構化的場景，線特征更具優勢，更能準確地表達環境的結構信息。其中線特征的參數化方法是研究基于線特征 SLAM 算法的基礎［4］。文獻［5］采用Plücker參數化的方式，將直線以 Plücker坐標來表示，其優點是可以很方便地對線特征使用線性投影模型，但Plücker參數化不利于后端對SLAM的非線性優化。而將空間直線進行Cayley參數化的優勢主要是用其表示的空間直線具有4個自由度，沒有一致性約束，也不存在冗余維度［6］，因此這類參數化方式能夠更好地適用于非線性優化算法。本文重點闡述了直線的參數化問題。

SLAM的求解是一個狀態估計問題，其解決方法可大致分為兩類:一類是基于濾波器的方法［7-10］，另一類則是基于圖優化的方法。基于濾波器的方法對于非線性系統很難得到精確的最優濾波解，為了解決該問題，文獻［11］提出了擴展濾波器。而圖優化在機器視覺和機器人領域都有大量研究，對SLAM問題具有魯棒性，比較容易實現。

除此之外，機器人的應用場合往往要求機器人能夠即時定位，當機器人快速移動時，由于線特征的提取和跟蹤算法處理速度相對較慢，會造成移動和定位不同步，單次定位處理的時間越長，前后兩幀圖像之間的重疊越少，無疑會降低機器人定位的精度。而光流法經過長時間的發展，在利用圖像關系估計運動對象的位姿時，不僅可以滿足一定的精度要求，而且也有良好的穩定度。更為重要的是，光流法計算速度塊，基于光流法的視覺里程計可以達到實際應用對實時性的要求［12］，但是光流法精度不高。本文提出將光流法與線特征匹配相融合，一方面提高光流法的定位精度，另一方面提高算法的實時性。

2 直線的表征

本文在對空間中的三維直線進行表征時，結合了Plücker和 Cayley兩種參數化方式。采用Plücker坐標有利于線特征的線性投影計算，而采用Cayley參數化方式便于在后端對線特征的位置進行非線性優化。

2．1 直線的 Plücker坐標

Plücker直線坐標也被稱作線坐標，在處理線幾何等問題時有獨特的優點。

設空間直線 L 經過 A(x1，y1，z1)、B(x2，y2，z2)兩點，那么直線L的方向向量為:

要確定L的位置，還需求出直線L相對于原點的線矩向量:

那么該直線的Plücker坐標為L=(n，v)。

直線的Plücker坐標具有如下性質:

(1)滿足 Plücker約束v⊥n，即v·n=0。

2．2 Plücker 直線位置的初始化

由于使用單目攝像頭存在尺度局限性，某一特征的完整位置信息很難從單一圖像中采集到，比如無法準確地從單幅圖片中計算出線特征的方向以及深度。所以，在SLAM問題中，單目SLAM是部分可觀測問題。那么，在描述和檢測線特征時，就涉及到特征位置的初始化，具體來說，特征位置初始化指的是特征的3D位置信息通過圖像中的特征觀測信息來獲得。文獻［5］在以Plücker方式表征直線的基礎上提出了非延遲的初始化方法，該方法與延遲類的初始化方法相比，其優點是能夠利用更多的特征信息，而且在面臨低視差的情況時仍舊有效。本文即采用該方法進行直線位置的初始化。

2．3 直線的 Cayley表征方式

Cayley表達式定義為一個4維向量W=(d，s)，其中，d表示直線到坐標系原點的距離，s=表示該直線在空間中的旋轉信息。

以下為將Plücker直線 L轉換為 Cayley直線的過程:

根據式(1)計算出Plücker直線L上距離坐標原點最近點P0:

按照式(3)構建正交矩陣Q:

根據式(4)計算出一個由向量s構成的反對稱矩陣:

至此，Cayley直線的坐標表達式W=(d，s)已經求出。

將Cayley直線轉換為 Plücker直線的過程亦需要構造矩陣Q，可通過式(5)求出:

根據Q的定義，可得Plücker直線坐標為:

2．4 線特征的優化

常見的易造成圖片質量下降的因素如圖像壓縮、圖像模糊或者存在噪聲等問題容易使匹配上的線特征和預測的線特征之間存在誤差，對SLAM問題的結果產生影響，所以有必要進行優化。本文的線特征優化方法是采用文獻［4］中的優化方法，線特征在優化更新過程中采用Cayley參數化方式。與點特征不同的是，線特征的誤差模型不再是簡單的特征投影預測值與測量值之間的差，在本文中，線特征的誤差計算模型為:

其中，l={l1，l2，l3}T表示圖像中線特征的預測值。(p1，p2)表示圖像中線特征測量值。上式結果為一個二維向量e=(e1，e2)，其模就是誤差值，直觀理解如圖1所示。

3 基于線特征的位姿估計

性變換前的直線L=(u，m)與線性變換后的L'=(u'，m')之間存在約束關系為:

其中，R是變換時的旋轉矩陣，T是平移矩陣，∧表示叉乘操作。由于空間直線段的方向相比其他量更加精確［13］，所以把運動估計的問題分為以下兩個子問題:

(1)計算旋轉矩陣R。

首先構造關于R的最小化目標函數。假設檢測到的直線集為{Li=(ui，mi)，i=1，2，…，N}，那么最小化目標函數。如式(6)所示:

其中，|q|=1為單位四元數，×表示四元數乘法操作。將式(7)中的每一項都乘以

其中，表達式u'i×q－q×ui是關于變量q的線性函數，進一步分析可知，存在矩陣:，得到:

使得u'i×q－q×ui=Aiq，其中珘u是向量u的反對稱矩陣，所以，式(8)進一步變化為:

同一直線在兩個圖像幀之間滿足線性變換，線

(2)計算平移矩陣T。

構造關于T的最小化目標函數，如式(10)所示:

將式(10)對T求偏導數，并令其等于零，得到:

4 圖優化

針對傳統的利用濾波方法估計位姿存在線性化誤差、更新效率低下等問題，本文依據SLAM研究中的圖優化思想，設計了一種基于非線性優化的位姿優化方法，同時實現局部位姿優化和全局位姿優化。

4．1 模型的建立

在機器人視覺估計位姿領域，經典的SLAM數學模型由三部分組成，分別是狀態變量、運動方程和觀測方程［15］。本文的應用對象機器人在感知環境時，主要以單目視覺傳感器，即單目攝像機作為輸入，因此本文的算法構建出的目標函數只借助狀態變量和觀測方程，目標函數為:

其中，C代表樣本最優一致集，xk代表第k時刻機器人的移動位姿，在圖優化里被稱作一個pose節點;yj代表第k時刻能被機器人視覺傳感器觀測到的路標，在圖優化里被稱作一個 point節點;zk，j則代表pose節點與 point節點的約束。需要指出的是，當優化過程是閉環檢測時，zk，j代表 pose節點與 pose 節點的約束。e(xk，yj，zk，j)，則是向量誤差函數，反映了兩個節點滿足約束條件的程度，在圖優化里被表示為兩個節點之間的連線，也被稱作邊。

最終所求的 e(xk，yj，zk，j) 的二次型(式(14))，實質上是在求解最大似然，即在假設噪聲是高斯分布之后，將整個SLAM問題等效為一個最小二乘問題［16，17］。可借助通用圖優化工具 g2o來解決上述非線性最小二乘問題。

4．2 圖的建立

上述 SLAM問題全部抽象成節點與邊，節點代表優化變量，包括 pose節點和 point節點，邊代表誤差項約束條件，包括 pose-point約束和posepose約束。pose-point約束依賴于相機觀測產生的約束，如圖2a所示。pose-pose約束則依賴于系統中閉環檢測產生的約束，如圖2b和圖2c所示，可有效地抑制系統的累積誤差，從而得到信息一致的地圖。它的值越小，表示兩個節點滿足匹配約束的程度越高，值為0時，表示兩個節點完全滿足匹配約束。

目標函數的物理意義:這里假設了誤差函數符合高斯分布(0，Ω)，則SLAM問題的貝葉斯概率描述為，求解適合的(x，y)，使系統最有可能產生當前的觀測數據Z，即最大似然估計為:

5 融合算法

由上述分析可知，對于圖像中線特征描述、匹配和優化需要一定的過程，這就導致移動機器人的定位難以獲得實時性。為了不影響機器人的移動速度，引入光流法。光流法因其計算速度快，所以定位頻率高，對幀間發生的小位移有較高的精度，十分適合機器人的實時定位［18］。但是，單純使用光流法，容易造成累積誤差隨機器人的位移增大而增大。所以，本文提出了一種特征法與光流法的融合算法，利用光流法與本文所述的線特征法二者之間的互補性，兼顧系統的準確性和實時性。Lucas-Kanade算法是經典的光流求解算法，本文采用此算法求解光流。

5．1 基于光流計算的運動估計

基于光流的運動估計首先要通過光流法確定圖像序列中前后兩幀之間的對應關系，然后利用視覺理論中的對極幾何約束來求出采集當前幀時機器人的位姿。值得說明的是，在利用光流求解時，為了突出光流法的魯棒性和實時性，不同于利用線特征估計位姿的求解方法，而是根據光流法跟蹤前后像素強度差值的均方差的大小自動切換求解位姿模型。均方差的值越大，說明跟蹤前后的視差越大，則利用本質矩陣模型求解;均方差的值越小，說明跟蹤前后的視差越小，則采用單應矩陣模型求解。

5．2 融合算法

具體融合算法流程圖如圖3所示。

令式(15)中xk－1取為光流法計算出來的前10幀累積的機器人位姿;觀測數據z是通過線特征點匹配法計算出來的機器人的位姿。通過求解優化目標函數融合兩種方法得到的數據。由于光流難免會受到光照變化的影響，所以計算出的結果會導致精度有所偏差。同時在攝像頭拍攝地面的情況下，地面上線特征較少，誤配的比例較高。將光流和特征結果按照上述流程圖優化融合的好處就在于減小估計均方誤差，提高精度和實時性。

6 實驗

6．1 點特征和線特征性能比較實驗

為驗證采用線特征方式相比于點特征是否能夠有更好的匹配跟蹤的效果和反映場景的結構信息，采用普通RGB相機采集圖4所示場景的一組圖像序列，圖片尺寸大小為744×992，分別用點特征和線特征構造三維地圖，構建結果如圖5所示，并在此基礎上比較了點特征和線特征的校正特性。從圖5中可以看出，采用點特征構建出的場景比較稀疏，而線特征則較好地反映了場景的結構信息。

點特征線特征校正性能比較實驗:

首先隨意選取圖像序列里的任一圖像幀作為實驗對象，設定像素真實位置，然后利用點、線特征校正估計出像素位置，得出與真實位置之間的誤差，實驗結果如表1所示。

Table 1 Comparison experiment of feature alignment for point-line feature表1 點特征和線特征校正特性比較

由表1可知，在快速性方面，線特征比點特征用時少;在精準度方面，線特征的誤差也比點特征的誤差低。其原因主要是，采用線特征的方式可以利用線的梯度信息，這樣不僅能提高極線搜索的速度，而且也使結果更加準確。而采用點特征在匹配過程中容易出現由于非線性相似度函數多極點而產生誤差的現象。

6．2 快速性比較實驗

為了驗證光流法的引入能夠提高機器人定位的實時性，特設立了該項實驗。該實驗以實驗室搭建的移動機器人作為平臺，分別運行未引入光流法和引入光流法的算法，并且在室外場景中行駛同一段路徑。圖6是所得到的實驗結果對比圖，其中，橫軸DL代表機器人的移動距離，縱軸t是算法的處理時間。由圖6可知，未引入光流法時，基于線特征的匹配算法平均處理時間為315．69 ms;而引入光流法后，融合算法的平均處理時間為69．38 ms。這是因為在融合光流法的SLAM算法中，耗時較多的線特征提取和匹配以及優化算法只處理每隔10幀的圖像信息，每2幀之間用光流法計算，所以融合算法提高了機器人SLAM問題的實時性。

6．3 算法整體性能比較實驗

為了驗證本文提出的基于圖優化的線特征融合光流SLAM算法的準確性，本實驗采用Tum數據集中的多組數據來進行評估。圖7分別表示“desk”“room”和“floor”數據經過本文算法得到的軌跡和Tum自帶的真實軌跡對比效果圖。

圖7中實線代表攝像機在采集數據時的真實運動軌跡，虛線代表本算法得到的軌跡。同時，本文算法也與其他算法做對比，表2記錄了本文算法和目前其他主流算法在不同數據集下，各自計算出的攝像機的三維運動軌跡和真實軌跡之間的均方根誤差。通過和ORB-SLAM、RGBD-SLAM算法均方根誤差的對比，可評估本文算法對機器人位姿跟蹤的性能。從圖7和表2中可以看出，本文算法能夠有效實現機器人的運動軌跡跟蹤。

Table 2 Comparison of algorithm performance表2 算法性能評估 m

7 結束語

本文針對機器人SLAM問題的準確性和實時性提出了線特征融合光流的單目SLAM算法。對于線特征的表征問題，本文算法將Plücker坐標和Cayley參數化方式相結合，在便于計算線性投影的同時，也有利于后端的非線性優化，使圖優化能夠運用到基于線特征的機器人位姿估計問題上。將光流法與線特征法相融合，有效避免了線特征處理時間長、光流法精度不高的缺點，突出了光流法的實時性和線特征的準確性。實驗表明，本文算法的定位精度比較高，具有一定的準確性和魯棒性。但是，如何避免生成的三維線段模型中存在過多的雜線是今后需要深入研究的內容。