面向低光弱紋理環境的SLAM系統實驗教學平臺設計

葉 濤， 趙云龍， 汪壽安， 李允旺， 嚴翔明

（中國礦業大學（北京）機械與電氣工程學院，北京 100083）

0 引 言

同步定位與建圖（Simultaneous Localization and Mapping，SLAM）是機器人領域中的一項關鍵技術，應用該技術的機器人能在未知環境中同時實現自身定位和構建地圖。對于機器人專業學生而言，SLAM 課程具有重要的學術意義和實踐價值。由于歷史原因以及教學和實驗條件的限制，SLAM 課程主要側重于理論教學，缺乏實際應用，導致學生對該課程作用的認識不足，教學效果并不理想。學生普遍反映，該系列課程所涉及的技術過于抽象和枯燥，側重點不夠突出，影響了對該課程的學習興趣和積極性。

為解決上述問題，本文提出一種改進視覺點線特征提取方法并搭建面向低光弱紋理環境的SLAM實驗教學平臺，旨在為學生提供探索低光弱紋理環境下機器人導航與定位的實踐機會。通過在模擬場景中進行實驗操作，學生能深入理解SLAM 算法在復雜環境下的應用和挑戰，提高培養解決實際問題的能力。該教學平臺有助于拓展學生的學術視野，激發學生對該領域的研究興趣。

1 案例工程背景

低光弱紋理空間常見于地下停車庫、暗室等日常場景以及地鐵隧道、礦井巷道等工業場景。這些場景光線照度低、缺乏明顯紋理特征、全球定位系統（Global Positioning System，GPS）［1］信號缺失等特點，機器人在上述復雜場景中難以實現精準、魯棒的地圖定位。隨著SLAM技術的興起，機器人使用的各種傳感器可實現4 自由度（Four Degrees of Freedom，4DOF）的位姿估計。單目視覺方法具有結構簡單、成本低、無距離限制和實時性良好等優點。由于其缺乏尺度信息、視場角有限以及受弱紋理結構影響較大，僅依靠純視覺傳感器難以達到所需效果。融合慣性測量單元（Inertial Measurement Unit，IMU）傳感器是解決上述問題的有效手段。相比于基于單目視覺的方法，慣性導航系統能在短時間內提供相對準確的角加速度和線加速度，數據更新率更高且不依賴于任何外部信息。由于IMU傳感器的噪聲和偏置，定位誤差隨時間累積導致定位不穩定且不準確。相比之下，采用視覺和慣性融合的方法［3-4］能克服這些缺點并提高自主定位的精度和狀態估計的可靠性。

文獻［5-7］中提出松耦合VIO 算法傳感器融合（sensor fusion），該算法充分融合了視覺和慣性傳感器的優點，實現了更為精確和魯棒的定位和導航。Falquez等［8］提出基于直接視覺里程計法取得了更好的效果，一些基于濾波方式的VIO 也被提出［9-10］。優化的全向視覺同時定位地圖構建（Omnidirection Visual SLAM，ORB-SLAM）等算法［11-13］與濾波算法相比能在保證實時性的同時有效提高檢測精度。Qin 等［14］提出單目視覺慣性系統（VINS-Mono），能在執行初始化過程中為最小化視覺和慣性誤差的非線性優化過程提供初始值，通過設計重定位和姿態圖優化模塊實現在復雜環境下實現高精度的定位和導航。盡管上述單目視覺慣性算法取得了不錯的性能，但地鐵隧道、礦井巷道、工業管道等空間弱紋理、低照度、空間狹窄等實際復雜場景為視覺慣性SLAM 提出了新的挑戰：在單目相機定位過程中，通常使用人工定義的角點特征識別環境中的幾何特征，這種角點特征在弱紋理環境下難以提取，嚴重影響了SLAM的整體性能，也限制了機器人在這種環境的自主導航能力。上述環境中仍然包含大量可用于提取的線特征，基于這些線特征構建的環境地圖具有更為豐富的幾何信息，能有效提升算法的定位精度和魯棒性。文獻［15］中提出一種從三維直線運動矩陣中提取常用運動矩陣的方法，并對通用矩陣進行校正，使其能準確地表示運動。文獻［16］中使用線特征研究視覺慣性里程計的可觀測性，解決了基于慣性測量和線的視覺觀測估計車輛在三維運動狀態的問題。He等［17］構造了一種結合點線特征的緊耦合單目VIO 系統。該算法實現局部位姿估計，沒有考慮全局的定位誤差，在低光照弱紋理環境中的檢測魯棒難以得到保證。

綜上所述，機器人在低光弱紋理場景下如何維持SLAM系統的估算精度、實時性、累計誤差之間的均衡仍是VIO所面臨的實際應用難題。本文提出一種以相機和慣性導航系統作為傳感器輸入的面向低光弱紋理環境的視覺慣性融合的魯棒SLAM 算法實驗平臺，實現機器人在此類復雜環境下的定位與導航的高精度和魯棒性。通過該實驗平臺與授課相結合的方式可鞏固學生所學的理論知識，提高學生的創新實踐能力和解決工程問題的能力。

2 案例設計

本文所提面向低光弱紋理環境的視覺慣性融合的魯棒SLAM系統框架如圖1 所示。該系統輸入為相機和IMU數據，其算法處理框架由初始化和前端與后端優化兩部分組成。前端對傳感器輸出的數據進行預處理，提取點特征和線特征，并進行視覺初始化以建立視覺觀測模型。同時，構建IMU 預積分觀測模型，并將這兩種觀測模型結合，構建視覺慣性里程計。將先驗信息、IMU殘差和點特征殘差輸入緊耦合滑動窗口，通過回環檢測判斷當前位置是否需要位姿圖優化，以獲得關鍵幀。系統對輸出經優化后的位姿進行狀態估計。

圖1 視覺慣性融合SLAM系統框架

2.1 點線特征的提取和模型觀測

針對低光照弱紋理環境下角點特征提取困難，線特征計算成本過高，難以保證算法的實時性，本文對LSD算法進行優化：通過研究隱藏參數的調整簡化線特征的檢測，針對原有N層高斯金字塔進行多尺度表達的創建調整固定采樣比例和高斯模糊次數，改善線特征提取圖像梯度的一致性；通過長度拒絕策略過濾掉短線特征，根據調節圖像縮放系數和最小密度閾值設計長度拒絕系數；采用LBD（Line Band Descriptor）描述子［22］對旋轉和尺度縮放具有較高的魯棒性，可有效提高匹配效率和計算速度。通過上述優化提高了算法在低光照和弱紋理環境下線段特征的提取和跟蹤性能，為SLAM系統提供更可靠和高效的視覺特征，提升定位和導航的精度和魯棒性。

在完成圖像線特征的關聯后，對世界坐標系下的相機位姿、路標觀測以及線段的重投影誤差進行建模。設世界系下相機第j幀的位姿為，在該幀觀測到的第k個特征點為，其在圖像上的投影為，第l條線段的首尾點可表示為，在圖像平面對應的齊次坐標點為，其點特征的重投影誤差和線特征的重投影誤差分別為：

式中：dpj，l為到、的投影線段的距離；lj，l為相機光心與線段端點構成平面的法線，線特征重投影誤差對線段的雅可比矩陣為：

式中：Rjw為世界系下相機第j幀的位姿為中的平移分量；fx、fy分別為相機的焦距；opx、opy、opz和oqx、oqy、oqz分別為點在相機坐標系x y z坐標軸上的分量。線特征重投影如圖2 所示。

圖2 線特征重投影

2.2 IMU預積分觀測模型構建

對于IMU 系統，主要考慮旋轉分量R、平移分量p、角速度ω、線速度v和加速度a。由旋轉運動學以及歐拉積分的離散形式解析，通過指數映射將李代數中的旋轉向量對應到李群中的旋轉矩陣，并將受到噪聲與重力影響的陀螺儀測量的角速度和加速度計測量的線加速度進行解析替換，通過對采樣頻率進行固定，每個離散時刻由k=0，1，…表示，可得關于旋轉R、速度v和平移p的3 個離散運動方程：

式中：ΔRij為預積分旋轉量；Δvij為預積分速度量；Δpij為預積分平移量。對預積分旋轉量的測量值和理論值進行解析，把噪聲項分離：

式中：Δ為旋轉量預積分測量值，它由陀螺儀測量值和對陀螺bias的估計得到；δ→φij為測量噪聲。同理，對線速度和位姿的理論值和測量值進行解析，分離噪聲項：

由于旋轉量的測量值、一階雅克比、Δt采樣時間這些都是已知量，為零均值高斯噪聲，（一階近似）也為零均值高斯噪聲。同理可得速度量和平移量的噪聲項都具有高斯分布的形式：

當bias變化較小時，假設預積分觀測隨零偏呈線性變化，在此基礎上進行線性化并保留一階項，對原始觀測值進行修正，可得：

在圖優化系統中，對IMU的殘差計算：

2.3 緊耦合滑動窗口和位姿圖優化

在包含了點、線和IMU測量信息的緊耦合滑動窗口中定義所有狀態向量：

式中：xk為第k幀中IMU在世界坐標系下的信息；pbwk、Rbwk、vbwk分別為位置量、旋轉量和速度量；ba、bg分別為加速度偏差和陀螺儀偏差；nk、np和nl分別為滑動窗口中關鍵幀、空間點和線的總數；λ 為點特征與其第1個觀察到的關鍵幀的反距離；o為3D線的4 參數正交表示。定義滑動窗口中的目標函數

式中：Iprior為先驗信息，它是在滑動窗口［23］中邊緣化舊幀時獲得的；eimu、epoint分別為點重投影殘差和IMU 殘差；eloop為閉環約束；eline為線重投影殘差。在滑動窗口后，構建一個具備全局信息的因子圖優化框架，如圖3所示。該框架考慮了先驗信息、IMU里程計因子、IMU預積分因子以及線特征路標和點特征路標對相機關鍵幀位姿的影響。

圖3 因子圖概覽

3 實驗平臺開發

本文對所提視覺慣性緊耦合SLAM算法進行了一系列實驗，并與其他先進的視覺慣性SLAM 算法進行了比較。這些實驗在開源的M2DGR 數據集上進行，該數據集為室內外地面機器人收集了長期具有挑戰性的序列，并配備完整的傳感器套件。實驗中，本文使用了分辨率為640 × 480、頻率為15 Hz 的Inivation DVXplorer事件相機和9 軸IMU（頻率為100 Hz）傳感器。實驗計算機采用AMD R5 2600 CPU，主頻為3.4 GHz，六核心和32 GB 運行內存。所有算法均采用C++實現，并在Ubuntu 18.04 系統上使用ROS 的Melodic版本執行。

為評估所提方法的準確度和誤差狀態，在M2DGR數據集hall_04 和street_03 序列上進行定量分析。所有算法均在開啟回環的情況下進行測試。圖4 ～6 展示了低光照弱紋理的實驗環境。其中，圖4、5 顯示了夜晚黑暗室外環境，圖6 顯示了無光照條件的室內環境。

圖4 低光照弱紋理的實驗環境-1

圖5 低光照弱紋理的實驗環境-2

圖6 低光照弱紋理的實驗環境-3

圖7 、8 展示了不同實驗序列絕對軌跡誤差（ATE）和軌跡。圖7 展現了hall_04 序列絕對軌跡誤差SE（3）分析和軌跡圖，圖8 展示了street_03 序列絕對軌跡誤差SE（3）分析和軌跡圖。圖7（a）、（b）和圖8（a）、（b）展示的灰色直線為APE（絕對位姿誤差），藍色直線為均方根誤差，綠色線為誤差中位數，紅色線為誤差平均值，紫色線為標準差所在范圍區間，可見，本文所提方法的縱坐標RMSE 等值具有更小的誤差值。在圖7（c）、（d）和圖8（c）、（d）展示的軌跡誤差圖中，右側的藍色部分為較小的誤差值，而紅色部分為較大的誤差值。可見，本文所提方法在右側標記的軌跡誤差值較小，藍色部分線段的分布更為密集。

圖7 hall_04 序列的絕對軌跡誤差SE（3）分析和軌跡圖

圖8 street_03 序列的絕對軌跡誤差SE（3）分析和軌跡圖

圖9 、10 分別展示了vins_mono 方法和ours 方法的軌跡xyz_view和rpy_view 對比，表1、2 分別列出了hall_04 和street_03 序列不同算法和真值之間絕對軌跡誤差統計。

表1 hall_04 序列不同算法和真值之間絕對軌跡誤差統計

表2 street_03 序列不同算法和真值之間絕對軌跡誤差統計

圖9 vins_mono和ours的軌跡xyz_view對比

圖10 vins_mono和ours的軌跡rpy_view對比

綜合對表1、2 和圖7、10 的數據分析，本文研究的算法在低光照、弱紋理場景的定位精度與vins_mono方法比有著明顯的改善，取得了更小的絕對軌跡誤差，在rpy_view的變化上也更加穩定。

4 結 語

本文所提改進視覺點線特征提取的方法，旨在解決弱紋理環境下基于視覺點特征數據誤關聯問題，能有效提升算法在弱紋理場景的定位精度，并在此基礎上設計一種面向低光弱紋理環境的視覺慣性融合魯棒SLAM軟件實驗平臺。通過引入該實驗教學平臺，學生將有機會深入了解SLAM 技術的實際應用，并掌握在低光弱紋理環境中解決導航與定位問題的關鍵技術。通過讓學生團隊合作完成巡檢機器人SLAM實驗的方式能有效培養學生的個人和協作能力，提高機器人專業本科生的專業能力和競爭力，為其未來職業發展奠定堅實基礎。