葉 濤， 趙云龍， 汪壽安， 李允旺， 嚴(yán)翔明

（中國礦業(yè)大學(xué)（北京）機(jī)械與電氣工程學(xué)院，北京 100083）

0 引 言

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

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

1 案例工程背景

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

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

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

2 案例設(shè)計

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

圖1 視覺慣性融合SLAM系統(tǒng)框架

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

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

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

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

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

圖2 線特征重投影

2.2 IMU預(yù)積分觀測模型構(gòu)建

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

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

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

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

當(dāng)bias變化較小時，假設(shè)預(yù)積分觀測隨零偏呈線性變化，在此基礎(chǔ)上進(jìn)行線性化并保留一階項，對原始觀測值進(jìn)行修正，可得：

在圖優(yōu)化系統(tǒng)中，對IMU的殘差計算：

2.3 緊耦合滑動窗口和位姿圖優(yōu)化

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

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

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

圖3 因子圖概覽

3 實驗平臺開發(fā)

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

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

圖4 低光照弱紋理的實驗環(huán)境-1

圖5 低光照弱紋理的實驗環(huán)境-2

圖6 低光照弱紋理的實驗環(huán)境-3

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

圖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 序列不同算法和真值之間絕對軌跡誤差統(tǒng)計。

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

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

圖9 vins_mono和ours的軌跡xyz_view對比

圖10 vins_mono和ours的軌跡rpy_view對比

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

4 結(jié) 語

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