融合點線特征的視覺慣性SLAM算法

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

關鍵詞： "視覺同時定位與地圖構建； 視覺慣性融合； 稀疏建圖； 點線特征提取

中圖分類號： "TP242 """文獻標志碼： A

文章編號： "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 引言

隨著計算機領域的優秀算法不斷涌現，移動機器人越來越智能。同時定位與地圖構建（SLAM）算法是目前的核心技術難點和研究熱點，它的出現使得移動機器人在GPS信號較弱的室內環境中，也能通過搭載相應的傳感器獲取自身狀態和周圍的環境信息，從而實現地圖構建和環境認知，自主地完成特定任務。

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

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

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

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

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

1 系統架構

系統框架由圖1所示的四個線程組成，即跟蹤、局部建圖、回環檢測和全局BA優化。

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

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

回環檢測線程進行位置識別與回環修正。采用DBoW2詞袋重定位系統［17］，實現位置識別和回環檢測的長期數據關聯。其中在位置識別時，進行候選檢測和計算相似變換群Sim3和SE3;在回環修正時，檢測整體回環并通過位姿圖優化來修正累計漂移誤差。

完成回環檢測后進行最后的全局優化并更新地圖。在全局優化中使用了滑動窗口模型，通過最小化目標函數來優化滑動窗口中的所有狀態變量來計算最優結構和運動解，并利用滑動窗口模型來限制需要優化的關鍵幀的數量以此來控制優化規模，防止突然產生大量的關鍵幀優化需求導致優化不及時，造成追蹤丟失和建圖誤差。這種方法提高了系統的實時性，并優化了建圖效果。

2 線特征提取與匹配

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

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

2.1 邊緣特征檢測和線特征提取

本文算法與現有的邊緣檢測算法相比有以下優點：其他邊緣檢測算法檢測到的邊緣像素通常是獨立、不相交、不連續的實體;本算法生成一組干凈的、連續的邊緣片段，即相連的邊緣像素鏈。給定一幅灰度圖像，EDLines分四步進行邊緣檢測：

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

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

c）選取錨點。選取梯度算子最大值的像素作為錨點，因為這些像素有很高的概率成為邊緣元素。

d）形成邊緣像素鏈。從一個錨點開始，計算相鄰像素的梯度大小和方向，通過梯度最大值選取下一個錨點。最后將相鄰錨點進行連接。因為錨點大多數為邊緣元素，由此可形成邊緣像素鏈，完成邊緣特征檢測。

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

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

（1）

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

2.2 相鄰幀匹配

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

為了克服直線檢測的碎片化問題，在直線檢測算法中對 N 張原始圖像通過一組尺度因子和高斯模糊算法進行降采樣，由此生成圖像的尺度空間金字塔。

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

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

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

接下來計算每條頻帶的描述子 BD j， 進而構成LBD描述子：

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

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

然后計算 BD j。對于B j的第k行，將該行內四個 方向上的像素梯度進行累加可得，如式（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）

其中高斯權重 λ=f g（k）f l（k ）。

通過疊加頻帶 B j中所有行的這四個累積梯度，頻帶描述矩陣 BDM "j構 造如式（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的行數。當j為1或m時，取n=2w，其他情況時n=3w 。

然后利用矩陣 BDM "j的均值向量 M "j和標準差向量 S "j來構造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）

通過對 LBD 的均值和標準方差部分分別進行歸一化，同時對每個 LBD維度進行約束，得到單元LBD。

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

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

3 視覺慣性傳感器融合

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

3.1 視覺慣性里程計預積分

在純視覺系統中，估計狀態僅包括當前攝像機姿態，而在視覺慣性系統中，假設相機和IMU坐標系的轉換是已知且固定的，仍需要計算額外的狀態向量。

狀態向量 s "i由世界坐標系中的相機方向D i、位置P i、速度V i，以及加速度計偏差Ba i和陀螺儀偏差Bg i 構成，如式（6）所示。

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

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

3 。

K k表示時間k之前所有關鍵幀的集合。估計的所有關鍵幀的狀態如 式（7）所示。

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

系統的輸入是相機和IMU的測量值。 C i是關鍵幀i處的相機測量值，χ ij表示在兩個連續關鍵幀i和j 之間的IMU測量集合。到時間 k為止的測量值集合為Z k {C i，χ ij} （i，j）∈K k 。

對于視覺慣性SLAM，按照文獻［15］中的理論，需要預先整合連 續幀i和i+1 之間的IMU測量數據，然后將得到的IMU預積分的旋轉、速度和位置測量向量分別記為Δ ω i，i+1、 Δ υ i，i+1、 Δ ρ i，i+1，由此得到整個測量向量的信息矩陣 Σ "ψ i－1，i。基于這些預積分項和狀態，采用文獻［16］中慣性剩余項γ ψ 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）

偏差估計在初始化過程中會發 生少量δb的變化。一種解決方案是當偏差發生變化時重新計算測量值，然而這會增加較大的計算量。所以，給定偏差更新b← +δb，可 以使用一階展開來更新測量值，如式（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

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

γ ij=τ ij－Π（ Euclid Math OneJAp

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

其中 Π ： Euclid Math TwoRAp

3→ Euclid Math TwoRAp

n為對應相機模型的投影函數;τ ij為j點在圖像i處的觀測值，其信息矩陣為 Σ "ij;用 Euclid Math OneTAp

表示從IMU信息到相機模型的剛性轉換;⊕是特殊歐氏群SE（3）對 Euclid Math TwoRAp

3元素的變換運算。

結合慣性殘差項和視覺殘差項，視覺慣性SLAM可描述為基于關鍵幀的最小化問題［23］。給定一組 k+1關鍵幀及其狀態F k {F 0，F 1，…，F k}和一組 3D 點及其狀態P {P 0，P 1，…，P l－1}，則視覺 慣性聯合優化問題可表述為

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點 j 的一組關鍵幀。

對于重投影誤差，使用G2o［24］提供的魯棒核函數ρHub來降低誤匹配率；對于慣性殘差項，因為不存在誤關聯的問題，所以可以將其忽略。

3.2 視覺慣性分步聯合初始化

在本節中，使用分步聯合初始化的方法對視覺慣性的參數和偏差進行初始化。

1） 純視覺初始化

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

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

2）純慣性初始化

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

V "k={μ， Euclid Math OneGAp

， Euclid Math OneBAp

， "0：k} ""（12）

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

是重力方向；Euclid Math OneBAp

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

因此最大化后驗分布如式（13）所示。

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

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

由馬爾可夫性質可知，某一時刻的圖像測量只依賴于此時的狀態。根據測量之間的標準獨立假設，結合式（6）可將式（13）表示為

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

考慮到測量的獨立性，純慣性最大后驗估計問題可以表示為

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）

最大后驗估計 V ′ k對 應式（15）的最大值，或者負對數后驗估計的最小值。對IMU的預積分和先驗分布取負對數和高斯誤差，負對數后驗估計可以寫成殘差的平方和。最終得到優化問題，如式（16）所示。

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

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

在進行優化時，需要定義一個收斂［24］來更新優化期間的重力方向估計，如式（17）所示。

Euclid Math OneGAp

′= Euclid Math OneGAp

exp （δα" Euclid Math OneGAp

，δβ" Euclid Math OneGAp

，0） ""（17）

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

在完成慣性優化后，幀姿態和速度以及3D地圖點將按照估計的比例尺進行縮放，并進行旋轉以使 Z 軸與估計的重力方向進行對齊。在此期間，將更新各類偏差并重復進行慣性測量單元的預積分，以此來減少線性化誤差。

3）視覺慣性聯合初始化

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

在這個初始化階段，所有關鍵幀具有相同的偏差，并且先驗信息與純慣性初始化階段中的相同。忽略傳感器不確定性的話會產生很大的不可預測的誤差，因此需要將傳感器的不確定性在初始化階段進行處理。

4 實驗與評估

實驗分為兩部分進行。首先與LSD算法對比來評估EDLines線特征提取匹配算法的性能。接著在公開數據集上評估本文算法和六個經典SLAM算法的性能，這六個SLAM算法包括三個純視覺算法和三個視覺慣性算法。

4.1 圖像匹配

4.1.1 線特征提取實驗

分別選取公開數據集EuRoC中不同場景中的圖片進行實驗，平均提取的特征數目和運行時間分別如圖4、5所示。

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

4.1.2 特征匹配實驗

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

由圖6、7可以較為直觀地看出，本文的算法誤匹配出現的情況較少，匹配準確率更高。但如果對此結果不加處理，同樣會產生較多的誤匹配和冗余匹配，浪費計算資源。因此可以引入RANSAC算法改善這一問題。

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

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

下面的一組圖展示了系統運行時對點線特征的提取、跟蹤情況以及建圖效果。圖9表示在系統運行過程中相機處于黑暗的場景時點特征和線特征的提取情況。圖10分別表示建圖時只生成點特征、線特征和融合點線特征的三種情況，可以看出點線特征融合建圖效果更為直觀。

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

4.2 定位精度實驗

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

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

在MH_04_difficult和MH_05_difficult困難序列中，微型飛行器會在光線變化更大或更空曠的位置采集圖像，并且初始化時間更短，采集過程中設備移動速度更快。這對特征提取和初始化部分提出了更高的要求。VR1_02_difficult和VR2_03_difficult序列是最為困難的兩個序列，因為它們是通過手持相機進行采集的，并且在相機運動過程中包含大量的無規則高速運動，產生了大量模糊的圖像，對特征提取造成較大的干擾。

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

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

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

通過圖11、12可以看出，本文算法估計的軌跡誤差在整條軌跡上分步相對均勻，整體誤差較小。其中誤差較大的位置是飛行器或手持相機突然轉向和光線變化較大區域，相機平緩運動時誤差極小，整體來看可以較好地完成位姿估計任務。

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

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

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

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

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

5 結束語

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

參考文獻：

[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] 謝洪樂，陳衛東，范亞嫻，等.月面特征稀疏環境下的視覺慣性SLAM方法［J］.航空學報，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相機的視覺慣性SLAM方法［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.