基于點線特征的視覺慣性SLAM算法研究

摘要：針對目前單目視覺SLAM易受光照、環境、紋理影響等問題，文章提出一種基于點線特征的單目視覺慣性SLAM（Simultaneous Localization and Mapping）方法。首先，在視覺慣性系統VINS-Mono的基礎上，提取EDLines線特征，并采用LBD描述子匹配線特征；其次，在線特征擬合階段，采用RANSAC方法剔除外點，提高擬合精度；最后，根據所提取的點線特征數量，提出特征篩選策略，保證系統的實時性和定位精度。通過在數據集上的實驗分析表明，該算法相較VINS-Mono算法的魯棒性及在定位精度上均有一定的提高，其在實際應用中具有廣闊的潛力。

關鍵詞：單目視覺慣性；點線特征；特征篩選

中圖分類號：TP391.41 """""""文獻標識碼：A"""""""""文章編號：1672-4437（2024）04-0048-06

0 引言

移動機器人技術近年來備受關注，其應用場景也逐漸復雜多樣。在人工智能興起之后，移動機器人逐漸向自主化和智能化進步。面對復雜環境，移動機器人如何根據搭載的傳感器獲取周圍環境信息實現準確定位是該領域的核心問題，其定位的精度、實時性及穩定性均會對移動機器人應用產生重要影響。

同步定位與建圖技術的發展可以追溯到1986年，當時在無人機、自主車輛和機器人等領域出現了對自主導航技術的迫切需求。Smith等^[1]首次提出了空間不確定性估計的概念，為后續SLAM技術的研究奠定了基礎。同步定位與建圖^[2]方法根據所使用傳感器種類的不同，可以將SLAM技術分為視覺SLAM和激光SLAM。其中視覺慣性SLAM因價格便宜、定位效果好備受社會關注：Leutenegger等于2015年提出了OKVIS方法^[3]，利用Harris角點^[4]和BRISK描述子^[5]進行前端匹配跟蹤，后端采用滑動窗口進行優化，同時處理視覺和IMU信息。為了降低計算量和消除重復積分，Foster等在2016年提出了IMU預積分理論^[6]，融入SVO框架^[7]，實現視覺和慣性信息緊密耦合。2017年，Mur-Artal等人結合ORB-SLAM^[8]和IMU預積分理論提出了VIORB-SLAM^[9]，通過閉環檢測減少累積誤差。同年，香港科技大學Shen S J團隊提出了VINS-Mono^[10]，基于滑動窗口優化的單目視覺慣性SLAM系統，通過視覺慣性對齊進行初始化和回環檢測，優化全局位姿圖消除整體漂移，在室內和室外場景驗證了高定位精度。2019年，該團隊基于VINS-Mono提出了VINS-Fusion^[11]，將VINS-Mono拓展為雙目與IMU、GPS融合。

移動機器人能在正常環境中提取特征點并進行跟蹤，確定自身定位，但其在某些復雜環境下，如光照較弱的弱紋理場所，往往提取不到足夠的特征點，因此僅僅依靠點特征可能無法滿足移動機器人的定位需求，進而導致定位和建圖的準確性下降甚至系統無法運行。

針對上述問題，本研究基于VINS-Mono系統框架，在視覺前端部分加入EDLines線特征^[12]，并在線段擬合階段改用RANSAC方法剔除外點，然后利用LBD描述子進行線特征匹配；對獲取到的點線特征采取特征篩選策略，設定數量閾值，解決復雜環境下特征提取不足的問題。根據上述改進方法，構建基于點線特征的視覺慣性SLAM系統。實驗表明，該方法能有效提高系統定位精度。

1 系統框架

系統采用點線融合和非線性優化方法，實現移動機器人的自身定位，系統框架如圖1所示。首先，在圖像幀中提取和跟蹤點線特征，并將提取到的特征進行篩選；其次，獲取IMU數據進行IMU預積分，通過初始化獲取關鍵參數；最后，利用點線視覺重投影殘差和IMU預積分殘差構建非線性優化函數，通過最小化殘差獲取精確的自身定位信息。

2 視覺慣性系統模型

2.1 單目視覺模型

單目相機所使用的模型即針孔相機模型，如圖2所示：為相機坐標系，根據右手定則，O為相機的光心即針孔模型中的針孔、軸指向相機前方、軸向右、軸向下；為物理成像平面坐標系；點和點為對應的空間點和成像點；?為焦距即物理成像平面到光心的距離；為世界坐標系。

圖2 針孔相機模型

隨后將點從物理成像平面轉到像素平面：像素坐標系原點位于圖像左上角，軸向右與軸平行，軸與軸平行，即像素坐標系與成像平面之間相差了一個縮放和原點得平移。假設像素坐標在軸上縮放了倍，在軸上縮放了倍，原點平移了。那么，的坐標與像素坐標的關系為：

將其寫成齊次坐標并換成矩陣形式：

其中把由參數組成的矩陣稱為相機的內參數矩陣；為外參矩陣，由相機系到世界系之間的旋轉和位移組成。

2.2 慣性模型

IMU作為慣性傳感器，通過加速度計和陀螺儀測得線性加速度和旋轉角速度，以及在測量時產生的隨機游走誤差和白噪聲，具體模型如下：

其中為IMU坐標系到世界坐標系的轉換，表示從IMU坐標系到世界坐標系的旋轉，為隨時間緩慢變化的隨機游走噪聲，為服從高斯分布的白噪聲，為重力加速度在世界坐標系下的表示。且該模型假設運行區域是個平面，不考慮地球自轉，的方向固定且大小固定，即認為世界坐標系W是慣性系。

根據運動學方程可得到從時間到時間的狀態：

其中為位移、為速度、為旋轉量，噪聲項為的離散形式。

由于VINS采用了BA優化^[13]的思想，每次優化都會導致IMU重新積分，這一過程會增加系統的計算時間，進而推出IMU預積分理論。利用IMU預積分公式，分離出測量值中的噪聲項，根據測量值和真實值計算出IMU預積分殘差，構建IMU約束并優化狀態量。

2.3 點特征參數化

環境中的路標點一般是基于世界坐標系使用三個參數進行參數化，但這種參數化方式在后端優化中增加了計算量，并且隨著特征點的深度增加，優化的誤差也會更大。因此本研究采用逆深度對3D點進行參數化，即使用一個參數實現點特征的參數表示。

當相機得到一幀圖像后，可以得到特征點在相機坐標系下的坐標為、相機內參矩陣為，則可以得到特征點的齊次像素坐標：

其中為特征點的逆深度、相機坐標可通過觀測得到。通過式（5）即可用一個參數表示一個路標點，將三個優化變量減少為一個優化變量，并且使用逆深度也可以減小在深度過大時的誤差。

3 視覺慣性系統模型的改進

3.1 EDLines線特征的提取及優化

在弱紋理環境下，單純依靠點特征容易導致定位失敗，因此本系統在視覺前端融入線特征，以點線融合的方式保證在復雜環境下的穩定定位。本研究使用EDLines算法提取圖像中的線特征，并對其進行改進。

EDLines是一種無參數的線段檢測器，具備高效、快速特點。其原理如下：首先，對輸入的灰度圖像進行濾波處理，得到平滑的圖像；然后，計算圖像中每個像素的梯度方向和梯度值；接著，根據梯度圖中梯度值的峰值點確定錨點；最后，通過連接這些錨點，并將它們繪制成線段，完成邊緣的提取。

定義線特征提取公式為：

其中A為一條線段，n為A的線段長度，k為A上與A方向一致的點的數目，為N×N圖像幀中所有可能的線段數量，方向一致定義為線段方向與像素梯度方向處于22.5°之內，即p取1/8。若NFAlt;1，則保留線段，否則丟棄。此方法同時能篩選掉短線段。

在線段擬合階段，EDLines算法采用的是最小二乘法^[14]，該方法利用局內和局外所有點計算模型。由于提取的錨點存在較多局外點，進而會影響直線擬合的準確性，所以本研究采用RANSAC^[15]算法，僅利用局內點的數據擬合線段，如圖3所示，擬合出的直線精確度更高。

將本系統在EuRoC數據集中運行，可得到如圖4所示的效果圖。其中（a）采用最小二乘法擬合圖中線特征，（b）采用RANSAC法擬合圖中線特征，可以看出，改進后的EDLines算法剔除了較多的不穩定線段，保留了較長的穩定線段。

3.2 特征篩選策略

由于系統加入了線特征，增加了特征提取的計算量，為了減少引入線特征后增加的計算量，本研究提出一種針對點特征和線特征的數據選擇策略。該策略通過設置線特征數量的閾值，將點特征作為主要參考，而線特征作為輔助。設定閾值時，考慮系統的實時性能和定位精度之間的平衡。較高的線特征閾值雖然可以提高特征匹配的準確性，但會增加計算復雜度，降低實時性；而較低的線特征閾值則相反。通過動態調節閾值數量，適應不同環境下的特征數量變化。例如，在點特征豐富的場景中，減少線特征閾值以降低計算復雜度；在點特征稀少的環境中，提高線特征閾值以確保足夠的特征用于匹配。

將本系統在COCO數據集多個場景（如室內、室外、特征豐富、特征稀少等）中進行多次實驗，在不同點特征數量下選取不同線特征閾值，系統地調整閾值并記錄實時性能與特征數量，尋找最優閾值范圍（如圖5）。

最終對比不同數據集序列的特征處理時間與特征數量，得出特征篩選策略（見表1），根據圖像幀中提取出的點特征數目來決定后續提取的線特征數量。

如圖6所示，（a）（b）是點特征較多的場景，其中（a）圖提取了較多線特征，會影響系統實時性，因此采用特征篩選策略減少圖中線特征的數量，改進后如圖（b）所示；（c）（d）是點特征較少的弱紋理場景，其中（c）圖提取了較少的點特征，會影響系統精度，因此采用特征篩選策略增加圖中線特征的數量，改進后如圖（d）所示。

根據圖6可統計出具體的特征提取數量，如表2所示，紋理較強的場景中，線特征的提取數量減少了一倍多；紋理較弱的場景中，線特征的提取數量增長了一倍多。由此可以看出，采用該特征篩選策略，能盡量均衡系統提取到的特征，進而保證實時性和定位精度。

4 實驗結果與分析

為驗證本算法的精度，采用公開的EuRoC數據集^[16]進行相關性能分析。EuRoC數據集包括在微型飛行器上收集到的11個飛行序列數據。該數據集的圖像采集頻率為20Hz，IMU的采集頻率為200Hz，并且該數據集提供真實軌跡。

本研究采用EuRoC數據集中Machine Hall場景下的MH_01_easy、MH_03_medium、MH_05_difficult來驗證算法的定位精度，采用無人車實驗平臺記錄的序列驗證算法的定位精度。

為測試本研究的定位精度，將系統與經典算法VINS-Mono進行對比，在EuRoC數據集上測試兩者的絕對軌跡誤差（Absolute Trajectory Error，ATE）。所選取的三個序列場景從簡單到困難，周圍環境的紋理依次變弱，能較合適地驗證本算法改進視覺特征提取的有效性。圖7為在三個序列下VINS-Mono與本研究算法的軌跡誤差對比，其中（a）（b）、（c）（d）、（e）（f）分別為VINS-Mono算法和本研究算法在EuRoC數據集MH_01_easy、MH_03_medium和MH_05_difficult序列下的絕對軌跡誤差，可以看出本研究算法的定位誤差要低于VINS-Mono算法（見表3）。從表3可以看出，與VINS-Mono算法相比，本研究算法的絕對軌跡誤差均有不同程度的減少，其中均方根誤差（RMSE）平均減少了約35%。

5 結論

本研究提出了一種基于點線特征融合的單目視覺慣性SLAM方法，該方法結合了點線視覺特征，通過改進線特征提取方式以及特征篩選策略，提高了移動機器人的定位精度。其中在所選EuRoC數據集中均方根誤差平均減少了35%。

然而，這種方法仍然存在一些局限性。當周圍環境存在較多運動物體時，由于提取了較多的點線特征，將這些特征用于移動機器人定位，反而會降低定位精度。因此，如何在高動態環境下，保證移動機器人高精度的自主定位是下一步要開展的工作。

參考文獻：

[1]SMITH R C， CHEESEMAN P.On the representation and estimation of spatial uncertainty[J].The international journal of Robotics Research，1986， 5（4）：56-68.

[2]田野，陳宏巍，王法勝，等.室內移動機器人的SLAM算法綜述[J].計算機科學，2021，48（09）：223-234.

[3]LEUTENEGGER S， LYNEN S， BOSSE M， et al. Keyframe-based visual-inertial odometry using nonlinear optimization[J].International Journal of Robotics Research，2015，34（3）：314334.

[4]GLOVER A， DINALE A， ROSA L D S， et al.luvharris： A practical corner detector for event-cameras[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2021，44（12）：10087-10098.

[5]LEUTENEGGER S， CHLI M， SIEGWART R Y.BRISK：Binary robust invariant scalable keypoints[C]//2011 International Conference on Computer Vision.IEEE， 2011.

[6]CHANG L， NIU X， LIU T.GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-integration[J].Sensors，2020，20（17）：4702.

[7]FORSTER C， PIZZOLI M， SCARAMUZZA D.SVO：Fast semi-direct monocular visual odometry[C]//2014 IEEE International Conference on Robotics and Automation.IEEE，2014.

[8]MUR-ARTAL R， MONTIEL J M M， TARDOS J D.ORB-SLAM： a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics，2015，31（5）： 1147-1163.

[9]MUR-ARTAL R， TARDOS J D.Visual-inertial monocular SLAM with map reuse[J].IEEE Robotics and Automation Letters，2017，2（2）：796-803.

[10]QIN T， LI P， SHEN S.Vins-mono： A robust and versatile monocular visual-inertial state estimator[J].IEEE Transactions on Robotics，2018， 34（4）：1004-1020.

[11]QIN T， PAN J， CAO S， et al.A general optimization-based framework for local odometry estimation with multiple sensors[J/OL].（2019-01-11）[2024-7-20].https：//arxiv.org/abs/1901.03638.

[12]ZHAO S， ZHANG T， WEI H.Fast visual inertial odometry with point–line features using adaptive EDLines algorithm[J].Measurement Science and Technology，2022，33（10）：105401.

[13]明德琦.基于關鍵幀的單目視覺慣性SLAM[D].鎮江：江蘇科技大學，2023.

[14]陸健.最小二乘法及其應用[J].中國西部科技，2007（12）：19-21.

[15]CHUM O， MATAS J， KITTLER J.Locally optimized RANSAC[C]//Pattern Recognition： 25th DAGM Symposium， Magdeburg， Germany， September 10-12， 2003， Proceedings.Springer Berlin Heidelberg， 2003：236-243.

[16]BURRI M， NIKOLIC J， GOHL P， et al. The EuRoC micro aerial vehicle datasets[J].The International Journal of Robotics Research，2016，35（10）： 1157-1163.