崔東亮 冷建偉

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

0 引 言

當今社會人工智能技術發(fā)展愈發(fā)迅速，實際應用的過程中對于智能機器人的智能化程度要求愈發(fā)提高，同步定位與地圖構建SLAM(Simultaneous Location And Mapping)問題開始逐步成為了研究的熱點。SLAM問題的目的是如何處理當機器人被置于陌生的環(huán)境中時，其怎么通過自身配備的各種的傳感器，借由記錄計算自身位移數(shù)據(jù)，以此來實現(xiàn)自主定位以及同步構建增量式地圖。在機器人自主移動的領域，SLAM技術有著至關重要的作用。在該系統(tǒng)中，攝像頭因為其低廉的成本、便攜的體積，并且可以采集到豐富的場景數(shù)據(jù)而成為主流的傳感器。目前主流的SLAM系統(tǒng)利用從收集圖像中提取的點特征來進行定位與建圖。常用的點特征相關算法有SIFT(Scale Invariant Feature Transform)、SURF(Speed Up Robust Features)、ORB(Oriented FAST and Rotated BRIEF)等成熟跟蹤算法。

但是基于點特征構建的地圖中，對于某些點特征較少的環(huán)境，只依靠點云無法準確地反映場景中的結構信息，點特征與線特征相互對比之下，線特征的維度要比點特征高一個維度，同時線特征在自然環(huán)境之中也大量存在，對于一些欠點環(huán)境，線特征具有更大的優(yōu)勢，可以更好地還原出場景的信息。文獻[1]提出了基于純幾何學的線特征檢測算法，對提取出的線特征，通過線段間的幾何特征進行匹配，而非傳統(tǒng)基于灰度的描述子匹配方法，使得整體算法對于光照變化具有很強魯棒性，解決了傳統(tǒng)算法由于光照的動態(tài)變化導致匹配失敗的問題，同時由于略去了描述子的計算過程，減少了算法的計算時間。但是算法本身匹配的條件較為嚴苛，導致算法的匹配數(shù)量較少，如果在下一幀中沒有檢測到對應的匹配特征，會直接的降低算法的整體匹配精度。為了解決這一問題，本文將基于幾何學線特征與點特征的匹配算法互相融合。通過融合實時性比較強的ORB算法，增加特征匹配數(shù)，構建點線匹配模型，引入Huber核函數(shù)進行優(yōu)化求解，提出一種具有良好實時性與準確性，同時對于光照魯棒性較強的點線融合算法。

1 點線特征提取與匹配

1.1 點線特征提取算法

特征點的提取和幀與幀之間特征的匹配是點特征相關的視覺里程計的主要組成部分。本文使用ORB[2]與LSD[3]算法作為工具來提取圖像中的點線特征。ORB點特征為在圖像金字塔中提取的具有方向信息的FAST(feature from accelerated segment test)[4]角點。對于線特征，LSD算法將誤差在一定閾值內(nèi)且梯度方向相似的像素點進行合并，在線性時間里提取圖像局部范圍內(nèi)亞像素級的線段作為匹配特征。

1.2 點線融合算法

由上一節(jié)可知，圖像中的線特征匹配及優(yōu)化需要花費一定的時間，使得難以保證機器人位置的實時性。為了解決這一問題，本文引入基于點特征的ORB算法。ORB算法在匹配速度上有著明顯的優(yōu)勢，能夠解決機器人實時定位的問題，但相比于其他速度較慢的基于點特征的匹配算法，ORB算法的累積誤差會隨著機器人的位移而增大。所以本文綜合上述兩種匹配算法，提出了一種點線融合的算法，可以在保證算法實時性的前提下，解決系統(tǒng)累積誤差過大的問題，同時由于基于純幾何線特征的特性，算法對于那些特征較少、光照變化頻繁的場景有著很好的魯棒性。融合算法示意如圖1所示。

1.3 線特征匹配算法

考慮到直線段在連續(xù)幀上的幾何一致性，即圖像中的方向、直線段之間的重疊和極線約束，才能沿著連續(xù)視頻成功地跟蹤直線段，為了給對應直線段增加描述以獲得魯棒匹配，需要引入一些匹配規(guī)則來處理與這種純幾何線匹配相關的模糊性。

1.3.1約束條件

為了優(yōu)化運行時間，增強算法對于光照的魯棒性，本文采用基于純幾何學的線特征匹配算法。對于連續(xù)圖像中標記好線特征的兩幅圖像I1和I2，由直線段的端點坐標s和e將兩幀圖像的線特征分別定義為L1={si,ei|i∈1,2,…,m}，L2={sj,ej|j∈1,2,…,m}，由此對于第k條線段，其向量表示為：

(1)

為了描述線特征的有效匹配，將兩幀圖像中成功匹配的線段定義為M12={(li,lj)|li∈L1，lj∈L2}，其中匹配的線特征需要滿足平行、長度近似相等、具有足夠的重疊部分、符合兩個圖片的極線約束這些條件。本文通過下式計算線特征之間的夾角來判斷是否符合平行條件：

(2)

然而僅通過是否平行進行判斷會造成許多錯誤匹配，因此通過判斷線特征是否可以重疊并計算其重疊部分來加以約束。定義ρ∈[0,1]來表示兩線特征間的重疊比例，其中ρ=1表示兩特征可以完全重疊，ρ=0表示兩特征不能重疊。兩特線特征之間的長度比例定義如下：

(3)

最后考慮兩特征的極線約束：

(4)

角度由線特征中點連線向量xij與X軸方向向量決定，其中xij=mi-mj，η1表示X軸方向向量，mk=(sk+ek)/2表示線特征中點。

1.3.2匹配規(guī)則

本文通過定義一個n維的向量來代表線特征間的匹配關系：

ωi=[ωi0…ωij…ωin]T

(5)

式中：ωi表示兩幀圖像中圖I1第i條線特征對應的匹配向量，ωi0，ωi2,…,ωin表示圖I1第i條線特征與圖I2中每一條線特征之間的匹配關系，ω=1表示兩線特征滿足匹配條件，ω=0表示無法匹配。

根據(jù)下式定義線特征間的誤差向量βij(圖I1中的第i條特征與圖I2中的第j條特征間的匹配誤差)：

(6)

目標向量為：

(7)

通過連接所有誤差向量可以得到一個4×n的誤差矩陣Ai：

Ai=[βi0…βij…βin]

(8)

算法的目的是找到匹配向量ωi使得匹配誤差和可以滿足約束Aiωi=b，這樣可以保證對于圖I1中的線特征i，在圖I2中只有一條最佳匹配的線特征與其對應，即問題轉化為：

(9)

其約束條件與上述公式對應，其中ε>0。式(9)的解可以利用文獻[5]提出的同倫法對下式求解得到：

(10)

1.4 點特征匹配算法

ORB算法通過計算特征描述子來進行特征匹配，算法首先采用窮舉法選擇出n對相關系數(shù)接近0.5的像素點對，然后根據(jù)下式計算二進制特征描述子：

(11)

式中：(x，y)為算法隨機選取的像素點對，區(qū)塊p中p(x)、p(y)表示像素點x、y坐標處的對應的灰度大小，由上述方式可以得到一個n位的二進制特征描述子：

(12)

本文使用K鄰近算法[6]進行特征匹配，匹配距離采用Hamming距離。Hamming距離經(jīng)常在二進制比較中使用，使用異或操作，方法簡單，計算速度較快，對比較后的距離給予一個合適的閾值便可以根據(jù)需求篩選出正確匹配。

2 位姿估計與優(yōu)化

2.1 點線觀測模型

(13)

2.2 空間線Lw與相機位姿的誤差模型

圖2 線特征誤差示意圖

2.3 點線融合誤差模型

最后考慮點線融合后的誤差模型，根據(jù)相機第n幀的位姿Tcw，n與該幀觀測到的第m點特征Pw，m的重投影誤差和該幀觀測到的第q條線特征Lw，q的重投影誤差為：

(14)

式中：K表示相機模型的內(nèi)參矩陣；j表示圖像中第j個點或線，d(Luv,n,q,KTcw,nLw,q)表示空間直線與相機位姿的重投影誤差。

在對相機位姿進行求解的過程中，將目標函數(shù)定義為重投影誤差的二范數(shù)的平方和。計算過程中錯誤的匹配對會導致二范數(shù)的增速過快，常規(guī)算法著重于調整錯誤誤差項的值，存在著忽略其他正確誤差項的問題。固本文在目標函數(shù)中引入了Huber核函數(shù)，以此來減少誤差項對優(yōu)化結果的影響，Huber核函數(shù)為：

(15)

由上式可以看出，當誤差e大于閾值δ時，目標函數(shù)的增長由二次函數(shù)變?yōu)橐淮魏瘮?shù)，由這種方式來抑制錯誤匹配對的影響，使得整個優(yōu)化求解更為穩(wěn)定。加入Huber核函數(shù)的目標函數(shù)分別為：

(16)

式中:Ωp和Ωl為點、線的協(xié)方差矩陣，Hp和Hl為點、線的Huber魯棒核函數(shù)。通過最小化目標函數(shù)的方式來對相機的位姿進行求解：

x*=arg minF(x)

(17)

式中:x*為待優(yōu)化的變量，即相機的位姿。

2.4 位姿估計

相機的位姿估計問題轉化為了求解非線性最小二乘法問題。目前解決這一問題的方法主要有高斯-牛頓法和列文伯格-馬爾夸特兩種方法。高斯-牛頓法是對目標函數(shù)進行一階泰勒展開，在對結果進行整理后得到增量方程，但是該方法也存在著一定的缺陷，如果求得的增量值Δx過大，會使得算法結果不能夠收斂。列文伯格-馬爾夸特法在一定程度上修正了這個問題，根據(jù)文獻[9]，本文采用列文伯格-馬爾夸特算法對優(yōu)化問題進行求解。

3 實 驗

為了驗證融合算法的整體性能，所有實驗都運行在Intel核心I5-3370 CPU和4 GB RAM上，沒有GPU并行。實驗選用Tum數(shù)據(jù)集來進行性能評估，分別在普通環(huán)境下、欠點環(huán)境下、光照變化環(huán)境下，與LSB+LBD算法、ORB算法進行性能對比。

3.1 數(shù)據(jù)集預處理

為了驗證光照變化對于算法性能的影響，實驗前首先對選定數(shù)據(jù)集進行模擬光照變化處理，分別對部分圖像的曝光度、白平衡進行處理，以模擬光照強度變化以及光源變化的影響，數(shù)據(jù)集前一部分不做任何處理，三分之一處加入模擬光照變化處理，部分數(shù)據(jù)處理效果如圖3所示。

(a) 原始數(shù)據(jù)集

(b) 模擬曝光變化圖3 部分圖像預處理

3.2 算法整體性能比較

跟蹤效果如圖4所示，可以看出在未做模擬光照處理部分三種算法跟蹤效果相近，進入模擬光照變化區(qū)域后，三種算法對應的軌跡均發(fā)生了不同程度的偏移。在desk數(shù)據(jù)集中(普通環(huán)境)，融合算法產(chǎn)生的偏移量與其他兩種算法相比較小，優(yōu)于ORB算法，在nostructure數(shù)據(jù)集中(欠點環(huán)境)，融合算法表現(xiàn)依然優(yōu)異，表明融合算法可以很好地適應光照變化的影響。同時通過兩組實驗對比可以看出在欠點環(huán)境下基于線特征的LSD+LBD算法匹配效果要優(yōu)于ORB算法，這也從側面反映了點線融合算法在欠點環(huán)境下的優(yōu)勢。

(a) desk數(shù)據(jù)處理對比

(b) nostructure數(shù)據(jù)處理對比圖4 跟蹤效果對比圖

為了更準確地反映算法性能，表1記錄了三種算法在對于不同數(shù)據(jù)集，普通條件下和模擬光照變換條件下跟蹤軌跡與實際軌跡之前的均方根誤差。結合表1和圖4可以看出，融合算法能夠有效地實現(xiàn)軌跡的跟蹤，且對光照具有較強的魯棒性。

表1 融合算法性能評估(普通環(huán)境/模擬光照變換環(huán)境)

為了驗證算法的實時性，實驗跟蹤記錄了幀與幀之間的匹配時間，如圖5所示。

圖5 處理時間

LSD+LBD算法平均處理時間為75.32 ms，ORB算法平均處理時間為27.89 ms，融合算法平均處理時間為34.43 ms，對比可知，融合算法雖然稍慢于ORB算法，但足夠滿足實時性要求。

4 結 語

本文采用改進的純幾何學線特征匹配算法，對光照變化魯棒性較好、計算速度快，并融合ORB算法以提高特征匹配數(shù)量。結合兩者優(yōu)點，改善了傳統(tǒng)點特征算法在欠點環(huán)境下表現(xiàn)不佳、線特征算法計算量過大等問題，解決了傳統(tǒng)算法對于光照變化敏感的問題，同時對融合算法模型進行優(yōu)化，提高了在系統(tǒng)匹配的準確率。實驗表明該算法定位精確度比較高，同時可以滿足SLAM系統(tǒng)實時性的需求，但如何進一步提高算法整體效率仍有待研究。