一種基于光流-線特征的單目視覺-慣性SLAM算法

夏琳琳，沈 冉，遲德儒，崔家碩，蒙 躍

（東北電力大學 自動化工程學院，吉林 132012）

作為智能機器人的核心技術，自主導航為移動機器人實現各項高級功能（如人機互動、多機協同等）奠定了基礎，自主導航能力也逐漸成為衡量移動機器人性能的重要指標[1,2]。傳統的慣性導航系統、全球衛星導航系統很難獨立完成相關任務[3]，而基于視覺的同時定位與地圖創建（Simultaneous Localization and Mapping，SLAM）則既能實現高精度的導航定位，又可以實時構建周圍環境的地圖，為自主導航提供了新的解決方案。

經過三十余年的發展，視覺SLAM 已經逐步趨于成熟，一批開源算法相繼問世，表1總結了近年經典的視覺SLAM 研究方案。這些方案主要分為兩類：一類是基于圖像特征點的間接法（特征點法），一類是基于圖像灰度變化的直接法。特征點法依靠人工設計的局部圖像特征，通過檢測、匹配與追蹤前后兩幀圖像中的特征點，來獲取相機的位姿；直接法采用最小化光度誤差來尋求圖像中像素間的對應關系，避免了特征提取和匹配，計算效率優于特征點法，且可適用于特征缺失（弱紋理或重復紋理）的環境。

表1 經典開源視覺SLAM 算法Tab.1 Classic open-source visual SLAM algorithms

近年，視覺SLAM 已轉向為以非線性優化理論為主導的批量狀態估計，逐步形成了前端實時跟蹤相機位姿、后端優化歷史相機位姿和環境地圖的兩級結構。研究者一邊在陸續開源的相對成熟的算法框架中獲得研究上的極大便利，一邊也在積極探索并提出眾多工程性的解決方案。這其中，越來越多的研究者偏向于視覺SLAM 前端，即視覺里程計（Visual Odoemtry，VO）的研究。在已有的代表性成果方面，比較著名的有R.Wang與J.Engel等人提出的基于單目或雙目相機的稀疏直接里程計DSO（Direct Sparse Odometry）[8,9]及C.Forster 等人提出的融合特征點法和稀疏直接法的視覺里程計SVO（Semi-direct VO）[10]。

上述VO 改進方案旨在克服相機在短時、較高速運動情況下出現的運動模糊、累積漂移及定位失敗等問題。為滿足移動機器人長航時精確定位和短時快速運動下穩定導航的任務需求，將視覺信息與慣性導航進行數據融合的視覺慣性里程計（Visual Inertial Odometry，VIO）已成為SLAM 工業化應用的主要方向。早期的研究者沿用機器人學中的濾波理論融合慣性測量單元（Inertial Measurement Units，IMU）與相機信息，以處理視覺SLAM 的并行狀態估計問題：多狀態約束卡爾曼濾波器（Multi-State Constraint Kalman Filter，MSCKF）主要通過將特征點轉化為相機位姿間的幾何約束，首次實現了視覺-慣性緊耦合算法[11]。文獻[12]中的ROVIO（Robust VIO）算法則將圖像光度誤差作為Kalman 濾波器觀測更新項，利用多級圖像塊跟蹤狀態，采用緊耦合方式聯合估計IMU 偏差與相機的外參。由于ROVIO 采用地標參數化的逆深度估計，故不需要明顯的單目遲延初始化過程。

自2013年，隨著稀疏矩陣和非線性優化理論的逐步完善[13,14]，更多的學者采用非線性優化的方式來求解SLAM 問題—如S.Leutenegge 等人[15]提出的關鍵幀機制下的非線性優化慣性視覺里程計OKVIS（采用圖像特征點檢測），其通過構建聯合優化IMU 誤差項與地標重投影誤差項的非線性函數、限制進入關鍵幀窗口中的狀態變量規模，提升了SLAM過程的實時性。近期，香港科技大學的沈劭劼團隊[16]開源了VINS-Mono 算法（前端提取并跟蹤圖像Fast 角點），其通過高效的初始化、緊耦合非線性優化方法得到高精度的VIO，并利用回環檢測、全局位姿圖優化等機制使VINS-Mono 具有良好的魯棒性，與OKVIS 相比定位精度提高了一倍。

綜上所述，目前已有的SLAM 方案主要采用相對成熟的特征點法進行圖像追蹤。這一方法在短時高速運動或弱紋理環境中的應用仍面臨計算耗時、實時性有限等挑戰，為了滿足機器人載體在上述環境中的導航、定位需求，本文基于VINS-Mono 的算法框架，結合融合點、線特征的VIO 研究，提出了一種在弱紋理環境中基于光流-線特征的單目視覺-慣性SLAM 算法（Particle-Line Visual Inertial SLAM,PL-VINS）。算法首先提取圖像中的Shi-Tomas 角點和LSD（Line Segment Detection）線特征，在視覺初始化的基礎上，通過視覺、慣性聯合初始化過程得到系統初始時刻的狀態估計值；其次，構建PL-VINS 的后端優化目標函數及滑動窗口邊緣化策略，通過非線性優化求解載體的位姿；在此基礎上，為應對隨增量定位過程不斷增長的累積誤差，建立回環檢測和重定位機制，并通過全局位姿優化保證載體軌跡和地圖的全局一致性。

1 系統框架搭建

本文以緊耦合方式，利用非線性優化方法實現視覺與慣性信息的深度融合，算法包含五部分，分別為：測量數據預處理、系統初始化、后端滑動窗口模型優化、回環檢測與重定位及全局位姿圖優化。PL-VINS系統算法框架如圖1所示。

圖1 PL-VINS 系統算法框架Fig.1 The algorithmic framework of PLP-VINS

● 測量數據預處理：分為圖像處理和IMU 數據處理。圖像處理時，分別提取圖像中Shi-Tomas 角點及LSD線特征，利用金字塔LK（Lucas-Kanade）光流法跟蹤相鄰幀的特征點，通過隨機采樣一致性去除異常點，將有效特征點存儲到圖像序列中，并導入后端進行優化求解；而對于圖像中檢測到的LSD 線特征則利用LBD（Line Band Descriptor）描述子將其與前一幀中線段進行匹配。針對IMU 數據則進行預積分處理，從而得到當前時刻的速度、位姿。

● 系統初始化：單目視覺傳感器無法直接獲得準確的圖像尺度信息，為此，本文將視覺與慣性聯合初始化以獲取尺度信息。先利用視覺SFM（Struct From Motion）求解兩幀圖像間的位姿及3D 點逆深度，隨后通過IMU 預積分模型所產生的偏移誤差去修正陀螺儀數據，并根據其與視覺運動信息的關系，構建優化函數求解出初始的速度、尺度、重力及偏置等。

● 后端滑動窗口模型優化：采用基于滑動窗口模型的非線性估計器，根據視覺約束、IMU 約束以及回環約束構建聯合優化函數并求解出滑動窗口內所有幀間位置、速度、旋轉及偏置等。

● 回環檢測與重定位：通過DBoW 詞袋模型和BRIEF 描述子進行回環檢測，建立局部滑動窗口與回環候選幀之間的鏈接。如存在回環，則利用重定位過程維持當前滑動窗口與過去時刻的位姿圖對齊，并將全部回環的位姿作為常量，利用所有IMU 測量值、局部視覺測量以及從回環中提取的特征對應值，共同優化滑動窗口，從而降低系統的累積誤差及計算量。

● 全局位姿圖優化：重定位之后局部滑動窗口與過去時刻位姿對齊，而視覺慣性信息可以提供橫滾角和俯仰角數據，故只存在 4 個自由度（Degrees-of-Freedom，DOF）誤差（即三軸位置誤差和航向角誤差）。只需在位姿圖中添加關鍵幀并對其4DOF 進行優化，即可保證全局軌跡的一致性。

2 視覺-慣性信息融合方法

2.1 系統初始化

初始化是為了獲取系統在后端優化所需的必要參數和載體初始時刻的狀態初值，主要包括陀螺儀偏置、重力加速度、絕對尺度以及初始時刻前幾個關鍵幀的位姿、速度和三維路標點的位置。其中，絕對尺度和重力加速度在系統運行時幾乎不會發生較大的變化。PL-VINS 系統初始化主要分為兩步：一是純視覺單目初始化，利用視覺SFM 求解出初始時刻內關鍵幀的位姿信息以及三維路標點的位置信息；二是利用視覺-慣性松耦合聯合優化出尺度、重力加速度、相機速度、陀螺儀的零偏信息。

2.2 基于滑動窗口的后端非線性優化

本文采用的是基于滑動窗口模型的非線性優化方法，即保證優化變量的個數維持在一定范圍，通過滑動窗口動態地增加或移除優化變量，只讓當前一段時間內的關鍵幀數據參與到位姿解算過程。

若將滑動窗口中第i時刻完整的狀態向量定義為：

本文將線特征信息引入到視覺-慣性SLAM 中，故目標優化函數包含四類殘差項，分別為邊緣化的先驗殘差項、IMU 測量殘差項、視覺重投影殘差項（包括點特征重投影誤差和線重投影誤差）及回環檢測殘差項。優化目標函數形式如下：

式(3)中，B為窗口內IMU 測量集合，D和L分別為圖像幀中至少兩次觀測的點特征和線特征的集合。代表邊緣化后的先驗信息，Hp是先前優化信息的信息矩陣，為IMU 預積分噪聲項的協方差，、為視覺觀測的噪聲協方差；ρ為抑制離群點的魯棒核函數。本文采用柯西核函數CauchyLoss 作為魯棒核函數，即

采用合適的魯棒核函數可以保證優化過程的平穩性，削弱異常值對狀態變量優化過程的影響。需要特別強調的是，回環檢測部分的殘差只有在出現視覺回環且需要重定位時才將第v幀的位姿和相關特征點作為視覺約束項參與到后端非線性優化的整體目標函數中，并固定第v幀的參數用以優化窗口內其他參數。

本文的PL-VINS 系統采取雙向邊緣化策略來控制滑動窗口內狀態變量，具體策略是：當次新幀為關鍵幀時，邊緣化最舊幀，但將最舊幀的點線路標點和相關聯的IMU 數據轉化為先驗信息，參與優化目標函數，即本小節提到的先驗殘差｛rp,Hp｝；當次新幀為非關鍵幀時，則直接舍掉次新幀和其對應的視覺測量值，但保留其相關聯的IMU 測量值，從而保證IMU 預積分的連貫性。邊緣化過程如圖2所示。

圖2 邊緣化策略示意圖Fig.2 Diagrammatic representation of marginalization strategy

2.3 回環檢測與重定位

回環檢測是SLAM 后端的重要一環。PL-VINS 采用BRIEF 描述子的DBoW2 詞袋進行回環檢測，由于系統前端提取的Shi-Tomas 角點數量有限（通常在70個左右），很難滿足檢測需要。為此，本文對非線性優化后的新關鍵幀重新提取500個FAST角點用于回環檢測，同時對所有新、舊角點進行BRIEF 描述。隨后，依托詞袋模型計算當前幀的相似度分數，并與關鍵幀數據庫中所有歷史幀進行比對。在此基礎上，進行回環一致性檢測，評分大于0.015 則認為是回環候選幀。在檢測到回環后，本文根據BRIEF 描述子對新、舊角點進行鄰域匹配，并對匹配點進行隨機一致性采樣以去除匹配異常點。當匹配點的索引值大于50，則認定該候選幀為正確的回環幀。當檢測到當前幀與歷史幀（設為第v 幀）存在回環關系后，進行緊耦合重定位。

將回環幀的位姿和相關路標點作為視覺約束項，加入到后端非線性優化目標函數中，見式(4)。

式(4)中，?為回環檢測到特征集合，(i,v) 為回環幀v中觀測到的第i個特征，與其他殘差項不同的是，回環幀的位姿將通過下一節中介紹的位姿圖獲得。但如果是第一次重定位，則以過去時刻的VO 估計的位姿為當前回環幀的位姿，并將其作為優化過程的常值。

2.4 全局位姿圖優化

重定位之后，局部滑動窗口移動與歷史位姿對齊，進行全局圖優化則是為了確?；谥囟ㄎ唤Y果對歷史位姿進行全局一致性匹配。由于PL-VINS 系統中載體的水平姿態（橫滾角及俯仰角）完全可測，因此，只有三軸位置和航向角這4個自由度存在累積漂移，需進行位姿圖優化。當關鍵幀從滑動窗口中被邊緣化之后，會被添加進位姿圖中，該幀會作為位姿圖中的一個頂點，通過下述的兩類邊與其他頂點相連接：

① 序列邊

經過前端VIO 解得的兩幀間的相對位姿為：

其中，i、j分別代表邊緣化的關鍵幀和歷史幀，、分別為兩幀之間的相對位置和相對航向。第i幀和第j幀的殘差可以表示為

② 回環邊

對于存在回環關系的兩幀，回環邊只包含4DOF 的相對位姿，可通過重定位的結果得到。所有序列邊和回環邊整體的優化目標函數可表示為

其中，ζ代表序列邊，τ代表回環邊，ρ則采用Huber核函數，定義見式(8)。至此系統完成了全局位姿優化過程。

式中δ為控制誤差降到多小時變為平方誤差的超參數，當Huber 損失s在|s|δ≤時，等價為均方誤差，否則等價為絕對誤差。

3 實驗與分析

本文針對PL-VINS 系統開展了基于公開數據集EuRoc 的性能評估實驗，對其導航定位精度、魯棒性等進行仿真實驗分析與評價。

3.1 仿真實驗環境及實驗平臺

圖3 EuRoc 數據采集硬件系統Fig.3 EuRoc data acquisition hardware system

EuRoc 數據集的采集載體是無人機AscTec Firefly，其所測視覺及慣性數據來自人為裝配的慣性視覺傳感器；其軌跡真值由兩個運動捕捉系統進行輔助測量，分別是Vicon 動作捕捉系統及Leica 激光追蹤器。前者采集六旋翼飛行器的6DOF 位姿變換，后者負責三維位置追蹤。該EuRoc 數據采集硬件系統如圖3所示。

PL-VINS 算法運行的硬件設備為DELL Inspiron 15-3542 型筆記本計算機，CPU 為Intel i5-4210U@1.7GHZ×4，所用操作系統為 Ubuntu16.04LTS，PL-VINS 的整體架構搭載在機器人操作系統ROS（版本為Kinetic1.12.14）上。PL-VINS 系統以C++語言開發，系統架構參考開源算法VINS-Mono。程序主要依托四個開源庫：計算機視覺庫OpenCV3.3.1、矩陣運算庫3.3.1、非線性優化求解庫CeresSlover 以及詞袋模型庫DboW2。地圖可視化部分采用的是ROS 自帶的Rviz三維可視化工具。

3.2 PL-VINS 仿真實驗與分析

3.2.1 點線特征

為展現PL-VINS 系統運行過程中對點、線特征的提取和跟蹤情況，本文截取了運行中的兩幅追蹤圖像，如圖4所示。

圖4 點線特征跟蹤的效果Fig.4 Point and line feature tracking performances

圖4中顏色越紅代表跟蹤的時間越長，圖中存在一定量的紅色線段，而且是點特征所無法有效跟蹤的部分，由此可見點、線融合的特征跟蹤可以使SLAM 算法獲得更豐富、更有效的特征信息。

3.2.2 導航定位精度

實驗選用的EuRoC 數據集主要為MH_01_easy 運動場景序列。鑒于PL-VINS 是單目視覺，本實驗僅采用雙目攝像機左相機拍攝圖像。在同等軟硬件條件下，獨立運行開源算法 VINS-Mono 和本文提出的PL-VINS，將二者結果進行對比，評價PL-VINS 系統的導航定位精度。

圖5為PL-VINS 運行時通過Rviz 可視化界面觀測到的載體飛行軌跡。圖中左上部顯示的是回環檢測情況；左下部是對當前幀提取出點、線特征，進行跟蹤顯示的情況，其中，點、線越紅則說明該特征追蹤的時間越長，屬于良好的路標點；圖的右側則實時展現真實軌跡與估計軌跡之間的差異情況。

圖5 PL-VINS 系統在Rviz 中的可視化界面Fig.5 Visual interface of PL-VINS in Rviz

圖6為PL-VINS 系統在MH_01_easy 序列上的絕對位姿誤差（APE）。讀取顏色條量化結果可知，平均誤差在10 cm 左右，精度上相對良好。

圖6 PL-VINS 在MH_01_easy 序列上的三維絕對位姿誤差Fig.6 3D absolute pose error of PL-VINS on MH_01_easy sequence

圖7 MH_01_easy 在X-Z 軸向投影的軌跡誤差Fig.7 MH_01_easy trajectory error in X-Z axial projection

為進一步細化軌跡誤差，將圖6中的軌跡投影到X-Z 軸所在的二維平面進行細致觀察，如圖7所示。從圖中可以看出，當載體突然轉向或是快速運動時，造成了PL-VINS 估算的位姿出現偏差，即誤差水平平均在40 cm 以內，在平緩運動時誤差可以縮小到1.6 cm，整體來看，PL-VINS 可以較為魯棒地完成位姿跟蹤任務。

圖8為三軸位置誤差，藍線為系統輸出隨時間變化的軌跡，虛線為軌跡真值，兩條線重合會顯示藍色?？梢钥闯?，圖中大部分的預測值與真值重合，說明整體上系統輸出的信息較為準確。同時可知，主要誤差出現在Z 軸，這是由于相機在此時發生了跟蹤較少特征點的模糊運動，而IMU 受偏置和噪聲影響亦輸出了不精確的導航參數，二者此時無法實現有效功能互補，導致精度下降，但由于全局優化的作用，誤差在整體上并未發散。

圖8 三軸位置誤差Fig.8 Three-axis position error

圖9為PL-VINS 系統與開源VINS-Mono 系統的導航誤差統計特性對比圖，量化指標包括均方根誤差、誤差中值、誤差均值、誤差極值及誤差平方和。如圖9（a），PL-VINS 在均方根誤差（控制在25 cm 以內）、誤差中值及誤差均值上的表現明顯優于VINS-Mono，但在誤差極值表現上不如VINS-Mono，究其原因，主要是線特征的引入雖然降低了累積誤差，但卻導致了誤差的趨向多極化，即線特征帶來的誤差形式與點特征并不一致。圖9（b）給出了PL-VINS 與VINS-Mono 的絕對位姿誤差柱狀統計特性比較，不難發現，在整體上，不論是誤差分布范圍還是誤差分布趨勢，PL-VINS 都明顯優于VINS-Mono。

圖9 PL-VINS 與VINS-mono 在MH_01_easy 序列上的誤差統計特性比較Fig.9 Error statistic property comparisons between PL-VINS and VINS-Mono on MH_01_easy sequence

進一步選取兩個不同難度（medium、difficult）的運動場景序列進行實驗，對所設計的PL-VINS 系統的魯棒性進行驗證。對比全局軌跡誤差及APE 各項參數指標，結果分別如圖10、圖11所示。

圖10 MH_03_medium 序列情形Fig.10 MH_03_medium sequence case

圖11 MH_05_medium 序列情形Fig.11 MH_05_medium sequence case

根據上述結果圖可知，PL-VINS 在不同難度數據集上均有良好的表現，特別是在MH_03_medium 序列上，PL-VINS 展現出較VINS-Mono 全面優異的性能。但隨著數據集運行難度的上升，PL-VINS 與VINS-Mono 之間的精度差距逐漸減小，以至于在MH_05_difffcult 序列上，兩者的位姿估算精度幾乎相當。根據MH_05_difffcult 的場景特點—載體快速運動、場景昏暗，可以初步認為在快速運動且光照不利的條件下，PL-VINS 受到了很大的挑戰，這主要是由于PL-VINS 采用了對光照變化比較敏感的光流跟蹤法，且快速運動本身不利于點、線追蹤。但即便在上述快速、非理想光照的天然不利條件下，PL-VINS 依然與VINS-Mono 精度相當，達到了本次設計的要求。

根據表2及表3的實驗數據，可得到如下結論：PL-VINS 將線特征引入到VINS 中，可有效提升系統導航的定位精度（APE 均方根統計值均低于VINS-Mono），且在快速運動及昏暗場景等不良條件下均有良好表現。

表2 EuRoc 數據集上PL-VINS 絕對位姿誤差的統計特性（單位：米）Tab.2 The absolute pose error statistic property of PL-VINS in EuRoc datasets（Unit:m）

表3 VINS-Mono 與PL-VINS 在EuRoc 數據集上的APE 均方根誤差比較（單位：米）Tab.3 APE mean square error comparison of VINS-Mono and PL-VINS in EuRoc datasets（Unit:m）

4 結 論

本文提出一種基于光流-線特征的單目視覺-慣性SLAM 算法PL-VINS，有效抑制弱紋理環境下視覺SLAM 運行不穩定及長航時累積誤差問題。通過將回環檢測誤差、慣性誤差及點線視覺重投影誤差整合，構建目標優化函數，以及制定特定的邊緣化策略，實現視覺SLAM 的后端非線性優化；通過采用BRIEF描述子的DBoW2 詞袋進行回環檢測并進行4DOF 的位姿圖優化，保證系統長期運行時全局軌跡與地圖的一致性。

通過開源數據集EuRoc 進行PL-VINS 性能評估測試，可以得到如下結論：

（1）將線段特征提取、Shi-Tomas 角點提取及光流法相結合是可行的，算法能夠提取到更多的環境特征，在走廊等弱紋理環境下可以提高其對環境的適應性，增強系統的魯棒性；

（2）在點-線SLAM 中引入回環檢測及重定位環節，并將回環檢測殘差加入到優化目標函數中可有效提高算法的定位精度，確保了長航時全局軌跡的一致性；

（3）所設計的PL-VINS 算法在導航定位精度上要高于VINS-Mono，且增強了VINS 在弱紋理環境下的適應性，即在快速運動或昏暗場景等不利的條件下，PL-VINS 仍能正常工作且定位精度與VINS-Mono 相當，這為后續視覺SLAM 研究提供了一定的技術支持與模型參考。