一種基于線特征約束的無人機雙目視覺SLAM算法

金凌云, 王從慶, 李宏光

(南京航空航天大學自動化學院,南京 211000)

0 引言

同時定位與地圖構建(Simultaneous Localization and Mapping,SLAM)是機器人在未知環境下進行作業的關鍵技術[1]。在未知環境下,機器人通過相機、IMU等傳感器獲取信息,SLAM算法利用獲取到的信息構建所處環境地圖,提供機器人的位姿信息,并隨著機器人的移動實現連續地圖構建與定位。根據所用傳感器不同,SLAM可以分為激光SLAM和視覺SLAM。相較于激光SLAM所用的激光雷達等測距傳感器,視覺SLAM所用的相機具有體積小、成本低等特點[2-3],適合用于無人機等小負載、低功耗的設備上。

目前有很多基于優化的視覺SLAM算法,其中,提取點特征的視覺SLAM算法包括ORB-SLAM系列[4-6]、VINS-Mono[7]、VINS-Fusion[8]等。ORB-SLAM[4]是一個基于特征點的實時單目SLAM系統,通過提取圖像中的ORB特征實現機器人的定位與建圖,并且通過改進ORB特征使其具有旋轉和縮放不變性,進一步提升算法的魯棒性;ORB-SLAM2[5]在ORB-SLAM的基礎上加入了對雙目相機和RGB-D相機的支持,進一步拓展了算法的應用場景;VINS-Mono是第一個前端通過LK光流[9]實現特征點跟蹤和匹配的單目VIO算法,由于不需要像ORB-SLAM一樣提取ORB特征,減少了前端所需時間,簡化了代碼;VINS-Fusion在VINS-Mono的基礎上增加了對雙目相機的支持,使其具有單目、雙目、單目+IMU、雙目+IMU 4種工作模式,其在多個數據集上的表現都十分優異。

提取點線特征的視覺SLAM算法包括PL-VIO[10]、PL-VINS[11]、PL-SLAM[12]等,其中,線特征的提取主要使用直線分割檢測(Line Segment Detector,LSD)算法,并計算線帶描述子(Line Band Descriptor,LBD)以便后續進行線特征匹配[13-14]。PL-VIO在VINS-Mono的基礎上加入了線特征的提取及后端優化,其精度相較于VINS-Mono有所提高,但由于線特征的提取和匹配需要大量的運算,所以PL-VIO并不能夠實時運行;PL-VINS通過調整OpenCV提取線特征的參數,加快了前端提取線特征的速度,使得算法能夠實時運行;PL-SLAM是雙目點線特征的視覺SLAM算法,其在前端、后端和回環部分均加入了線特征,進一步提高了算法的精度,并且具有實時性。

在VINS-Fusion算法中,后端考慮了雙目相機觀測特征點的3種情況,即單目相機在兩幀下觀測同一點、雙目相機在兩幀下觀測同一點、雙目相機在一幀下觀測同一點,并分別計算了殘差塊和雅可比矩陣。為了更好地利用線特征對相機位姿進行約束,本文針對雙目線特征的SLAM算法,將雙目相機觀測線特征分為了3種情況:只有左目相機或只有右目相機在兩幀下觀測到同一條線段;雙目相機在兩幀下觀測到同一線段;雙目相機在一幀下觀測到同一線段。并基于上述線特征約束計算了這3種情況對應的殘差塊及雅可比矩陣,改進了VINS-Fusion算法并在EuRoC數據集[15]上進行了驗證,結果表明本文所提算法可以在保證實時性的同時提高定位精度。

1 系統框架

點線視覺SLAM算法后端處理流程如圖1所示。

對于前端提取到的點/線特征,首先進行三維重建獲取深度信息,并將圖像和IMU信息進行視覺慣性對齊,然后計算先驗約束、預積分約束和重投影約束放入基于滑窗的非線性優化中進行優化,最后根據邊緣化的是最老幀還是次新幀進行地圖點/線管理。

2 線特征后端約束

2.1 空間直線的兩種參數化方法

空間中一條直線有4個自由度,所以只需要4個參數就可以表示一條直線。普呂克參數化方法[16]包含6個參數,主要用于空間直線的平移與旋轉,由于過參數化的問題,其不能用于無約束的優化問題。而空間直線的正交表示只需要4個參數,在優化問題時較為方便,并且2種參數化方法可以很方便地相互轉化,因此在空間變換時使用Plücker坐標表示,在優化時使用正交表示 。

空間直線的普呂克坐標表示為L=(nT,dT)T∈R6,如圖2所示。

圖中:c為相機光心;d∈R3為直線的方向向量,n∈R3是由直線和相機光心構成的平面π的法向量。當給定世界坐標系到相機坐標系的變換矩陣Tcw,可實現普呂克坐標變換,即

(1)

式中:Lc為直線在相機坐標系下的坐標;Lw為直線在世界坐標系下的坐標;Rcw表示世界系到相機系的旋轉矩陣;[·]×表示三維向量的反對稱矩陣。從不同的兩幀圖像觀測到一個新的線特征時,可用如下方法初始化線的普呂克坐標:將兩幀圖像的相機光心c1、c2分別與直線L確定兩個平面π1和π2,這兩個平面可以確定空間唯一直線。有了這兩個平面可以得到對偶普呂克矩陣

(2)

從對偶矩陣中可提取普呂克坐標。

在優化時需要使用直線的正交表示(U,W)∈SO(3)×SO(2)。已知直線的普呂克坐標L=(nT,dT)T,對其進行QR分解得到

(3)

式(3)第1項為正交矩陣U,表示的是相機坐標系到直線坐標系的旋轉,即

(4)

式中,ψ=[ψ1ψ2ψ3]T。由于正交矩陣U已經包含了3個自由度,所以式(3)第2項W只有1個自由度,可以利用三角函數矩陣表示為

(5)

式中,φ為旋轉角。至此,正交表示的4個自由度已經求出,在優化時,使用O=[ψφ]T作為空間直線更新的最小表示。

2.2 線特征的殘差模型

空間中一直線L=[nTdT]T可以通過相機內參矩陣K投影至像素平面:

(6)

式中:l1,l2,l3為直線在像素平面內的坐標;fx,fy,fz為x,y,z3個方向上的焦距;cx,cy,cz為像素坐標系的原點。而線特征的重投影誤差rl定義為圖像中觀測線段的端點到空間重投影回像素平面的直線的距離,

(7)

為了最小化線的視覺重投影誤差rl,需要計算其對于約束變量的雅可比矩陣,由鏈式求導法則可得

(8)

1) 單目相機在兩幀下看到該線段,約束變量有第i幀IMU系的位姿Twbi、第j幀IMU系的位姿Twbj、相機外參Tbc。對上述約束變量求偏導得到雅可比矩陣為

(9)

式中:Rwbi表示左目IMU系到世界系的旋轉矩陣,twbi表示左目IMU系到世界系的平移矩陣;(nbi,dbi)為左目IMU系下直線的普呂克坐標;Rwbj表示右目IMU系到世界系的旋轉矩陣,twbj表示右目IMU系到世界系的平移矩陣;(nw,dw)表示直線在世界系下的普呂克坐標;Rbc表示相機到IMU的旋轉矩陣,Rbjbi表示左目IMU到右目IMU的旋轉矩陣,Rcjci表示左目相機到右目相機的旋轉矩陣;(nci,dci)表示左目相機系下直線的普呂克坐標。

2) 雙目相機在兩幀下看到該線段,約束變量有第i幀IMU系的位姿Twbi,第j幀IMU系的位姿Twbj,左目相機外參Tbci,右目相機外參Tbcj,對上述約束變量求偏導得到雅可比矩陣為

(10)

式中:Rwbj表示右目IMU系到世界系的旋轉矩陣;twbj表示右目IMU系到世界系的平移矩陣;(nbj,dbj)為右目IMU系下直線的普呂克坐標;Rbci表示左目相機到IMU系的旋轉矩陣,tbci表示左目相機到IMU系的平移矩陣;Rbcj表示右目相機到IMU系的旋轉矩陣,tbcj表示右目相機到IMU系的平移矩陣;(ncj,dcj)表示右目相機系下直線的普呂克坐標。

3) 雙目相機在一幀下看到該線段,為了保持下標的一致性,此處仍用cj表示右目相機在同一幀下的觀測。約束變量有左目相機外參Tbci,右目相機外參Tbcj,由于同一幀下IMU系并不發生變化,所以有Lbi=Lbj。對上述約束變量求偏導得到雅可比矩陣為

(11)

上述雙目相機觀測線段的3種情況如圖3所示。

圖3 雙目相機觀測線段

3 實驗與分析

利用Ceres-solver的解析求導功能,將上文計算出的殘差及雅可比矩陣填入定義好的矩陣塊中,并基于此改進VINS-Fusion算法后端的非線性優化部分。實驗的運行環境為Ubuntu18.04和ROS Melodic,CPU為i7-12700F@2.1 GHz,內存大小為32 GiB。在EuRoC公開數據集上驗證所提算法并與VINS-Fusion算法進行比較,評價指標為絕對軌跡誤差(Absolute Trajectory Error,ATE)、相對位姿誤差(Relative Pose Error,RPE)。

3.1 EuRoC數據集及實驗結果

EuRoC數據集為微型飛行器(Micro Aerial Vehicle,MAV)上收集的視覺慣性數據集,包含MH_01-05、V1_01-03、V2_01-03等11組數據,每組數據都包含雙目圖像和IMU信息,用于SLAM算法的輸入,同時包含姿態和位置的真值信息,用于后續的指標評估。將本文算法與VINS-Fusion算法在數據集上運行,得到的軌跡誤差熱力圖如圖4所示(圖中僅展示11組數據中的3組)。

圖4 三維軌跡誤差熱力圖

將運行得到的位姿數據與序列真值進行比較,實現絕對軌跡誤差和相對位姿誤差的計算。兩種算法在EuRoC數據集上的運行結果如表1、表2所示,表中加粗數值為位姿數據最優值。

表1 絕對軌跡誤差對比

表2 相對位姿誤差對比

由表中數據可知,本文算法在ATE上的精度提升最大為24.3%(V1_03),最小為1.7%(V2_02);在RPE上的精度提升最大為45.5%(MH_03),最小為0.5%(MH_01),整體相較于VINS-Fusion算法精度平均提升13.2%。

3.2 實時性評估

為了驗證本文所提算法的實時性,選取后端非線性優化所需時間和系統整體運行所需時間作為評價指標,結果如表3所示。

表3 非線性優化及系統整體運行耗時對比

可以看出,本文算法在運行EuRoC數據集時平均每幀非線性優化耗時37 ms,整體系統運行耗時97 ms,即平均每秒可處理10幀,可以滿足實時性要求。

4 結束語

本文針對低紋理環境下的無人機雙目視覺SLAM算法提出了一種新的線特征約束方法,可在特征點較少的情況下通過線特征的提取和約束仍然具有較高的定位精度和實時性。所得結果表明,本文所提算法相較于雙目點特征和單目點線特征的視覺SLAM算法在低紋理環境下具有更高的精度,同時實時性也得到了保證。在后續工作中,可以進一步發揮線特征的約束作用,嘗試將線特征加入回環檢測中,進一步提升SLAM系統的精度。