改進ORB 特征的機器人RGB-D SLAM算法

伍錫如，黃榆媛，王耀南

1.桂林電子科技大學 電子工程與自動化學院，廣西 桂林 541004 2

.湖南大學 電氣與信息工程學院，長沙 410082

1 引言

隨著魚眼相機、深度相機和全景相機等先進設備的出現與計算機視覺技術的日益成熟，視覺同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）成為國內外學者的重點研究領域。特別是在機器人、AR/VR、自動駕駛和無人機等應用中涌現出許多創新性成果[1-5]。SLAM技術是機器人實現自主移動的關鍵技術[6]，基于視覺的SLAM相比于激光SLAM具有功耗低、成本低、小型化等優點，理論和應用價值突出[7]。2010年微軟公司推出的RGB-D深度相機Kinect，由于其低成本、輕量化、能采集豐富的圖像信息等優勢，廣泛應用到視覺SLAM技術的研究和開發中[8-11]。文獻[8]首次采用深度相機進行室內環境3D密集地圖的構建。Endres F等人[10]設計了一個經典的RGB-D SLAM系統，該系統具有較好的魯棒性和準確性，能在較長軌跡和有挑戰性的場景中追蹤機器人的運動。文獻[11]中介紹了第一個基于單目、雙目和RGB-D相機的完整SLAM系統，全程使用ORB[12]特征，兼具地圖重用、閉環檢測與重定位的功能，并且可實時在CPU上工作。

經過30多年的發展，機器人視覺SLAM的整體算法框架包括：前端視覺里程計、后端優化、回環檢測和地圖構建。從概念上來說，機器人視覺SLAM需要完成的工作是用視覺傳感器采集到的圖片數據，精確地估計出機器人的運動位姿，并同步構建周圍場景的地圖，以便于進行后續的路徑規劃或運動控制[13]。早期的視覺SLAM沿用機器人領域的濾波方法，如卡爾曼濾波[14]、擴展卡爾曼濾波[15]、粒子濾波[16]等，而現在應用計算機視覺領域的圖優化方法成為主流[17-21]。文獻[18]的視覺SLAM系統最早引入關鍵幀機制，通過串接關鍵幀而非采集到的全部圖片完成軌跡和地圖的優化。文獻[19]總結和分析了基于關鍵幀的SLAM系統，并對SLAM研究中存在的外部干擾、地圖維護、重定位等問題提供指導意見。KinectFusion[20]是Newcombe等人設計的首個用Kinect深度相機完成的三維重建系統，它從深度圖中讀取點云數據，根據迭代最近點（Iterative Closest Point，ICP）算法估計位姿來拼接點云，基于網格模型實時重建場景。Kintinuous[21]是KinectFusion的改進版本，增加回環檢測與優化，融合直接法使得處理速度更快，適用于大場景重建。文獻[10]的RGB-D SLAMv2是一個優秀且完整的SLAM系統，有前后端、閉環檢測和地圖構建，在使用SIFT特征時要用GPU加速。從這些優秀的文獻工作來看，在靜態不變、無人為干擾、光照不明顯的應用環境中，視覺SLAM技術已經發展得較為成熟。但是現實的工作環境往往更加復雜多變，如何在已有的系統框架上通過改進算法提升視覺SLAM技術的實時性、魯棒性和準確性，從而應用到實際場景中，成為該技術領域的一大挑戰和難題。

本文提出一種新的RGB-D SLAM算法，旨在提高機器人視覺SLAM算法實時性、定位和建圖精度。主要貢獻在于：（1）基于四叉樹（Quad-tree，Q-tree）算法改進ORB特征提取，減少冗余點，加速誤匹配點的剔除；（2）應用EPnP和ICP組合算法求解得到較精準的機器人位姿；（3）基于關鍵幀和詞袋（Bag of Words，BoW）模型構建快速回環檢測，減小累積誤差；（4）使用光速法平差（Bundle Adjustment，BA）優化位姿圖得到最優軌跡，構建3D彩色點云地圖，并以八叉樹地圖（Octomap）形式進行存儲。通過公開數據集和實際移動機器人平臺驗證算法性能。

2 機器人RGB-D SLAM系統架構

圖1 機器人RGB-D SLAM系統框圖

根據文獻[10]的系統框架，本文的機器人RGB-D SLAM系統框圖如圖1所示。前端負責讀取傳感器數據和估計位姿。Kinect相機作為環境感知設備能同時讀取RGB彩色圖和深度圖。本文選擇特征法開展研究，因為它不易受動態障礙物、光線等的影響，可穩定運行在移動機器人平臺。常用的圖像特征有SIFT[22]、SURF[23]和ORB，其中的ORB算法因為較優的快速性和實時性得到廣泛應用。為使提取的ORB特征點均勻分布，本文先引入Q-tree來管理FAST角點，然后采用快速最近似最近鄰（Fast Library for Approximate Nearest Neighbors，FLANN）算法匹配特征，最后在隨機采樣一致性（Random Sample Consensus，RANSAC）算法的基礎上，迭代使用EPNP和ICP方法求解不同坐標系下的幾何關系，獲得機器人位姿。由于相機獲取的數據存在不可避免的外界干擾，位姿估計必然會出現累積漂移，所以需要回環測和后端優化來減小累積誤差，進一步優化位姿。回環檢測的目的是判別機器人是否重新回到之前已經走過的區域，檢測到的有效回環信息能將具有累積誤差的軌跡拉回正確的地方。實現回環檢測的原理是對關鍵幀進行數據相似性的計算，具體的方法是使用BoW模型來創建字典，通過字典對關鍵幀之間的相似性做出量度，進而判斷回環的存在。在后端中，需要用到優化理論構造圖優化模型，這個模型使用估計出的位姿和回環檢測數據構造位姿圖，通過BA算法優化位姿圖的結構，得到長時間內機器人的最優軌跡，從而構建全局一致的機器人運動軌跡和環境地圖。這部分工作將基于通用圖形優化器G2O進行求解。在已知機器人的最優軌跡后，可通過軌跡來重建環境地圖。3D點云地圖數據規模較大，為降低硬件內存要求，使用緊湊的Octomap進行數據存儲。Octomap易于信息的壓縮和更新，同時也可以根據應用需求調節分辨率得到不同精度的導航地圖。

3 機器人RGB-D SLAM算法實現

本章介紹整個算法的實現流程。圖2是整體算法流程圖。該算法的輸入數據為Kinect深度相機于同一時間戳采集到彩色RGB圖像幀和深度圖像幀，它們在時空上是對齊的。算法前端，對RGB圖像幀使用基于四叉樹的ORB特征算法來提取均勻的Fast關鍵點，獲得這些關鍵點的BRIEF描述子；通過FLANN算法完成描述子的特征匹配，并根據篩選條件剔除錯誤匹配點對；按照RANSAC迭代方式，結合EPNP和ICP算法估計出機器人的位姿。同時，每一時間戳的圖像幀經過關鍵幀判別后存儲到關鍵幀數據集中，該集合經冗余處理后用于詞袋模型的回環檢測。在后端，前端計算的機器人位姿和回環檢測數據被構造成位姿圖，基于BA算法求解得到最優的機器人位姿軌跡。根據最優軌跡和對應的3D點云重建出運行環境的彩色點云地圖，點云地圖在轉換后被表示成緊湊的Octomap地圖。

圖2 整體算法流程圖

3.1 前端

3.1.1 基于Q-tree的ORB特征提取

相比于像素或直方圖的圖像內容描述方法，基于外觀的圖像底層特征保留了豐富的原始信息，能更好地區分和表達圖像內容。根據硬件條件和實時性的需求，選擇同時兼顧特征檢測速度和穩定性能的ORB算法對場景內容進行描述。

（1）基于Q-tree的FAST關鍵點檢測

對同一張圖片提取不同的空間尺度信息，得到圖像局部和全局尺度上的一系列多分辨率圖像集合，構建高斯金字塔（見圖3），再從每一層中進行FAST角點檢測，解決FAST算法本身沒有尺度不變特性的問題。

圖3 多尺度高斯金字塔

采用灰度質心法計算FAST關鍵點的主方向，增加旋轉描述。假設點O(x,y)是某一待檢測的特征點，取包含點O的一個小圖像塊U計算(p+q)階鄰域矩：

式中，I(x,y)為點O的灰度值。

通過鄰域矩來求圖像塊U的質心C：

連接O和C獲得方向向量OC，則關鍵點O的主方向為：

均勻地對環境進行采樣是進行良好特征匹配的前提，但在研究中發現檢測出的FAST角點在一張圖片中會堆積成點斑，不利于后期的特征匹配。本文結合Q-tree均勻算法[24]加以改進，減少冗余特征點的同時，能使提取的關鍵點更均勻地遍布在整個圖像中。

圖4是Q-tree區域分割及數據存儲的示意圖。對每張圖像提取到的FAST角點進行Q-tree均勻處理，步驟如下（一個子區域對應樹狀結構中的一個子節點）：

步驟1初始化父節點O，將2D空間均勻分割成4個子區域(A,B,C,D)，得到初始的Q-tree結構。

圖4 Q-tree數據結構示意圖

步驟2將檢測出的FAST角點映射到初始Q-tree中，根據角點位置把角點劃分到不同的子區域。

步驟3按照子區域存在的角點數量判斷是否進行再分割：若角點個數小于等于1，則不再分割；反之，則再次均勻四分割并劃分角點。

步驟4重復步驟3，直至遞歸劃分出的子區域中存在的角點個數小于等于1或達到期望提取的角點數量為止。

步驟4中停止分割的條件是每個子區域中的角點數都不大于1，這在實際中很難達到，因為檢測出的角點如存在扎推現象，相鄰的多個角點幾乎是重疊的，所以當子區域的角點個數大于1時，需要通過非極大值抑制[23]條件，只保留Harris響應強度最大的較優角點，去除Harris響應強度小的冗余點。

（2）BRIEF描述子計算

BRIEF[25]用隨機點對生成的描述子的抗噪性能不佳，處理方法是使用積分圖做平滑處理，通過高斯濾波降噪。依據取法之間相關性最小的規則，在特征點的31×31像素鄰域中選擇5×5子窗口對，比較隨機N對子窗口對的像素灰度值之和得到一個N維的二進制描述子，對于一個平滑后的圖像鄰域Δ：

其中，λ(Δ;g,h)表示二值化的結果，I(Δ,g)和 I(Δ,h)是鄰域Δ內的兩個對應子窗口g、h各自的灰度值之和。fN(Δ)是獲得的二進制比特串，N=256。

依據FAST關鍵點的主方向對描述子進行旋轉，克服BRIEF描述子不具備旋轉不變性的缺點。這個方向信息用關鍵點31×31像素鄰域內的N對(xj,yj)構成2×N的矩陣Τ：

由旋轉矩陣R和式（3）中的主方向φ對Τ旋轉，得到校正后的Τφ矩陣：帶方向的描述子[12]可表示為：

在檢測出特征點并計算對應描述子后，通過描述子計算圖片間的漢明距離進行匹配。暴力匹配法對于特征點多的場景運算成本高，不利于機器人SLAM的實時運行，選用FLANN[26]算法完成特征匹配。同時根據工程經驗，用漢明距離小于最小距離的2倍為篩選依據，去除錯誤的匹配點對。

3.1.2 機器人位姿估計

在使用深度相機獲得深度圖的情況下，可以得到特征點的距離信息，所以根據ORB特征匹配后的結果對機器人的位姿進行估計，通常可以考慮兩種情形，一種是根據已知3D點與其在相機中的投影2D點求解PNP問題，另一種就是使用匹配好的兩組3D點云求解ICP問題。在SLAM中，常見的一些做法是使用P3P或者非迭代的EPNP算法估計位姿再進行優化，這種做法沒有有效利用更多的匹配點信息，而直接使用ICP算法求解則需要依賴較好的初始值，不然極易在迭代中陷入局部最優，且在深度相機不能獲得距離信息后難以執行，可見這些方法在噪聲干擾或誤匹配時會導致算法失效。本研究在這些算法的基礎上進行優化，使用RANSAC算法簡單有效地過濾誤匹配，通過迭代的方式提高匹配點對的信息利用率，把良好的匹配數據應用到EPNP[27]算法中改善穩健性，在有了較好的初始值情況下，進一步使用ICP[28]算法求解機器人位姿。

EPNP算法的思想是用4個非共面虛擬控制點的加權和表示世界坐標系下的3D點坐標。為保證算法的穩定性，需要根據世界坐標系下匹配點集的質心以及主成分分析法得到的三個主方向選擇控制點。它每次隨機地從所有匹配好的點對中選擇四個點對進行計算，匹配點相對于世界坐標系的坐標為,i=1,2,…,n ，相對于相機坐標系的坐標為,i=1,2,…,n，同時，假設4個控制點的坐標表示那么在世界坐標系下，匹配點與4個虛擬控制點的線性關系為：

由于相機坐標系與世界坐標系是相似變換的關系，在相機坐標系下也存在：

根據相機的投影模型有：

式中，si為尺度因子，[uivi]T表示 pci對應的2D投影坐標；K是相機的內參數矩陣，其中 fu,fv代表相機焦距，(uc,vc)為相機中心，該矩陣可以通過標定獲得。

將公式（10）和（11）代入公式（12）中展開得：

把式（14）寫成矩陣形式：

向量x位于M的右零空間[27]，存在下列關系：

其中，vi是矩陣M的零特征值相應的N特征向量，通過計算MTM的特征向量可以解出vi。βi的求解與N的取值有關，詳細求解參照文獻[27]。

求解出βi后，依據公式（16）就能得到x，即相機坐標系下4個虛擬控制點的坐標。再將x代入式（11）計算出匹配點在相機坐標系下的3D坐標。

在已知匹配點在世界坐標系下和相機坐標系下的兩組3D點后，使用ICP[28]算法估計機器人的位姿，獲得旋轉矩陣R和平移向量t。算法的操作流程為：

（1）假設兩組點分別為P和Q：

存在歐式變換R,t，滿足?i,pi=Rqi+t。

（2）第i對點的誤差定義為：

構建最小二乘的目標函數F，求出一個R,t使誤差平方和得到極小值：

（3）求解式（19），先計算兩組點的質心坐標 ?,?′，得到每個點的去質心坐標κi,κ′i：

（4）由優化的目標函數求旋轉矩陣R：

（5）按照（4）中的 R，進一步求出t：

至此，就能求得機器人的估計位姿R,t。

用每次估計出的位姿計算反投影誤差，確定內點的數量，然后通過內點數量判斷出有效的位姿估計，最后選擇具有最大內點數的位姿作為此次的最準確估計。在RANSAC算法框架中，以上過程不斷迭代，獲得當前幀對應的最優位姿估計。

3.2 基于關鍵幀的回環檢測

根據降低控制問題規模和保證稀疏性的需要，采用關鍵幀技術進行有效采樣能提高視覺SLAM系統的實時性和穩定性。而且在后端優化中使用成功的回環信息有利于構建全局一致的軌跡和地圖。

3.2.1 關鍵幀選擇

兩張圖片之間相似性的可靠測量是許多關鍵幀選擇算法的基礎。本文研究了基于特征匹配的相似性量度S[29]的方法。S與兩張圖片的距離函數Λ相關：

式中，Σt和Σi分別表示當前圖像幀Ft及該幀對應的最近上一關鍵幀(KF)i中視覺特征的個數，M表示兩個圖像幀匹配的視覺特征個數。Λmax和Λmin是圖像之間距離函數Λ期望取到或者能夠取到的最大值和最小值，S 的取值范圍是[0，1]。

將系統輸入的第一幀作為初始關鍵幀，下一個關鍵幀的選擇判據有兩個：（1）距離上一個關鍵幀插入已經過去超過20幀；（2）檢測到的內點數要大于設定的最小閾值，并且相似性量度S小于設定的最大閾值。滿足（1）、（2）中一個條件即將當前幀設為關鍵幀。

3.2.2 BoW模型

BoW模型的建立是基于外觀的，它應用于回環檢測時可以脫離視覺SLAM系統前、后端估計的幾何信息，比基于圖像特征的回環檢測方式更加快速和有效。在BoW中涉及單詞和字典的概念，每個單詞代表某一類特征的組合，多個單詞組成字典。根據文獻[30]，使用類似于層次聚類的k叉樹來生成字典。圖5是深度為l的k叉樹字典數據結構圖。從所有訓練圖片中提取ORB特征，在根節點用k-means++算法均勻地將所有特征點進行層次聚類。這里使用文獻[11]中的ORB特征類型字典用于回環的檢索。

圖5 k叉樹字典結構

回環檢測前要刪除關鍵幀集合中的冗余幀，刪除的標準是如果有不少于3個的其他關鍵幀能夠觀測到某個關鍵幀90%以上的路標點，則把該關鍵幀視作冗余關鍵幀并剔除[11]。回環檢測在去除冗余幀的關鍵幀集合中進行，通過比較不同關鍵幀間的相似性判斷回環，將回環信息作為約束交給后端處理。

3.3 后端優化與建圖

3.3.1 基于BA的圖優化

視覺SLAM中的優化問題可以用一個由頂點和邊組成的圖表示，通過優化圖的結構獲得待優化變量的最優解。圖6給出了簡單的圖優化模型。路標點和位姿作為頂點是待優化的變量，使用運動模型和觀測模型得到頂點之間的約束邊，可以是估計位姿約束或者回環約束。

圖6 圖優化模型

基于圖優化模型，視覺SLAM的優化能直觀地用數學語言表達成一個非線性最小二乘問題，通過BA[31]方法在G2O庫中求最優解。視覺SLAM過程的基本數學表示包括運動方程xk和觀測方程zk。

這里的uk是傳感器的輸入，wk為噪聲。pj是在位置xk觀測到的路標點，vk,j為觀測中的噪聲。

由公式（25）知道，運動和觀測過程都存在噪聲干擾，實際的觀測值和用算法計算出來的觀測值存在誤差，圖模型的邊就代表了這個誤差項e。本研究只對位姿xk做優化處理，優化的目標函數F(X)為：

其中，X是全部位姿變量xk的集合，D代表估計位姿的時間序列；xi,xj是i、j時刻估計出的機器人位姿，zij表示這兩個頂點的實際約束，而z?ij是通過計算得到的約束；Ωij代表約束的信息矩陣，即兩頂點不確定性的度量，其中的每個Ωij作為ei、ej的系數，是對ei、ej誤差項相關性的預估。此時，優化問題的求解變成尋找一個最優位姿序列X?，使得目標函數F(X)取得最小值，即：

考慮優化算法的魯棒性，對目標函數用魯棒Huber核函數包裝，避免優化過程專注于錯誤數據的優化，而忽略正確約束的影響。整體實現在G2O庫中進行，使用列文伯格-馬夸爾特下降策略和設置稀疏求解器完成。

3.3.2 地圖構建

構建地圖是對機器人運行環境進行建模的過程。把關鍵幀數據處理成點云，根據優化后的位姿使用點云庫PCL拼接點云，得到3D彩色點云地圖。目前由離散路標點構建的稀疏點云地圖很難與導航、避障、路徑規劃等應用接軌，并且數據的存儲對硬件內存要求高。研究中將點云地圖轉化成Octomap，八叉樹節點能保留該點的占據情況，樹狀存儲結構占據內存小，也使得數據的更新和維護靈活和快速。在Octomap庫實現點云地圖到Octomap的轉換，其可視化界面有查看、縮放旋轉、調節分辨率等操作。

4 實驗與分析

為了驗證本文算法的可行性和高效性，使用公開的RGB-D數據集進行驗證，并在室內環境對多履帶式全向移動機器人開展實驗。通過對比實驗分析設計算法的實時性、定位及建圖效果。

4.1 實驗準備

全部實驗在Ubuntu16.04 LTS環境下完成，硬件平臺是64 bit的ThinkPad-T460 PC，處理器為Intel Core i5-6200U@2.30 GHz×4。實驗數據是德國慕尼黑工業大學的TUM數據集[32]，數據序列的詳細信息見表1。

表1 實驗數據的信息

實驗前，在ROS Kinetic環境下對Kinect v1（見圖7）進行標定，使用8×6×0.028的棋盤格，得到的結果如下。

相機內參數矩陣：

畸變參數：

D=[0.005 7-0.085 5 0.012 88-0.000] 3 0 （30）

實驗中，主要參數配置有：相機幀率30 f/s，每張圖片提取的ORB特征點數量為1 000，尺度金字塔層數設為8且層與層之間的尺度因子為1.2。

圖7 Kinect v1與標定實驗

4.2 改進算法的相關性能分析

4.2.1 基于Q-tree的ORB算法檢測性能

圖8為使用基于Q-tree均勻算法的FAST角點檢測處理前后的角點分布情況對比。顯然，與原始的ORB算法相比，經過改進算法處理后，可以在整張圖像中提取出分布較均勻的角點。由式（23）、（24）可知，誤匹配點對會嚴重影響圖像相似度的計算結果，這里使用FLANN算法快速匹配特征并剔除誤匹配對，獲得良好的圖像配準結果（見圖9）。可見，改進處理后的ORB特征算法具有較穩定和優越的場景表達性能，適用于描述和匹配圖像。

圖8 角點檢測對比

圖9 改進算法的特征匹配對比圖

4.2.2 實時性和定位誤差對比

使用表1的數據，將RGB-D SALMv2[10]算法與改進算法的運行時間、均方根誤差（Root Mean Square Error，RMSE）進行對比，統計數據于表2。

表2 RGB-D SLAMv2算法與改進算法的性能指標對比

由表2可以看出，本文算法的運行時間和定位誤差比RGB-D SLAMv2算法顯著減少，算法實時性和準確性得到極大提升。針對這12個數據序列，從數據處理時間來看，RGB-D SLAMv2算法平均每幀數據的處理時間約為0.153 s，本文算法的處理時間約為0.080 s，從兩種算法估計出的軌跡與地面真實軌跡的RMSE來看，本文算法的平均RMSE低于RGB-D SLAMv2算法，其中RGB-D SLAMv2的平均RMSE約為0.258 m，本文算法的平均RMSE約為0.084 m。根據以上指標的統計結果，證明改進算法有更好的定位實時性和準確性。

為了更加直觀地顯示本文算法的定位準確性，采用文獻[33]的定量評價方法對估計軌跡與真實地面軌跡進行誤差量化分析。圖10是估計軌跡與真實軌跡的對比圖，其中（a）、（b）、（c）和（d）分別對應表1的fr1/xyz、fr1/floor、fr2/pioneer_360和fr2/pioneer_slam數據序列，灰色虛線reference表示真實軌跡，帶顏色的實線表示算法的估計軌跡，圖右側從紅到藍的色帶，顯示誤差的大小程度。圖11為4組數據序列的估計軌跡與真實軌跡絕對位姿誤差（Absolute Pose Error，APE）隨時間的變化趨勢曲線，表3為誤差量化指標數據，包括最大值max、最小值min、均值mean、中位數median和標準差std。

表3 誤差量化指標 m

圖10 估計軌跡與真實軌跡對比圖

圖11 絕對位姿誤差變化趨勢圖

從圖10可以看出估計軌跡能準確貼合真實軌跡，全局一致性較好。分析圖11的趨勢曲線和表3的數據可知，算法在類似fr1代表的靜態室內環境中誤差值與變化波動都較小，而類似fr2代表的移動機器人上采集的數據序列，由于機器人的運動會有一些較大的誤差波動，但經過本文算法處理后，誤差會平緩減小，保證總體誤差在較小的可接受范圍。

4.3 建圖效果評估

本節用公開的TUM數據集和實驗室機器人平臺自測數據進行實驗，以便評估算法的建圖效果。

4.3.1 TUM數據集建圖

將本文算法與RGB-D SLAMv2算法構建的地圖進行對比。圖12（a）、（b）和（c）分別表示使用fr1/xyz、fr1/desk2和fr2/pioneer_360數據序列構建的三維地圖，每組對比圖的左邊是用RGB-D SLAMv2算法構建的地圖，右邊是用本文算法構建的地圖。可見RGB-D SLAMv2算法和本文算法都能有效地構建環境地圖，有較好的三維重建效果。其中對于室內具有較多物件的豐富場景，兩種算法構建的地圖沒有明顯的優劣差距，但是對于大范圍且較空曠的環境，改進算法的優勢比較明顯，構建的地圖更清晰直觀。

4.3.2 自測實驗場景建圖

圖13 實驗平臺及環境

圖14 自測實驗場景下的建圖對比

圖13 給出了機器人平臺和實驗環境。圖（a）為自主開發的多履帶式全向移動機器人，圖（b）是實驗場景。圖14（a）、（b）為兩種算法構建出的環境地圖對比，圖（c）是由圖（b）轉換的Octomap。在實驗過程中，發現機器人在跑RGB-D SLAMv2算法時會出現算法中斷的現象，為保證實際建圖，用手柄操作機器人在實驗室中間空曠地面向前移動，均勻且緩慢運行。相比之下，本文算法運行較穩定，在一定程度上能減少大部分的光照干擾，使用關鍵幀的建圖方式忽略了不必要的部分場景，地圖更簡潔直觀。

5 結束語

本文提出了一種改進的機器人RGB-D SLAM算法，通過Q-tree均勻算法改進的ORB特征提取能有效剔除冗余特征點，使得保存下來的較優特征點均勻分布，從而促進良好的點對匹配，減少了錯誤匹配對機器人位姿估計帶來的不利影響；在解算機器人位姿時，應用EPNP和ICP算法的組合優化模式，充分利用更多數據在迭代過程中獲得最優的思想，提升機器人位姿估計的準確度；進一步地應用關鍵幀的有效回環和圖優化技術減少累積誤差，達到使優化后的運動軌跡和地圖全局一致的目的。基于實驗研究和對比，在TUM數據集和機器人平臺的實驗結果表明，算法的實時性及魯棒性提高，估計的軌跡能較好地貼合數據集的真實軌跡，重建出清晰直觀的場景地圖。

進一步的研究會集中在聯合激光雷達、IMU等較精確的設備優化位姿估計，引入深度學習方法賦予機器人語義識別能力并創建實時八叉樹地圖實現機器人的自主導航控制等。