基于動(dòng)態(tài)點(diǎn)線耦合的單目視覺(jué)慣性SLAM算法

吳 桐,黃宜慶,張開平

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

0 引言

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

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

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

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

1.1 算法框架

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

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

1.2 線特征殘差模型

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

(1)

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

(2)

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

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

(3)

其中,s為空間線中端點(diǎn)的齊次坐標(biāo)。

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

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

(4)

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

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

(5)

2 點(diǎn)線耦合

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

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

定義線段提取的動(dòng)態(tài)閾值為L(zhǎng)min,則Lmin滿足如下公式:

(6)

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

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

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

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

(7)

其中,Lli表示提取到線段的長(zhǎng)度,當(dāng)Lli大于或等于閾值Lmin時(shí),線段被保留下來(lái),反之線段會(huì)被剔除。

2.2 提取點(diǎn)線耦合特征

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

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

圖2 點(diǎn)特征搜索線特征示意圖

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

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

(8)

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

2.3 點(diǎn)線耦合殘差模型

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

圖3 點(diǎn)線耦合殘差模型

ax+by+c=0.

(9)

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

(10)

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

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

(11)

(12)

3 實(shí)驗(yàn)與分析

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

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

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

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

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

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

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

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

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

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

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

表1 不同算法的位姿估計(jì)誤差

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

4 結(jié)語(yǔ)

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