ORB-SLAM2車載雙目視覺的自主導航性能評估

沈燕芬，張小紅，2，朱 鋒

(1.武漢大學 測繪學院，武漢 430079；2.地球空間信息技術協同創(chuàng)新中心，武漢 430079)

0 引言

近幾年來，隨著無人機、無人駕駛汽車、智能移動機器人相關技術的飛速發(fā)展，視覺導航作為一種非傳統的定位技術被廣泛應用。文獻[1-2]提到：視覺導航僅利用單一相機傳感器完成，精度穩(wěn)定且無需場景和運動的先驗信息，可避免由于傳感器精度降低或低成本慣性測量單元(inertial measurement unit，IMU)漂移嚴重引起的誤差發(fā)散問題；其被動無源的特點，也彌補了全球衛(wèi)星導航系統(global navigation satellite system，GNSS)在復雜城市或室內環(huán)境下信號易受遮擋而無法定位的缺陷。

自1982年文獻[3]首次提出了一套完整的基于計算機視覺的理論框架以來，視覺導航領域的相關算法和應用層出不窮。視覺導航算法根據其對圖像信息的利用程度可分為特征法和直接法。直接法直接對圖像所有像素點進行操作，可充分利用環(huán)境幾何信息，其代表作為文獻[4]的基于直接法的大范圍單目即時定位和地圖構建方法(large-scale direct monocular simultaneous localization and mapping，LSD-SLAM)，在精確計算自身位姿的前提下，還可構建出大規(guī)模的半稠密地圖。而單目定位構圖(monocular simultaneous localization and mapping，MonoSLAM)、平行跟蹤與地圖構建(parallel tracking and mapping，PTAM)等算法則采用了對圖像關鍵信息進行檢測提取的特征法。MonoSLAM基于擴展卡爾曼濾波器實現，將相機位姿及地圖點3D坐標作為狀態(tài)向量進行觀測更新，算法計算復雜度較高，且線性化也造成了一定誤差。文獻[5]提出基于關鍵幀提取及光束法平差優(yōu)化的PTAM算法，首次將定位與構圖分為2個獨立工作的線程。2014年文獻[6]提出的半稠密單目視覺里程計算法(semi-direct monocular visual odometry，SVO)則融合了特征法與直接法，首先采用加速分割檢測特征(features from accelerated segment test，FAST)角點跟蹤關鍵點，再根據關鍵點的周圍信息估計幀間位姿，其實時性高、算法簡潔，非常適用于平臺受限的場合，如無人機、手持增強現實(augmented reality，AR)及虛擬現實(virtual reality，VR)設備等，該法對位姿估計精度依賴性強，易出現丟失和重定位的情況。為達到實時性的自主導航定位要求，文獻[7]將特征信息在用于位姿估計后不進行保留，后續(xù)優(yōu)化僅依據位姿關系，減輕了存儲計算負擔，其基于光束法平差(bundle adjustment，BA)的實時視覺定位構圖算法成為了在大范圍內進行定位構圖的開創(chuàng)性作品。文獻[8]的基于外觀的實時定位構圖方案(real-time appearance-based mapping，RTAB-MAP)則從內存管理機制角度出發(fā)，將定位點根據被訪問的頻率及存在的時間長度設置權重，以此為依據進行分類與修剪，從而提高了回環(huán)檢測的效率。

文獻[9-10]提出的基于旋轉不變特征點的實時定位構圖系統(oriented brief-SLAM，ORB-SLAM)被公認為現有綜合性能最好的視覺導航算法：在整體架構上延續(xù)了PTAM的多線程思想；但改進了PTAM需手動進行初始化的缺陷，擴展了應用環(huán)境的多樣性和系統的交互性。ORB-SLAM實現了基于單目相機的實時算法，而ORB-SLAM2在此基礎上擴展為雙目、RGBD相機，并被廣泛應用于采集車、無人機及室內外機器人等設備。

本文采用不同采集環(huán)境的車載數據對雙目視覺自主導航性能進行評估，重點分析其整體定位定姿精度、累積誤差隨距離的變化情況以及回環(huán)檢測對定位結果的影響。

1 利用ORB-SLAM2進行位姿估計

ORB-SLAM2整個系統分為位置跟蹤、局部地圖優(yōu)化、回環(huán)檢測3個并行處理的線程，其整體架構如圖1所示。

圖1 ORB-SLAM2算法整體架構

跟蹤線程主要負責每幀影像的位姿估計。經畸變矯正的雙目序列影像和相機的標定參數輸入至系統后，采用具有方向信息的多尺度FAST算子對影像進行特征提取，并用256位的具有方向信息的二元加速魯棒特征(oriented binary robust independent elementary features，rBRIEF)二進制描述符表示特征點。根據小孔成像原理以及雙目相機高度一致、光軸平行的特點，左右影像匹配后可重構出每對匹配點在相機坐標系中的坐標。將第一幀影像的左相機坐標系作為系統參考坐標系，通過序列影像的幀間匹配可將特征點的世界坐標進行傳遞和擴展，在幀間進行3D-3D對應估計可獲得參考坐標系與每幀影像的相對位姿關系。設第i幀中某匹配特征點在該幀相機坐標系中的坐標為(x,y,z)，與i-1幀進行匹配可得該點在系統參考坐標系中的坐標為(X,Y,Z)，即

(1)

(2)

式中：(uj,vj)為第j個特征點的像素坐標；N為匹配到的特征點總數；ρ為Huber損失函數用于外點的剔除；project()表示3D-2D重投影函數；Xj為第j個匹配點在系統參考坐標系中的x坐標；Σ為和每個特征點的尺度相關的信息矩陣。

在對每幀影像進行以上操作的同時，局部地圖優(yōu)化線程存儲關鍵點和關鍵幀，實時地優(yōu)化局部若干幀的位姿及相應3D點的位置，對誤差進行修正；而回環(huán)檢測線程則通過回環(huán)探測和融合實現全局位姿優(yōu)化。

通過位姿估計得到幀間的旋轉平移關系后，可以初始位置為坐標原點，僅考慮車輛在系統參考坐標系的運動情況，反演出載體的行駛軌跡為

(3)

式中：P0為初始時刻相機在系統參考坐標系的位置坐標；Pi表示第i幀影像的相機拍攝中心在系統參考坐標系下的位置坐標；m為總幀數。若已知初始坐標原點的絕對位置，則可通過坐標變換遞推得車輛在世界坐標系下的絕對行駛軌跡，實現車載雙目視覺導航。

2 ORB-SLAM2位姿估計的關鍵技術

ORB-SLAM2可有效對目標物體進行跟蹤，即使丟幀也可快速重定位，能夠實現可靠的自主導航；其關鍵技術集中在特征提取與匹配和位姿精度優(yōu)化2部分。文獻[11]提到，特征提取與匹配的準確度保證了每幀左右及幀間影像重構的三維點的準確度，從而決定了位姿估計的整體精度，故提高特征提取與匹配的正確率是算法提升自主導航魯棒性的有力措施；而多次的位姿精度優(yōu)化則是在此基礎上的雙重保障。

1)特征提取與匹配：特征在提取時候做了網格處理，從而避免了特征點選取的不均勻性，在左右影像間進行特征匹配時相繼用到了帶狀搜索、絕對差值和SAD(sum of absolute differences，一種圖像匹配算法)滑窗以及拋物線擬合以確保匹配點達到亞像素級的精度；幀間匹配，在缺乏先驗信息時，拋棄了傳統的高維圖像特征描述子比對，轉而使用詞袋模型(bag of words，BoW)對描述子進行分類，通過類別節(jié)點的設置大大簡化了運算，反之則通過重構特征點、三角化或相似變換約束縮小搜索范圍以加速匹配，并對特征點加入了正向景深、視差距離、反投影誤差、尺度及旋轉一致性的嚴格判定。

2)位姿精度優(yōu)化：除單幀位姿優(yōu)化外，系統還將在關鍵幀插入后對局部窗口幀及地圖點進行局部光束法平差(local bundle adjustment，Local BA)，在成功進行回環(huán)修正后對所有關鍵幀及地圖點進行全局光束法平差(full bundle adjustment，Full BA)。Local BA對由關鍵幀和地圖點的連接關系進行優(yōu)化，構造而成的共視圖(covisibility graph，CG)圖2所示。

圖2 Local BA優(yōu)化共視

如圖所示，鄰接共視幀KL1、KL2、KL3的位姿及其共視點PL1、PL2，PL3的位置在優(yōu)化中被修正，而包含共視點但非鄰接關鍵幀KF1、KF2的位姿則在優(yōu)化中被設為固定，只參與計算，不進行優(yōu)化。若幀間特征點的匹配關系(用χ表示)已知，則其優(yōu)化函數可表示為

(4)

式中：R、t分別為對應幀的旋轉矩陣和平移向量；下標l表示對應的共視幀；下標k表示共視幀與關鍵幀的交集。當通過一系列自適應閾值篩選得到可靠的候選回環(huán)幀并進行回環(huán)修正后，系統會另開辟線程執(zhí)行Full BA。Full BA是Local BA的擴展，除初始幀外，所有關鍵幀和地圖點均被優(yōu)化；故可有效解決因誤差累積導致的尺度漂移問題，大大提高系統的定位測姿精度，提升雙目自主導航的可靠性與穩(wěn)定性。

3 實驗與結果分析

為了對基于ORB-SLAM2的車載雙目視覺自主導航進行全面的性能評估，本節(jié)將采用不同道路采集環(huán)境的數據對其算法的整體定位測姿性能及其累積誤差隨載體行駛距離的變化、回環(huán)檢測對自主導航結果的影響等方面做細致分析。

本次分析采用的數據為卡爾斯魯厄工學院和豐田技術學院(Karlsruhe Institute of Technology and Toyota Technological Institute，KITTI)聯合提供的數據集的序列影像，其采集環(huán)境主要為高速公路、公路及居民街區(qū)。實驗設備包含1套OXTS RT 3003的GPS/IMU組合導航系統、4個灰點公司的Flea 2系列相機、4個埃蒙德的4～8 mm變焦鏡頭，分析數據僅涉及其中2個灰色相機。相機與地平面平行安裝，采樣幀率為10 Hz，其實驗設備和不同環(huán)境的采集影像如圖3所示。

3.1 整體精度分析

以差分GPS/IMU的組合定位結果為參考值，解算軌跡與參考值的偏差值大小，結果由不同灰度值表示，如圖4所示，灰度越深代表誤差越大。

圖3 實驗裝備與數據采集

圖4 整體軌跡誤差

由圖4可知，所有軌跡均與真值重合度較高，整體定位精度約為1.02 %，即行駛100 m偏離真值1 m，角度誤差為0.001 9 deg/m，各方向定位精度在4～5 m左右，定姿精度在0.6～0.7 deg之間，如表1所示。

由表1可知：回環(huán)軌跡的整體定位定姿精度顯著優(yōu)于無回環(huán)軌跡；單閉環(huán)軌跡在回環(huán)融合附近的誤差修正效果明顯，而無回環(huán)軌跡隨著行駛距離的增加誤差明顯逐漸發(fā)散。對多回環(huán)影像序列2、序列3的異常軌跡分析發(fā)現，大轉彎、照片過曝或陰影遮擋會影響定位測姿精度，而影像中的在相機視野內持續(xù)運動物體會嚴重干擾位姿估計，如圖5所示，3號軌跡中異常軌跡段中，有一輛摩托跟隨行駛了500多米。

圖5 3號序列影像中特殊場景分析

3.2 累積誤差隨距離變化的影響分析

增量式的位姿估計會導致絕對位置誤差不斷累積，位姿愈發(fā)發(fā)散，從而最終限制系統的整體精度。

選取無回環(huán)的9號高速公路影像(全長2 451 m)及多回環(huán)的1號居民街區(qū)影像(全長2 204 m)進行處理，并統計其累積誤差(滑窗寬度為100 m，統計窗口內為位置誤差及姿態(tài)估計誤差的均方根誤差)，如圖6所示。

圖6 位置、姿態(tài)累積誤差隨距離的變化

由圖6可知：高速公路環(huán)境下的行駛路線，各方向位置精度隨距離增長呈明顯線性發(fā)散趨勢，行駛到2 500 m左右時，側向精度將近達到25 m，而居民街區(qū)軌跡由于多次的回環(huán)修正，各方向精度均始終保持在2 m左右；姿態(tài)精度在2種采集模式下無明顯規(guī)律可循，均始終保持在1 deg以內，原因在于相對姿態(tài)誤差在局部的位姿優(yōu)化中有很大程度的修正，而回環(huán)影像的優(yōu)勢主要在于可修正由誤差積累所導致的尺度漂移的問題。故無回環(huán)軌跡各方向的位置精度隨距離呈約1 %的線性發(fā)散，而經過回環(huán)修正則可使精度始終保持在2 m，提高了車載自主導航的魯棒性。

3.3 回環(huán)修正對結果的影響分析

為進一步定量分析回環(huán)修正對定位測姿精度的改善情況，本小節(jié)選取4組長距離回環(huán)序列影像進行分析，對有回環(huán)修正和無回環(huán)修正2種模式下的定位測姿精度分別進行統計，并重點對6號序列影像進行細致分析，如表2所示。

表2 回環(huán)修正結果精度統計

由表可知，針對不同序列影像，閉環(huán)檢測平均約可改善36 %的位置精度和50 %的姿態(tài)估計精度。

進一步分析比較6號影像軌跡(如圖7所示)可以發(fā)現：沒有進行回環(huán)修正的軌跡尺度漂移明顯，各方向的位置逐漸偏離參考值，誤差逐漸增大，姿態(tài)誤差也存在發(fā)散趨勢，但幅度較?。环粗?，成功的回環(huán)修正可使得起點與終點高度重合，明顯修正位置誤差，起到抑制誤差發(fā)散、優(yōu)化整體位姿精度的作用。

圖7 6號影像軌跡比較

4 結束語

本文介紹了利用ORB-SLAM2車載雙目視覺自主導航的基本位姿估計原理及其關鍵技術，重點對ORB-SLAM2的自主導航性能進行評估，分析了其整體定位測姿精度、累積誤差隨行駛距離的變化情況以及回環(huán)修正對結果的具體影響。對一定量序列影像數據分析得出：該算法的整體定位相對精度約為1.02 %，定姿相對精度約為0.001 9 deg/m；在無回環(huán)探測時，位置誤差隨行駛距離呈現明顯的1 %線性發(fā)散；成功閉環(huán)探測約可提高36 %的位置精度和50 %的姿態(tài)精度，且無發(fā)散情況，定位精度始終保持在2 m左右。故該算法存在和其他導航系統或傳感器，如GNSS、IMU等進行組合導航的理論基礎。目前視覺導航發(fā)展的難點和熱點為高精度、高效率及多傳感器融合，故在接下來的研究中，將著重對視覺導航和IMU及GNSS組合系統的相對姿態(tài)標定以及組合導航方案進行深入研究，望實現一套完整可靠、低成本、高精度的雙目視覺輔助慣導/GNSS的組合導航實時系統。

[1] SCARAMUZZA D，FRAUNDORFER F.Visual odometry：Part I：the first 30 years and fundamentals[J].IEEE Robotics & Automation Magazine，2011，18(4)：80-92.

[2] 權美香，樸松昊，李國.視覺SLAM綜述[J].智能系統學報，2016，11(6)：768-776.

[3] 盧維.高精度實時視覺定位的關鍵技術研究[D].杭州：浙江大學，2015.

[4] ENGEL J，SCH?PS T，CREMERS D.LSD-SLAM：large-scale direct monocular SLAM[C]//Anon.Proceedings of European Conference on Computer Vision.Zurich：Springer，2014：834-849.

[5] KLEIN G，MURRAY D.Parallel tracking and mapping for small AR workspaces[EB/OL].[2017-02-19].http：//www.robots.ox.ac.uk/～gk/publications/KleinMurray2007ISMAR.pdf.

[6] FORSTER C，PIZZOLI M，SCARAUZZA D.SVO：fast semi-direct monocular visual odometry[EB/OL].[2017-02-19].http：//rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf.

[7] KONOLIGE K，AGRAWAL M.FrameSLAM：from bundle adjustment to real-time visual mapping[J].IEEE Transactions on Robotics，2008，24(5)：1066-1077.

[8] LABBé M，MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics，2013，29(3)：734-745.

[9] MUR-ARTAL R，MONTIEL J M M，TARDS J D.ORB-SLAM：a versatile and accurate monocular SLAM system[J].IEEE Transactions on Robotics，2017，31(5)：1147-1163.

[10] MUR-ARTAL R，TARDS J D.ORB-SLAM2：an open-source slam system for monocular，stereo，and RGB-D cameras[J].IEEE Transactions on Robotics，2016，33(5)：1255-1262.

[11] FRAUNDORFER F，SCARAMUZZA D.Visual odometry：Part II：matching，robustness，optimization，and applications[J].IEEE Robotics & Automation Magazine，2012，19(2)：78-90.