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

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

0 引言

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

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

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

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

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

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

圖1 ORB-SLAM2算法整體架構(gòu)

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

(1)

(2)

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

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

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

(3)

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

2 ORB-SLAM2位姿估計的關(guān)鍵技術(shù)

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

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

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

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

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

(4)

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

3 實驗與結(jié)果分析

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

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

3.1 整體精度分析

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

圖3 實驗裝備與數(shù)據(jù)采集

圖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ā)現(xiàn)，大轉(zhuǎn)彎、照片過曝或陰影遮擋會影響定位測姿精度，而影像中的在相機視野內(nèi)持續(xù)運動物體會嚴重干擾位姿估計，如圖5所示，3號軌跡中異常軌跡段中，有一輛摩托跟隨行駛了500多米。

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

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

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

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

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

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

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

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

表2 回環(huán)修正結(jié)果精度統(tǒng)計

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

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

圖7 6號影像軌跡比較

4 結(jié)束語

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

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

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

[3] 盧維.高精度實時視覺定位的關(guān)鍵技術(shù)研究[D].杭州：浙江大學(xué)，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.