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

賈 哲，冷建偉

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

1 引言

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

SLAM的求解是一個狀態(tài)估計問題，其解決方法可大致分為兩類:一類是基于濾波器的方法［7-10］，另一類則是基于圖優(yōu)化的方法?；跒V波器的方法對于非線性系統(tǒng)很難得到精確的最優(yōu)濾波解，為了解決該問題，文獻(xiàn)［11］提出了擴(kuò)展濾波器。而圖優(yōu)化在機(jī)器視覺和機(jī)器人領(lǐng)域都有大量研究，對SLAM問題具有魯棒性，比較容易實(shí)現(xiàn)。

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

2 直線的表征

本文在對空間中的三維直線進(jìn)行表征時，結(jié)合了Plücker和 Cayley兩種參數(shù)化方式。采用Plücker坐標(biāo)有利于線特征的線性投影計算，而采用Cayley參數(shù)化方式便于在后端對線特征的位置進(jìn)行非線性優(yōu)化。

2．1 直線的 Plücker坐標(biāo)

Plücker直線坐標(biāo)也被稱作線坐標(biāo)，在處理線幾何等問題時有獨(dú)特的優(yōu)點(diǎn)。

設(shè)空間直線 L 經(jīng)過 A(x1，y1，z1)、B(x2，y2，z2)兩點(diǎn)，那么直線L的方向向量為:

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

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

直線的Plücker坐標(biāo)具有如下性質(zhì):

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

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

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

2．3 直線的 Cayley表征方式

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

以下為將Plücker直線 L轉(zhuǎn)換為 Cayley直線的過程:

根據(jù)式(1)計算出Plücker直線L上距離坐標(biāo)原點(diǎn)最近點(diǎn)P0:

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

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

至此，Cayley直線的坐標(biāo)表達(dá)式W=(d，s)已經(jīng)求出。

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

根據(jù)Q的定義，可得Plücker直線坐標(biāo)為:

2．4 線特征的優(yōu)化

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

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

3 基于線特征的位姿估計

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

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

(1)計算旋轉(zhuǎn)矩陣R。

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

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

其中，表達(dá)式u'i×q－q×ui是關(guān)于變量q的線性函數(shù)，進(jìn)一步分析可知，存在矩陣:，得到:

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

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

(2)計算平移矩陣T。

構(gòu)造關(guān)于T的最小化目標(biāo)函數(shù)，如式(10)所示:

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

4 圖優(yōu)化

針對傳統(tǒng)的利用濾波方法估計位姿存在線性化誤差、更新效率低下等問題，本文依據(jù)SLAM研究中的圖優(yōu)化思想，設(shè)計了一種基于非線性優(yōu)化的位姿優(yōu)化方法，同時實(shí)現(xiàn)局部位姿優(yōu)化和全局位姿優(yōu)化。

4．1 模型的建立

在機(jī)器人視覺估計位姿領(lǐng)域，經(jīng)典的SLAM數(shù)學(xué)模型由三部分組成，分別是狀態(tài)變量、運(yùn)動方程和觀測方程［15］。本文的應(yīng)用對象機(jī)器人在感知環(huán)境時，主要以單目視覺傳感器，即單目攝像機(jī)作為輸入，因此本文的算法構(gòu)建出的目標(biāo)函數(shù)只借助狀態(tài)變量和觀測方程，目標(biāo)函數(shù)為:

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

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

4．2 圖的建立

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

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

5 融合算法

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

5．1 基于光流計算的運(yùn)動估計

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

5．2 融合算法

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

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

6 實(shí)驗(yàn)

6．1 點(diǎn)特征和線特征性能比較實(shí)驗(yàn)

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

點(diǎn)特征線特征校正性能比較實(shí)驗(yàn):

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

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

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

6．2 快速性比較實(shí)驗(yàn)

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

6．3 算法整體性能比較實(shí)驗(yàn)

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

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

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

7 結(jié)束語

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