基于動態點線耦合的單目視覺慣性SLAM算法

吳 桐,黃宜慶,張開平

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

0 引言

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

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

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

1 本文算法結構

1.1 算法框架

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

圖1 視覺慣性SLAM系統框架結構

1.2 線特征殘差模型

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

(1)

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

(2)

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

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

(3)

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

1.3 滑動窗口優化

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

(4)

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

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

(5)

2 點線耦合

2.1 設定線特征提取的動態閾值

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

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

(6)

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

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

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

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

(7)

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

2.2 提取點線耦合特征

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

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

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

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

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

(8)

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

2.3 點線耦合殘差模型

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

圖3 點線耦合殘差模型

ax+by+c=0.

(9)

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

(10)

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

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

(11)

(12)

3 實驗與分析

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

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

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

進一步,在MH_04_difficult、V1_03_difficult和V2_03_difficult三個難度大的序列中,將本文算法與VINS-Mono算法、PL-VIO算法進行對比,得到各自的三維絕對軌跡誤差,實驗結果如圖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算法相比,取得了更小的定位誤差,再次驗證了本文算法要優于另兩種算法。

圖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。由此可知,本文算法的絕對位姿誤差的分布區間均比VINS-Mono算法和PL-VIO算法小,進而可得,本文算法的絕對位姿誤差分布范圍更集中,說明采用本文算法的系統魯棒性更強,使系統的穩定性更好。

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

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

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

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

4 結語

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