董靜薇　蔣燚銘　韓知雨

摘 要：針對視覺同步定位與地圖構建領域中基于深度相機傳感器的算法前端跟蹤耗時長，精度差的問題，提出了一種RGB-D SLAM算法的前端視覺里程計實現(xiàn)方案。依據(jù)傳統(tǒng)的視覺里程計實現(xiàn)方案，采用ORB特征提取算法對圖像進行特征提取并計算描述子，與局部地圖中地圖點進行特征匹配，最后采用PnP算法計算相機位姿。為提高算法跟蹤的精準度，提出在計算相機位姿后，利用三角測量進行局部地圖中地圖點深度的更新。實驗采用TUM數(shù)據(jù)庫中fr1系列數(shù)據(jù)集進行測試，結果表明，該算法在保證實時性的同時，使跟蹤過程中的均方根誤差平均減小了9.21%。且fr1系列兩種數(shù)據(jù)集通過視覺里程計最后生成點云地圖的圖像信息熵比傳統(tǒng)RGB-D SLAM系統(tǒng)平均提高了11.21%。

關鍵詞：RGB-D SLAM; ORB特征提取;PnP算法;三角測量;局部地圖

DOI：10.15938/j.jhust.2020.06.023

中圖分類號： TP242.6

文獻標志碼： A

文章編號： 1007-2683（2020）06-0157-08

RGB-DSLAM Visual Odometry Optimization Algorithm

DONG Jing-wei1，2， JIANG Yi-ming1，2， HAN Zhi-yu1，2

（1.School of Measurement and Control Technology and Communications Engineering， Harbin University of Science and Technology， Harbin 150080， China;

2.The Higher Educational Key Laboratory for Measuring and Control Technology and Instrumentations of Heilongjiang Province， Harbin University of Science and Technology， Harbin 150080， China）

Abstract：In view of the long time-consuming and poor precision of RGB-D SLAM algorithm in the field of visual SLAM， an implementation scheme of RGB-D SLAM visual odometry is proposed. Based on the traditional visual odometer implementation scheme， the ORB feature extraction algorithm is used to extract the features of the image and calculate the descriptor to match the features of the map points in the local map. Finally， the PnP algorithm is used to calculate the camera pose. In order to ensure the accuracy of the tracking， it is proposed to use the triangulation algorithm to update the depth of the map points in the local map after calculating the camera pose. The experiment uses fr1 series data set in tum database to test. The experimental results show that the scheme can reduce the root-mean-square error in the tracking process by 9.21% on average while ensuring real-time performance. The image quality of the point cloud map generated is also improved by 11.21%.

Keywords：RGB-D SLAM; ORB feature extraction; PnP algorithm; triangulation algorithm; local map

0 引 言

隨著人工智能的到來，近年來視覺同步定位與地圖構建（Simulation Localization and Mapping， SLAM）技術在現(xiàn)實生活中被廣泛應用，如掃地機器人，無人駕駛，VR虛擬現(xiàn)實等。視覺SLAM是指以搭載相機為傳感器主體，在沒有任何環(huán)境先驗信息的情況下，在運動過程中建立環(huán)境模型，同時估計自己的運動。根據(jù)主傳感器的不同，視覺SLAM主要分為三類，分別為Monocular SLAM（單目SLAM）[1]、Stereo SLAM（雙目SLAM）、以及RGB-D SLAM（深度相機SLAM）[2]，其中Monocular SLAM目前已實現(xiàn)的方案主要有MonoSLAM、PTAM（parallel tracking and mapping）等，Stereo SLAM主要的實現(xiàn)方案目前較少只有RTAB-MAP（real time appearance-based mapping）方法，而RGB-D SLAM的應用較廣，主要的實現(xiàn)方案有DVO （dense visual odometry）、RTAB Map、ORB-SLAM2等。RGB-D SLAM 是指把深度相機作為主傳感器進行信息獲取，深度相機造價低，且具有可以同時獲取彩色圖和深度圖的優(yōu)點，促使更多的學者把RGB-D相機數(shù)據(jù)作為研究視覺SLAM的傳感器輸入數(shù)據(jù)。

siui=Kexp（ζΛ）Pi（2）

現(xiàn)實情況下因為相機位姿未知以及觀測點噪聲的原因，所以該等式存在誤差，因此我們把誤差項求和，構造最小二乘問題，同時找到相機位姿的最優(yōu)解，使得誤差最小化，最小二乘公式如下：

ζ*=argmin12∑ni=1ui-1siKexp（ζΛ）Pi22（3）

式（3）是指真實像素點的位置與空間3D點根據(jù)當前初始位姿投影到像素平面的像素點位置之間的誤差。為求解式（3），使用李代數(shù)構建無約束的優(yōu)化問題，并通過高斯牛頓優(yōu)化算法進行求解，首先對該問題進行線性化：

e（X+Δx）≈e（X）+JΔx（4）

式中：e為像素坐標誤差（2維）;X為相機位姿（6維）;J是一個2×6的雅克比矩陣，描述了相機誤差對相機位姿的一階變換關系，用李代數(shù)的擾動模型進行求解J：

P′=（exp（ζΛ）P）1∶3=[X′，Y′，Z′]T（5）

式（5）中P′表示空間點Pi變換到相機坐標系下的坐標，上式表示取該坐標的前3維。根據(jù)相機針孔模型有：

su=KP′

susvs=fx0cx0fycy001X′Y′Z′（6）

消去s得：

u=fxX′Z′+cx，v=fyY′Z′+cy（7）

左乘擾動量并求導得：

eδζ=limδζ→0e（δζζ）δζ=eP′P′δζ（8）

式（8）中指李代數(shù)上的左乘擾動，等式右端第一項是指誤差項關于投影點的導數(shù)：

eP′=-uX′uY′uZ′vX′vY′vZ′=-fxZ′0-fxX′Z′2

0fyZ′-fyY′Z′2（9）

式（8）等式右端第二項是指變換成空間坐標后對李代數(shù)的導數(shù)：

（TP）δζ=I-P′Λ0T0T（10）

取前3維得：

P′δζ=I-P′Λ（11）

將兩項相乘得到了2×6的雅克比矩陣：

eδζ=-

fxZ0-fxX′Z′2-fxX′Y′Z′2fx+fxX′2Z′2-fxY′Z′

0fyZfyY′2Z′2-fy-fyY′2Z′2fyX′Y′Z′2fyX′Z′（12）

至此推導出了觀測相機方程關于相機位姿的導數(shù)矩陣，為優(yōu)化過程提供梯度的方向，指導優(yōu)化迭代。利用高斯牛頓法優(yōu)化出精度更高的相機位姿。

2 改進算法

本文采用Monocular SLAM中常用的構建局部地圖的方法。使當前幀只與局部地圖中的地圖點進行匹配，并計算位姿，這樣可 以大大減小由于相鄰幀之間位姿誤差的積累而產(chǎn)生的漂移現(xiàn)象。而后為減少從深度圖中采集到的地圖點對應的空間點深度值的誤差，本文提出采用三角化算法來計算出空間點的深度值，并與采集到的空間點深度值進行加權數(shù)值融合，爭取達到提高構建的局部地圖質量的目的。

局部地圖僅包含與當前幀有相同特征點的地圖點和關鍵幀，所以計算量小，不會大幅度增加算法的運行時間，但是卻提高了算法的精確度，同時也為后端的回環(huán)檢測提供了更加精準的地圖點深度的數(shù)據(jù)。

2.1 構建局部地圖

構建局部地圖是指向地圖中插入關鍵幀以及地圖點等信息[19]，局部地圖描述了附近的特征點信息，只保留距離相機當前位置較近的特征點，把遠的視野外的特征點信息刪掉。如圖3顯示了構建局部地圖的具體過程。

在正確的計算PnP相機位姿后，要經(jīng)過地圖點的更新，局部地圖構建過程如下：

1）從第一幀彩色圖提取其ORB特征點，并根據(jù)深度圖計算對應特征點深度，并把第一幀當做關鍵幀，取全部的特征點為地圖點，加入到局部地圖中。

2）正常工作模式下，利用PnP求解位姿后，更新地圖點，把彩色圖中沒有匹配上的特征點求取其深度信息，添加到地圖點中，把地圖點中匹配成功率低的點刪掉。

3）關鍵幀檢測，設置閾值，當PnP求取的位姿結果大于閾值時，則把當前幀添加進關鍵幀，其上所有特征點取其深度以及描述子加入到局部地圖中。這樣完成了整個局部地圖的構建的過程。

2.2 三角化局部地圖點深度

在采用了PnP算法估計出了相機位姿的基礎上，對RGB-D SLAM算法的視覺里程計前端提出進一步的改進，通過估計出的位姿對局部地圖中地圖點的深度進行進一步優(yōu)化，這樣可以提高下一幀匹配的準確度以及減小重投影的誤差[20-21]。

三角測量是指通過在兩處同時觀察到同一個點的夾角，從而確定該點的距離，如圖4所示。

左右兩圖分別為圖像I1，I2以左圖為參考幀，右圖的位姿為T，O1，O2分別為兩個位置的相機光心，點P為空間點，p1，p2分別為空間點在兩幀圖像中對應的特征點，由圖中可以看到由于噪聲原因，直線O1，p1與直線O2，p2并沒有交于點P，按照對極幾何定義：

s1x1=s2Rx2+t（13）

式中：s1，s2分別為特征點的深度;x1，x2為特征點的歸一化坐標;R，t為相機位姿，上式左乘xΛ1得：

s1xΛ1x1=0=s2xΛ1Rx2+xΛ1t（14）

因為估計的相機位姿存在誤差，現(xiàn)構建最小二乘問題求解s2，這里的優(yōu)化變量為s2，即求得當s2為何值時，等式最小，最后利用式s2求得s1。

θ=argmin12∑ni=0‖s2xΛ1Rx2+xΛ1t‖22（15）

由于是前端的位姿估計，此處并沒有選擇計算量較大的深度濾波進行深度估計，數(shù)據(jù)融合算法采取普通的加權數(shù)據(jù)融合算法。將上一步求得的深度數(shù)據(jù)s1與從深度圖中采集得到的數(shù)據(jù)融合：

s1=x+v1（16）

s0=x+v0（17）

式中s表示深度數(shù)據(jù);v表示兩深度估計值存在的隨機誤差，且設vi（i=0，1）為存在的誤差，vi～N（0，σ2i），且兩估計值分別獨立。

設x的估計值與觀測值s成線性關系，為x的無偏估計則：

=w1s0+w2s1（18）

式中Ω=（w1，w2）為兩個估計值的權值。設估計誤差=x-取代價函數(shù)為的均方誤差，則：

D=E（2）=E{[x-w1（x+s0）-w2（x+s1）]2}（19）

E（）=E[x-w1（x+s0）-w2（x+s1）]=0（20）

式中E（v1）=E（v2）=0，E（x）=E（），則有：

w2=1-w1（21）

則代價函數(shù)可寫為

D=E（2）=

E[w21v21+（1-w1）2v22+2w1（1-w1）v1v2]（22）

D=E（2）=w21σ21+（1-w1）2σ22（23）

為使J最小，求導有

DΩ=0（24）

最優(yōu)權值為

w1=σ22σ22+σ21，w2=σ21σ22+σ21（25）

則最優(yōu)估計量為

=σ22s0σ22+σ21+σ21s1σ22+σ21（26）

3 仿真實驗結果與分析

為驗證算法的有效性，本章選取TUM數(shù)據(jù)集中的fr1系列數(shù)據(jù)集進行測試，首先進行特征匹配實驗，然后通過定位誤差與運行速度實驗和點云建圖實驗，對傳統(tǒng)算法和本文算法進行評估，最后得出實驗結論。

本算法的實驗平臺為Intel i5 6200U處理器（2.3GHz），4 GB RAM，64位Ubuntu14.04操作系統(tǒng)。

3.1 ORB特征匹配實驗結果

特征匹配實驗選取fr1/xyz中的部分數(shù)據(jù)進行實驗。ORB特征匹配結果如圖5、6所示，其中圖5為直接匹配的結果，圖6為去除誤匹配之后的匹配效果。圖6的特征匹配相比于圖5減小了67%，也就是正確匹配率為33%。這里設置的篩選依據(jù)是，漢明距離小于最小距離的2倍。

3.2 定位誤差與運行速度分析

本次選取fr1/room數(shù)據(jù)集以及fr1/xyz數(shù)據(jù)集進行測試，進行5次重復試驗，在實驗過程中記錄算法的運行時間以便對算法的實時性進行評估，

表1、2為本文算法與其他兩種相關算法的跟蹤精度和運行時間的對比，均方根誤差體現(xiàn)了數(shù)據(jù)的真實軌跡和算法跟蹤的軌跡之間的誤差。

從表1、2可以看出：

1）針對前兩行，SIFT算法與ORB算法的比較，SIFT算法的均方根誤差比ORB算法平均降低了9.962%，而對運算速度而言，ORB算法卻較SIFT算法提升了90%，相較于均方根誤差的變化，運算速上的差異則更加明顯，由此證明本文采用的ORB特征提取算法極大提高了算法的實時性。

2）針對后兩行，同樣使用ORB特征提取的兩個算法，本文算法相較于ORB+PnP算法，均方根誤差平均降低了9.21%，而算法的運行時間卻僅增加了0.002s，對于fr1這樣每秒取30幀的數(shù)據(jù)集，該算法的處理幀速度可滿足SLAM算法的時實同步跟蹤定位的要求。

綜上所述，在滿足處理幀速度的條件下，本文算法減小了均方根誤差，提高了跟蹤的精度。

3.3 局部地圖構建實驗結果

同步定位與地圖構建技術的建圖方式有很多種，其中主流的建圖方式有點云建圖、八叉樹建圖、網(wǎng)格建圖。點云建圖是指用一組離散的點來表示的地圖，其中每個點都包含x，y，z三維坐標信息。相比于點云建圖，八叉樹建圖法以及網(wǎng)格建圖法作為底層技術更加方便向上層應用來提供信息。但是建圖結果不清晰，導致實驗對比困難，所以為驗證本文算法的有效性，本文采取最基本的點云建圖方式進行實驗對比分析。

本文的點云建圖實驗選取fr1系列的fr1/desk數(shù)據(jù)集以及fr1/floor數(shù)據(jù)集中的全部數(shù)據(jù)進行點云建圖，建圖結果如圖7～10所示。圖7、8為采用fr1/desk數(shù)據(jù)集的建圖結果，圖9、10為采用fr1/floor數(shù)據(jù)集的建圖結果。其中圖7、9為前端沒有三角測量的RGB-D SLAM算法生成的點云圖，圖8、10則為使用本文算法最后生成的點云圖。獲得的數(shù)據(jù)結果如表3、4所示。

如圖7～10所示，圖8相較于圖7圖像輪廓更加清晰，圖像質量更好。而圖10相較于圖9圓圈處建圖范圍更廣（如圖中紅線處圈出的地板所示）。

下面采用圖像質量評判的方法對以上三組對比圖進行定量分析。本文采取的圖像質量評判標準分別為信息熵，亮度均值以及標準差。

1）圖像信息熵通過統(tǒng)計每個像素點灰度值的總期望得出，信息熵值大說明該圖像的輪廓更加清楚。

2）亮度均值是衡量一個圖像中像素的灰度值的平均數(shù)，亮度均值大的圖像說明該圖像亮度越大，點云建圖結果更加符合真實場景。

3）圖像標準差反應一個圖像高頻部分的大小，標準差大的圖像對比明顯，圖像更加清晰。

以上兩組對比圖像的像素分別為522×300，440×390。量化分析的實驗結果如表3、4所示：

從表3、4可以看出，對于fr1/desk、fr1/floor數(shù)據(jù)集，本文算法相較于ORB+PnP算法生成點的點云圖，雖然亮度均值及標準差相差無幾，但是信息熵都有明顯提高，分別提高了15.78%、6.64%。可見圖8、10的建圖結果更加清晰，圖像信息含量更高。

綜上所述，兩種數(shù)據(jù)集的測試結果都表明，使用本文算法構建的點云地圖更加清晰，本文算法確實通過三角化地圖點深度提高了構建的點云地圖的質量。

4 結 論

本文主要把傳統(tǒng)的RGB-D SLAM算法的前端視覺里程計部分進行改進，在保證了算法處理幀速度的同時，在前端視覺里程計部分加入了三角測量算法來提高定位的穩(wěn)定性。最后采用TUM數(shù)據(jù)集中的fr1系列數(shù)據(jù)集進行了均方根誤差，運行速度以及生成點云圖的實驗。實驗結果證明本方案在定位精度上有明顯提高，其中均方根誤差平均減少9.21%。改進后算法生成的點云圖相較于傳統(tǒng)算法生成的點云圖信息熵提高了11.21%，可以為后端的回環(huán)檢測提供良好的基礎數(shù)據(jù)。

fr1數(shù)據(jù)集使用的Kinect相機的運行速度是0.41m/s，旋轉速度是23.33deg/s，運行相對較慢。下一步的研究是如何在運行速度較快時實現(xiàn)準確的定位與建圖。

參考文獻：

[1] LEES H， CIVERA J. Loosely-coupled Semi-direct Monocular SLAM [J]. IEEE Robotices & Automation Letters， 2018， 4（2）： 399.

[2] KERL C， STURM J， CREMERS D. Dense Visual SLAM for Rgb-d Cameras[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. USA Piscataway： IEEE， 2014： 2100.

[3] HENRY P， KRAININ M， HERBST E， et al. Rgb-d mapping： Using Depth Cameras for Dense 3D Modeling of Indoor Environments[C]//The 12th International Symposium on Experimental Robotics. 2010：22.

[4] ENDRESS F， HESS J， STURM J， et al. 3-D Mapping with an Rgb-d Camera [J]. IEEE Transactions on Robotics， 2014， 30（1）： 177.

[5] ENGELHARD N， ENDRESS F， HESS J， et al. Real-time 3D Visual SLAM with a Hand-held Rgb-d Camera[C]//Proceedings of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum， Sweden Vasteras， 2011 ： 1.

[6] KERL C， STURM J， CREMERS D. DenseVisual SLAM for Rgb-d Cameras[C]// In 2013 IEEE/RSJ Internation Conference on Intelligent Robots and Systems， 2013：2100.

[7] LABBE M， MICHAUD F. Online Global Loop Closure Detection for Large-scale Multi-session Graph-based SLAM[C]// In 2014 IEEE /RSJ International Conference on Intelligent Robots and Systems， 2014：2661.

[8] MURARTAL R， TARDOS J D. Orb-SLAM： A Versatile and Accurate Monocular SLAM System[C]//IEEE Transactions or Robotics. IEEE， 2015 ： 1147.

[9] MURARTAL R， TARDOS JD. Orb-SLAM2： an Open-source SLAM System for Monocular， Stereo， and Rgb-d Cameras [J]. IEEE Transactions on Robotics， 2017， 35（5） ： 1255.

[10]陳邵， 郭宇翔， 高天嘯， 等. 移動機器人RGB-D視覺SLAM算法[J].農(nóng)業(yè)機械學報， 2018， 49（10） ： 38.

CHEN Shao， GUO Yuxiang， GAO Tianxiao， et al. Rgb-d Visual SLAM Algorithm for Mobile Robots [J]. Transactions of the Chinese Society for Agricultural Machinery， 2018， 49（10）： 38.

[11]李戈星， 劉士榮， 仲朝亮， 等. 基于改進關鍵幀選擇的RGB-D SLAM算法[J].大連理工大學學報， 2017，57（4）： 411.

LI Yixing， LIU Shirong， ZHONG Chaoliang， et al. Rgb-d SLAM Algorithm Based on Improved Key-frame Selection [J]. Journal of Dalian University of Technology，2017，57（4）： 411.

[12]HENRY P， KRAININ M， HERBST E， et al. Rgb-d Mapping： Using Kinect-style Depth Cameras for Dense 3D Modeling of Indoor Environments [J]. International Journal of Robotics Research， 2012， 31（5）： 647.

[13]丁文東， 徐德， 劉希龍， 等. 移動機器人視覺里程計綜述[J]. 自動化學報， 2018， 44（3） ：385.

DING Wendong， XU De， LIU Xilong， et al. Review on Visual Odometry for Mobile Robots [J]. Acta Automatica Sinica， 2018， 44（3）： 385.

[14]龔學銳， 閔華松. 基于ROS的慣性和視覺里程計的機器人室內(nèi)定位[J]. 組合機床與自動化加工技術， 2019， 5（5） ：106.

GONG Xuerui， MIN Huasong. Indoor Positioning by Visual-odometry Based on Ros [J]. Modular Machine Tool & Automatic Manufacturing Technique，2019，5（5）：106.

[15]張震， 鄭宏. 一種結合ORB特征和視覺詞典的RGB-D SLAM算法[J].計算機工程與應用， 2018， 54（12）： 170.

ZHANG Zhen， ZHENG Hong.Rgb-d SLAM Algorithm based on Orb Features and Visual Dictionary [J]， Computer Engineering and Applications， 2018， 54（12）：170.

[16]LEPETIT V， MORENO F， FUA P， et al. Epnp： an Accurate o（n） Solution to the PnP Problem [J]. International Journal of Computer Vision， 2008， 81（2）：155.

[17]RUBLEE E， RABAUD V， KONOLIGE K， et al. Orb： an Efficient Alternative to Sift or Surf[C]//International Conference on Computer Vision， 2011： 2564.

[18]董靜薇， 趙春麗， 海博. 融合同態(tài)濾波和小波變換的圖像去霧算法研究[J]. 哈爾濱理工大學學報， 2019， 24（1）：66.

DONG Jingwei， ZHAO Chunli， HAI Bo. Research on Image De-fog Algorithm based on Fusion Homomorphic Filtering and Wavelet Transform. Journal of Harbin University of Science and Technology， 2019， 24（1）：66.

[19]劉志斌， 吳顯亮， 徐文麗， 等. 視覺SLAM中的基于誤匹配風險預測的特征選擇[J].機器人， 2010，32（5）： 635.

LIU Zhibin， WU Xianliang， XU Wenli， et al. Erroneous Matching Risk Prediction based Feature Selection for Visual SLAM [J]. Robot， 2010， 32（5）：635.

[20]PRAKHYA SM， LIU B， LIN W， et al. Sparse Depth Odometry： 3D Keypoint based Pose Estimation from Dense Depth Data[C]//IEEE International Conference on Robotices and Automation， 2015： 4216.

[21]彭清漪， 夏林元， 吳東金. 基于光度與點線特征融合的半直接單目視覺定位算法[J]. 傳感器與微系統(tǒng)， 2020， 39（4）：110.

PENG Qingyi， XIA Linyuan， WU Dongjin. Semi-direct Monocular Visual Localization Algorithm based on Photometry and Point/line Feature Fusion [J]. Transducer and Microsystem Technologies， 2020， 39（4） ：110.

[22]方正， 趙世博， 李昊來. 一種融合稀疏幾何特征與深度流的深度視覺SLAM算法[J]. 機器人， 2019， 41（2） ： 185.

FANG Zheng， ZHAO Shibo， LI Haolai. A Depth Vision SLAM Algorithm Combining Sparse Geometric Features with Range Flow [J]. Robot， 2019， 41（2） ：185.

（編輯：溫澤宇）

收稿日期： 2015-05-31

基金項目： 國家自然科學基金（61601149）;黑龍江省自然科學基金（QC2017074）.

作者簡介：

董靜薇（1972—），女，教授，碩士研究生導師;

韓知雨（1994—），男，碩士研究生.

通信作者：

蔣燚銘（1994—），女，碩士研究生，E-mail：2585098146@qq.com.