大尺度弱紋理場景下多源信息融合SLAM算法

朱葉青，金 瑞，趙良玉

(北京理工大學(xué)宇航學(xué)院，北京 100081)

0 引 言

近年來，以無人車和無人機(jī)為代表的自主機(jī)器人數(shù)量及需求不斷增加[1]。同時定位與地圖創(chuàng)建(Simultaneous localization and mapping, SLAM)作為其自主導(dǎo)航的關(guān)鍵技術(shù)[2]，允許機(jī)器人在未知環(huán)境中，依靠自身所帶傳感器遞增式的創(chuàng)建環(huán)境地圖，并同時給出機(jī)器人所在位置，受到了國內(nèi)外諸多學(xué)者的關(guān)注和研究。視覺慣性融合的SLAM系統(tǒng)可通過其慣性測量單元(Inertial measurement unit，IMU)測量到的加速度和角速度信息來改善相機(jī)因強(qiáng)烈光照變化等引起的問題，其在小范圍內(nèi)定位精度高、不易受外界干擾、信息量獲取豐富，成為近年來的熱門研究方向[3]。文獻(xiàn)[4]提出了一種對比度增強(qiáng)的尺度不變特征變換(Scale-invariant feature transform，SIFT)算法，降低了特征匹配的時間。文獻(xiàn)[5]使用點(diǎn)云迭代最近點(diǎn)(Iterative nearest point，ICP)算法獲取初始位姿，可有效規(guī)避誤匹配從而獲得更加精準(zhǔn)的相對位姿。然而在弱紋理場景中，由于較難提取大量有效特征點(diǎn)，基于特征點(diǎn)法的視覺慣性SLAM性能通常會降低、定位精度會下降，紋理較差的圖像序列或由于運(yùn)動模糊等原因造成系統(tǒng)完全失效。雖然直接法在某種程度上緩解了特征點(diǎn)的依賴問題，但是稠密和半稠密的直接跟蹤法計算量較大，無法在一些計算能力有限的平臺上使用[6]。

如在城市樓房、街道等人造場景[7]中，通常包含豐富的線性元素，即在缺乏可靠特征點(diǎn)的情況下，弱紋理場景下仍然包含許多可以提取的線特征。線特征相對于點(diǎn)特征而言，是一種更高層次的特征，利用線特征構(gòu)建的環(huán)境地圖具有更直觀的幾何信息，同時也能提高SLAM系統(tǒng)的精度和魯棒性。對于線特征的表示，文獻(xiàn)[8]提出了正交表示法。文獻(xiàn)[9]使用線特征研究了視覺慣性里程計(Visual inertial odometry, VIO)的可觀測性。文獻(xiàn)[10]構(gòu)造了一種結(jié)合點(diǎn)線特征的緊耦合單目VIO系統(tǒng)，但算法只是實(shí)現(xiàn)了局部位姿估計,沒有考慮全局的定位誤差。

在大尺度場景中，為了提高狀態(tài)估計的精度和魯棒性，多源信息融合的方法已成為一種主流趨勢。根據(jù)系統(tǒng)使用的傳感器類型不同，將已有的研究工作分為局部定位和全局感知定位。對于局部定位，通常采用相機(jī)[11-12]、IMU、激光雷達(dá)[13]和RGB-D傳感器進(jìn)行六自由度狀態(tài)估計。局部定位傳感器的不足主要包括：相機(jī)在快速移動或者光照發(fā)生劇烈變化時特征點(diǎn)容易跟蹤丟失、激光雷達(dá)對玻璃等物體存在反射、IMU長時間運(yùn)行會存在累計誤差產(chǎn)生漂移等。全局感知定位包含全球?qū)Ш叫l(wèi)星系統(tǒng)(Global navigation satellite system, GNSS)、磁力計、氣壓計等。全局感知定位的測量相對于地球框架，獨(dú)立于起始點(diǎn)，可有效避免累計漂移的問題。然而，全局測量通常含有噪聲且采集頻率較低，若只有全局傳感器不足以實(shí)現(xiàn)六自由度狀態(tài)的實(shí)時估計，在做高精度定位時往往無法單獨(dú)使用。

為了實(shí)現(xiàn)精確的全局感知定位，可以將全局傳感器與局部傳感器融合在一起。文獻(xiàn)[14]中引入一種多維觀測噪聲尺度因子，并在此基礎(chǔ)上提出一種基于無跡卡爾曼濾波的自適應(yīng)SLAM算法。文獻(xiàn)[15]通過基于貝葉斯濾波的視覺SLAM方法來估計空間翻滾非合作目標(biāo)的相對位姿。文獻(xiàn)[16]提出一種基于擴(kuò)展卡爾曼濾波(Extended Kalman filter, EKF)的視覺慣性和全球定位系統(tǒng)(Global positioning system, GPS)測量相融合的無漂移估計算法。文獻(xiàn)[17]采用無跡卡爾曼濾波(Unscented Kalman filter, UKF)算法對視覺、激光雷達(dá)和GPS測量信息進(jìn)行融合。然而使用濾波方法對時間同步非常敏感，由于狀態(tài)傳播無法逆向，所以延遲的測量處理較麻煩，需要特殊的排序機(jī)制以確保來自多個傳感器的所有測量均為有序。與基于濾波的方法相比，基于優(yōu)化的方法在該方面具有明顯優(yōu)勢。其可以在不同的點(diǎn)重復(fù)狀態(tài)向量的線性化，可存儲較長一段時間的測量值。文獻(xiàn)[18]使用基于優(yōu)化的框架將局部VIO與GPS測量值融合，定位精度比文獻(xiàn)[16]更高。文獻(xiàn)[19]提出了一種基于優(yōu)化的支持多種傳感器類型的融合框架，但是該框架沒有考慮弱紋理場景下的特征跟蹤問題。

為此，本文提出一種視覺慣性與全球?qū)Ш叫l(wèi)星系統(tǒng)多源信息融合的SLAM算法。首先利用基于點(diǎn)線的特征跟蹤算法，解決了弱紋理場景相鄰幀圖像之間運(yùn)動估計精度不高的問題。同時對于具有重復(fù)線特征的弱紋理場景，將線特征表示為直線端點(diǎn)的線性約束，整合到基于特征點(diǎn)的算法中，有效改善了算法的魯棒性。最后為實(shí)現(xiàn)大尺度弱紋理場景下的精準(zhǔn)狀態(tài)估計，通過視覺慣性信息和GNSS測量信息融合的算法，實(shí)現(xiàn)了局部精確和全局無漂移的位姿估計。

1 系統(tǒng)整體框架

系統(tǒng)整體框架如圖1所示，主要包括前端點(diǎn)線特征的跟蹤及相機(jī)與IMU的數(shù)據(jù)預(yù)處理、后端視覺慣性SLAM位姿圖聯(lián)合優(yōu)化、GNSS位姿融合3個部分。

圖1 系統(tǒng)整體框架Fig.1 System framework

在預(yù)處理階段，慣性測量單元的數(shù)據(jù)采用IMU預(yù)積分的形式[20]，在短期內(nèi)提供相鄰幀之間的相對位姿估計。相鄰圖像幀之間采用光流法進(jìn)行前端數(shù)據(jù)關(guān)聯(lián)。利用慣性測量數(shù)據(jù)優(yōu)化視覺測量的尺度因子，并對齊兩個傳感器的位姿估計結(jié)果，實(shí)現(xiàn)視覺慣性聯(lián)合位姿校準(zhǔn)。

在后端視覺慣性SLAM位姿圖聯(lián)合優(yōu)化部分，基于視覺慣性里程計滑動窗口優(yōu)化方法，將新增的關(guān)鍵幀依次通過滑動窗口進(jìn)行優(yōu)化。在全局位姿圖優(yōu)化時，聯(lián)合優(yōu)化位移、偏航角、俯仰角和滾轉(zhuǎn)角，實(shí)現(xiàn)六自由度的位姿圖優(yōu)化，最終將得到的局部位姿與GNSS全局位姿進(jìn)行融合，實(shí)現(xiàn)魯棒、高精度的位姿估計。

2 點(diǎn)線特征跟蹤算法

2.1 點(diǎn)線特征表示及觀測模型

基于點(diǎn)特征提取和描述的視覺SLAM系統(tǒng)目前已經(jīng)比較成熟，由于特征點(diǎn)位置在圖像中比較好定位并加以區(qū)分，降低了數(shù)據(jù)關(guān)聯(lián)的難度，目前大部分視覺慣性融合的SLAM算法都是基于點(diǎn)特征。線特征的優(yōu)點(diǎn)是在較大的視角變化下比較穩(wěn)定，并且具有較高的光照不變性；在弱光照下線段檢測比點(diǎn)特征檢測具有更強(qiáng)的魯棒性。其缺點(diǎn)是缺少有效的描述匹配方法，線段的檢測較不穩(wěn)定，使得基于線特征的SLAM實(shí)現(xiàn)起來具有一定的難度。

相比于霍夫直線變換等經(jīng)典的直線檢測算法，LSD(Line segment detector, LSD)算法是一種局部提取直線的方法[21]，其運(yùn)行速度更快，能在線性時間內(nèi)得出亞像素級精度的檢測結(jié)果。LSD算法以灰度圖像為輸入，其輸出值為一系列檢測出的線段。該算法在設(shè)計過程中被設(shè)定為在任何數(shù)字圖像中都無需參數(shù)調(diào)節(jié)，并且可以自動控制誤檢的數(shù)量，是一種魯棒性好、精度高的直線檢測算法，使用LSD直線檢測算法和LBD(Line band descriptor, LBD)描述子[22]，對旋轉(zhuǎn)與尺度縮放具有較高的魯棒性，可以達(dá)到更優(yōu)的匹配效果，計算速度更快。

對于直線表示方法，文獻(xiàn)[10]在初始化、坐標(biāo)變換和重投影時使用普呂克坐標(biāo)表示，后端優(yōu)化采用正交表示。本文對于具有重復(fù)圖案或弱紋理的場景，引入一種線性誤差，將其表示為直線端點(diǎn)上的線性約束，該約束可整合到兩種先進(jìn)的基于特征點(diǎn)算法的線性公式OPnP[23]和EPnP[23]中。此外，由于遮擋或誤檢，直線可能只被部分觀測，本文線特征參數(shù)化方式可將其集成到EPnP和OPnP算法中。

圖2 線特征的重投影誤差Fig.2 Re-projection error of line feature

定一個平面，其單位法向量li,k定義為[24]：

(1)

(2)

(3)

其中:投影點(diǎn)xi,j=[xi,j,yi,j]T。

(4)

(5)

其中:Op和Oq分別表示Pw,k與Qw,k對應(yīng)的相機(jī)坐標(biāo)，可通過式得到，οpx,οpy,οpz與οqx,οqy,οqz分別表示Op與Oq在相機(jī)坐標(biāo)系中x,y,z軸上的分量。式(6)中Riw和tiw分別表示世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移：

(6)

(7)

2.2 IMU預(yù)積分及觀測模型

(8)

(9)

其中:

(10)

其中:αbibj、βbibj、qbibj分別為預(yù)積分得到的位置、速度和旋轉(zhuǎn)值。式(11)為連續(xù)時間下未考慮偏置和噪聲的預(yù)積分模型，而現(xiàn)實(shí)中IMU測量值通常含有噪聲，且為離散時間形式。在離散時間條件下，采用中值積分的方式可得到IMU測量值，將幀k和幀k+1之間的IMU預(yù)積分表示為：

(11)

(12)

(13)

則IMU的測量殘差可寫為式(14)：

(14)

其中:[·]xyz是以四元數(shù)形式表示的旋轉(zhuǎn)誤差的實(shí)部。

3 視覺慣性SLAM位姿圖聯(lián)合優(yōu)化

在視覺慣性里程計后端聯(lián)合優(yōu)化部分，通過最小化所有測量殘差項(xiàng)之和來優(yōu)化滑動窗口中的所有狀態(tài)變量如式(15):

(15)

4 GNSS位姿融合

(16)

(17)

由于視覺慣性局部估計在小范圍內(nèi)是精確的，因此可以利用兩幀之間的相對姿態(tài)?？紤]到兩個連續(xù)幀t-1幀和t幀，局部因子采用式(18)所示的視覺慣性因子表征:

(18)

(19)

5 仿真校驗(yàn)與分析

5.1 數(shù)據(jù)集實(shí)驗(yàn)

仿真采用開源機(jī)器人操作系統(tǒng)(Robot operating system,ROS)Kinetic，使用C++語言基于Ubuntu 16.04環(huán)境下的Microsoft Visual Studio Code 2019平臺開發(fā)，計算機(jī)處理器為Intel Xeon CPU E5645，主頻2.40 GHz，內(nèi)存為48 GB。

5.1.1EuRoC數(shù)據(jù)集

為了驗(yàn)證弱紋理和光照變化較大場景下算法定位精度及魯棒性，本文選用EuRoC數(shù)據(jù)集[25]。EuRoC數(shù)據(jù)集是由蘇黎世聯(lián)邦理工大學(xué)采集的微型飛行器(Micro aerial vehicle，MAV)視覺慣性數(shù)據(jù)集。數(shù)據(jù)集包括立體圖像(Aptina MT9V034全局快門，單色WVGA，相機(jī)頻率20FPS)，并同步IMU(ADIS 16448, 200 Hz)數(shù)據(jù)以及精準(zhǔn)的地面真實(shí)狀態(tài)(動作捕捉系統(tǒng)VICON，激光追蹤器Leica MS50)。數(shù)據(jù)集提供了所有外部和內(nèi)部參數(shù)。在仿真實(shí)驗(yàn)中，只使用左攝像頭的圖像，與OKVIS[26]以及VINS-mono[27]兩種比較先進(jìn)的單目VIO算法進(jìn)行比較。OKVIS是基于點(diǎn)特征的滑動窗口優(yōu)化算法，可應(yīng)用于單目和立體相機(jī)模式。VINS-mono是具有閉環(huán)的完整的視覺慣性SLAM系統(tǒng)，在滑動窗口中使用點(diǎn)特征優(yōu)化IMU狀態(tài)。

圖3所示為EuRoC數(shù)據(jù)集序列中室內(nèi)弱紋理場景圖。其中圖(a1)～圖(a7)為數(shù)據(jù)集中的原始場景圖；圖(b1)～圖(b7)為點(diǎn)特征提取圖；圖(c1)～圖(c7)為線特征提取得到的效果圖?？煽吹皆诎^大面積平面的圖(a1)、圖(a4)、圖(a5)光照較暗的圖(a2)、圖(a3)，以及光照過強(qiáng)而反光的圖(a6)、圖(a7)等弱紋理場景中，點(diǎn)特征提取較為稀疏，而利用線特征提取算法則可以提取大量有效信息從而更加直觀描述幾何結(jié)構(gòu)，為后續(xù)視覺里程計根據(jù)圖像信息估計相機(jī)運(yùn)動提供更加豐富且魯棒的特征信息。

圖3 弱紋理場景下特征提取對比Fig.3 Comparison of feature extraction in weak texture scenes

表1 EuRoC數(shù)據(jù)集上位移與旋轉(zhuǎn)均方根誤差比較Table 1 The root mean square error of translation and rotation error on the EuRoC MAV dataset

表1通過不同算法在EuRoC數(shù)據(jù)集11個序列估計的軌跡和真實(shí)軌跡的均方根誤差來比較算法的定位性能，圖4為位移均方根誤差及旋轉(zhuǎn)均方根誤差對比柱狀圖，橫坐標(biāo)為數(shù)據(jù)集序號。可以看出，由于所提出的算法在點(diǎn)特征提取的基礎(chǔ)上加入了線特征提取，所以均方根誤差明顯低于基于點(diǎn)特征的

圖4 位移均方根誤差以及旋轉(zhuǎn)均方根誤差RMSE對比Fig.4 Comparison of root mean square error (RMSE) for translation and rotation using the EuRoc dataset

VIO算法OKVIS與VINS-Mono。與同樣是點(diǎn)線特征提取的PL-VIO相比，后者在V1_02_medium數(shù)據(jù)集上運(yùn)行時，出現(xiàn)追蹤失敗導(dǎo)致嚴(yán)重漂移沒有得到最終軌跡，表明本文所提出的算法具有更好的魯棒性。另外，在快速運(yùn)動、特征稀疏的數(shù)據(jù)集V2_03上仍能夠保持0.2 m左右的高精度定位，精度提升了19%左右，較未添加線特征和全局定位的VINS-Mono算法提升39%左右。且根據(jù)柱狀圖的分布可知，所提出算法的定位誤差及旋轉(zhuǎn)誤差的波動更小，定位估計具有更好的一致性，實(shí)現(xiàn)了弱紋理環(huán)境下魯棒精準(zhǔn)的定位。

圖5給出了PL-VIO和所提出算法估計的軌跡，顏色由藍(lán)到紅，誤差逐漸增大。比較MH_04_difficult、MH_03_medium、V1_03_difficult三個序列上的軌跡，其中圖5(a)、(c)、(e)為PLO-VIO的軌跡結(jié)果，圖5(b)、(d)、(f)為所提出算法的軌跡結(jié)果?？梢钥闯觯寒?dāng)攝像機(jī)快速旋轉(zhuǎn)時，本文所提算法比PL-VIO產(chǎn)生的誤差更小。此外，在發(fā)生快速旋轉(zhuǎn)的序列上，觀測視角會發(fā)生很大變化，光照條件對跟蹤點(diǎn)特征也非常具有挑戰(zhàn)性。為了更加直觀的查看不同算法的軌跡對比圖，圖6給出了EuRoC數(shù)據(jù)集MH_04上的測試結(jié)果軌跡對比圖?？梢园l(fā)現(xiàn)，所提出算法的定位軌跡能更接近真實(shí)軌跡，定位誤差小于PL-VIO、OKVIS和VINS-mono，表明所提出算法的定位更準(zhǔn)確。

圖5 EuRoC數(shù)據(jù)集上與PL-VIO的軌跡對比Fig.5 Trajectories comparison of the proposed methods verus PL-VIO using the EuRoc dataset

5.1.2KITTI數(shù)據(jù)集

為了驗(yàn)證算法在大尺度場景中的定位精度以及魯棒性，本文選用KITTI數(shù)據(jù)集[28]。KITTI數(shù)據(jù)集由德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合記錄，是目前最大的自動駕駛場景下的計算機(jī)視覺算法評測數(shù)據(jù)集。KITTI包含市區(qū)、鄉(xiāng)村和高速公路等場景采集的真實(shí)圖像數(shù)據(jù)，有各種程度的遮擋與截斷。其中采用的傳感器包含立體相機(jī)(相機(jī)型號：PointGrey Flea 2，單色1382×512像素，相機(jī)頻率為10PFS)和GPS，地面真實(shí)狀態(tài)由慣性導(dǎo)航系統(tǒng)(OXTS RT 3003)提供。所提出算法從GNSS測量數(shù)據(jù)中獲取緯度、經(jīng)度和高程。相機(jī)用于局部狀態(tài)估計，在全局優(yōu)化中，局部結(jié)果與GNSS測量值融合。將所提出算法與比較先進(jìn)的VIO算法融合GPS算法VINS-Fusion[19]進(jìn)行比較。VINS-Fusion[19]是VINS-Mono的擴(kuò)展，支持多種視覺慣性傳感器類型，

圖6 不同算法在EuRoC數(shù)據(jù)集MH_04_difficult上的軌跡對比Fig.6 Comparison of the estimated trajectories by different algorithms using the EuRoc sequence MH_04_difficult

如單目加IMU，雙目相機(jī)加IMU以及與GPS的融合。文獻(xiàn)[19]在仿真驗(yàn)證中使用Horn方法作為對齊真值，采用表2中Raw Data數(shù)據(jù)序列對應(yīng)的KITTI中00-10數(shù)據(jù)序列的真值進(jìn)行對齊。

從表2中的均方根誤差(Root mean square error, RMSE)可以看出，本文所提算法在所有序列上的性能均優(yōu)于VINS-Fusion，表明大尺度場景下同樣融合GNSS的前提下添加線特征可有效提高估計軌跡的精度，而且為了更加直觀的比較軌跡的定位精度,使用KITTI數(shù)據(jù)集中長度最長(長度為3.7 km)的數(shù)據(jù)序列10_03_drive_0027，將兩種方法估計的軌跡進(jìn)行對比如圖7所示，可以看出所提出算法的軌跡更加貼合真值，尤其在拐角處VINS-Fusion的軌跡與真值相差較大，所提出的算法在大尺度運(yùn)行場景下魯棒性更強(qiáng)、定位精度更高。

表2 KITT上位移均方根誤差RMSE比較Table 2 The RMSE of translation error on the KITTI raw data

圖7 不同算法在KITTI數(shù)據(jù)集上的定位軌跡對比Fig.7 Comparison of the estimated trajectories by different algorithms using KITTI dataset.

5.1.3AGZ數(shù)據(jù)集

為了驗(yàn)證算法在含有豐富線性特征的大尺度弱紋理人造場景(如城市樓房、街道等)中的定位精度以及魯棒性，選用AGZ數(shù)據(jù)集[7]進(jìn)行驗(yàn)證。AGZ數(shù)據(jù)集是由蘇黎世聯(lián)邦理工大學(xué)，使用裝載GoPro Hero 4相機(jī)的微型飛行器在城市街道低海拔(離地5～15 m)進(jìn)行拍攝，記錄圖像間隔為30 ms。數(shù)據(jù)集總長度2 km，包括時間同步的高分辨率圖像(1920×1080×24位)、GPS以及IMU傳感器數(shù)據(jù)、地面谷歌街景圖像和地面真實(shí)數(shù)據(jù)，因此該數(shù)據(jù)集適合評估和檢測基于圖像的定位、單目視覺里程計和在線三維重建環(huán)境等算法。

圖8是AGZ中部分蘇黎世城市街景圖像，可以看出，城市戶外街景中存在大量弱紋理墻壁及玻璃窗，在地面和墻面上幾乎提取不到點(diǎn)特征，而在地面與墻面的交界處，窗戶的邊緣等卻存在豐富的線段特征。圖8(a1)、(b1)為提取點(diǎn)特征的場景圖，圖8(a2)、(b2)對應(yīng)其提取線特征的對比圖，圖8(c1)、(c2)、(c3)和圖8(d1)、(d2)、(d3)為不同場景下線特征提取的效果圖?？煽吹皆诎^大面積墻壁的圖8(c1)、(c2)、含有長距離街道的圖8(d1)、(d2)，以及大量窗戶的圖8(c3)、(d3)等場景中，利用線特征提取算法可以提取大量有效信息從而更加直觀描述幾何結(jié)構(gòu)，為視覺里程計根據(jù)圖像信息估計相機(jī)運(yùn)動提供更加豐富的特征信息。將所提出算法在AGZ數(shù)據(jù)集上得到的軌跡與GNSS得到的軌跡在谷歌地圖上對比，結(jié)果如圖9所示，藍(lán)色為GNSS的數(shù)據(jù)，紅色為所提算法的運(yùn)行軌跡?？梢钥闯鲈谑彝獯蟪叨冗\(yùn)行過程中，算法可以有效的校正GNSS產(chǎn)生的定位誤差，產(chǎn)生的軌跡與地圖的路線吻合更好，即算法具有大尺度下姿態(tài)估計準(zhǔn)確的優(yōu)點(diǎn)。

圖9 本文算法軌跡與GNSS對比Fig.9 Comparison of trajectory with GNSS

6 結(jié) 論

1)提出了一種基于點(diǎn)線特征的跟蹤算法，解決了弱紋理場景下基于特征點(diǎn)的視覺慣性里程計前端數(shù)據(jù)關(guān)聯(lián)誤差較大的問題，定位精度較未添加線特征的VINS-Mono算法提升約19%左右，有效提高了弱紋理場景相鄰幀圖像之間的運(yùn)動估計精度。

2)通過引入線性誤差表示，將線性特征表示為直線端點(diǎn)上的線性約束，從而將線特征整合到基于特征點(diǎn)算法的線性公式中，提升了算法的魯棒性。

3)提出了一種視覺慣性與GNSS多源信息融合的SLAM算法，解決了大尺度場景下累計誤差導(dǎo)致的定位性能下降問題。通過多個不同數(shù)據(jù)集的測試驗(yàn)證，所提出算法相較同類算法具有更高的定位準(zhǔn)確度和魯棒性。