閆莉萍, 李程偉, 徐柏凱, 夏元清, 肖 波

(1. 北京理工大學(xué)自動化學(xué)院, 北京 100081; 2. 北京郵電大學(xué)人工智能學(xué)院, 北京 100876)

0 引 言

同時定位與建圖(simultaneous localization and mapping,SLAM)技術(shù)發(fā)展到如今已有幾十年的歷史,目前以激光雷達作為主傳感器的SLAM技術(shù)比較穩(wěn)定、可靠,仍然是主流的技術(shù)方案。最近幾年,隨著計算機視覺技術(shù)的快速發(fā)展,SLAM技術(shù)越來越多地被應(yīng)用于家用機器人、無人機、無人駕駛、虛擬現(xiàn)實等領(lǐng)域,基于視覺的SLAM(visual SLAM,VSLAM)技術(shù)逐漸開始嶄露頭角[1]。不過,相比于激光SLAM,VSLAM存在著魯棒性不足、精度較差等問題,因此將輪速計、慣性測量單元(inertial measurement unit,IMU)、全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)與衛(wèi)星圖像引入到VSLAM系統(tǒng)成為了一個研究熱點[2-3]。在此過程中,IMU以其低廉的價格、較輕的重量以及與相機互補的特性得到了廣泛關(guān)注[4-5]。

相較于單一的視覺里程計(visual odometry,VO),視覺慣性里程計(visual-inertial odometry,VIO)的精度與魯棒性取得了顯著的提升。然而在一些低紋理的人造場景中,傳統(tǒng)的僅采用點特征作為視覺約束的VIO系統(tǒng)仍然不能滿足要求。與此同時,低紋理場景往往是人造場景,這意味著該類場景一般富含類似家具、墻壁、電器等結(jié)構(gòu)化物體。結(jié)構(gòu)化物體一般包含許多線段、平面等高級幾何特征,這些高級幾何特征能夠提供更強的幾何約束,且對光照變化不敏感。因此,近年來,學(xué)者們提出了一系列組合點特征與線面等高級幾何特征的SLAM系統(tǒng)。比如,Zuo等[6]提出了一個基于點線特征、可以在各種場景下魯棒運行的雙目視覺SLAM系統(tǒng),He等[7]提出了一個以點線特征為路標的緊耦合單目VIO,趙良玉等[8]提出了一種基于點線特征并融合IMU的雙目視覺的慣性SLAM系統(tǒng)。面特征也在近年獲得了廣泛的應(yīng)用[9-11]。高級幾何特征的引入有效地提升了SLAM系統(tǒng)的魯棒性和精度,然而相對于只擁有三自由度的點特征,線面特征自由度更高,更難被初始化。此外,線面特征還存在不穩(wěn)定、容易被遮擋、提取時間長等問題,且與點特征相同,線面特征只能提供局部的約束,這將造成累計誤差的存在。

與此同時,低紋理人造場景,如辦公室、工廠及地下車庫等具有比較規(guī)則的結(jié)構(gòu)特征,滿足曼哈頓世界由3個相互正交的主導(dǎo)方向組成的假設(shè),由這3個相互正交的主導(dǎo)方向組成的結(jié)構(gòu)特征可作為一個全局路標,以提供全局約束。與此同時,與每個主導(dǎo)方向平行的線段特征在圖像平面中交于消失點(vanishing point,VP),因此可以通過對線段特征采用VP提取算法對結(jié)構(gòu)特征進行提取。結(jié)構(gòu)特征的引入有效地消除了旋轉(zhuǎn)累計誤差,該方法已經(jīng)被研究者應(yīng)用在單目[12]、雙目[13]及RGBD(red-green-blue-depth)相機[14]中。

本文提出了一種高效的VP提取方法,在此基礎(chǔ)上將其引入到一個基于優(yōu)化的緊耦合單目VIO中。在提升了VIO系統(tǒng)的精度與魯棒性的同時保持了系統(tǒng)的實時性。本文的主要貢獻如下:(1) 提出了一種快速且準確的VP提取算法;(2) 提出了一種初始化全局結(jié)構(gòu)特征的方法;(3) 提出了一種點特征、線段特征和結(jié)構(gòu)特征緊耦合的單目VIO。

1 相關(guān)工作

1.1 視覺慣性SLAM

視覺和IMU定位方案存在一定的互補性:IMU適合計算短時間、快速的運動;而視覺適合計算長時間、慢速的運動。因此,可以利用視覺信息來估計IMU的零偏;同時利用IMU信息為相機提供快速運動時的初始定位。VIO可以被劃分為松耦合和緊耦合兩類。松耦合[15-16]將IMU定位與視覺的位姿直接進行融合。緊耦合[17-18]融合過程則會影響視覺和IMU的參數(shù)(如IMU的零偏和視覺的尺度)。目前更常用的方案為緊耦合,因為緊耦合可以一次性建模所有的運動和測量信息,更容易達到最優(yōu)的狀態(tài)估計。視覺融合IMU問題可以分為基于濾波[19-20]和基于優(yōu)化[21-22]兩大類。在計算資源受限、待估量比較簡單的情況下,以擴展卡爾曼濾波為代表的濾波方法更加有效,在計算資源充足的情況下,基于優(yōu)化的方法精度更高。

視覺慣性SLAM通常通過優(yōu)化視覺重投影殘差與IMU測量殘差組成的代價函數(shù)來獲得狀態(tài)的最優(yōu)估計。然而,由于IMU的采樣頻率高,無法將數(shù)據(jù)都放到狀態(tài)變量中。為了簡化計算,將兩幀圖像間的IMU測量值進行積分,得到一個兩幀圖像之間的IMU約束。標準的IMU積分方法與IMU初始狀態(tài)緊密相關(guān),當初始狀態(tài)發(fā)生改變時,需要重新對所有的IMU測量值進行積分。針對該問題,提出了預(yù)積分理論[23]。

1.2 提取VP

VP提取算法主要可分為4類,第1類為窮舉法[24-25],此類方法基于VP的正交性窮舉出所有的候補VP,并通過線段特征構(gòu)建極坐標網(wǎng)格對候補VP進行查詢驗證,最終得到最優(yōu)的VP估計。第2類為EM法[26],即在E-step中根據(jù)當前估計的消失點,找到最佳線段分類,在M-step中基于分類的線段,對消失點重新進行估計,然后反復(fù)迭代執(zhí)行 E-step和 M-step,直至收斂。第3類則基于隨機抽樣一致(random sample consensus,RANSAC)算法。該類方法通常首先定義一個最小解集(minimum sample size,MSS),然后應(yīng)用 RANSAC迭代生成候補消失點,直至從中選擇出一個最好的解。目前,研究者已提出了1-line[27]、3-line[28]、4-line[29]等多種 MSS 模型。這些模型很好地滿足了消失點的正交約束,同時在效率和精度之間取得很好的平衡。第4類則是基于優(yōu)化的方法[30],該方法旨在尋求VP估計這一數(shù)學(xué)問題的全局最優(yōu)解。

1.3 結(jié)構(gòu)SLAM

人造環(huán)境中包含著豐富的結(jié)構(gòu)信息,例如平行性、正交性和共面性。這些結(jié)構(gòu)信息能夠為SLAM系統(tǒng)提供有效的幾何約束,使之獲得更加準確和魯棒的位姿估計。在此基礎(chǔ)上,一系列的結(jié)構(gòu)SLAM系統(tǒng)被提出?；跀U展卡爾曼濾波器(extended Kalman filter, EKF) 框架,Camposeco等[12]將VP融入到了一個緊密耦合的VIO以提高定位精度,StructSLAM[31]則引入了與曼哈頓世界主導(dǎo)方向平行的結(jié)構(gòu)線段特征,Zou等[32]采用了由多個曼哈頓世界模型組成的亞特蘭大世界模型,使VIO系統(tǒng)對于復(fù)雜的人造環(huán)境更加魯棒?；邳c線(point and line,PL)-SLAM[33],文獻[30]根據(jù)VP所蘊含的正交性、平行性和共面性[34]等幾何約束,提出了一種后端優(yōu)化策略,有效地減少了累計誤差的存在,提高了PL-SLAM的精度。Kim等[35]則將旋轉(zhuǎn)與平移解耦,首先于RGBD圖像上提取結(jié)構(gòu)特征,然后通過追蹤結(jié)構(gòu)特征獲得相機的旋轉(zhuǎn)量,最后根據(jù)追蹤到的點特征及旋轉(zhuǎn)量獲得相機的平移。

2 框架概述

(1)

圖1展示了本文所提出的VIO系統(tǒng)的基本框架。它以圖像數(shù)據(jù)及IMU數(shù)據(jù)為輸入,輸出每幀圖像對應(yīng)的位姿估計。其中前端模塊負責對點、線與結(jié)構(gòu)特征進行提取和匹配,同時對IMU測量數(shù)據(jù)進行預(yù)積分。后端則通過非線性優(yōu)化的方法對視覺特征重投影誤差與IMU預(yù)積分誤差組成的代價函數(shù)進行優(yōu)化。接下來,將詳細說明系統(tǒng)的每個模塊。

圖1 基于結(jié)構(gòu)特征的VIO框架Fig.1 VIO framework based on structural features

3 前端

前端負責處理來自傳感器的數(shù)據(jù),同時決定當前圖像幀是否為關(guān)鍵幀。在此過程中,IMU測量值被用來對當前幀的位姿進行一個初始估計,同時通過預(yù)積分操作獲得當前幀與上一幀的預(yù)積分約束,具體細節(jié)將于第3.1節(jié)中進行闡述。針對圖像序列,當新的一幀圖像到來時,系統(tǒng)便開始并行地提取點特征和線段特征。其中點特征由KLT (Kanade Lucas Tomasi)稀疏光流法[36]進行跟蹤和匹配, 線段特征由邊緣繪制直線(edge drawing lines, EDLines)[37]算法進行提取。EDLines算法是一種高效的線段提取算法,其速度是傳統(tǒng)線段特征提取(line segment detector, LSD)算法的5～10倍,線段特征的匹配則通過線段帶描述子(line band descriptor, LBD)算子+幾何驗證的方法。LBD算子是一個256維的向量,它能夠描述線段特征周圍環(huán)境的信息。兩個相匹配的線段應(yīng)該符合如下3個準則:(1) 兩條線段特征的LBD描述子之間的距離小于某個閾值;(2) 兩條線段特征中點之間的距離應(yīng)小于某個給定值d;(3) 兩條線段特征的角度差應(yīng)小于閾值θ。

其中,后兩條準則是基于線段的幾何特性給定的,圖2展示了由這兩條策略剔除的誤匹配線段。其中相同顏色的線段代表一對匹配的線。

圖2 誤匹配線段Fig.2 Mismatched line segment

基于線段特征,對每幀圖像的結(jié)構(gòu)特征進行提取和匹配,其具體細節(jié)將于第4.2節(jié)中進行闡述。關(guān)鍵幀則根據(jù)以下3個標準決定[38]:(1) 當前幀和次新幀之間的視差大于某閾值;(2) 當前幀觀測到了許多新的路標;(3) 當前幀沒有追蹤到足夠的路標。

這3條準則保證了VIO系統(tǒng)能夠魯棒、實時地運行。

3.1 IMU預(yù)積分

IMU包含一個3軸加速度計和一個3軸陀螺儀,可用于測量其本身相對于慣性坐標系的加速度和角速度,其測量值受零偏和噪聲影響,具體公式如下:

(2)

(3)

(4)

式中:?表示四元數(shù)的乘法操作。

(5)

為避免在初始時刻i的IMU狀態(tài)發(fā)生變化時需對測量值重新積分,可將初始狀態(tài)從積分中分解出來,即:

(6)

(7)

由于IMU測量值是離散且包含噪聲的,本文采用中值積分的方法對k和k+1之間的IMU測量值進行積分,預(yù)積分測量如下:

(8)

(9)

3.2 結(jié)構(gòu)特征的提取與初始化

結(jié)構(gòu)特征為3個相互正交的主導(dǎo)方向組成且行列式為1的正交基,3個主導(dǎo)方向可通過對該幀圖像上的線段特征進行消失點估計獲得。本文采用了一種高效魯棒的算法對結(jié)構(gòu)特征進行提取與匹配,并在此基礎(chǔ)上對全局結(jié)構(gòu)特征進行了初始化。

3.2.1 結(jié)構(gòu)特征提取算法

VIO系統(tǒng)初始化成功后,首先基于重力向量,采用窮舉+隨機采樣一致或最小二乘法對圖像的結(jié)構(gòu)特征進行提取,主要分為如下3個步驟。

(1) 構(gòu)建極坐標柵格

極坐標柵格基于等效球構(gòu)建,如圖3所示。

圖3 等效球Fig.3 Equivalent sphere

等效球是一個球心位于相機焦點的單位球,其X軸和Y軸與圖像平面的x軸和y軸平行,Z軸則從焦點指向像主點。給定像主點(x0,y0)T和焦距f,圖像平面上任意一個像素點(x,y)T都能夠通過如下公式獲得其等效球坐標系下的坐標:

(10)

其經(jīng)緯度坐標表示為

(11)

式中:φ∈[0,π/2],λ∈[0,2π]。

設(shè)定極坐標柵格G的大小為90×360,精度為1°。對i=1,2,3,…,90,j=1,2,3,…,360,有G(i,j)的初始值為0。

選取圖像上任意一對線段特征l1,l2求其交點p,基于式(10)、式(11)求其對應(yīng)的經(jīng)緯度坐標(φ,λ),進而將其對應(yīng)的柵格進行更新:

G(φdeg,λdeg)=G(φdeg,λdeg)+‖l1‖×‖l2‖×sin(2θ)

(12)

式中:‖l‖為對應(yīng)線段長度,θ為兩個線段的夾角,最后對得到的極坐標柵格應(yīng)用3×3的高斯濾波器。極坐標柵格構(gòu)建過程是記錄每對線段對每個單元響應(yīng)的過程。

(2) VP假設(shè)生成與驗證

如圖4(a)所示,將初始化得到的重力向量投影到圖像平面上,獲得第一個VP的初始估計VPinit,然后計算所有線段特征與該VP向量的夾角,夾角小于4°的線段屬于該VP對應(yīng)的結(jié)構(gòu)線段聚類L1。

若采用窮舉+隨機采樣一致的VP提取算法,假定圖像中有N個線段,其中對應(yīng)于滅點VP1,VP2,VP3的線段數(shù)量分別為n1,n2,n3,則對于任意兩條線段,其對應(yīng)于同一個滅點的概率P為

(13)

假設(shè)所有線段中異常線段的數(shù)量約占1/2,那么則有:

(14)

(15)

設(shè)置信系數(shù)η為0.999 9,則在該置信系數(shù)下,需要迭代的最少次數(shù)為

nit=ln(1-η)/ln(1-P)=105

(16)

因此,在50%的線段異常比例以及0.999 9的置信系數(shù)下,至少需要105次迭代才能得到至少一個正確的VP1。

若L1包含的線段特征大于4時,還可通過構(gòu)建最小二乘問題來對VPinit進行優(yōu)化,獲得第一個VP的假設(shè)VP1:

(17)

式中:S∈R3×n,n為L1包含的線段特征的個數(shù);K為相機內(nèi)參;ljstart,ljend分別對應(yīng)第j條線段起點和終點的歸一化坐標。然后,如圖4(b)所示,基于消失點的正交性,給定第一個VP,即VP1=(X1,Y1,Z1)T后,第二個VP(VP2)必然位于其超圓上。因此,以精度1°對該超圓進行均勻采樣獲得VP2的360個假設(shè)。 其中第i個假設(shè)的經(jīng)度λ=i×2π/360,緯度φ則通過如下兩個約束求解:

圖4 VP提取過程Fig.4 VP extraction process

(18)

X1×X2+Y1×Y2+Z1×Z2=0

(19)

最后,如圖4(c)所示,VP3由VP1和VP2叉乘可得。

將步驟(2)中360個消失點假設(shè)VP1,VP2,VP3用極坐標表示(λ1,φ1),(λ2,φ2),(λ3,φ3),然后基于極坐標柵格查詢每個VP假設(shè)的線段響應(yīng):

T=G(φ1,λ1)+G(φ2,λ2)+G(φ3,λ3)

(20)

線段響應(yīng)最大的VP假設(shè)為最優(yōu)估計,得到當前幀消失點估計后,對當前幀的線段特征進行分類。得到與VP1,VP2,VP3向量夾角小于4°的結(jié)構(gòu)線段特征集合L1,L2,L3。

(3) 基于線段匹配關(guān)系計算VP

基于下一幀與當前幀線段匹配關(guān)系,獲得與下一幀匹配成功的結(jié)構(gòu)線段集合,并按照包含結(jié)構(gòu)線段特征的數(shù)量對其由大到小進行排序,得到L1p,L2p,L3p。若L2p包含的結(jié)構(gòu)線數(shù)量小于6,則繼續(xù)采用式(1)、式(2)所述算法對下一幀的VP進行提取。否則,便可以基于匹配到的結(jié)構(gòu)線段集合對下一幀的結(jié)構(gòu)特征進行提取。即基于式(13)得到結(jié)構(gòu)線段集合L1p,L2p所對應(yīng)的消失點估計值VP1init,VP2init,然后將兩者叉乘獲得VP3init。最后,對VP1init,VP2init,VP3init進行施密特正交化,得到該幀的VP向量VP1,VP2,VP3,并在此基礎(chǔ)上獲得該幀的結(jié)構(gòu)線段集合,用于對后續(xù)圖像幀進行處理。

基于當前幀與次新幀結(jié)構(gòu)線段的匹配數(shù)量分別采取窮舉+RANSAC或最小二乘求解的方式對結(jié)構(gòu)特征進行提取,有效地保證了在引入結(jié)構(gòu)特征這一前提下VIO系統(tǒng)的實時性。圖5(a)和圖5(b)分別展示了兩種方式提取的結(jié)構(gòu)特征對應(yīng)的結(jié)構(gòu)線。

圖5 不同算法提取的結(jié)構(gòu)特征對應(yīng)的結(jié)構(gòu)線Fig.5 Structural lines corresponding to structural features extracted by different algorithms

3.2.2 初始化全局結(jié)構(gòu)特征

VIO系統(tǒng)初始化成功后,將滑窗內(nèi)的每幀圖像對應(yīng)的結(jié)構(gòu)特征MFi轉(zhuǎn)化到世界坐標系下。

(21)

(22)

距離小于0.1的結(jié)構(gòu)特征為內(nèi)點。若內(nèi)點比例大于0.6,則對所有內(nèi)點進行slerp插值得到全局結(jié)構(gòu)特征MFW,否則,認定初始化失敗,滑動窗口移動,重新按照上述流程對全局結(jié)構(gòu)特征進行初始化。

4 后端

后端負責對前端獲得的初始狀態(tài)進行優(yōu)化,在后端優(yōu)化過程中,首先通過三角化獲得點線特征于3D空間的初始值,然后對包含點線特征的重投影誤差、IMU預(yù)積分誤差、結(jié)構(gòu)特征所重投影誤差的代價函數(shù)進行優(yōu)化,進而獲得狀態(tài)向量的估計。優(yōu)化結(jié)束后,基于優(yōu)化結(jié)果對異點進行剔除,其中包含逆深度為負的點特征,重投影誤差過大的線特征和結(jié)構(gòu)特征。

4.1 緊耦合非線性優(yōu)化

當全局結(jié)構(gòu)特征初始化成功后,時刻i滑動窗口的狀態(tài)向量定義如下:

(23)

式中:xi包含滑窗第i幀圖像對應(yīng)的IMU平移、旋轉(zhuǎn)、速度、加速度計零偏與角速度計零偏;λk為第k個路標點的逆深度;Ol為第l個路標線的正交表示;MFW為結(jié)構(gòu)特征于世界坐標系下的矩陣表示。下標n,m,o則分別是IMU、路標點、路標線的起始索引;N為滑窗內(nèi)關(guān)鍵幀數(shù)量;M,O分別為滑窗內(nèi)路標點和路標線的數(shù)量。

通過優(yōu)化如下代價函數(shù)對上述狀態(tài)進行優(yōu)化:

(24)

本文采用LM(levenberg-marquardt)算法對該優(yōu)化問題進行求解,即通過如下公式迭代更新初始值來獲得最佳估計:

(25)

式中:⊕操作符用于表示更新操作。

4.2 IMU測量誤差

(26)

誤差對應(yīng)的雅克比矩陣[34]為

(27)

(28)

(29)

(30)

4.3 點特征測量誤差

(31)

式中:

(32)

(33)

(34)

(35)

4.4 線特征測量誤差

給定一個普呂克坐標描述的3D線段L=[n,d]T,其中n∈R3為線段與世界坐標系原點組成平面的法向量,d∈R3為線段的方向向量,其投影到相機平面的投影方程為

(36)

(37)

采用普呂克坐標描述的線段特征具有6個自由度,是一種過參數(shù)的表述形式,無法直接在后端優(yōu)化過程中使用。因此,本文在后端優(yōu)化中采用了具有4個自由度的正交形式描述線段特征[39]。普呂克坐標與正交形式的轉(zhuǎn)換公式如下:

(38)

式中:U為一旋轉(zhuǎn)矩陣,具有3個自由度,W則只有一個自由度。3D線段L在相機i歸一化平面上的重投影誤差定義為

(39)

d(s,l)為投影線段兩個端點與觀測線段的距離:

(40)

誤差對應(yīng)的雅克比矩陣為

(41)

(42)

式中:ui為U的第i列,w1=cosφ,w2=sinφ,K為內(nèi)參矩陣,同時有:

(43)

(44)

4.5 結(jié)構(gòu)特征測量誤差

由于結(jié)構(gòu)特征是一個由3個相互正交的主導(dǎo)方向組成且行列式為1的正交基,因此它可以用來表示一個旋轉(zhuǎn)量。當全局結(jié)構(gòu)特征初始化成功后,通過引入絕對旋轉(zhuǎn)殘差項,可有效減少旋轉(zhuǎn)累計誤差。絕對旋轉(zhuǎn)殘差定義如下:

(45)

其中MFci為結(jié)構(gòu)特征于第i幀圖像的觀測。誤差對應(yīng)的雅克比矩陣為

(46)

對于四元數(shù)q=[qw,qx,qyqz]T有:

(47)

(48)

5 實驗結(jié)果

在公開數(shù)據(jù)集EuRoc[40]上對本文所提系統(tǒng)的精度和實時性進行了測試。該數(shù)據(jù)集由無人機采集,記錄了傳感器在一個工廠和兩個不同房間內(nèi)采集到的數(shù)據(jù),其中包含了雙目圖像、IMU測量,以及通過光學(xué)運動捕捉系統(tǒng)獲得的真實值。本文采用了左眼相機拍攝的圖像和IMU測量,所有的實驗都運行在一臺處理器為i5-9400(2.90 GHz)、內(nèi)存為8GB、系統(tǒng)為ROS Kinetic的電腦上。

5.1 精度評估

由于管道、機械、家具等物體的存在,EuRoc數(shù)據(jù)集擁有豐富的結(jié)構(gòu)線段,基于這些結(jié)構(gòu)線段特征提取的結(jié)構(gòu)特征,可有效減少旋轉(zhuǎn)的累積誤差。圖5、圖6分別展示了在工廠和房間內(nèi)提取的結(jié)構(gòu)特征。

圖6 V1_01_easy序列某幀圖像提取的結(jié)構(gòu)特征Fig.6 Sequence V1_01_easy structural features extracted from a frame image

本節(jié)將本文所提快速曼哈頓框架VIO(fast Manhattan frame VIO,FMF-VIO)與PL-VIO[5]和沒有回環(huán)檢測的單目視覺慣性狀態(tài)估計器(monocular visual-inertial system, VINS-Mono)[38]、開放式視覺慣性系統(tǒng)(open visual-inertial system, OpenVINS)[41]進行了對比;同時為了證明本文所提VP提取算法的有效性,實驗還和采用文獻[24]提出的VP檢測算法的VIO系統(tǒng)的曼哈頓框架(Manhattan frame, MF)VIO進行了對比。其中PL-VIO和VINS-Mono均采用作者提供的默認參數(shù)。實驗結(jié)果如圖7、圖8和表1所示。

圖7 MH_02_easy數(shù)據(jù)集上PL-VIO與FMF-VIO的誤差Fig.7 Error between MF-VIO and FMF-VIO on dataset MH_02_easy

圖8 MH_04_difficult數(shù)據(jù)集上MF-VIO與FMF-VIO的誤差Fig.8 Error between MF-VIO and FMF-VIO on dataset MH_04_difficult

表1 平移和旋轉(zhuǎn)的均方根誤差Table 1 Root mean square error of piston and rotation

表1列出了EuRoc數(shù)據(jù)集下各種系統(tǒng)的平移和旋轉(zhuǎn)的均方根誤差,并加粗和標記了每個序列效果最好的兩個結(jié)果。由于OpenVINS在部分數(shù)據(jù)集下會出現(xiàn)跟蹤失敗的現(xiàn)象,故在表1中用斜杠表示?？梢钥闯?與不包含線段特征與結(jié)構(gòu)特征的OpenVINS[41]相比,本文提出的FMF-VIO系統(tǒng)在精度相近的情況下具有更好的魯棒性。與包含線段特征但不包含結(jié)構(gòu)特征的PL-VIO相比,FMF-VIO在大多數(shù)序列上的旋轉(zhuǎn)精度和平移精度均有所提升,只有在MH_01_easy數(shù)據(jù)集上的平移精度有所下降,這是由記錄該序列時無人機存在長時間的緩慢移動進而造成結(jié)構(gòu)特征測量無法有效利用造成的。與同樣利用了結(jié)構(gòu)特征的MF-VIO系統(tǒng)相比,本文提出的FMF-VIO系統(tǒng)與之達到了相似的精度,但顯著提高了系統(tǒng)的實時性(關(guān)于實時性的具體分析見下一小節(jié))。

圖7直觀展示了PL-VIO和FMF-VIO兩種算法在MH_02_easy序列下旋轉(zhuǎn)與平移的誤差情況,其中紅色曲線代表FMF-VIO算法,藍色曲線代表PL-VIO算法。從圖7可以看出,本文提出的FMF-VIO算法具有更小的平移和旋轉(zhuǎn)誤差。這與表1得出的結(jié)論一致。

圖8展示了MF-VIO和FMF-VIO在MH_04_difficult數(shù)據(jù)集下的旋轉(zhuǎn)誤差和平移誤差,MF-VIO和FMF-VIO分別采用了不同結(jié)構(gòu)特征提取算法。其中紅色和藍色曲線分別表示FMF-VIO和MF-VIO算法的誤差情況。從圖8可以看出,本文提出的FMF-VIO系統(tǒng)得到的旋轉(zhuǎn)誤差和平移誤差與MF-VIO得到的誤差相當,這也與表1中的數(shù)據(jù)一致。

綜上所述,本節(jié)的實驗結(jié)果表明,本文提出的FMF-VIO的精度明顯優(yōu)于PL-VIO,與OpenVINS和MF-VIO相當,且比OpenVINS具有更好的魯棒性。

5.2 實時性評估

SLAM是一種對實時性要求很高的系統(tǒng),結(jié)構(gòu)特征的引入在提升系統(tǒng)旋轉(zhuǎn)精度的同時,也對系統(tǒng)的實時性產(chǎn)生了影響。本節(jié)將在EuRoc數(shù)據(jù)集上對本文提出的基于重力向量和最小二乘法的曼哈頓框架下的VP提取算法(VP extraction algorithm based on gravity vector and least squares in the Manhattan frame,G2MF)與僅基于重力向量的曼哈頓框架下的VP提取算法(VP extraction algorithm based on gravity vector in the Manhattan frame,GMF)和文獻[24]提出的基于曼哈頓框架的VP提取算法(VP extraction algorithm based on Manhattan frame,MF)的實時性進行了對比分析。實驗結(jié)果如表2所示。表2列出了本文算法和對比算法在每一個序列中對每幀圖像提取結(jié)構(gòu)特征的平均耗時。

表2 VP提取算法的實時性展示Table 2 Real time display of VP extraction algorithm ms

由表2可以看出,本文提出的算法G2MF處理單幀圖像的平均耗時要遠遠小于文獻[24]所提MF算法。與GMF算法相比,本文所提出的算法實時性也有所提升。

本文還在 MH_05_difficult數(shù)據(jù)集上評估了運行PL-VIO與FMF-VIO的平均執(zhí)行時間,該數(shù)據(jù)集是一個典型的人造場景的圖像序列,具有豐富的結(jié)構(gòu)線段且相機運動較為劇烈。表3展示了兩個系統(tǒng)在不同單元的執(zhí)行時間。由表3可以看出,通過將LSD算法替換為EDLines,同時采用更為魯棒、高效的結(jié)構(gòu)特征提取算法,FMF-VIO在引入結(jié)構(gòu)特征的前提下,實時性依然得到了有效的提升。在Euroc數(shù)據(jù)集上的實時運行結(jié)果顯示,FMF-VIO的速度是PL-VIO的兩倍。

表3 PL-VIO 與FMF-VIO的不同單元每幀平均執(zhí)行時間Table 3 Average execution time per frame of PL-VIO and FMF-VIO different units ms

綜上所述,本節(jié)的實驗結(jié)果表明,本文提出的FMF-VIO系統(tǒng)在實時性方面,均明顯優(yōu)于PL-VIO和MF-VIO。

綜合第5.1節(jié)和第5.2節(jié)兩部分的實驗結(jié)果可知,本文提出的FMF-VIO方法是有效的。

6 結(jié)束語

為了提升VIO的精度,本文于線段中提取結(jié)構(gòu)特征,并基于結(jié)構(gòu)特征的正交特性將其引入到VIO中,有效地提升了VIO的精度。與此同時,為了保證VIO系統(tǒng)的實時性,本文還將線段提取算法由LSD替換為EDLines,并基于重力向量與結(jié)構(gòu)線段的相鄰幀匹配關(guān)系提出了一種魯棒且高效的結(jié)構(gòu)特征提取算法。在EuRoc數(shù)據(jù)集上對本文提出的VIO進行了測試,結(jié)果表明本文提出的系統(tǒng)在保持高精度的同時,可以實時地運行。

在未來,將嘗試通過神經(jīng)網(wǎng)絡(luò)引入語義特征,進一步提高SLAM的魯棒性和潛在應(yīng)用價值。