基于改進ORB-SLAM2的果園噴藥機器人定位與稠密建圖算法

叢佩超 崔利營 萬現(xiàn)全 李佳星 劉俊杰 張 欣

(廣西科技大學機械與汽車工程學院, 柳州 545616)

0 引言

當前,我國水果品類豐富、產(chǎn)量穩(wěn)居世界第一,但其種植區(qū)域多分布在丘陵地區(qū)。受地形條件影響,果園的農(nóng)藥噴施還以人工操作為主,該方式存在成本高、效率低、危害身體健康、環(huán)境污染大等問題。為解決上述問題,用于除草、噴藥、采摘等工作的果園機器人不斷涌現(xiàn),其相關(guān)技術(shù)的研究已成為機器人領(lǐng)域的熱點問題[1-2]。

果園噴藥機器人是將環(huán)境感知、目標識別、導(dǎo)航定位和運動控制等技術(shù)高度集成的智能化系統(tǒng),精準定位技術(shù)是其中的關(guān)鍵問題之一。目前廣泛使用的定位方法有:全球?qū)Ш叫l(wèi)星系統(tǒng)(Global navigation satellite system, GNSS)、激光雷達定位、視覺定位[3-4]。室外場景下GNSS的應(yīng)用最為廣泛,其無遮擋情況下的定位精度可達厘米級[5-7]。雖然衛(wèi)星定位在室外空曠環(huán)境下的表現(xiàn)良好,但在枝葉茂密的果園環(huán)境中,遮擋造成的衛(wèi)星信號丟失,會嚴重影響定位精度[3]。激光雷達定位精度高、抗干擾能力強[8-9]。激光雷達雖能獲得較高精度的位置信息,但在運動過程中易產(chǎn)生點云畸變和誤匹配,且多線激光雷達價格較高[10]。視覺定位通過單目、雙目和RGB-D相機不僅能夠獲得豐富的位置信息,而且其硬件成本低廉,功耗低、體積小、安裝方便,適用于大范圍的工作場景,被廣泛地應(yīng)用于移動機器人的導(dǎo)航問題[11]。文獻[12-14]研究的視覺定位技術(shù)通常先由相機獲取環(huán)境的目標信息,再通過圖像分割等方法對目標圖像進行處理,并利用Hough變換擬合導(dǎo)航路線,提高導(dǎo)航的定位精度。然而,果園噴藥機器人的作業(yè)環(huán)境復(fù)雜多樣,茂密的枝葉易導(dǎo)致圖像分割失敗,繼而影響果園噴藥機器人導(dǎo)航過程中的定位精度。視覺同時定位與建圖(Simultaneous localization and mapping,SLAM)技術(shù)可較好地解決復(fù)雜場景下的定位與建圖問題,已成為當前農(nóng)業(yè)機器人領(lǐng)域的研究熱點。

迄今為止,比較經(jīng)典的視覺SLAM算法有MonoSLAM[15]、PTAM[16]、SVO[17]、ORB-SLAM[18]等。其中,MUR-ARTAL研究團隊[18]提出的ORB-SLAM系統(tǒng),利用單目相機完成了全自動初始化,其在室內(nèi)、外環(huán)境下均可實時運行,且具有較好的魯棒性,與當時其它單目SLAM算法相比,實現(xiàn)了前所未有的性能。之后,該團隊[19]又提出了一種基于特征點法的ORB-SLAM2算法,它主要由跟蹤、局部建圖、回環(huán)檢測三大線程組成,在大場景下實現(xiàn)了移動機器人的高精度定位與稀疏點云地圖構(gòu)建。李盛輝等[20]提出了一種基于全景相機的視覺SLAM算法,該方法可應(yīng)用于真實農(nóng)業(yè)環(huán)境下的智能車輛自主導(dǎo)航,其定位平均誤差0.108 m。李月華等[21]提出了一種基于人工信標修正的視覺SLAM算法,該算法利用雙目相機實現(xiàn)了工廠環(huán)境下自動引導(dǎo)車的自主定位,其定位誤差不超過0.1 m。黎達等[22]提出了一種雙目視覺SLAM算法,利用地面、樹木等周圍環(huán)境的紋理信息,可實時確定相機傳感器的空間位置和姿態(tài),其均方根誤差不超過0.9 m。董蕊芳等[23]提出了一種改進型的ORB-SLAM2算法,通過融合環(huán)境的3D、2D占據(jù)信息、路標點空間位置以及視覺特征等信息,實現(xiàn)了輪式機器人在森林公園內(nèi)的準確定位,其定位誤差不超過1.43 m。上述視覺SLAM方法在農(nóng)、林等復(fù)雜環(huán)境下的定位雖然取得了一定的效果,但相應(yīng)的精度還有很大提升空間,特別是針對稠密果園環(huán)境的視覺定位和建圖方法仍然比較欠缺[24]。因此,研究適用于果園噴藥機器人的定位與稠密建圖方法,具有重要的現(xiàn)實意義[25]。

基于以上分析,為進一步改善果園噴藥應(yīng)用場景下機器人定位與建圖能力,本文提出一種改進的ORB-SLAM2定位與稠密建圖算法,以O(shè)RB-SLAM2為基礎(chǔ),圍繞圖像像素亮度的差異性,引入噪聲抑制機制與高斯濾波,優(yōu)化ORB特征點的提取過程;借助局部建圖線程的優(yōu)質(zhì)關(guān)鍵幀,運用點云濾波處理與位姿處理技術(shù),構(gòu)建稠密建圖線程。

1 算法框架及視覺SLAM理論基礎(chǔ)

1.1 算法框架

ORB-SLAM2算法是基于特征點法的實時SLAM開源算法,但其構(gòu)建的三維稀疏點云地圖僅保留圖像的部分ORB特征點,且建立的地圖表征不夠充分,缺少占據(jù)信息,難以描繪地圖中的障礙物,因此,ORB-SLAM2算法僅適用于機器人的空間定位[23]。此外,果園環(huán)境復(fù)雜多樣、紋理缺失,也易造成果園農(nóng)藥噴灑機器人的視覺定位精度不足,無法滿足路徑規(guī)劃等任務(wù)的需求[26]。針對上述問題,本文提出基于改進ORB-SLAM2算法的果園噴藥機器人定位與稠密建圖算法,如圖1所示。該算法基于ORB-SLAM2算法框架,保留原有的局部建圖與回環(huán)檢測線程[19],在跟蹤線程中優(yōu)化FAST角點、描述子閾值,利用高斯濾波對圖像進行處理,并采用圖像下采樣方法形成高斯圖像金字塔,實現(xiàn)ORB特征提取過程的優(yōu)化,以降低劣質(zhì)特征點數(shù)量,提高算法的關(guān)鍵幀質(zhì)量及特征匹配準確率,從而改善算法的定位精度。此外,本文算法增加稠密建圖線程,將相機周圍場景以三維稠密點云地圖的形式呈現(xiàn),實現(xiàn)稠密地圖的構(gòu)建功能,以備果園噴藥機器人后續(xù)自主導(dǎo)航。

圖1 基于改進ORB-SLAM2算法的視覺定位與稠密建圖算法框架

1.2 視覺SLAM理論基礎(chǔ)

為實現(xiàn)果園噴藥機器人的高精度定位與三維稠密地圖構(gòu)建,需有豐富的圖像信息作為保證?；诮?jīng)濟性考慮,本文采用Astra RGB-D相機。果園噴藥機器人通過RGB-D相機獲取環(huán)境信息,如圖2所示,機器人在pose1、pose2、…、poseN處觀測果園環(huán)境中路標點y1、y2、…、yn。通過視覺SLAM算法處理圖像數(shù)據(jù),根據(jù)相機的運動方程、觀測方程估計相機位姿,再借助坐標系轉(zhuǎn)換關(guān)系確定機器人的位置與姿態(tài),以實現(xiàn)其自身的精準定位[27]。

圖2 果園場景下的相機運動狀態(tài)

RGB-D相機的運動方程為

xt=f(xt-1,pt,qt)

(1)

其觀測方程為

zt,i=h(yi,xt,wt,i)

(2)

式中xt——相機t時刻位置

pt——測量t時刻數(shù)據(jù)

qt——測量t時刻噪聲

wt,i——觀測t時刻路標點噪聲

yi——相機觀測到的路標點

zt,i——相機在xt處對路標點yi的觀測數(shù)據(jù)

Astra RGB-D相機由RGB彩色相機、紅外傳感器和接近傳感器3部分組成,結(jié)構(gòu)如圖3所示。該相機通過搭載的光學相機和紅外相機獲取RGB圖像,利用紅外結(jié)構(gòu)光原理獲取目標圖像對應(yīng)的深度信息,為三維稠密地圖重建提供豐富的三維場景點云數(shù)據(jù),再由相機點云坐標系轉(zhuǎn)換為全局點云坐標系,最終實現(xiàn)環(huán)境信息重建[28]。

圖3 Astra RGB-D結(jié)構(gòu)

點云坐標系變換關(guān)系式為

Wo,j=To,ciWci,j

(3)

式中To,ci——第i個關(guān)鍵幀位姿

Wci,j——第i個關(guān)鍵幀的坐標系點云

Wo,j——全局坐標系點云

2 同時定位與三維稠密地圖構(gòu)建

2.1 ORB特征點提取優(yōu)化

ORB特征由Oriented FAST角點、BRIEF描述子組成,其不僅計算速率和精度較高,還具備旋轉(zhuǎn)、光照和尺度的不變性,以及匹配速度快、實時性高等特點。因此,ORB特征點提取在算法中起著至關(guān)重要的作用,其結(jié)果將直接影響定位的精度及建圖效果[29]。但ORB-SLAM2提取的特征點分布不均勻,且輸出特征點重疊較多、質(zhì)量較差。為解決該問題,本文提出一種如圖4所示的特征點提取算法,其主要由5部分組成:FAST角點提取、BRIEF描述子計算、旋轉(zhuǎn)角度計算、圖像金字塔層數(shù)優(yōu)化、高斯處理。

圖4 特征點提取流程圖

本文算法根據(jù)像素亮度不同進行FAST角點提取與描述子計算,引入噪聲抑制機制,以優(yōu)化ORB特征點的處理流程,如圖5所示。首先,在圖像中選取像素塊P,假設(shè)其像素亮度為LP,并設(shè)置亮度閾值為A0。以P為中心,在半徑為r的圓上選取16個像素點,若存在連續(xù)12個像素點的亮度不在區(qū)間[LP-A0,LP+A0]之內(nèi),則像素塊P可列為優(yōu)質(zhì)特征點。對圖中所有像素均重復(fù)以上步驟,篩選出圖像中所有的優(yōu)質(zhì)特征點。其次,利用OpenCV庫中的imread函數(shù)檢測出Oriented FAST角點位置,依據(jù)角點位置計算BRIEF描述子,設(shè)置其提取數(shù)量閾值范圍為[A1,A2],并在描述子閾值范圍內(nèi)選取關(guān)鍵點,且剔除范圍以外的關(guān)鍵點,以提高SLAM系統(tǒng)特征點的質(zhì)量。最后,采用OpenCV庫中的BruteForceMatcher函數(shù)結(jié)合漢明距離對2幅圖像的描述子進行匹配,計算相鄰點之間的極小值距離,若描述子間漢明距離大于2倍極小值,判斷其為誤匹配。此時,運用隨機采樣一致性算法減少誤匹配,抑制噪聲,剔除外點,以提高系統(tǒng)定位精度。

圖5 ORB特征點提取

ORB-SLAM2算法構(gòu)建圖像金字塔是對原圖像進行多尺度結(jié)構(gòu)表達。首先,按照圖像比例因子進行逐級向下采樣;其次,采樣后的圖像按不同分辨率由高到低向上排列,進行不同層的角點匹配,從而解決尺度不變性問題。但當描述子數(shù)量較少時,易出現(xiàn)特征點提取與匹配精度下降的問題[30]。為解決上述問題,通過優(yōu)化圖像金字塔層級,對圖像進行高斯濾波,利用OpenCV的pyrDown函數(shù)形成高斯圖像金字塔,如圖6所示。高斯圖像金字塔從第K層生成第K+1層,利用高斯濾波對第K層進行卷積,刪除所有的偶數(shù)行與列,具體計算過程為

圖6 高斯圖像金字塔

圖7 稠密建圖流程圖

FK=pyrDown(FK+1)

(4)

式中FK、FK+1——第K、K+1次下采樣圖像

pyrDown——下采樣函數(shù)

所形成的高斯圖像金字塔可用于構(gòu)建圖像的尺度,避免引入其它噪聲,從而提高本文算法的特征點提取與匹配精度。

2.2 三維稠密地圖構(gòu)建

針對ORB-SLAM2所建立地圖的表征不夠充分、缺少占據(jù)信息問題,本文在ORB-SLAM2架構(gòu)基礎(chǔ)上添加稠密地圖構(gòu)建線程,如圖1、7所示,該線程以局部建圖線程的優(yōu)質(zhì)關(guān)鍵幀為基礎(chǔ),構(gòu)建相機周圍果園環(huán)境的三維稠密點云地圖。首先,基于RGB-D相機的成像原理,如圖8所示,運用點云恢復(fù)算法、統(tǒng)計濾波方法,將輸入關(guān)鍵幀的2D像素坐標轉(zhuǎn)換為3D點云坐標,從而形成像素點云隊列[31]。其計算過程為

圖8 針孔相機成像原理圖

(5)

式中px、py——水平與垂直方向的相機焦距

qx、qy——像素坐標與成像平面的平移量

(x,y,z)——圖像坐標系坐標

(u,v)——圖像坐標

d——深度相機測量出的像素點距離

n——實際距離和測得距離d的比例系數(shù)

同時,運用位姿處理技術(shù),將相機位姿生成位姿隊列。其次,點云隊列與位姿隊列采取點云融合拼接算法,進行點云地圖拼接,完成新舊點云地圖疊加,點云拼接公式為

(6)

式中Rwc——3×3旋轉(zhuǎn)矩陣

twc——3×1平移向量

由于高采樣頻率、運動速度的約束,相鄰關(guān)鍵幀之間存在大量的冗余信息,導(dǎo)致系統(tǒng)運算速度較慢,本文通過體素濾波處理掉該冗余信息,以獲得新的局部點云地圖。最后,進行點云地圖更新,輸出三維稠密點云地圖,并將稠密建圖部分編寫成獨立的pointcloud_mapping功能包。生成的稠密點云地圖可通過Octomap_Server轉(zhuǎn)換成八叉樹地圖,為果園噴藥機器人在復(fù)雜場景下的自主導(dǎo)航提供環(huán)境地圖[31-33]。

稠密建圖線程若單獨運行,系統(tǒng)需重新進行特征點提取與匹配等工作,會產(chǎn)生大量重復(fù)計算。為減少稠密建圖計算量,本文基于ORB-SLAM2系統(tǒng)的ROS節(jié)點進行改進。ORB-SLAM2系統(tǒng)主要分為3個節(jié)點:相機驅(qū)動節(jié)點、位姿估計節(jié)點、建圖節(jié)點,如圖9所示。相機驅(qū)動節(jié)點采集視覺傳感器數(shù)據(jù),位姿估計節(jié)點提供相機位姿(TCW),建圖節(jié)點接收圖像數(shù)據(jù)和位姿數(shù)據(jù),進行點云拼接[34]。本文在ORB-SLAM2的建圖節(jié)點中增加KeyFrame輸出接口與位姿發(fā)布話題,通過TrackRGBD接口函數(shù)調(diào)用KeyFrameDataBase庫,逐層添加關(guān)鍵幀狀態(tài)標志位,利用Tracking::Track函數(shù)計算位姿,NeedNewKeyFrame函數(shù)選取ORB-SLAM2生成的優(yōu)質(zhì)關(guān)鍵幀,作為稠密建圖線程的輸入,從而減少處理幀數(shù),節(jié)約運算資源,保證系統(tǒng)的實時性,關(guān)鍵幀選取過程如圖10所示。

圖9 基于ROS的ORB-SLAM2系統(tǒng)節(jié)點框架

圖10 關(guān)鍵幀選取示意圖

3 仿真與實驗

為驗證本文算法的有效性與實用性,分別進行數(shù)據(jù)集仿真與真實場景實驗。仿真分析選用自帶標準軌跡與比較工具的TUM數(shù)據(jù)集,將本文算法與ORB-SLAM2進行對比,驗證其定位精度與稠密建圖效果。果園真實場景選取廣西壯族自治區(qū)柳州市某處柑橘種植場地,利用果園噴藥機器人實體樣機的移動底盤進行實物驗證。上述仿真、真實場景實驗的計算機采用Intel core i7-6700HQ處理器、32 GB運行內(nèi)存的ThinkPad T460P型便攜式計算機,其操作系統(tǒng)為Ubuntu 18.04。

3.1 數(shù)據(jù)集仿真分析

TUM數(shù)據(jù)集為Kinect視覺傳感器采集的標準數(shù)據(jù)集,該數(shù)據(jù)集中包含彩色圖、深度圖及相機運動過程中的真實軌跡(groundtruth)[23]。仿真分析利用TUM數(shù)據(jù)集的freiburg1_room序列進行驗證,該序列具有場景大、運動回環(huán)等特點,適于評估SLAM系統(tǒng)的位姿估計與回環(huán)檢測能力?；谏鲜鲂蛄蟹謩e測試ORB-SLAM2及本文所提算法,其運行效果如圖11所示。由圖11可知,本文算法相較ORB-SLAM2可實時地構(gòu)建三維稠密點云地圖。

圖11 運行效果對比

借助測評工具EVO對圖11得到的運行結(jié)果進行處理,以評估上述兩種SLAM算法的定位精度。首先,將兩種SLAM算法運行后的估計位姿KeyFrame Trajectory與數(shù)據(jù)集真實位姿(groundtruth)進行對比,結(jié)果如圖12所示;其次,分別獲取兩種SLAM算法對應(yīng)的絕對軌跡誤差(APE,圖13)、相對軌跡誤差(RPE,圖14);最后,運行兩種算法各15次,求取相應(yīng)的絕對軌跡誤差與相對軌跡誤差平均值,結(jié)果如表1、2所示。其中,采用最大誤差(max)、平均誤差(mean)、誤差中值(median)、最小誤差(min)、均方根誤差(RMSE)、方差(SSE)與標準差(STD)作為算法的評價指標。

表1 絕對軌跡誤差平均值

表2 相對軌跡誤差平均值

圖12 真實位姿與估計位姿對比

圖13 絕對軌跡誤差對比

圖14 相對軌跡誤差對比

圖12表明:本文算法相比ORB-SLAM2算法的估計位姿更接近真實位姿。觀察圖13、14可知,本文算法相較ORB-SLAM2的單幀誤差波動、絕對和相對軌跡誤差范圍更小;此外,分析表1、2可知,本文算法的絕對和相對軌跡誤差平均值均有所下降。其中,絕對軌跡平均誤差0.043 612 m,降低44.01%;均方根誤差0.048 419 m,降低43.26%;標準差0.021 032 m,降低46.68%。相對軌跡平均誤差0.018 938 m,降低7.93%;均方根誤差0.026 485 m,降低9.89%;標準差0.018 515 m,降低11.81%。數(shù)據(jù)集仿真結(jié)果表明:本文改進算法的估計位姿更接近真實位姿,定位精度也較ORB-SLAM2算法有明顯提升。

3.2 果園噴藥機器人定位與建圖實驗

為進一步驗證本文算法在真實場景下的定位與稠密建圖效果,以柑橘果園為應(yīng)用場景,選取36 m×16 m的測試范圍,如圖15所示。將ORB-SLAM2及本文算法分別在果園噴藥機器人實體樣機的移動底盤上加以測試。移動底盤搭載Astra RGB-D相機,主控為ThinkPad T460P型便攜式計算機,如圖16所示。

圖15 真實果園場景

圖16 實驗平臺

3.2.1算法性能測試

在果園內(nèi)的不同工況下進行多次實驗,根據(jù)特征點匹配數(shù)量、算法初始化時間、關(guān)鍵幀匹配數(shù)量測試算法性能。

兩種算法在運行過程中,通過調(diào)整機器人位置改變相機在環(huán)境中的位姿,每種算法進行20次特征點匹配實驗,實驗過程及結(jié)果如圖17、18所示。實驗結(jié)果表明,在相同場景下本文算法匹配到的特征點數(shù)量普遍高于ORB-SLAM2算法,特征點數(shù)量平均提升19.03%。

圖17 特征點匹配實驗(部分)

圖18 特征點匹配數(shù)量結(jié)果

在晴天與陰雨天兩種工況下對同一場景進行30次初始化時間與關(guān)鍵幀匹配數(shù)量測試,分別計算其平均值,結(jié)果如表3、4所示。實驗表明,本文算法30次初始化時間的均值為15.19 s。在晴朗天氣條件下初始化時間較短,兩類算法同一工況下的平均初始化時間相差不到4.3 s。本文算法通過優(yōu)化ORB特征點的提取過程,濾除劣質(zhì)特征點,減少了關(guān)鍵幀數(shù)量,其平均值為105幀,保證了本文算法在兩種工況下關(guān)鍵幀的平均值均低于ORB-SLAM2算法。

表3 初始化時間平均值

表4 關(guān)鍵幀匹配平均數(shù)量

3.2.2果園噴藥機器人定位實驗

通過實驗平臺直線運動與回環(huán)運動輸出的結(jié)果,驗證本文算法的定位精度。首先,在果園內(nèi)選取一條長10 m、寬1.8 m的直線通道,道路兩邊種滿果樹;然后,實驗平臺以速度0.6 m/s沿紅色直線噴藥通道行駛,如圖19所示;本文算法的運行效果如圖20所示。

圖19 實驗平臺直線行駛

圖20 直線運動過程

上述測試過程中獲取的實驗數(shù)據(jù)使用EVO軌跡功能進行處理,直線運動中部分軌跡坐標信息如表5所示,其中,Tx、Ty、Tz為當前關(guān)鍵幀位置坐標,Qx、Qy、Qz、Qw為位姿四元數(shù),整理后的實驗結(jié)果如圖21所示。

表5 直線運動軌跡坐標

圖21 直線運動軌跡

通過觀察圖21直線運動軌跡與分析表5實驗數(shù)據(jù)可知,本文通過優(yōu)化跟蹤線程中ORB特征點的提取過程,將劣質(zhì)ORB特征濾除,不但提高了算法生成的關(guān)鍵幀質(zhì)量,且增強了關(guān)鍵幀選取的可靠性與特征匹配的準確性,從而使本文算法的直線運動估計位姿較接近噴藥機器人實際的直線運動軌跡。

利用回環(huán)檢測消除果園噴藥機器人運行過程中的累計誤差,以提高其定位精度。在真實果園場景內(nèi)選取長10 m、寬3.6 m的場地進行回環(huán)實驗,實驗過程與結(jié)果如圖22、23所示。觀察圖23可知,通過噴藥機器人在果園內(nèi)的閉環(huán)運動,本文算法的估計軌跡坐標基本重合,具有良好的回環(huán)檢測性能。雖然,室外地形復(fù)雜,光線不均會導(dǎo)致部分軌跡位姿產(chǎn)生漂移,但本文算法仍可有效繪制果園噴藥機器人的位姿及軌跡,表明其在回環(huán)運動時的定位效果較為理想。通過上述實驗分析可知,本文算法具有較好的定位精度和魯棒性。

圖22 回環(huán)運動過程

圖23 回環(huán)運動軌跡

3.2.3果園噴藥機器人稠密建圖實驗

在真實果園場景內(nèi)選取兩條直線噴藥道路,場地長30 m、寬5.4 m。實驗平臺以速度0.6 m/s沿著噴藥路徑運動,利用本文算法進行實時三維稠密地圖構(gòu)建,得到的點云地圖如圖24a所示,將其與真實場景(圖24b)進行對比,以驗證本文算法的稠密建圖效果。

圖24 稠密地圖構(gòu)建

觀察圖24可知,本文算法構(gòu)建出的三維稠密點云地圖清晰地展現(xiàn)了真實果園場景內(nèi)草地與果樹的三維空間位置,且未出現(xiàn)重疊、變形等情況,與現(xiàn)實場景基本吻合。因此,本文算法具備較強的三維稠密點云地圖構(gòu)建能力。

綜上可知,相較ORB-SLAM2算法,本文所提算法的絕對軌跡、相對軌跡誤差均有所下降,定位精度也獲得提升。在真實果園場景下,本文算法特征匹配數(shù)量增多,初始化時間接近ORB-SLAM2算法,且在直線運動與回環(huán)運動中均實現(xiàn)了有效定位,室外場景也能構(gòu)建出有效的三維稠密點云地圖,可為果園噴藥機器人的導(dǎo)航定位打下堅實基礎(chǔ)。

4 結(jié)論

(1)通過改進FAST角點、描述子閾值,利用圖像金字塔法與高斯濾波算法優(yōu)化圖像特征點的提取過程,剔除劣質(zhì)ORB特征點,提高了關(guān)鍵幀匹配的可靠性,并通過實驗證明了本文算法在freiburg1_room序列中比ORB-SLAM2的運行軌跡效果更優(yōu),定位精度更高,其絕對軌跡的平均誤差、均方根誤差與相對軌跡的平均誤差、均方根誤差分別降低44.01%、43.26%、7.93%和9.89%。

(2)基于ORB-SLAM2算法框架引入稠密建圖線程,構(gòu)建RGB-D相機運動模型,利用點云恢復(fù)、點云拼接與濾波處理技術(shù),實現(xiàn)了三維稠密點云地圖構(gòu)建;并在ORB-SLAM2算法的建圖節(jié)點中引用NeedNewKeyFrame函數(shù),選取局部建圖線程所生成的優(yōu)質(zhì)關(guān)鍵幀圖像,從而減少了系統(tǒng)計算量。在真實果園場景實驗驗證了本文算法的實際性能,實驗結(jié)果證明本文算法的ORB特征點匹配數(shù)量平均提升19.03%,且關(guān)鍵幀數(shù)量的平均值為105幀,系統(tǒng)初始化時間的均值為15.19 s;此外,果園噴藥機器人可通過本文算法在直線、回環(huán)運動中實現(xiàn)準確定位,并能獲取機器人所處環(huán)境的三維場景信息,構(gòu)建出的稠密點云地圖可為果園噴藥機器人的自主導(dǎo)航提供基礎(chǔ)。