雙目點(diǎn)線特征與慣導(dǎo)融合的機(jī)器人SLAM算法研究

馮 波，劉桂華，曾維林，余東應(yīng)，張文凱

(西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621010)

E-mail：fb_941219@163.com

1 引 言

近年來，同步定位與建圖(Simultaneous Localization and Mapping，SLAM)已經(jīng)發(fā)展成為移動機(jī)器人領(lǐng)域的研究熱點(diǎn)，其被認(rèn)為是實(shí)現(xiàn)自主導(dǎo)航的核心環(huán)節(jié)，SLAM包含兩個主要任務(wù)，即定位和建圖.機(jī)器人在未知環(huán)境中，通過傳感器在運(yùn)動過程中的檢測周圍特征來獲取機(jī)器人的位姿，然后從機(jī)器人角度進(jìn)行環(huán)境的地圖構(gòu)建[1，2].在激光SLAM與視覺SLAM中，視覺SLAM因其成本低廉，能獲取豐富的信息，成為了目前SLAM方案中的熱門方法.

視覺SLAM利用獲取的圖像以估計相機(jī)運(yùn)動.根據(jù)位姿估計的方式分類，可分為直接法和特征法.

直接法如DTAM[3]，LSD-SLAM[4]和DSO[5]等根據(jù)圖像的像素亮度信息估計相機(jī)運(yùn)動，并通過最小化亮度誤差進(jìn)行優(yōu)化求解.然而，它的使用前提條件是基于灰度不變假設(shè)，即在連續(xù)的圖像幀中固定相同空間點(diǎn)的像素灰度級.因此，在照明情況復(fù)雜的應(yīng)用場景不能滿足利用直接法的條件.

特征點(diǎn)法，按照提取的特征類型，主要分為點(diǎn)、線(邊緣)以及點(diǎn)線結(jié)合的3種特征SLAM算法.

點(diǎn)特征：主要使用SIFT[6]，ORB[7]或SURF[8]提取與匹配點(diǎn)特征.根據(jù)特征匹配結(jié)果通過增量束調(diào)整最小化重投影誤差來估計相機(jī)位姿[9].線特征：Schenk Fabian和Fraundorfer Friedrich提出了一種基于邊緣的RGBD-SLAM，RESLAM[10].通過邊緣在不同的光照條件下比其他區(qū)域的強(qiáng)度值更穩(wěn)定這一特性構(gòu)建系統(tǒng).點(diǎn)和線特征：采用點(diǎn)、線特征相結(jié)合的方式求解相機(jī)位姿.最具有代表性的是Pumarola等提出的PL-SLAM[11]、謝曉佳等人提出的基于點(diǎn)特征與線特征的雙目視覺SLAM[12]、基于單目點(diǎn)線特征結(jié)合[13-15]以及基于雙目的點(diǎn)線特征系統(tǒng)[16].

以上系統(tǒng)定位精度依賴于提取的特征質(zhì)量，這在某些紋理較少的場景缺乏魯棒性，在實(shí)際應(yīng)用中比較困難.同時視覺SLAM也存在缺點(diǎn)，例如單目視覺無法獲取絕對尺度，單目純旋轉(zhuǎn)機(jī)器人運(yùn)動無法估計，機(jī)器人快速運(yùn)動時容易跟丟等.Baofu fang等人提出的解決由運(yùn)動等造成的圖像模糊匹配難的點(diǎn)線方案[17]，與僅適用視覺傳感器的方案相比，具有很好的實(shí)用性.當(dāng)前多傳感器與視覺的融合SLAM系統(tǒng)逐步發(fā)展，尤其是與IMU的融合，因IMU的快速響應(yīng)、不受成像質(zhì)量影響、可估計絕對尺度等特點(diǎn)恰好與視覺互補(bǔ)而成為多傳感器視覺SLAM的一個熱點(diǎn)研究.

對于相機(jī)與IMU融合而言，根據(jù)方式視覺慣性里程計(VIO)系統(tǒng)可分為兩個主流：松耦合與緊耦合.松耦合為IMU與相機(jī)獨(dú)立進(jìn)行位姿估計，然后對結(jié)果進(jìn)行融合；緊耦合為將IMU狀態(tài)量與相機(jī)狀態(tài)量合并在一起，共同構(gòu)建運(yùn)動方程和觀測方程，然后進(jìn)行位姿估計，且緊耦合通常精度更高、更魯棒[18]，如VINS-Mono[19].在VINS-Mono基礎(chǔ)上秦通等人又相繼提出了VINS-Fusion[20]，其給出了單目與IMU、雙目、雙目與IMU、雙目與GPS幾種不同的融合模式，在精度與穩(wěn)定性上均有一定程度的提高，但是其為了提高運(yùn)行速度前端使用opencv的Shi-Tomas角點(diǎn)(又稱GFTT，Good Feature To Track)提取與光流跟蹤，這在一定程度上損失了系統(tǒng)精度.為了解決特征點(diǎn)過少同時兼顧精度，賀一家等人提出了結(jié)合點(diǎn)線特征的視覺慣性里程計PL-VIO[21]，視覺提取點(diǎn)線特征的同時與IMU傳感器進(jìn)行緊耦合來估計相機(jī)位姿運(yùn)動，PL-VIO為基于VINS-Mono[19]開發(fā)，其前端的特征點(diǎn)與VINS-Mono一樣采用FAST角點(diǎn)提取與光流跟蹤，這在精度上也有一定的損失.

針對室內(nèi)場景實(shí)際應(yīng)用中有時會存在重復(fù)紋理以及光照變化復(fù)雜等問題，本系統(tǒng)前端的特征提取使用點(diǎn)線特征融合方案，其中點(diǎn)特征提取使用ORB特征點(diǎn).同時由于一些低成本IMU傳感器測量結(jié)果受噪聲影響較大，導(dǎo)致使用尺度恢復(fù)的結(jié)果不穩(wěn)定.此時加入雙目傳感器獲取的深度信息進(jìn)行初始化可以為后端非線性優(yōu)化的迭代求解提供一個更好的初始值[22].故本文對雙目圖像提取ORB點(diǎn)特征與線特征信息融合雙目相機(jī)深度信息與IMU傳感器進(jìn)行緊耦合優(yōu)化估計機(jī)器人位姿，本文將其命名為基于優(yōu)化的ORB點(diǎn)特征與線特征的雙目慣性視覺里程計(Stereo Visual-Inertial state estimator based on optimized ORB point feature and line feature，OOL-VINS).

2 OOL-VINS系統(tǒng)總覽

如圖1所示，整個系統(tǒng)可分為3個部分:1)前端視覺里程計.執(zhí)行點(diǎn)線特征的提取與跟蹤，另一方面獲取頻率遠(yuǎn)高于視覺的IMU信息，利用IMU響應(yīng)快速、不受成像質(zhì)量影響等性質(zhì)，與跟蹤匹配的視覺特征融合計算出相機(jī)的位姿并創(chuàng)建新的關(guān)鍵幀;2)系統(tǒng)初始化.初始化采用松耦合的方式獲取初始值.利用立體匹配或SFM求解滑動窗口內(nèi)所有幀的位姿(以第一幀作為參考坐標(biāo)系)和所有路標(biāo)點(diǎn)的3D位置.然后將SFM的結(jié)果與IMU預(yù)積分的結(jié)果進(jìn)行對齊，實(shí)現(xiàn)對陀螺儀偏置的校正，再求解每一幀對應(yīng)的速度，重力向量方向;3)后端優(yōu)化.后端優(yōu)化使用滑動窗口算法，限制關(guān)鍵幀數(shù)量，防止滑窗內(nèi)的優(yōu)化變量個數(shù)隨時間不斷增加.

圖1 系統(tǒng)框架設(shè)計Fig.1 System framework design

3 OOL-VINS傳感器測量與殘差模型

OOL-VINS前端將點(diǎn)線特征進(jìn)行數(shù)學(xué)建模加上IMU數(shù)據(jù)傳入后端進(jìn)行機(jī)器人位姿估計以及優(yōu)化.前端可分為視覺部分與IMU預(yù)積分部分，也可以分為3個部分：ORB點(diǎn)特征提取與跟蹤、線特征的提取與跟蹤、IMU預(yù)積分，如圖2所示.

3.1 點(diǎn)線特征的提取與匹配

基于OpenCV提取的ORB特征過于集中，會降低SLAM的精度，對于閉環(huán)檢測來說，也會降低一幅圖像上的信息量.故本文采用ORB-SLAM[23]中的特征點(diǎn)提取策略，通過網(wǎng)格化圖像進(jìn)行均勻的特征提取.匹配時，為了限制待匹配特征集大小，而不是對整張圖上的每一個特征點(diǎn)都進(jìn)行匹配計算，從而減小計算開銷，加快匹配速度，對不同階段采取不同的特征匹配方式[23].

線特征的提取與匹配基于LSD[24]算法與LBD[25]算法，傳統(tǒng)的LSD算法對于圖像遮擋、局部模糊等情況，由于區(qū)域增長算法的特性，一條線段會割裂成多段.這使得線特征在SLAM中的匹配不是一對一，而是多對多的，且分割點(diǎn)在鄰近的幀中不同，導(dǎo)致線特征匹配難度增大.故本文中線特征的提取采用謝曉佳等人改進(jìn)的LSD算法[12].

3.2 點(diǎn)線特征的殘差模型

本小節(jié)中，通過前面所述的特征提取與匹配結(jié)果，構(gòu)建點(diǎn)線特征的殘差模型.

3.2.1 點(diǎn)特征的殘差模型

(1)

其中，zi為點(diǎn)Pi為雙目匹配求得的深度值，K為相機(jī)內(nèi)參，ξ為前一幀到當(dāng)前幀變換矩陣的李代數(shù)：

(2)

圖2 視覺重投影誤差與IMU預(yù)積分Fig.2 Visual reprojection error and IMU pre-integration

3.2.2 線特征的殘差模型

線特征的殘差模型同樣使用重投影誤差表示，如圖2所示.即線特征重投影誤差表示為：下一幀檢測到的匹配線段端點(diǎn)M′，N′∈3到上一幀檢測線段的端點(diǎn)M，N∈3投影至下一幀所得的端點(diǎn)m，n∈3所組成直線的距離之和.

(3)

(4)

其中，s為端點(diǎn)M′、N′的坐標(biāo)，l為直線方程，顯然，l1、l2為直線方程系數(shù).

3.3 IMU測量與殘差模型

忽略地球旋轉(zhuǎn)，一個6軸IMU(3軸線加速度，3軸角速度)的測量模型[26]為分別對加速度計測量與陀螺儀測量.

qwbj=qwbi?qbibt

(5)

將積分項(xiàng)從世界坐標(biāo)系轉(zhuǎn)為IMU坐標(biāo)系進(jìn)行求解，則積分為相對于IMU坐標(biāo)系前一時刻的狀態(tài)而不是世界坐標(biāo)系，如圖2所示，以q為例，轉(zhuǎn)換后積分項(xiàng)則為相對于IMU坐標(biāo)系的狀態(tài)，p、v同理.IMU的預(yù)積分量是對兩個關(guān)鍵幀之間的測量值進(jìn)行積分，相當(dāng)于幀與幀之間的增量[28].則廣義的IMU測量值為：

(6)

同時通過公式(5)轉(zhuǎn)換后求得的積分量是在連續(xù)時間模型基礎(chǔ)上，忽略了偏置和噪聲，而實(shí)際IMU的測量是離散時間模型.考慮進(jìn)偏置與高斯白噪聲使用基于誤差隨時間變化的遞推[26](狀態(tài)轉(zhuǎn)移模型)預(yù)測預(yù)積分量，即廣義的IMU預(yù)測值記為αbibj，βbibj，qbibj，則IMU的殘差模型為：

(7)

4 OOL-VINS初始化算法設(shè)計

OOL-VINS視覺里程計部分算法設(shè)計基于VINS-Mono[19]系統(tǒng)，初始化采用松耦合獲取初始值，首先用SFM求解滑動窗口內(nèi)所有幀的位姿(以第1幀作為參考坐標(biāo)系，無尺度信息)，同時結(jié)合雙目對特征點(diǎn)匹配所得的深度信息恢復(fù)所有特征點(diǎn)的3D位置(帶尺度信息).然后將SFM的結(jié)果與IMU預(yù)積分結(jié)果進(jìn)行對齊，實(shí)現(xiàn)對陀螺儀偏置的校正，再求解每一幀對應(yīng)的速度，求解重力向量方向，恢復(fù)單目相機(jī)的尺度因子.VINS-Mono對于機(jī)器人在特殊運(yùn)動情況(例如在局部以恒定加速度運(yùn)動)以及使用低成本IMU傳感器時(IMU測量噪聲較大)，VIO初始化不能很好的恢復(fù)尺度[22]，此時使用雙目加IMU測量值進(jìn)行初始化能夠解決此時尺度不可觀問題[29].

OOL-VINS在點(diǎn)線跟蹤模塊和IMU預(yù)積分結(jié)束及數(shù)據(jù)傳入初始化模塊后，首先對于每個圖像幀結(jié)合雙目相機(jī)匹配獲取的深度信息獲取特征的3D點(diǎn).區(qū)別于VINS-Mono松耦合時對視覺只使用了RGB信息而采用5點(diǎn)法(或8點(diǎn)法)求解相機(jī)位姿，OOL-VINS使用已經(jīng)帶有尺度信息的數(shù)據(jù)利用PNP算法進(jìn)行相機(jī)初始化.

對于純視覺的SFM，假設(shè)已知相機(jī)與IMU之間的外參qbc，pbc，則可以利用外參數(shù)以相機(jī)坐標(biāo)系第1幀c0為世界坐標(biāo)系構(gòu)建等式：

(8)

(9)

(10)

(11)

對于純視覺，利用外參旋轉(zhuǎn)約束估計陀螺儀偏置后，再使用平移約束估計重力方向，速度，以及尺度信息初始值即待估計的變量為：

(12)

為了實(shí)現(xiàn)“基礎(chǔ)厚、口徑寬、能力強(qiáng)、素質(zhì)高”的人才培養(yǎng)目標(biāo)，在課程內(nèi)容設(shè)置上應(yīng)充分考慮到石油類高校非焊接專業(yè)學(xué)生相應(yīng)的基礎(chǔ)知識和專業(yè)應(yīng)用特點(diǎn)，如石油化工機(jī)械主要涉及到各種化工容器、反應(yīng)塔、加熱爐和換熱器的制造與安裝。油氣儲運(yùn)專業(yè)涉及到各種儲油罐、油氣管道、油槽車和油輪等制造工程。因此，需要從《金屬焊接》課程本身出發(fā)，使課程內(nèi)容與相應(yīng)支持課程內(nèi)容相融合，從而構(gòu)成“學(xué)有基礎(chǔ)、用有對象”的課程知識體系。

(13)

即尺度已知，在OOL-VINS中只需初始化速度和重力向量.此時區(qū)別于VINS-Mono使用IMU預(yù)積分量αbkbk+1、βbkbk+1，OOL-VINS使用γbkbk+1構(gòu)建等式以待優(yōu)化量χI為變量最小化目標(biāo)函數(shù)進(jìn)行求解.需要注意的是IMU預(yù)積分量中包含初始化過程需要優(yōu)化的IMU偏置變量，這將導(dǎo)致每次迭代優(yōu)化時得到新的偏置量需要反復(fù)對IMU預(yù)積分來求γbkbk+1，這將十分耗時，故在優(yōu)化時假設(shè)預(yù)積分的變化量與偏置成線性關(guān)系，則為γbkbk+1：

(14)

(15)

此時將公式(15)轉(zhuǎn)換為一個線性最小二乘問題進(jìn)行求解即可：

(16)

初始化χI完后，再對重力向量進(jìn)行精細(xì)優(yōu)化，相對于VINS-Mono省去了最后將相機(jī)坐標(biāo)系旋轉(zhuǎn)到世界坐標(biāo)系的過程，因?yàn)榧円曈X初始化時是將c0視作世界坐標(biāo)系，在恢復(fù)尺度后需要將相機(jī)坐標(biāo)系旋轉(zhuǎn)到世界坐標(biāo)系，而加入深度信息后的初始化過程不會存在這樣的問題.

5 OOL-VINS初始化算法設(shè)計

初始化結(jié)束后將前端的數(shù)據(jù)傳入后端執(zhí)行集成深度信息的VIO流程.一個基于優(yōu)化的SLAM后端典型求解思路是首先確定系統(tǒng)中所有傳感器的約束關(guān)系(因子圖[30])；然后針對每個約束，確定3個要素：待優(yōu)化的狀態(tài)量，每個測量元素傳遞模型即確定協(xié)方差(SLAM中假設(shè)狀態(tài)量的噪聲滿足高斯分布)，每個測量元素的誤差項(xiàng)表示；最后計算各個狀態(tài)量相對于誤差項(xiàng)的偏導(dǎo)數(shù)即Jacobian矩陣，調(diào)用非線性求解器ceres-solver(1)http：//ceres-solver.org/(使用GN，LM等迭代法)，求解BA問題.

圖3 OOL-VINS后端處理流程Fig.3 OOL-VINS back-end processing flow

在OOL-VINS中，如第1節(jié)所述，后端求解為最小化圖像平面中視覺特征的重投影誤差和IMU預(yù)積分誤差來優(yōu)化相機(jī)位姿和3D地圖點(diǎn)、線.在沒有深度信息的VINS-Mono[19]中，視覺特征的深度信息依靠前后幀的三角化獲取，而在OOL-VINS中深度的獲取方法更加魯棒：根據(jù)相機(jī)的基線距離求得雙目匹配的深度范圍，對于深度在此范圍內(nèi)的則采用雙目匹配獲取深度信息；反之，則采用前后幀三角化獲取深度信息，并使用逆深度作為待優(yōu)化變量，如圖3所示，圖中邊緣化操作將在后文闡述.

圖4 OOL-VINS因子圖Fig.4 OOL-VINS factor graph

同時，為了提高效率OOL-VINS采用滑動窗口進(jìn)行因子圖優(yōu)化的求解，系統(tǒng)因子圖見圖4.圓形節(jié)點(diǎn)為待優(yōu)化的機(jī)器人位姿或3D視覺特征(Landmark)；方塊圖形為視覺測量值和IMU預(yù)積分值，其連接并約束圓形節(jié)點(diǎn)；OOL-VINS系統(tǒng)使用滑動窗口求解因子圖優(yōu)化，在i時刻的滑動窗口中的待優(yōu)化狀態(tài)量為：

χ=[xn，xn+1，…，xn+N，λm，λm+1，…，λm+M，Ll，Ll+1，…，Ll+L]T

(17)

(18)

即待優(yōu)化向量包括滑動窗口中的N個關(guān)鍵幀相機(jī)的狀態(tài)p，v，q，b以及視覺特征：M個點(diǎn)特征的逆深度與L個線特征的正交坐標(biāo)參數(shù)化表示.

由于系統(tǒng)采用滑動窗口的形式來限制優(yōu)化變量的數(shù)目，求解時待估計量均為滑動窗口內(nèi)的狀態(tài)變量.當(dāng)有新的一個關(guān)鍵幀(關(guān)鍵幀的選取策略沿用VINS-Mono中的策略[19])加入到滑動窗口時，需要去除滑動窗口中一個舊的關(guān)鍵幀及其相關(guān)的狀態(tài)量，以保證滑窗內(nèi)的狀態(tài)量數(shù)目穩(wěn)定.但被刪除的關(guān)鍵幀通過IMU預(yù)積分和觀測到的某些地圖點(diǎn)，會與滑動窗口內(nèi)的其他某些關(guān)鍵幀之間產(chǎn)生約束關(guān)系，如果直接把該關(guān)鍵幀從滑動窗口內(nèi)刪除，會直接丟失這些約束關(guān)系，從而降低滑動窗口內(nèi)狀態(tài)量的優(yōu)化結(jié)果.為了解決這個問題，需要在滑窗移動(增刪關(guān)鍵幀)時使用邊緣化(Marginalization)[31]進(jìn)行操作.

邊緣化后會得到一個先驗(yàn)殘差項(xiàng)，同時再加上其他的殘差項(xiàng)，如第1節(jié)所述，最后最小化所有測量殘差項(xiàng)之和，優(yōu)化求解所有狀態(tài)變量，整體目標(biāo)函數(shù)如下：

(19)

6 實(shí)驗(yàn)及結(jié)果分析

將OOL-VINS移植到了Intel i7、2.20GHz，CPU，8GB內(nèi)存ubuntu16.04操作系統(tǒng)上.在此硬件平臺上，進(jìn)行點(diǎn)和線特征的提取與匹配，初始化和定位精度實(shí)驗(yàn).

6.1 點(diǎn)線特征提取與匹配

點(diǎn)線特征的結(jié)合可以提高系統(tǒng)在低紋理環(huán)境下的魯棒性，以TUM[33]STRUCTURE VS.TEXTURE數(shù)據(jù)集為測試，選取freiburg3_structure_noteture_far中近鄰兩張圖片進(jìn)行低紋理環(huán)境的點(diǎn)線提取實(shí)驗(yàn)；以EuRoc MAV[34]V1_01_easy中近鄰兩張圖片進(jìn)行普通場景的點(diǎn)線特征提取實(shí)驗(yàn)；以TUM-VI[35]corridor1中近鄰兩張圖片進(jìn)行低紋理場景的點(diǎn)線特征提取實(shí)驗(yàn)，需要指出的是TUM-VI數(shù)據(jù)集相機(jī)模型為魚眼相機(jī)，故實(shí)驗(yàn)中圖片為矯正過后的圖片.實(shí)驗(yàn)結(jié)果如圖5所示，分別為EuRoc MAV數(shù)據(jù)集，TUM-VI走廊數(shù)據(jù)集；其中，圖5(a)、(b)為場景原始圖像；圖5(c)、(d)為點(diǎn)線特征提取與匹配結(jié)果圖.由圖5可知，在低紋理場景中點(diǎn)線特征相較于單獨(dú)的點(diǎn)特征與線特征匹配得都更多(具體成功匹配數(shù)目如表1所示)這對系統(tǒng)的前端跟蹤是十分有利且必要的.同時，點(diǎn)線特征提取與匹配程序使用雙線程獨(dú)立進(jìn)行，此時共耗時27ms；相比于ORB特征提取匹配24ms，線特征提取匹配26ms有一定的時間增加，但對于系統(tǒng)的實(shí)時性不會有影響.如表1所示.

圖5 ORB與線特征混合提取與匹配Fig.5 ORB and line feature mixing extracting and matching

表1 點(diǎn)線特征成功匹配數(shù)與平均耗時Table 1 Number of successful matching points and average time

6.2 系統(tǒng)初始化

本小節(jié)中，在上述硬件平臺上分別基于EuRoc和TUM-VI數(shù)據(jù)集進(jìn)行普通場景與低紋理場景的初始化實(shí)驗(yàn).

6.2.1 普通場景初始化

如第2小節(jié)所述，OOL-VINS系統(tǒng)的初始化采用點(diǎn)線視覺特征與IMU數(shù)據(jù)松耦合的設(shè)計，在初始化過程中能夠獲取更加豐富的視覺特征，使得系統(tǒng)更快的進(jìn)行初始化.在EuRoc數(shù)據(jù)集上對OOL-VINS、VINS-Mono、VINS-Fusion這3個系統(tǒng)進(jìn)行初始化實(shí)驗(yàn)對比，實(shí)驗(yàn)結(jié)果如圖6所示(以在V1_01_easy數(shù)據(jù)集上初始化為例)，其中，圖6(a)為V1_01_easy起始部分場景圖，圖6(b)為OOL-VINS初始化點(diǎn)線特征3D圖.

圖6 SLAM系統(tǒng)初始化Fig.6 SLAM system initialization

定量的初始化性能測試采用初始化所需要的圖像幀數(shù)、初始化消耗的時間以及第1幀初始化地圖的地圖點(diǎn)個數(shù)，實(shí)驗(yàn)結(jié)果如表2所示.表2結(jié)果表明OOL-VINS初始化所需幀數(shù)少于VINS-Mono、VINS-Fusion，同時初始化成功的視覺地圖點(diǎn)也更多，這為系統(tǒng)的運(yùn)行提供了一個更加精確的初始值.既實(shí)驗(yàn)表明OOL-VINS初始化更加快速穩(wěn)定.

6.2.2 低紋理場景初始化

同上一小節(jié)，在TUM-VI數(shù)據(jù)集上對OOL-VINS、VINS-Mono、VINS-Fusion這3個系統(tǒng)進(jìn)行初始化實(shí)驗(yàn)對比，實(shí)驗(yàn)結(jié)果如圖7所示(以在corridor1數(shù)據(jù)集上初始化為例)，其中，圖7(a)為V1_01_easy起始部分場景圖，圖7(b)為OOL-VINS初始化點(diǎn)線特征3D圖.

表2 SLAM系統(tǒng)初始化對比Table 2 SLAM system initialization comparison

圖7 SLAM系統(tǒng)初始化Fig.7 SLAM system initialization

定量描述結(jié)果如表3所示.由表3可知在低紋理場景中OOL-VINS初始化所需幀數(shù)少于VINS-Mono、VINS-Fusion，同時初始化成功的視覺地圖點(diǎn)也更多，為系統(tǒng)的運(yùn)行提供了一個更加精確的初始值.既實(shí)驗(yàn)表明OOL-VINS在普通場景與低紋理場景中初始化都更加快速穩(wěn)定.

表3 SLAM系統(tǒng)初始化對比Table 3 SLAM system initialization comparison

6.3 離線定位精度分析

本小節(jié)中，在上述平臺上分別基于EuRoc和TUM-VI數(shù)據(jù)集進(jìn)行普通場景與低紋理場景的定位精度分析實(shí)驗(yàn).

6.3.1 普通場景離線定位精度分析

采用EuRoc MAV數(shù)據(jù)集對OOL-VINS系統(tǒng)整體性能進(jìn)行實(shí)驗(yàn)分析，硬件配置同上.同時將OOL-VINS系統(tǒng)與開源系統(tǒng)PL-SLAM、VINS-Mono、VINS-Fusion、PL-VIO以及ORB-SLAM2進(jìn)行定位精度對比.

實(shí)驗(yàn)中將EuRoc MAV數(shù)據(jù)集名稱進(jìn)行簡寫：MH_05_difficult簡寫為MH01d，V1_01_difficult簡寫為V11d，其他依次類推.OOL-VINS 在EuRoc MAV數(shù)據(jù)集估計軌跡APE誤差熱力圖如圖8所示(以MH05d、V13d、V23d為例)，具體OOL-VINS與其他開源SLAM系統(tǒng)APE誤差值對比如表4所示，其中“*”表示跟蹤丟失.

圖8 EuRoc MAV各數(shù)據(jù)集軌跡APE誤差熱力圖Fig.8 AER error heat map of each trajectory of EuRoc MAV

表4中VINS-Mono采用閉環(huán)模式進(jìn)行軌跡保存，VINS-Fusion采用雙目+IMU閉環(huán)模式進(jìn)行軌跡保存；為保證配置參數(shù)相同只比較算法性能差異，ORB-SLAM2配置文件中“ORBextractor.nFeatures”設(shè)置為與OOL-VINS相同的800，同時注釋掉“Tracking”線程中的相關(guān)判斷語句.表中對絕對軌跡誤差量化到最大值、最小值、均方根4項(xiàng)進(jìn)行統(tǒng)計比較；從表4中可以看出OOL-VINS對于EuRoc MAV數(shù)據(jù)集均成功跟蹤，且定位精度優(yōu)于現(xiàn)有開源算法，在與VINS-Fusion、ORB-SLAM2比較時部分?jǐn)?shù)據(jù)集略遜于VINS-Fusion、ORB-SLAM2，但相差均在cm級別以內(nèi).表中PL-VIO對于數(shù)據(jù)集V12m一欄數(shù)值較大，實(shí)則是為PL-VIO在該數(shù)據(jù)集運(yùn)行時發(fā)生巨大軌跡漂移，但依然有軌跡數(shù)據(jù)保存.

在EuRoc MAV數(shù)據(jù)集上測試，跟蹤每幀平均耗時為0.0397s，即運(yùn)行幀率為25.2Hz，基本滿足實(shí)時性要求.以上實(shí)驗(yàn)結(jié)果表明OOL-VINS算法的定位精度與實(shí)時性均十分出色.

6.3.2 低紋理場景離線定位精度分析

采用TUM-VI[35]數(shù)據(jù)集對OOL-VINS系統(tǒng)整體性能進(jìn)行實(shí)驗(yàn)分析，硬件配置同上.同時將OOL-VINS系統(tǒng)與開源系統(tǒng)PL-SLAM、VINS-Mono、VINS-Fusion、ORB-SLAM2進(jìn)行實(shí)驗(yàn)對比.

因針對低紋理場景進(jìn)行實(shí)驗(yàn)，故只選取TUM-VI中corridor系列512×512分辨率數(shù)據(jù)集進(jìn)行測試；同時由于TUM-VI相機(jī)模型為魚眼相機(jī)，魚眼相機(jī)采集的圖像邊緣處畸變較大，矯正后依然有畸變這對低紋理場景提取特征點(diǎn)影響較大.

表4 OOL-VINS與開源算法絕對軌跡誤差對比Table 4 Comparison of absolute trajectory error between OOL-VINS and open source algorithms

以VINS-Fusion為例，如圖9所示，圖9(a)為未進(jìn)行掩膜mask處理的軌跡，其有著肉眼可見的漂移如圖中圓圈處，圖9(b)加掩膜后則沒有明顯漂移.又因?yàn)閂INS-Mono對魚眼相機(jī)做了掩膜處理，故為了在測試過程中保證前期圖像預(yù)處理相同，對VINS-Fusion、PL-SLAM以及ORB-SLAM2系統(tǒng)均添加了掩膜處理，處理效果如圖10所示.

圖9 VINS-Fusion加fisheye mask前后軌跡對比圖Fig.9 VINS-Fusion plus fisheye mask before and after thetrajectory comparison

圖10 TUM-VI數(shù)據(jù)集加fisheye mask前后對比圖Fig.10 Comparison of TUM-VI data set with fisheyemask before and after

TUM-VI數(shù)據(jù)集同樣給出了ground truth，但只給出了數(shù)據(jù)集開頭和結(jié)尾處的ground truth.此時同樣可以使用絕對軌跡誤差利用時間戳對齊對有g(shù)round truth部分進(jìn)行性能評價，OOL-VINS與開源SLAM系統(tǒng)APE誤差值對比如表5所示.

表5 OOL-VINS與開源算法絕對軌跡誤差對比Table 5 Comparison of absolute trajectory error between OOL-VINS and open source algorithms

其中，“*”表示跟蹤失敗.

表5中VINS-Mono采用閉環(huán)模式進(jìn)行軌跡保存，VINS-Fusion采用雙目+IMU閉環(huán)模式進(jìn)行軌跡保存.從表5中可以看出OOL-VINS對于TUM-VI走廊數(shù)據(jù)集均跟蹤成功，且定位精度均優(yōu)于現(xiàn)有開源算法，而ORB-SLAM2沒用融合IMU傳感器數(shù)據(jù).

OOL-VINS對TUM-VI走廊數(shù)據(jù)集的軌跡與誤差熱力圖Corridor5數(shù)據(jù)集為例，如圖11 Corridor5(a)、(b)所示，因ground truth只給出了起始與結(jié)尾部分，故絕對誤差熱力圖也只繪制出該部分.圖11 corridor5-all(a)、(b)為各數(shù)據(jù)集在corridor5上的對比.在TUM-VI corridor數(shù)據(jù)集上測試，跟蹤每幀平均耗時為0.0382s，即運(yùn)行幀率為26.18Hz，基本滿足實(shí)時性要求.實(shí)驗(yàn)結(jié)果表明OOL-VINS算法在低紋理場景表現(xiàn)更加出色，定位精度均優(yōu)于對比實(shí)驗(yàn)的開源SLAM系統(tǒng).

7 結(jié)束語

本文針對一些較低紋理的未知環(huán)境，提出一種基于優(yōu)化的點(diǎn)線特征雙目慣性視覺里程計OOL-VINS，其中點(diǎn)特征采用ORB特征點(diǎn).該算法結(jié)合兩種視覺特征融合IMU對傳感器信息進(jìn)行充分利用；設(shè)計了基于雙目的慣性里程計的初始化以及更加魯棒的獲取路標(biāo)點(diǎn)的3D信息；后端采用滑動窗口進(jìn)行優(yōu)化.在公共數(shù)據(jù)集上實(shí)驗(yàn)表明，OOL-VINS算法具有較高的定位精度與魯棒性，點(diǎn)線特征的提取和匹配在低紋理場景與普通場景均具有很好的表現(xiàn)，與現(xiàn)有的部分開源系統(tǒng)該算法擁有更小的絕對軌跡誤差.

圖11 OOL-VINS再TUM-VI corridor5數(shù)據(jù)集上的軌跡圖、絕對誤差熱力圖以及各開源算法在corridor5數(shù)據(jù)集上的對比Fig.11 OOL-VINS and TUM-VI corridor5 trajectory map，absolute error heat map and comparisonof various open source algorithms on the corridor5 data set

未來工作計劃在此系統(tǒng)基礎(chǔ)上實(shí)現(xiàn)稠密地圖重建.