融合點(diǎn)線特征的視覺慣性SLAM算法

摘 要： "為使移動(dòng)機(jī)器人在昏暗場(chǎng)景中獲得更高的定位精度以實(shí)現(xiàn)更好的建圖效果，針對(duì)特征點(diǎn)稀疏、相機(jī)移動(dòng)過快所導(dǎo)致的SLAM算法初始化困難、定位精度較低等問題，提出了一種融合點(diǎn)線特征的視覺慣性SLAM算法。通過EDLines線特征提取匹配算法來引入豐富的線特征，進(jìn)而提升特征跟蹤的魯棒性；利用慣性傳感器和視覺傳感器的互補(bǔ)優(yōu)勢(shì)，通過視覺慣性分步聯(lián)合初始化方法在初始化階段分三步進(jìn)行優(yōu)化，進(jìn)而提升初始化的精度和速度。實(shí)驗(yàn)表明，該算法所使用的線特征提取匹配算法相比傳統(tǒng)的LSD算法具有了更快的匹配速度和更低的誤匹配率，并且該算法在視覺慣性數(shù)據(jù)集中相機(jī)位姿的估計(jì)精度更高、魯棒性更強(qiáng)。

關(guān)鍵詞： "視覺同時(shí)定位與地圖構(gòu)建； 視覺慣性融合； 稀疏建圖； 點(diǎn)線特征提取

中圖分類號(hào)： "TP242 """文獻(xiàn)標(biāo)志碼： A

文章編號(hào)： "1001-3695（2022）02-004-0349-07

doi：10.19734/j.issn.1001-3695.2021.07.0270

Visual inertial SLAM algorithm fusing point and line feature

Fu Yua， Zheng Shuanga， Bie Tonga，b， Zhu Xiaoqinga，b， Wang Quanmina

（a.Faculty of Information Technology， b.Beijing Key Laboratory of Computational Intelligence amp; Intelligent System， Beijing University of Technology， Beijing 100124， China）

Abstract： "To solve the problems of difficult initialization and low positioning accuracy of SLAM algorithm caused by sparse feature points and fast camera movement in practical application scenes， this paper proposed a visual-inertial SLAM algorithm. It used EDLines feature extraction and matching algorithm to introduce rich line features to improve the robustness of feature tracking. Using the complementary advantages of the inertial sensor and the visual sensor， it carried out the optimization in three steps in the initial stage through the visual-inertial initialization by steps method， so as to improve the accuracy and speed of the initialization. Experiments show that the line feature extraction and matching algorithm used in this algorithm provides faster matching speed and lower mismatching rate than the traditional LSD algorithm. Moreover， the proposed algorithm has higher accuracy and stronger robustness in the estimation of camera pose in the visual-inertial data set.

Key words： "visual SLAM; visual-inertial; sparse mapping; point and line feature extraction

0 引言

隨著計(jì)算機(jī)領(lǐng)域的優(yōu)秀算法不斷涌現(xiàn)，移動(dòng)機(jī)器人越來越智能。同時(shí)定位與地圖構(gòu)建（SLAM）算法是目前的核心技術(shù)難點(diǎn)和研究熱點(diǎn)，它的出現(xiàn)使得移動(dòng)機(jī)器人在GPS信號(hào)較弱的室內(nèi)環(huán)境中，也能通過搭載相應(yīng)的傳感器獲取自身狀態(tài)和周圍的環(huán)境信息，從而實(shí)現(xiàn)地圖構(gòu)建和環(huán)境認(rèn)知，自主地完成特定任務(wù)。

自從這一問題提出以來，研究人員以提升精度和增強(qiáng)魯棒性為目標(biāo)，經(jīng)過幾十年的不斷發(fā)展，形成了基于激光傳感器和視覺傳感器的兩大SLAM方向。2007年Klein等人［1］提出了PTAM，他們采用了跟蹤與建圖雙線程的架構(gòu)，實(shí)現(xiàn)了在計(jì)算復(fù)雜度很高的視覺領(lǐng)域中的實(shí)時(shí)處理。同年提出Mono SLAM［2］是首個(gè)使用概率框架在線創(chuàng)建稀疏并保持持久的自然地標(biāo)地圖，算法由先驗(yàn)信息輔助系統(tǒng)完成初始化并用擴(kuò)展卡爾曼濾波法實(shí)時(shí)更新狀態(tài)。相較于高成本的激光SLAM，視覺 SLAM 可以獲取環(huán)境中大量的色彩以及紋理信息，并從中提取出更加豐富的場(chǎng)景信息，有利于之后的定位與導(dǎo)航應(yīng)用。

視覺SLAM算法主要分為直接法和特征點(diǎn)法。直接法利用圖像灰度值，通過最小化光度誤差來獲取機(jī)器人的位姿信息，計(jì)算像素深度，實(shí)現(xiàn)自身定位和建圖［3］。特征點(diǎn)法通過提取圖像特征點(diǎn)和描述子進(jìn)行特征點(diǎn)匹配，然后最小化重投影誤差計(jì)算相機(jī)位姿［4］。直接法相對(duì)于特征點(diǎn)法計(jì)算量較小，運(yùn)行速度較快，但極易受光照影響，對(duì)相機(jī)內(nèi)參要求很高，當(dāng)存在幾何噪聲時(shí)算法性能下降較快。

ORB-SLAM［5］框架是目前熱門的特征點(diǎn)法視覺框架，前端基于ORB特征，在減少了計(jì)算量的同時(shí)也比直接法精度更高。研究者們?cè)谶@個(gè)框架基礎(chǔ)上，提出了很多改進(jìn)的版本，其中ORB-SLAM2［6］是首個(gè)支持單目、雙目及RGB-D相機(jī)的開源SLAM算法，并且該算法的后端使用基于單目和雙目觀測(cè)的光束平差法，使其可以精確地估計(jì)軌跡的尺度。在2021年，Carlos等人［7］提出了能夠用于魚眼相機(jī)、單目相機(jī)、雙目相機(jī)、RGB-D相機(jī)的視覺慣性SLAM算法ORB-SLAM3，此算法的主要?jiǎng)?chuàng)新之一是多地圖系統(tǒng)，該系統(tǒng)依賴于一種新的地點(diǎn)識(shí)別算法來提升召回率，使算法能夠在長時(shí)間視覺信息不佳的情況下維持運(yùn)行，算法檢測(cè)到丟失時(shí)會(huì)立即啟動(dòng)一個(gè)新的地圖，當(dāng)識(shí)別到重新訪問已建圖區(qū)域時(shí)，該地圖能與以前的地圖無縫合并。上述三個(gè)算法運(yùn)行時(shí)盡管ORB特征在紋理良好的環(huán)境下精度較高，但是由于相機(jī)本身的硬件約束及成像原理導(dǎo)致在較暗光照等特征點(diǎn)缺失場(chǎng)景中無法提取到圖像中的豐富特征信息，甚至使SLAM系統(tǒng)崩潰。即使缺乏可靠的特征點(diǎn)，在這些場(chǎng)景中仍然包含許多可以提取的線特征，PL-SLAM［8］在ORB-SLAM框架基礎(chǔ)上融合了LSD線特征提取算法，提高了在復(fù)雜場(chǎng)景中的精度，不過該方法在相機(jī)位姿估計(jì)的精度和特征提取速度上仍然有提升空間。

為了適應(yīng)更多的場(chǎng)景，研究者們將IMU（inertial measurement unit）傳感器與視覺相融合以獲得性能的提升［9，10］。尤其在快速運(yùn)動(dòng)和強(qiáng)旋轉(zhuǎn)造成的成像模糊場(chǎng)景下，IMU仍然可以很好地感知運(yùn)動(dòng)，從而將姿態(tài)估計(jì)的誤差限制在可接受的范圍內(nèi)。視覺慣性SLAM系統(tǒng)可分為松耦合和緊耦合兩種類型。松耦合方法是融合視覺與IMU兩個(gè)模塊分別獨(dú)立進(jìn)行運(yùn)動(dòng)估計(jì)后的結(jié)果，其計(jì)算復(fù)雜度較小、擴(kuò)展性強(qiáng)，然而優(yōu)化效果較差，經(jīng)典的松耦合算法有Ethzasl_MSF［11］。緊耦合方法則是利用相機(jī)與IMU的原始數(shù)據(jù)共同構(gòu)建運(yùn)動(dòng)方程和觀測(cè)方程，聯(lián)合優(yōu)化，更容易獲得全局一致的位姿估計(jì)，雖然計(jì)算復(fù)雜度較高、擴(kuò)展性較差但是優(yōu)化效果更好、魯棒性更強(qiáng)，是目前研究的熱點(diǎn)方向。Mur-Artal等人［12］在ORB-SLAM框架的基礎(chǔ)上應(yīng)用緊耦合方案，加入IMU測(cè)量信息提出了VI ORB-SLAM算法，這是第一個(gè)能夠重用地圖的視覺慣性SLAM算法， 其初始化部分計(jì)算了尺度、重力方向、速度、陀螺儀和加速度計(jì)零偏，提升了相機(jī)位姿精度，但這導(dǎo)致初始化時(shí)間較長。同樣是基于ORB-SLAM框架的緊耦合方案，ORB-SLAM3在慣性測(cè)量單元初始化階段，完全依賴于最大后驗(yàn)估計(jì)，提升了初始化的速度。VINS-Mono［13］是緊耦合的另一經(jīng)典算法。該算法可以在初始化過程中對(duì)慣性測(cè)量單元的各初始狀態(tài)進(jìn)行標(biāo)定，但是該初始化方法忽略了慣性測(cè)量單元加速度偏置誤差和測(cè)量噪聲，從而影響了系統(tǒng)初始化的準(zhǔn)確度和運(yùn)行效率，并且只支持單目相機(jī)。在VINS-Mono的基礎(chǔ)上Qin等人［14］又提出了VINS-Fusion算法，支持多個(gè)傳感器如單目與IMU、雙目與IMU、雙目與GPS等。

相機(jī)和IMU具有良好的互補(bǔ)性，高頻率IMU可以彌補(bǔ)快速運(yùn)動(dòng)對(duì)相機(jī)的影響，反之相機(jī)能夠有效修正IMU的漂移，并且線特征的加入能使特征的跟蹤更加魯棒。所以本文基于ORB-SLAM框架，融合慣性傳感器來提升初始化的精度和速度，在前端視覺里程計(jì)部分加入基于LBD描述子的EDLine線特征提取匹配算法來解決傳統(tǒng)LSD算法在特征提取時(shí)線段不連續(xù)以及實(shí)時(shí)性較差的問題，提出了一種精度更高、魯棒性更強(qiáng)、實(shí)時(shí)性更好的視覺慣性SLAM算法。

1 系統(tǒng)架構(gòu)

系統(tǒng)框架由圖1所示的四個(gè)線程組成，即跟蹤、局部建圖、回環(huán)檢測(cè)和全局BA優(yōu)化。

RGB圖像和IMU數(shù)據(jù)被輸入到跟蹤線程中。首先對(duì)雙目相機(jī)的RGB圖像幀進(jìn)行點(diǎn)線特征提取，其中點(diǎn)特征沿用了ORB-SLAM2的ORB提取算法，步驟是對(duì)具有旋轉(zhuǎn)不變性的 FAST 角點(diǎn)進(jìn)行提取。線特征采用基于LBD描述子的EDLine線特征提取匹配算法，步驟是先進(jìn)行邊緣檢測(cè)得到邊緣像素鏈，再遍歷像素鏈進(jìn)行擬合得到線特征。使用文獻(xiàn)［15，16］中的理論，將IMU的加速度計(jì)數(shù)據(jù)和陀螺儀數(shù)據(jù)預(yù)先整合連續(xù)幀之間的IMU測(cè)量數(shù)據(jù)，然后將得到的IMU預(yù)積分的旋轉(zhuǎn)、速度和位置測(cè)量向量組合形成整個(gè)測(cè)量向量的信息矩陣，再將IMU運(yùn)動(dòng)模型在兩個(gè)視覺幀之間進(jìn)行預(yù)積分處理。接著進(jìn)行初始位姿估計(jì)，需要對(duì)提取出的點(diǎn)線特征進(jìn)行匹配：對(duì)于點(diǎn)特征，根據(jù)BRIEF 描述子的計(jì)算，通過詞袋模型進(jìn)行點(diǎn)特征匹配；對(duì)于線特征，計(jì)算LBD描述子，根據(jù)描述子進(jìn)行幀間匹配，再用RANSIC算法剔除誤匹配。根據(jù)匹配的特征和IMU預(yù)積分進(jìn)行初始位姿估計(jì)和局部建圖，然后對(duì)創(chuàng)建的局部地圖進(jìn)行追蹤，并選取新關(guān)鍵幀。

局部建圖線程對(duì)局部地圖進(jìn)行管理和優(yōu)化，此線程將跟蹤線程獲得的關(guān)鍵幀進(jìn)行插入，對(duì)點(diǎn)特征和線特征進(jìn)行三角化來創(chuàng)建新地圖點(diǎn)，再剔除冗余的關(guān)鍵幀、點(diǎn)特征和線特征。除了原始關(guān)鍵幀之外，使用視覺慣性BA來優(yōu)化地圖中的關(guān)鍵幀和點(diǎn)特征及線特征。此外，另一項(xiàng)重要工作是IMU初始化，目的是給整個(gè)系統(tǒng)提供優(yōu)化求解的初值，采用視覺和IMU 的緊耦合方案。在IMU初始化階段分三步進(jìn)行，在3.2節(jié)進(jìn)行具體介紹。最后對(duì)視覺慣性測(cè)量單元（visual inertial mea-surement unit，VIMU）比例尺參數(shù)進(jìn)行更新和細(xì)化。

回環(huán)檢測(cè)線程進(jìn)行位置識(shí)別與回環(huán)修正。采用DBoW2詞袋重定位系統(tǒng)［17］，實(shí)現(xiàn)位置識(shí)別和回環(huán)檢測(cè)的長期數(shù)據(jù)關(guān)聯(lián)。其中在位置識(shí)別時(shí)，進(jìn)行候選檢測(cè)和計(jì)算相似變換群Sim3和SE3;在回環(huán)修正時(shí)，檢測(cè)整體回環(huán)并通過位姿圖優(yōu)化來修正累計(jì)漂移誤差。

完成回環(huán)檢測(cè)后進(jìn)行最后的全局優(yōu)化并更新地圖。在全局優(yōu)化中使用了滑動(dòng)窗口模型，通過最小化目標(biāo)函數(shù)來優(yōu)化滑動(dòng)窗口中的所有狀態(tài)變量來計(jì)算最優(yōu)結(jié)構(gòu)和運(yùn)動(dòng)解，并利用滑動(dòng)窗口模型來限制需要優(yōu)化的關(guān)鍵幀的數(shù)量以此來控制優(yōu)化規(guī)模，防止突然產(chǎn)生大量的關(guān)鍵幀優(yōu)化需求導(dǎo)致優(yōu)化不及時(shí)，造成追蹤丟失和建圖誤差。這種方法提高了系統(tǒng)的實(shí)時(shí)性，并優(yōu)化了建圖效果。

2 線特征提取與匹配

SLAM領(lǐng)域普遍使用的經(jīng)典線特征匹配算法是LSD算法［18］，雖然LSD對(duì)大多數(shù)類型的圖像具有良好的效果，但是它消耗的計(jì)算資源較大，實(shí)時(shí)性較差。本章介紹在視覺里程計(jì)模塊引入的基于LBD描述子的EDLines［19］線特征提取匹配算法。此方法提高了相鄰圖像的數(shù)據(jù)關(guān)聯(lián)性，并通過RANSAC（random sample consensus）算法進(jìn)一步降低了誤匹配率，提高相機(jī)位姿估計(jì)精度。經(jīng)實(shí)驗(yàn)驗(yàn)證，此方法與LSD算法相比精度相當(dāng)，速度更快。

本文提出的基于LBD描述子的EDLines線特征提取匹配算法流程如圖2所示。

2.1 邊緣特征檢測(cè)和線特征提取

本文算法與現(xiàn)有的邊緣檢測(cè)算法相比有以下優(yōu)點(diǎn)：其他邊緣檢測(cè)算法檢測(cè)到的邊緣像素通常是獨(dú)立、不相交、不連續(xù)的實(shí)體;本算法生成一組干凈的、連續(xù)的邊緣片段，即相連的邊緣像素鏈。給定一幅灰度圖像，EDLines分四步進(jìn)行邊緣檢測(cè)：

a）平滑處理。首先對(duì)圖像進(jìn)行高斯濾波來抑制噪聲（5×5高斯核，默認(rèn) r =1）。

b）計(jì)算梯度大小和方向。使用Sobel梯度算子計(jì)算平滑處理后圖像中每個(gè)像素的梯度大小和方向。

c）選取錨點(diǎn)。選取梯度算子最大值的像素作為錨點(diǎn)，因?yàn)檫@些像素有很高的概率成為邊緣元素。

d）形成邊緣像素鏈。從一個(gè)錨點(diǎn)開始，計(jì)算相鄰像素的梯度大小和方向，通過梯度最大值選取下一個(gè)錨點(diǎn)。最后將相鄰錨點(diǎn)進(jìn)行連接。因?yàn)殄^點(diǎn)大多數(shù)為邊緣元素，由此可形成邊緣像素鏈，完成邊緣特征檢測(cè)。

得到相鄰邊緣像素鏈后，接下來的目標(biāo)是將這個(gè)鏈分割成一個(gè)或多個(gè)線段。其基本方法是依次遍歷像素，并使用最小二乘直線擬合方法對(duì)像素進(jìn)行直線擬合，直到誤差超過某一閾值。采用偏差絕對(duì)值之和最小的原則進(jìn)行擬合，如式（1）所示，其中 x i、y i是 像素坐標(biāo)。

min∑ "m i=1 |δ i|=∑ m i=1 |φ（x i）－y i|

（1）

當(dāng)誤差超過所設(shè)閾值時(shí)，則生成一個(gè)新的線段。然后遞歸處理鏈中剩余的像素，直到所有像素都被處理。

2.2 相鄰幀匹配

幀匹配部分，在EDLines算法提取出的線特征基礎(chǔ)上，為了評(píng)估成對(duì)線特征的幾何一致性，用LBD描述子［20］來描述成對(duì)線之間的幾何屬性。LBD描述子有如下優(yōu)點(diǎn)：a）由于采用多尺度線檢測(cè)策略，對(duì)圖像變換仍然具有魯棒性；b）設(shè)計(jì)的LBD描述子計(jì)算速度快，并且降低了圖匹配問題的維數(shù)；c）計(jì)算過程中對(duì)成對(duì)線特征的幾何一致性評(píng)估，在低紋理圖像中也有較好的效果。

為了克服直線檢測(cè)的碎片化問題，在直線檢測(cè)算法中對(duì) N 張?jiān)紙D像通過一組尺度因子和高斯模糊算法進(jìn)行降采樣，由此生成圖像的尺度空間金字塔。

將EDLines算法應(yīng)用于空間金字塔，在不同尺度上分別提取一組線特征。每條線的方向是通過使大多數(shù)邊緣像素從線的左側(cè)指向右側(cè)的梯度計(jì)算得出的。然后在尺度空間中尋找對(duì)應(yīng)的線來進(jìn)行匹配。將在同一尺度空間中提取的所有線特征分配不同的ID，并存儲(chǔ)到LineVec向量中。對(duì)整個(gè)金字塔進(jìn)行提取，得到一組LineVec向量。為了描述LineVec的局部外觀，為提取該線的降采樣圖像生成一個(gè)線描述子LBD。

LBD由線支持域LSR計(jì)算得到，這個(gè)支持域被劃分為一組頻帶，用{ B 1，B 2，…，B m} 的形式表示，每個(gè)頻帶有相同的寬度。將LSR中的點(diǎn)像素的梯度投影到局部二維坐標(biāo) 系d l和d ⊥中：g′=（g T ·d ⊥，g T ·d l） T （g′ d ⊥，g′ d l） T ，其中g(shù)為像素點(diǎn)梯度，g′為局部坐標(biāo) 系中的投影像素點(diǎn)。

SIFT［21］和MSLD［22］的研究顯示，可以將全局高斯函數(shù)和局部高斯函數(shù)沿LSR應(yīng)用于區(qū)域內(nèi)的每一行 d ⊥。對(duì)全局高斯函數(shù)，將全局加權(quán)系數(shù)f g（i）=（1/ 2 π "σ g） e －d2 "i/2σ2 "g "分配給LSR的第 i行，其中d i是區(qū)域內(nèi)的中心行到第i行的距離，此時(shí)可得σ g=（m·w－1）/2，其中m代 表LSR的行數(shù)， w代表每行的寬度。對(duì)于局部高斯函數(shù)，在局部區(qū)域B j、B j－1和B j+1，將局部加權(quán)系數(shù)f l （k） =（1/ 2 π "σ l ） e -d "k′2/2σ2 "l 分配給第k行，其中d′ k是局部區(qū)域中心B j的中心行到第k行的距離，此時(shí)σ l=w。 全局高斯函數(shù)的目的是減輕對(duì)LSR內(nèi)沿 d "⊥方向變化的敏感度。局部高斯函數(shù)的目的是減少邊界效應(yīng)，避免描述符在像素從一個(gè)頻帶移動(dòng)到下一個(gè)頻帶時(shí)突變。

接下來計(jì)算每條頻帶的描述子 BD j， 進(jìn)而構(gòu)成LBD描述子：

對(duì)于LSR中的 頻帶B j，頻帶描述子BD j是從B j及其最近的兩個(gè)頻帶B j－1和B j+1計(jì)算出來的。計(jì)算BD j后，將它們簡 單地串聯(lián)起來，就得到了LBD描述子，如式（2）所示。對(duì)于在LSR頂部和底部的頻帶 B 1和B m， 在計(jì)算描述子時(shí)在LSR之外的像素不會(huì)被考慮在內(nèi)。

LBD=（BD T "1，BD T "2，…，BD T "m） T "（2）

然后計(jì)算 BD j。對(duì)于B j的第k行，將該行內(nèi)四個(gè) 方向上的像素梯度進(jìn)行累加可得，如式（3）所示。

v1k j=λ∑ g′ d ⊥gt;0 g′ d ⊥，v2k j=λ∑ g′ d ⊥gt;0 －g′ d ⊥

v3k j=λ∑ g′ d ⊥gt;0 g′ d ⊥， v4k j=λ∑ g′ d ⊥gt;0 －g′ d ⊥ ""（3）

其中高斯權(quán)重 λ=f g（k）f l（k ）。

通過疊加頻帶 B j中所有行的這四個(gè)累積梯度，頻帶描述矩陣 BDM "j構(gòu) 造如式（4）所示。

BDM "j= "v11 j v12 j … v1n j

v21 j v22 j … v2n j

v31 j v32 j … v3n j

v41 j v42 j … v4n j

∈ Euclid Math TwoRAp

4×n """（4）

其中： n是B j的行數(shù)。當(dāng)j為1或m時(shí)，取n=2w，其他情況時(shí)n=3w 。

然后利用矩陣 BDM "j的均值向量 M "j和標(biāo)準(zhǔn)差向量 S "j來構(gòu)造BD j=（ M T "j， S T "j）∈ Euclid Math TwoRAp

8，并將其代入 LBD描述子式（5），可得

LBD=（ M T 1， S T 1， M T 2， S T 2，…， M T "m ， S T "m ）T∈ Euclid Math TwoRAp

8m ""（5）

通過對(duì) LBD 的均值和標(biāo)準(zhǔn)方差部分分別進(jìn)行歸一化，同時(shí)對(duì)每個(gè) LBD維度進(jìn)行約束，得到單元LBD。

得到LineVecs的LBD描述子 L r和L q后， 通過以下步驟來進(jìn)行匹配。首先進(jìn)行預(yù)處理來檢查LineVecs的一元幾何屬性和局部外觀相似性，以通過排除明顯的不匹配項(xiàng)得到候選匹配對(duì)。然后，為一組候選匹配 CM={（L1 r，L1 q），（L2 r，L2 q），…，（Lk r，Lk q）}建立關(guān)系圖，其中點(diǎn)表示潛在的對(duì)應(yīng)關(guān)系，線表示它們之間的匹配權(quán)重。根據(jù)CM 中候選匹配的權(quán)重構(gòu)造鄰接矩陣，從中得到匹配結(jié)果。

最后使用RANSAC算法剔除相鄰幀的誤匹配線段，得到更加準(zhǔn)確的匹配結(jié)果。

3 視覺慣性傳感器融合

本文在VI ORB-SLAM和文獻(xiàn)［14］的基礎(chǔ)上，融合ORB-SLAM2和ORB-SLAM3的初始化方法，提出了一種更加快速、準(zhǔn)確的視覺慣性分步聯(lián)合初始化的方法。

3.1 視覺慣性里程計(jì)預(yù)積分

在純視覺系統(tǒng)中，估計(jì)狀態(tài)僅包括當(dāng)前攝像機(jī)姿態(tài)，而在視覺慣性系統(tǒng)中，假設(shè)相機(jī)和IMU坐標(biāo)系的轉(zhuǎn)換是已知且固定的，仍需要計(jì)算額外的狀態(tài)向量。

狀態(tài)向量 s "i由世界坐標(biāo)系中的相機(jī)方向D i、位置P i、速度V i，以及加速度計(jì)偏差Ba i和陀螺儀偏差Bg i 構(gòu)成，如式（6）所示。

s "i{D i，P i，V i，Ba i，Bg i} ""（6）

其中：相機(jī)位姿可 以寫成{D i，P i}， IMU 的偏差可以寫成b i {Ba i，Bg i}，且V i，Ba i，Bg i∈ Euclid Math TwoRAp

3 。

K k表示時(shí)間k之前所有關(guān)鍵幀的集合。估計(jì)的所有關(guān)鍵幀的狀態(tài)如 式（7）所示。

S k={ s "i} i∈K k ""（7）

系統(tǒng)的輸入是相機(jī)和IMU的測(cè)量值。 C i是關(guān)鍵幀i處的相機(jī)測(cè)量值，χ ij表示在兩個(gè)連續(xù)關(guān)鍵幀i和j 之間的IMU測(cè)量集合。到時(shí)間 k為止的測(cè)量值集合為Z k {C i，χ ij} （i，j）∈K k 。

對(duì)于視覺慣性SLAM，按照文獻(xiàn)［15］中的理論，需要預(yù)先整合連 續(xù)幀i和i+1 之間的IMU測(cè)量數(shù)據(jù)，然后將得到的IMU預(yù)積分的旋轉(zhuǎn)、速度和位置測(cè)量向量分別記為Δ ω i，i+1、 Δ υ i，i+1、 Δ ρ i，i+1，由此得到整個(gè)測(cè)量向量的信息矩陣 Σ "ψ i－1，i。基于這些預(yù)積分項(xiàng)和狀態(tài)，采用文獻(xiàn)［16］中慣性剩余項(xiàng)γ ψ i，i+1 的定義，如式（8）所示。

γ ψ i，i+1=［γ "Δ ω i，i+1，γ "Δ υ i，i+1，γ "Δ ρ i，i+1］

γ "Δ ω i，i+1= log（Δ ω "T "i，i+1 ω "T "i ω "i+1）

γ "Δ υ i，i+1= ω "T "i（υ i+1－υ i－g Δ t i，i+1）－ Δ υ i，i+1

γ "Δ ρ i，i+1= "ω "T "i p j－p i－υ i Δ t－ g Δ t2 2 "－ Δ p "i，i+1 """（8）

偏差估計(jì)在初始化過程中會(huì)發(fā) 生少量δb的變化。一種解決方案是當(dāng)偏差發(fā)生變化時(shí)重新計(jì)算測(cè)量值，然而這會(huì)增加較大的計(jì)算量。所以，給定偏差更新b← +δb，可 以使用一階展開來更新測(cè)量值，如式（9）所示。

Δ ""i，i+1（Bg i）" Δ ""i，i+1（ g i） exp" " "Δ ""i，i+1" Bg δBg """"（9）

Δ ""i，i+1（Ba i，Bg i）" Δ ""i，i+1（ a i， g i）+" "Δ ""i，i+1 Ba δBa i+" "Δ ""i，i+1" Bg δBg i

Δ ""i，i+1（Ba i，Bg i） Δ ""i，i+1（ a i， g i）+" "Δ ""i，i+1" Ba δBa i+" Δ ""i，i+1" Bg δBg i

利用慣性殘差得到的坐標(biāo)系 i和三維點(diǎn)j在位置x j的重投影誤差γ ij ，如式（10）所示。

γ ij=τ ij－Π（ Euclid Math OneJAp

－1 i⊕x j） ""（10）

其中 Π ： Euclid Math TwoRAp

3→ Euclid Math TwoRAp

n為對(duì)應(yīng)相機(jī)模型的投影函數(shù);τ ij為j點(diǎn)在圖像i處的觀測(cè)值，其信息矩陣為 Σ "ij;用 Euclid Math OneTAp

表示從IMU信息到相機(jī)模型的剛性轉(zhuǎn)換;⊕是特殊歐氏群SE（3）對(duì) Euclid Math TwoRAp

3元素的變換運(yùn)算。

結(jié)合慣性殘差項(xiàng)和視覺殘差項(xiàng)，視覺慣性SLAM可描述為基于關(guān)鍵幀的最小化問題［23］。給定一組 k+1關(guān)鍵幀及其狀態(tài)F k {F 0，F(xiàn) 1，…，F(xiàn) k}和一組 3D 點(diǎn)及其狀態(tài)P {P 0，P 1，…，P l－1}，則視覺 慣性聯(lián)合優(yōu)化問題可表述為

min ""F k，p （∑ k i=1 ‖γψ i－1，i‖2 "Σ "ψ i－1，i+∑ l－1 j=0 "∑ n i∈φ j "ρHub （‖γ ij‖ "Σ "ij）） ""（11）

其中： φ j 是3D點(diǎn) j 的一組關(guān)鍵幀。

對(duì)于重投影誤差，使用G2o［24］提供的魯棒核函數(shù)ρHub來降低誤匹配率；對(duì)于慣性殘差項(xiàng)，因?yàn)椴淮嬖谡`關(guān)聯(lián)的問題，所以可以將其忽略。

3.2 視覺慣性分步聯(lián)合初始化

在本節(jié)中，使用分步聯(lián)合初始化的方法對(duì)視覺慣性的參數(shù)和偏差進(jìn)行初始化。

1） 純視覺初始化

在初始化階段，一些傳統(tǒng)的SLAM系統(tǒng)例如VI-DSO［25］會(huì)直接開始視覺慣性BA優(yōu)化，而繞過純視覺的初始化階段。本文添加了純視覺的初始化階段，并計(jì)算比例尺，利用ORB-SLAM提供的較為準(zhǔn)確的初始地圖，提高IMU初始化速度。

在純視覺初始化階段，運(yùn)行ORB-SLAM的初始化進(jìn)程，在相鄰兩幀之間采用特征點(diǎn)法進(jìn)行幀間匹配，并插入關(guān)鍵幀，初步形成一個(gè)由純視覺BA優(yōu)化的，包含有多個(gè)相機(jī)位姿和稀疏點(diǎn)云組成的初始化地圖，并在地圖內(nèi)固定相機(jī)軌跡，其軌跡可以表示為： "T "0：k=［m，n］ 0：k，其 中“-”表示按比例尺的變量。

2）純慣性初始化

在純慣性初始化階段，目標(biāo)是獲得慣性變量的最佳估計(jì)值。在最大后驗(yàn)估計(jì)的基礎(chǔ)上，僅使用固定的相機(jī)軌跡和這些關(guān)鍵幀之間的慣性變量。這些慣性變量可以疊加在純慣性狀態(tài)向量 V "k中 ，如式（12）所示。

V "k={μ， Euclid Math OneGAp

， Euclid Math OneBAp

， "0：k} ""（12）

其中： μ 是純視覺初始化階段的比例因子；Euclid Math OneGAp

是重力方向；Euclid Math OneBAp

是初始化時(shí)假定為常數(shù)的加速度計(jì)和陀螺儀偏差； "0：k 是從第一個(gè)關(guān)鍵幀到最后一個(gè)關(guān)鍵幀的按比例尺縮放的相機(jī)速度，它最初是通過固定的相機(jī)軌跡估計(jì)的。基于上述描述，只需考慮慣性測(cè)量值χ 0：k {χ 0：1，χ 1：2，…，χ k－1：k }。

因此最大化后驗(yàn)分布如式（13）所示。

p（ V "k|χ 0：k）∝p（χ 0：k| V "k）p（ V "0） ""（13）

其中： p（χ 0：k| V "k）代表似然值;p（ V "0）代表先 驗(yàn)值。

由馬爾可夫性質(zhì)可知，某一時(shí)刻的圖像測(cè)量只依賴于此時(shí)的狀態(tài)。根據(jù)測(cè)量之間的標(biāo)準(zhǔn)獨(dú)立假設(shè)，結(jié)合式（6）可將式（13）表示為

p（ V "0）∏ （i，j）∈K k p（χ ij|s i， s j）∏ i∈K k p（Z i|s i） ""（14）

考慮到測(cè)量的獨(dú)立性，純慣性最大后驗(yàn)估計(jì)問題可以表示為

V ′ k= arg max ""V "k （p（ V "0）∏ k i=1 p（χ i－1：i|μ， Euclid Math OneGAp

， Euclid Math OneBAp

， "i－1， "i）） ""（15）

最大后驗(yàn)估計(jì) V ′ k對(duì) 應(yīng)式（15）的最大值，或者負(fù)對(duì)數(shù)后驗(yàn)估計(jì)的最小值。對(duì)IMU的預(yù)積分和先驗(yàn)分布取負(fù)對(duì)數(shù)和高斯誤差，負(fù)對(duì)數(shù)后驗(yàn)估計(jì)可以寫成殘差的平方和。最終得到優(yōu)化問題，如式（16）所示。

V ′ k= arg max ""V "k （‖γ p‖2 "Σ "p+∑k i=1‖γ ψ i-1，i‖2 ""Σ "ψ i-1，i） ""（16）

式（16）的優(yōu)化方程與式（11）的不同之處在于它不包括視覺殘差，而是使用先驗(yàn)殘差 γ p 使得IMU偏差接近于零，并且式（16）的協(xié)方差由IMU特征給出。

在進(jìn)行優(yōu)化時(shí)，需要定義一個(gè)收斂［24］來更新優(yōu)化期間的重力方向估計(jì)，如式（17）所示。

Euclid Math OneGAp

′= Euclid Math OneGAp

exp （δα" Euclid Math OneGAp

，δβ" Euclid Math OneGAp

，0） ""（17）

其中：exp為從so（3）到SO（3）的指數(shù)映射。為了保證優(yōu)化期間比例因子保持正值，將其更新定義為 μ′=μ exp（ δμ ）。

在完成慣性優(yōu)化后，幀姿態(tài)和速度以及3D地圖點(diǎn)將按照估計(jì)的比例尺進(jìn)行縮放，并進(jìn)行旋轉(zhuǎn)以使 Z 軸與估計(jì)的重力方向進(jìn)行對(duì)齊。在此期間，將更新各類偏差并重復(fù)進(jìn)行慣性測(cè)量單元的預(yù)積分，以此來減少線性化誤差。

3）視覺慣性聯(lián)合初始化

在視覺慣性聯(lián)合初始化階段，這一階段可以用圖3表示。通過之前的方法對(duì)慣性和視覺參數(shù)有較好的估計(jì)，就可以進(jìn)行視覺慣性聯(lián)合初始化。

在這個(gè)初始化階段，所有關(guān)鍵幀具有相同的偏差，并且先驗(yàn)信息與純慣性初始化階段中的相同。忽略傳感器不確定性的話會(huì)產(chǎn)生很大的不可預(yù)測(cè)的誤差，因此需要將傳感器的不確定性在初始化階段進(jìn)行處理。

4 實(shí)驗(yàn)與評(píng)估

實(shí)驗(yàn)分為兩部分進(jìn)行。首先與LSD算法對(duì)比來評(píng)估EDLines線特征提取匹配算法的性能。接著在公開數(shù)據(jù)集上評(píng)估本文算法和六個(gè)經(jīng)典SLAM算法的性能，這六個(gè)SLAM算法包括三個(gè)純視覺算法和三個(gè)視覺慣性算法。

4.1 圖像匹配

4.1.1 線特征提取實(shí)驗(yàn)

分別選取公開數(shù)據(jù)集EuRoC中不同場(chǎng)景中的圖片進(jìn)行實(shí)驗(yàn)，平均提取的特征數(shù)目和運(yùn)行時(shí)間分別如圖4、5所示。

由圖4、5可知，本文算法與LSD算法相比提取的特征數(shù)基本相當(dāng)，但由于降低了匹配過程的運(yùn)算量，進(jìn)而大大縮短了算法的運(yùn)行時(shí)間。更快的提取速度將為相機(jī)位姿估計(jì)和整體的SLAM算法帶來更好的實(shí)時(shí)性。

4.1.2 特征匹配實(shí)驗(yàn)

在相鄰幀匹配部分，圖6、7分別展示了在EuRoC的同一場(chǎng)景中兩種算法的效果。場(chǎng)景選取了飛行器飛到黑暗場(chǎng)景的圖像幀進(jìn)行實(shí)驗(yàn)，因?yàn)樵诤诎祱?chǎng)景下特征的提取和幀間匹配更具挑戰(zhàn)性。黑暗場(chǎng)景中的紋理信息較少，點(diǎn)線特征的提取變得更加困難，并且在匹配時(shí)也更容易產(chǎn)生誤匹配的情況，因此采用黑暗場(chǎng)景進(jìn)行實(shí)驗(yàn)更能展示出不同算法的性能差別。其中圖6使用LSD算法，圖7使用本文算法。

由圖6、7可以較為直觀地看出，本文的算法誤匹配出現(xiàn)的情況較少，匹配準(zhǔn)確率更高。但如果對(duì)此結(jié)果不加處理，同樣會(huì)產(chǎn)生較多的誤匹配和冗余匹配，浪費(fèi)計(jì)算資源。因此可以引入RANSAC算法改善這一問題。

在本文算法的基礎(chǔ)上使用RANSAC算法消除誤匹配和冗余匹配，效果如圖8所示。

顯然，在增加RANSAC算法后，與圖7相比消除了部分的誤匹配和冗余線特征，線特征匹配的效果更好。

下面的一組圖展示了系統(tǒng)運(yùn)行時(shí)對(duì)點(diǎn)線特征的提取、跟蹤情況以及建圖效果。圖9表示在系統(tǒng)運(yùn)行過程中相機(jī)處于黑暗的場(chǎng)景時(shí)點(diǎn)特征和線特征的提取情況。圖10分別表示建圖時(shí)只生成點(diǎn)特征、線特征和融合點(diǎn)線特征的三種情況，可以看出點(diǎn)線特征融合建圖效果更為直觀。

由此可見，點(diǎn)線融合的特征提取與匹配可以使 SLAM 算法獲得更豐富、更有效的特征信息，提高SLAM算法的性能，在黑暗的弱紋理場(chǎng)景中也擁有良好的定位，并實(shí)現(xiàn)較好的建圖效果。

4.2 定位精度實(shí)驗(yàn)

使用微型飛行器（MAV）與手持相機(jī)分別收集的視覺慣性數(shù)據(jù)集EuRoC進(jìn)行評(píng)估。MAV序列的采集工具是Asctec Firefly六角旋翼直升機(jī)，上面搭載著雙目相機(jī)和慣性測(cè)量單元（IMU），MAV在大型工廠中采集數(shù)據(jù)，根據(jù)飛行的速度、照明情況和場(chǎng)景紋理，序列被分為容易、中等和困難。手持相機(jī)序列的采集工具也采用了雙目相機(jī)和IMU，采集者在兩個(gè)不同的小房間內(nèi)進(jìn)行移動(dòng)來采集數(shù)據(jù)，根據(jù)采集者的動(dòng)作復(fù)雜度和速度也分為了容易、中等和困難三種不同的序列。表1給出了以ATE（absolute trajectory error，絕對(duì)軌跡誤差）為指標(biāo)在數(shù)據(jù)集序列中進(jìn)行對(duì)比，ATE是估計(jì)位姿和真實(shí)位姿的直接差值，可以非常直觀地反映算法精度和軌跡全局一致性。

將本文算法和ORB-SLAM2［6］、VINS-Fusion［14］、VINS-Mono［13］、VI ORB-SLAM［12］、ORB-SLAM3［7］五種算法使用EuRoc數(shù)據(jù)集進(jìn)行對(duì)比。根據(jù)算法的種類不同可將其劃分為純視覺和視覺慣性融合兩類，如表1所示。

在MH_04_difficult和MH_05_difficult困難序列中，微型飛行器會(huì)在光線變化更大或更空曠的位置采集圖像，并且初始化時(shí)間更短，采集過程中設(shè)備移動(dòng)速度更快。這對(duì)特征提取和初始化部分提出了更高的要求。VR1_02_difficult和VR2_03_difficult序列是最為困難的兩個(gè)序列，因?yàn)樗鼈兪峭ㄟ^手持相機(jī)進(jìn)行采集的，并且在相機(jī)運(yùn)動(dòng)過程中包含大量的無規(guī)則高速運(yùn)動(dòng)，產(chǎn)生了大量模糊的圖像，對(duì)特征提取造成較大的干擾。

由表1可知，在純視覺對(duì)比算法中，ORB-SLAM2在絕大部分?jǐn)?shù)據(jù)集序列中能保持最好的性能。在視覺慣性融合對(duì)比算法中，VI ORB-SLAM和ORB-SLAM3性能突出。本文在ORB-SLAM框架的基礎(chǔ)上采用融合LBD描述子的EDLine線特征提取匹配算法和視覺慣性分步聯(lián)合初始化方法，雖然在MH_01_ easy、VR1_01_easy等簡單難度序列和MH_03_medium、VR1_02_ medium等中等難度的序列中，與ORB-SLAM2算法表現(xiàn)出基本相同的性能，但在MH_04_difficult、VR1_02_difficult等困難序列中，本文算法相比各類對(duì)比算法都有了提升，隨著數(shù)據(jù)集序列難度的增加，提升更為明顯。相比ORB-SLAM3，本文算法在多數(shù)MH序列上保持一定的領(lǐng)先，在VR序列上性能相近。因?yàn)镸H序列為工廠環(huán)境，飛行器在采集數(shù)據(jù)中會(huì)進(jìn)入較為黑暗的弱紋理場(chǎng)景，在此類場(chǎng)景中采用線特征提取算法可以提高跟蹤與建圖的精度。VR序列為室內(nèi)環(huán)境，特征較多，相機(jī)在錄制過程中有大幅度快速晃動(dòng)，更考驗(yàn)算法的魯棒性。

在MH_04_difficult序列中，純視覺的對(duì)比算法中表現(xiàn)最好的是ORB-SLAM2，其ATE為0.119，視覺慣性的對(duì)比算法中表現(xiàn)最好的是ORB-SLAM3，其ATE為0.099，而本文算法的ATE僅為0.063，表現(xiàn)出良好的性能。在MH_05_difficult序列中，ORB-SLAM2和ORB-SLAM3在對(duì)比算法中性能出色，其ATE分別為0.060和0.071，本文算法的ATE為0.056，略好于ORB-SLAM2。在VR1_02_difficult序列中，VINS-Fusion和VI ORB-SLAM在運(yùn)行過程中會(huì)產(chǎn)生長時(shí)間的相機(jī)位姿丟失和無法進(jìn)行特征提取的情況，不能完成基本任務(wù)；在可以完成基本任務(wù)的前提下，ORB-SLAM3和本文算法是效果最佳的算法，在完成任務(wù)的同時(shí)擁有最低的ATE，僅為0.025和0.026。在VR2_03_difficult序列中，ORB-SLAM2不能完成基本任務(wù)，在運(yùn)行過程中會(huì)持續(xù)無法估計(jì)相機(jī)位姿，在局部BA后產(chǎn)生極大的偏差；ORB-SLAM3是對(duì)比算法中效果最好的，達(dá)到了0.037的ATE，而本文算法實(shí)現(xiàn)了僅為0.032的ATE。

圖11和12更直觀地展示了在中等和困難序列VR2_02_medium和VR2_03_difficult中本文算法的誤差在整條軌跡上的分布。圖11代表兩個(gè)序列的APE，圖12代表兩個(gè)序列的RPE（relative pose error，相對(duì)位姿誤差）。圖中從深藍(lán)色到紅色的漸變色帶代表誤差大小，色帶頂部和底部的數(shù)值為誤差的最大值與最小值（見電子版）。軌跡顏色越接近深藍(lán)色表示軌跡與真實(shí)值的誤差越小。

通過圖11、12可以看出，本文算法估計(jì)的軌跡誤差在整條軌跡上分步相對(duì)均勻，整體誤差較小。其中誤差較大的位置是飛行器或手持相機(jī)突然轉(zhuǎn)向和光線變化較大區(qū)域，相機(jī)平緩運(yùn)動(dòng)時(shí)誤差極小，整體來看可以較好地完成位姿估計(jì)任務(wù)。

圖13展示了VR2_02_medium和VR2_03_difficult序列的軌跡估計(jì)情況。由于ORB-SLAM3的時(shí)間戳和真實(shí)值不一致，導(dǎo)致其提出運(yùn)行結(jié)果不能與真實(shí)值和其他算法進(jìn)行直接比較。故本文將提出的算法與兩種對(duì)比算法中性能突出的ORB-SLAM2和VI ORB-SLAM進(jìn)行更直觀的估計(jì)軌跡對(duì)比。圖中虛線代表真實(shí)值，藍(lán)色實(shí)線代表本文的算法，綠色實(shí)線和紅色實(shí)線分別代表ORB-SLAM2和VI ORB-SLAM算法（電子版）。

由圖13（b）的軌跡可以看出，在VR2_02_medium序列上，本文算法與真實(shí)軌跡貼合得最好，ORB-SLAM2和VI ORB-SLAM算法互有勝負(fù)，但總體效果遜色于本文算法。圖13（a）所示的VR2_03_difficult序列上可以看出，綠色的ORB-SLAM2算法屬于跟蹤失敗的狀態(tài)；紅色的VI ORB-SLAM雖然能完成軌跡估計(jì)的任務(wù)，但與真實(shí)軌跡的貼合程度不如本文的算法（電子版）。

由以上對(duì)比可以看出，本文的算法更貼近于真實(shí)軌跡，精度比ORB-SLAM2和VI ORB-SLAM更高，軌跡的估計(jì)效果更好，在較為困難的場(chǎng)景中具有更強(qiáng)的魯棒性。

接下來在誤差統(tǒng)計(jì)特性方面使用VR2_02_medium和VR2_ 03_difficult序列進(jìn)行進(jìn)一步對(duì)比。由于ORB-SLAM2在VR2_03_difficult序列運(yùn)行過程中丟失了相機(jī)位姿，所以只對(duì)比本文算法和VI ORB-SLAM兩種算法。圖14展示了在運(yùn)行階段的實(shí)時(shí)APE，可以看出本文算法整體上優(yōu)于ORB-SLAM2和VI ORB-SLAM兩種對(duì)比算法，在兩種算法的APE產(chǎn)生較大波動(dòng)的時(shí)候依然較為穩(wěn)定。圖15展示了整體的均方根誤差、誤差中值、誤差均值、誤差極值及誤差平方和，本文算法在各類指標(biāo)中均為最優(yōu)，誤差最大值小于另外兩種算法。圖16給出了絕對(duì)位姿誤差分布范圍，本文的算法誤差分布集中，效果穩(wěn)定，展現(xiàn)出較強(qiáng)的魯棒性。從圖17箱線圖展示的數(shù)據(jù)分布情況可以看出，本文算法的APE中位數(shù)和平均數(shù)小于其他算法，分布更加均勻。綜合來看，本文算法的表現(xiàn)優(yōu)于另外兩個(gè)算法。

綜上，通過實(shí)驗(yàn)對(duì)比可以發(fā)現(xiàn)，本文算法能更好地解決低紋理區(qū)域的位姿估計(jì)問題，并且提供了更好的IMU初始化效果和特征提取匹配效果，在不同難度的數(shù)據(jù)集上均有較好的表現(xiàn)，并且隨著難度上升，算法由于融合了線特征和IMU數(shù)據(jù)，有效提升了算法的精度和魯棒性，在快速運(yùn)動(dòng)及昏暗場(chǎng)景等復(fù)雜條件下均有良好表現(xiàn)。

5 結(jié)束語

本文提出了一種基于ORB-SLAM框架，融合IMU數(shù)據(jù)和線特征的視覺慣性SLAM算法。在IMU初始化環(huán)節(jié)，得益于視覺慣性分步聯(lián)合初始化的方法，本文方法相比傳統(tǒng)的純慣性初始化和隱式視覺慣性比例尺等初始化方法具有更優(yōu)秀的初始化效果，并在初始化階段對(duì)傳感器的不確定性進(jìn)行處理，提高了整體SLAM算法的精度。在特征提取環(huán)節(jié)，采用基于LBD描述子的EDLines線特征匹配算法來取代傳統(tǒng)的LSD算法，提高了線特征提取的速度與質(zhì)量，為整體的SLAM算法提供了更好的實(shí)時(shí)性、魯棒性以及更高的精度。實(shí)驗(yàn)表明，在相機(jī)快速移動(dòng)和場(chǎng)景紋理信息較少的情況下，本文的SLAM算法相比對(duì)比的SLAM算法具有更好的定位、追蹤和建圖效果，誤差更小，魯棒性更強(qiáng)。在未來的工作中，計(jì)劃進(jìn)一步利用視覺傳感器提供的豐富場(chǎng)景信息，繼續(xù)優(yōu)化幀間特征匹配，提高SLAM算法的性能，并通過創(chuàng)建稠密點(diǎn)云，完成高質(zhì)量地圖的生成，并將其應(yīng)用到機(jī)器人的路徑規(guī)劃與導(dǎo)航中。

參考文獻(xiàn)：

[1] "Klein G， Murray D. Parallel tracking and mapping for small AR workspace［C］//Proc of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.Piscataway，NJ：IEEE Press，2007：225-234.

［2] Andrew J D， Ian D R， Nicholas D M， "et al . MonoSLAM： real-time single camera SLAM［J］. IEEE Trans on Pattern Analysis and Machine Intelligence ，2007， 29 （6）：1052-1067.

［3] Engel J， Koltun V， Cremers D. Direct sparse odometry［J］. IEEE Trans on Pattern Analysis and Machine Intelligence ，2018， 40 （3）：611-625.

［4] Cadena C， Carlone L， Carrillo H， "et al . Past， present， and future of simultaneous localization and mapping： towards the robust perception age ［J］. IEEE Trans on Robotics ，2016， 32 （6）：1309-1332.

［5] Mur-Artal R， Montiel J M M， Tardós J D. ORB-SLAM： a versatile and accurate monocular slam system［J］. IEEE Trans on Robotics ，2015， 31 （5）：1147-1163.

［6] Mur-Artal R， Tardós J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo， and RGB-D cameras［J］. IEEE Trans on Robotics ，2017， 33 （5）：1255-1262.

［7] Carlos C， Richard E， Juan J G R， "et al . ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM［J/OL］. IEEE Trans on Robotics ，2021：article ID 9440682.（2021-04-23）［2021-08-24］.https：//doi.org/10.1109/TRO.2021.3075644.

［8] Pumarola A， Vakhitov A， Agudo A， "et al .PL-SLAM： real-time monocular visual SLAM with points and lines［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2017：4503-4508.

［9] 謝洪樂，陳衛(wèi)東，范亞嫻，等.月面特征稀疏環(huán)境下的視覺慣性SLAM方法［J］.航空學(xué)報(bào)，2021， 42 （1）：305-315.（Xie Hongle， Chen Weidong， Fan Yaxian， "et al . Visual-inertial SLAM in featureless environments on lunar surface［J］. Acta Aeronautica et Astronautica Sinica ，2021， 42 （1）：305-315.）

［10] 曹力科，肖曉暉.基于卷簾快門RGB-D相機(jī)的視覺慣性SLAM方法［J］.機(jī)器人，2021， 43 （2）：193-202.（Cao Like， Xiao Xiaohui. A visual-inertial SLAM method based on rolling shutter RGB-D ca-meras［J］. Robot ，2021， 43 （2）：193-202.）

［11] Weiss S M. Vision based navigation for micro helicopters［J/OL］. ETH Zurich ，2012：article ID 41345331.（2012-01-14）［2021-08-24］.https：//doi.org/10.3929/ethz-a-007344020.

［12] "Mur-Artal R， Tardós J D. Visual-inertial monocular SLAM with map reuse［J］. IEEE Robotics and Automation Letters ，2017， 2 （2）：796-803.

［13] Qin Tong， Li Peiliang， Shen Shaojie. VINS-Mono： a robust and versatile monocular visual-inertial state estimator［J］. IEEE Trans on Robotics ，2018， 34 （4）：1004-1020.

［14] Qin Tong， Shen Shaojie. Online temporal calibration for monocular visual-inertial systems［C］//Proc of IEEE International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2018：3662-3669.

［15] Todd L， Salah S. Visual-inertial-aided navigation for high dynamic motion in built environments without initial conditions［J］. IEEE Trans on Robotics， 2012， 28 （1）：61-76.

［16] Christian F， Luca C， Frank D， "et al . On-manifold preintegration for real-time visual-inertial odometry［J］. IEEE Trans on Robotics ，2017， 33 （1）：1-21.

［17] Galvez-Lpez D， Tardós J D. Bags of binary words for fast place recognition in image sequences［J］. IEEE Trans on Robotics ，2012， 28 （5）：1188-1197.

［18] Gioi R， Jakubowicz J， Morel J M， "et al . LSD： a line segment detector［J］. Image Processing on Line ，2012， 2 （4）：35-55.

［19] Cuneyt A， Cihan T. EDLines： a real-time line segment detector with a 1 detection control［J］. Pattern Recognition Letters ，2011， 23 （13）：1633-1642.

［20] Lilian Z， Reinhard K. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency［J］. Journal of Visual Communication and Image Representation ，2013， 24 （7）：794-805.

［21] "Lowe D G. Distinctive image features from scale-invariant keypoints［J］. International Journal of Computer Vision ，2004， 60 （2）：91-110.

［22] Wan Zhiheng， Wu Fuchao， Hu Zhanyi. MSLD： a robust descriptor for line matching［J］. Pattern Recognition ，2009， 42 （5）：941-953.

［23] Leutenegger S， Lynen S， Bosse M， "et al . Keyframe-based visual-inertial odometry using nonlinear optimization［J］. The International Journal of Robotics Research ，2015， 34 （3）：314-334.

［24] "Rainer K， Giorgio G， Hauke S， "et al . G2o： a general framework for graph optimization［C］//Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ：IEEE Press，2011：3607-3613.

［25] Lukas V S， Vladyslav U， Daniel C. Direct sparse visual-inertial odometry using dynamic marginalization［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2018：2510-2517.

［26] Agostino M. Closed-form solution of visual-inertial structure from motion［J］. International Journal of Computer Vision ，2014， 106 （2）：138-152.