基于動態(tài)點線耦合的單目視覺慣性SLAM算法

吳 桐,黃宜慶,張開平

(1.安徽工程大學(xué)電氣工程學(xué)院,安徽 蕪湖 241000;2.高端裝備感知與智能控制教育部重點實驗室,安徽 蕪湖 241000)

0 引言

近些年來,視覺同步定位與建圖(Visual Simultaneous Localization and Mapping,V-SLAM)[1-2]得益于其能夠通過可視化的圖像提供相當豐富的信息,在無人駕駛、虛擬現(xiàn)實[3]等領(lǐng)域中得到了廣泛的應(yīng)用。為了更好優(yōu)化V-SLAM算法,研究者們在V-SLAM框架中引入了視覺慣性里程計(Visual-Inertial Odometry,VIO)[4],這是一個將低成本的視覺慣性測量單元(Inertial Measurement Unit,IMU)集成到視覺里程計(Visual Odometry,VO)的系統(tǒng),通過系統(tǒng)中的視覺傳感器(如單目相機)和IMU相互結(jié)合,在提取特征信息方面取得了更好的效果[5-6],被廣泛應(yīng)用到實際場景中。

最初的VIO系統(tǒng)是基于點的提取和跟蹤來估計運動姿態(tài),例如QIN等[7]提出的VINS-Mono系統(tǒng)是利用OpenCV提供的提取算法對特征點進行光流跟蹤及提取,但是基于點的提取有局限性,在重復(fù)紋理等復(fù)雜的場景中提取的點較少,這很大程度上影響了運動估計姿態(tài)的準確性。為了克服這一缺陷,研究者們在基于點提取的基礎(chǔ)上,將線特征引入到視覺慣性系統(tǒng)中,PUMAROLA等[8]在ORB-SLAM[9]算法的基礎(chǔ)上加入線特征,提出線特征初始化算法。線特征包含的信息比點特征多,應(yīng)用于缺乏點特征的人造環(huán)境中,可以有效提高系統(tǒng)的魯棒性,基于此特點,研究者們又提出了點線融合的特征提取方法,該方法的優(yōu)點就是線特征與點特征完全互補,通過相互融合,具有很好的協(xié)同作用,有效地提高了系統(tǒng)的準確性與魯棒性。HE等[10]在VINS-Mono的系統(tǒng)框架上作了修改,提出了一種基于點線特征融合的緊耦合視覺里程計(PL-VIO),該系統(tǒng)使用OpenCV中的LSD(Line Segment Detector)[11]線段提取算法來檢測線特征,但是因為它具有高的計算成本,嚴重影響了PL-VIO系統(tǒng)的性能。劉建軍等[12]提出了基于點線特征的快速單目慣性SLAM算法,但是對系統(tǒng)的定位精度提升得不夠明顯,付煜等[13]用點線融合的方法提取了豐富的線特征,但是提取的線特征質(zhì)量較差。

針對上述算法存在的問題,本文在VINS-Mono算法的基礎(chǔ)上,給出了一種基于點線耦合的單目視覺慣性SLAM算法。通過設(shè)定提取線特征的動態(tài)閾值,提取新的點線耦合特征并構(gòu)造點線耦合殘差模型,然后將所提出的點線耦合殘差集成到后端滑動窗口優(yōu)化中,并構(gòu)建優(yōu)化成本函數(shù)對系統(tǒng)進行優(yōu)化,以提高系統(tǒng)的定位精度和魯棒性。

1 本文算法結(jié)構(gòu)

1.1 算法框架

本文算法的基本結(jié)構(gòu)與VINS-Mono類似,系統(tǒng)框架結(jié)構(gòu)如圖1所示。系統(tǒng)主要包括三個模塊,分別是測量預(yù)處理、全局滑動窗口優(yōu)化和閉環(huán)檢測。在測量預(yù)處理模塊中,對視覺傳感器(相機)采集到的圖像進行點線特征檢測及跟蹤。對于點特征,本文先使用Shi-Tomasi[14]算法檢測角點特征,再用KLT(Kanade-Lucas-Tomasi)[15]算法執(zhí)行點的跟蹤;對于線特征,在原來的LSD算法上進行改進,設(shè)置動態(tài)閾值,用于檢測線特征,然后采用幾何約束的線特征匹配法對線特征匹配跟蹤,最后將提取到的點線特征與IMU預(yù)積分視覺慣性對齊。在全局滑動窗口優(yōu)化模塊中,基于滑動窗口的非線性狀態(tài)估計器可以根據(jù)先驗信息,IMU殘差,閉環(huán)約束,點、線殘差以及本文構(gòu)建的點線耦合殘差構(gòu)造聯(lián)合優(yōu)函數(shù),計算滑動窗口中的幀的位置、速度、旋轉(zhuǎn)與偏差。對于回環(huán)檢測模塊和原理與ZHAO等[16]提出的PLI-VINS相似,本文就不再敘述。

圖1 視覺慣性SLAM系統(tǒng)框架結(jié)構(gòu)

1.2 線特征殘差模型

線殘差模型的建立與點殘差類似,將線殘差定義為線端點到投影線的距離。本文使用Plucker坐標來描述線段,假設(shè)在世界坐標系下的一條空間線Lw=(nw,sw)T,通過轉(zhuǎn)換矩陣Tcw=(Rcw,tcw)將世界坐標系下的空間線Lw轉(zhuǎn)換為相機坐標系下的直線Lc。

(1)

再將相機坐標系下的直線Lc投影到像素坐標平面上,得到投影線[17]L′:

(2)

其中,K為投影矩陣,nc可由式(1)求得。

基于此,可以定義線特征殘差[18]el為:

(3)

其中,s為空間線中端點的齊次坐標。

1.3 滑動窗口優(yōu)化

要進行滑動窗口優(yōu)化,首先要定義滑動窗口中的狀態(tài)變量,由VINS-Mono可知,可將完整的狀態(tài)變量定義為[7]:

(4)

其中,m,n,l分別表示滑動窗口中關(guān)鍵幀、特征點、特征線的數(shù)量,xk表示第k個滑動窗口的IMU狀態(tài),分別描述了在世界系中IMU的位置、姿態(tài)方向、速度、加速度計偏差和陀螺儀偏差。λn表示空間點的逆深度,Ol表示空間線的正交。

然后把邊緣化先驗殘差、IMU測量殘差、閉環(huán)信息殘差、點殘差和線殘差都加入到后端優(yōu)化成本函數(shù)C中,得到式(5):

(5)

2 點線耦合

2.1 設(shè)定線特征提取的動態(tài)閾值

傳統(tǒng)的線段提取算法存在短線較多和重疊線段不能有效剔除的問題,這些問題會導(dǎo)致視覺傳感器對相機位姿的估計不夠準確,且難以精確進行全局地圖構(gòu)建。對于點線融合的SLAM系統(tǒng),線特征是對點特征的補充,只需要保留較長的線特征而剔除較多的短線就可以對相機位姿進行精確估計。本文構(gòu)造了一種改進的線段提取算法,在線特征提取階段,設(shè)定一個根據(jù)具體場景而變化的動態(tài)線段提取閾值,這樣做的目的是對視覺傳感器檢測到實際場景的線特征進行保留和剔除,進一步提高相機位姿估計的準確性,從而提升了全局建圖的精確度。

定義線段提取的動態(tài)閾值為Lmin,則Lmin滿足如下公式:

(6)

其中,n表示在第m幀中線特征的提取數(shù)量,W與H分別表示當前幀的寬度與高度。

以上公式是根據(jù)每幀圖像的寬度、高度以及提取到的線特征數(shù)量設(shè)計而成,避免了傳統(tǒng)線特征提取算法由于設(shè)置固定閾值的原因致使提取有效的線特征較少或提取多余短線段的結(jié)果,從而對線特征有合理且有效的利用。

再將提取到的所有線特征定義為一個集合{l1,l2,…,ln},則線段長度篩選機制遵循以下公式:

Lli≥Lmin,i?{1,2,…,n},

(7)

其中,Lli表示提取到線段的長度,當Lli大于或等于閾值Lmin時,線段被保留下來,反之線段會被剔除。

2.2 提取點線耦合特征

LSD算法是檢測線特征的常用方法之一。由于該算法計算灰度圖像中每個像素的水平線角度并分割具有相似水平線角度的連接區(qū)域,可以在線特征的端點處檢測角點特征。因此在線段的末端檢測到的角點特征與線特征存在位置關(guān)系,這種位置關(guān)系被定義為點線耦合特征。

在本文中,將特征點與距其最近的特征線進行耦合,然后提取融合后的特征,具體的做法如下:使用K-D樹(K-Dimensional Tree)快速搜索算法,在以特征點為圓心、半徑為r的圓中搜索線特征的端點,將距離圓心最近的線特征端點所在的特征線視為距離特征點最近的特征線,并將其與特征點融合并提取(圖2)。

圖2 點特征搜索線特征示意圖

如果點線特征成功耦合后,可以得到一個關(guān)于點特征、線特征和幀三者之間的關(guān)系式:

H={h|h={p,l,ci,cj}},

(8)

其中,p表示點特征,l表示線特征,ci與cj表示不同的兩幀,h為點線耦合特征的描述符,H表示h的集合。

2.3 點線耦合殘差模型

在全局優(yōu)化中,本文采用BA(Bundle Adjustment)算法估計點特征的深度,點特征的深度估計的準確性和穩(wěn)定性受跟蹤幀數(shù)的影響。跟蹤幀數(shù)越多,深度估計就越準確、越穩(wěn)定,相反,若跟蹤幀數(shù)少,就可能導(dǎo)致不穩(wěn)定。在本文中,在滑動窗口內(nèi)設(shè)置一個點跟蹤幀數(shù)的閾值μ,若點跟蹤的幀數(shù)小于μ,就在滑動窗口優(yōu)化中引入本文構(gòu)造的點線耦合殘差,利用所提出的點線耦合殘差進行魯棒定位。點線耦合殘差模型如圖3所示。

圖3 點線耦合殘差模型

ax+by+c=0.

(9)

若已知直線的起點Lcjs=(xcjs,ycjs)和終點Lcje=(xcje,ycje),則可以計算出式(9)所表述直線的常系數(shù),如式(10)所示。

(10)

將直線的常系數(shù)集合定義為M(Lcj),稱之為直線的性質(zhì),如式(11)所示。

M(Lcj)=[a,b,c].

(11)

(12)

3 實驗與分析

本文算法的實驗平臺是一臺處理器為Intel?CoreTMi3-8100,運行內(nèi)存是8 GB,操作系統(tǒng)為Ubuntu 18.04 LTS 64位的計算機。

將本文算法在EuRoC數(shù)據(jù)集的MH_04_difficult序列進行了仿真實驗,得到了點線融合特征圖像和算法運行的軌跡,如圖4所示。圖4(a)中的線條表示線特征,小圓點表示點特征,從圖4(a)可以看出,在黑暗環(huán)境下,點特征稀疏,但本文算法提取到了豐富的線特征,且在線特征提取時,剔除了多而短小的線段,保留了比較長的線段。在圖4(b)中,顏色深的線表示本文算法的運行軌跡,顏色淺的線表示真實軌跡,可以看出本文算法的運行軌跡與真實軌跡一致。

(a)點線耦合圖 (b)運行軌跡 圖4 本文算法在MH_04_difficult序列的仿真圖

進一步,在MH_04_difficult、V1_03_difficult和V2_03_difficult三個難度大的序列中,將本文算法與VINS-Mono算法、PL-VIO算法進行對比,得到各自的三維絕對軌跡誤差,實驗結(jié)果如圖5至圖7所示。圖中的虛線與實線分別表示真實軌跡和估計軌跡,豎直條狀帶表示誤差等級,顏色越深誤差越小,顏色越淺誤差越大。從圖中可以看出,本文算法的估計軌跡顏色深的居多,表明本文算法相對于VINS-Mono算法與PL-VIO算法的定位誤差更小,從而具有更高的定位精度。

(a)VINS-Mono (b)PL-VIO (c)本文算法 圖5 三種算法在MH_04序列的三維絕對軌跡誤差圖

圖8是VINS-Mono算法、PL-VIO算法和本文算法在兩個序列中的實時絕對軌跡誤差曲線圖。

(a)V1_03序列 (b)V2_03序列圖8 三種算法在兩序列的實時絕對軌跡誤差曲線圖

從圖8可以直觀地看出,本文算法的軌跡誤差曲線整體上是低于另外兩種算法,且誤差的峰值比另外兩種算法小,表明本文算法與VINS-Mono算法、PL-VIO算法相比,取得了更小的定位誤差,再次驗證了本文算法要優(yōu)于另兩種算法。

圖9和圖10表示VINS-Mono算法、PL-VIO算法和本文算法在V1_03和V2_03序列中絕對位姿誤差的分布情況。從圖9可以看出,VINS-Mono算法、PL-VIO算法和本文算法在V1_03序列的絕對位姿誤差分布分別為0.05～0.40 m、0.05～0.40 m、0.05～0.30 m,從圖10可知,三種算法在V2_03序列的對位姿誤差分布依次為0.05～0.40 m、0.05～0.40 m、0.05～0.30 m。由此可知,本文算法的絕對位姿誤差的分布區(qū)間均比VINS-Mono算法和PL-VIO算法小,進而可得,本文算法的絕對位姿誤差分布范圍更集中,說明采用本文算法的系統(tǒng)魯棒性更強,使系統(tǒng)的穩(wěn)定性更好。

(a)VINS-Mono (b)PL-VIO (c)本文算法 圖9 三種算法在V1_03序列絕對位姿誤差分布

為了在數(shù)值上更好地說明本文算法的優(yōu)點,將本文算法在11個EuRoC公共數(shù)據(jù)集上進行多次仿真實驗取平均值,并與之前的VINS-Mono算法、PL-VIO算法的結(jié)果作對比。在EuRoC數(shù)據(jù)集下的不同算法的絕對軌跡的均方根誤差(RMSE)和標準差(STD)如表1所示。

表1 不同算法的位姿估計誤差

從表1數(shù)據(jù)可以直觀地看出,本文算法相較于VINS-Mono和PL-VIO算法,絕對軌跡誤差在大多數(shù)序列中有明顯降低,這表明算法的定位準確度有了明顯提升。本文算法在MH_03_medium序列中的軌跡精度比VINS-Mono算法提升了48.7%,在V2_03_difficult序列中的軌跡精度比PL-VIO提升了58.8%。

4 結(jié)語

本文構(gòu)造了一種動態(tài)點線融合的單目視覺慣性SLAM算法。在傳統(tǒng)點特征視覺SLAM算法基礎(chǔ)上加入了改進后的線特征提取算法,在線段處理階段,通過設(shè)置提取線特征的動態(tài)閾值,處理冗余的線段,實現(xiàn)對線特征的有效利用,提取新的點線耦合特征并構(gòu)造點線耦合殘差模型,用以構(gòu)造最小化成本函數(shù)。通過在EuRoC數(shù)據(jù)集上的仿真實驗表明,本文算法相較于VINS-Mono算法和PL-VIO算法,得到的位姿估計誤差均有明顯降低,所得到的估計軌跡更接近于真實的軌跡,表明本文所構(gòu)造的算法提升了系統(tǒng)的定位精度,使系統(tǒng)更具有魯棒性。