一種基于結(jié)構(gòu)特征的單目視覺慣性里程計(jì)

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

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

0 引 言

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

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

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

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

1 相關(guān)工作

1.1 視覺慣性SLAM

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

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

1.2 提取VP

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

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

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

2 框架概述

(1)

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

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

3 前端

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

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

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

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

這3條準(zhǔn)則保證了VIO系統(tǒng)能夠魯棒、實(shí)時(shí)地運(yùn)行。

3.1 IMU預(yù)積分

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

(2)

(3)

(4)

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

(5)

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

(6)

(7)

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

(8)

(9)

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

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

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

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

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

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

圖3 等效球Fig.3 Equivalent sphere

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

(10)

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

(11)

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

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

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

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

(12)

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

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

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

若采用窮舉+隨機(jī)采樣一致的VP提取算法,假定圖像中有N個(gè)線段,其中對(duì)應(yīng)于滅點(diǎn)VP1,VP2,VP3的線段數(shù)量分別為n1,n2,n3,則對(duì)于任意兩條線段,其對(duì)應(yīng)于同一個(gè)滅點(diǎn)的概率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次迭代才能得到至少一個(gè)正確的VP1。

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

(17)

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

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

(18)

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

(19)

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

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

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

(20)

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

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

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

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

圖5 不同算法提取的結(jié)構(gòu)特征對(duì)應(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)的每幀圖像對(duì)應(yīng)的結(jié)構(gòu)特征MFi轉(zhuǎn)化到世界坐標(biāo)系下。

(21)

(22)

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

4 后端

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

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

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

(23)

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

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

(24)

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

(25)

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

4.2 IMU測(cè)量誤差

(26)

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

(27)

(28)

(29)

(30)

4.3 點(diǎn)特征測(cè)量誤差

(31)

式中:

(32)

(33)

(34)

(35)

4.4 線特征測(cè)量誤差

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

(36)

(37)

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

(38)

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

(39)

d(s,l)為投影線段兩個(gè)端點(diǎn)與觀測(cè)線段的距離:

(40)

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

(41)

(42)

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

(43)

(44)

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

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

(45)

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

(46)

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

(47)

(48)

5 實(shí)驗(yàn)結(jié)果

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

5.1 精度評(píng)估

由于管道、機(jī)械、家具等物體的存在,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)檢測(cè)的單目視覺慣性狀態(tài)估計(jì)器(monocular visual-inertial system, VINS-Mono)[38]、開放式視覺慣性系統(tǒng)(open visual-inertial system, OpenVINS)[41]進(jìn)行了對(duì)比;同時(shí)為了證明本文所提VP提取算法的有效性,實(shí)驗(yàn)還和采用文獻(xiàn)[24]提出的VP檢測(cè)算法的VIO系統(tǒng)的曼哈頓框架(Manhattan frame, MF)VIO進(jìn)行了對(duì)比。其中PL-VIO和VINS-Mono均采用作者提供的默認(rèn)參數(shù)。實(shí)驗(yàn)結(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)的均方根誤差,并加粗和標(biāo)記了每個(gè)序列效果最好的兩個(gè)結(jié)果。由于OpenVINS在部分?jǐn)?shù)據(jù)集下會(huì)出現(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ù)集上的平移精度有所下降,這是由記錄該序列時(shí)無人機(jī)存在長(zhǎng)時(shí)間的緩慢移動(dòng)進(jìn)而造成結(jié)構(gòu)特征測(cè)量無法有效利用造成的。與同樣利用了結(jié)構(gòu)特征的MF-VIO系統(tǒng)相比,本文提出的FMF-VIO系統(tǒng)與之達(dá)到了相似的精度,但顯著提高了系統(tǒng)的實(shí)時(shí)性(關(guān)于實(shí)時(shí)性的具體分析見下一小節(jié))。

圖7直觀展示了PL-VIO和FMF-VIO兩種算法在MH_02_easy序列下旋轉(zhuǎn)與平移的誤差情況,其中紅色曲線代表FMF-VIO算法,藍(lán)色曲線代表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)特征提取算法。其中紅色和藍(lán)色曲線分別表示FMF-VIO和MF-VIO算法的誤差情況。從圖8可以看出,本文提出的FMF-VIO系統(tǒng)得到的旋轉(zhuǎn)誤差和平移誤差與MF-VIO得到的誤差相當(dāng),這也與表1中的數(shù)據(jù)一致。

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

5.2 實(shí)時(shí)性評(píng)估

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

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

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

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

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

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

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

6 結(jié)束語

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

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