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

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

0 引言

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

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

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

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

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

1.1 算法框架

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

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

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

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

圖2 果園場景下的相機(jī)運(yùn)動狀態(tài)

RGB-D相機(jī)的運(yùn)動方程為

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

(1)

其觀測方程為

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

(2)

式中xt——相機(jī)t時刻位置

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

qt——測量t時刻噪聲

wt,i——觀測t時刻路標(biāo)點噪聲

yi——相機(jī)觀測到的路標(biāo)點

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

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

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

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

Wo,j=To,ciWci,j

(3)

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

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

Wo,j——全局坐標(biāo)系點云

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ù)像素亮度不同進(jìn)行FAST角點提取與描述子計算,引入噪聲抑制機(jī)制,以優(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幅圖像的描述子進(jìn)行匹配,計算相鄰點之間的極小值距離,若描述子間漢明距離大于2倍極小值,判斷其為誤匹配。此時,運(yùn)用隨機(jī)采樣一致性算法減少誤匹配,抑制噪聲,剔除外點,以提高系統(tǒng)定位精度。

圖5 ORB特征點提取

ORB-SLAM2算法構(gòu)建圖像金字塔是對原圖像進(jìn)行多尺度結(jié)構(gòu)表達(dá)。首先,按照圖像比例因子進(jìn)行逐級向下采樣;其次,采樣后的圖像按不同分辨率由高到低向上排列,進(jìn)行不同層的角點匹配,從而解決尺度不變性問題。但當(dāng)描述子數(shù)量較少時,易出現(xiàn)特征點提取與匹配精度下降的問題[30]。為解決上述問題,通過優(yōu)化圖像金字塔層級,對圖像進(jìn)行高斯濾波,利用OpenCV的pyrDown函數(shù)形成高斯圖像金字塔,如圖6所示。高斯圖像金字塔從第K層生成第K+1層,利用高斯濾波對第K層進(jìn)行卷積,刪除所有的偶數(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)建相機(jī)周圍果園環(huán)境的三維稠密點云地圖。首先,基于RGB-D相機(jī)的成像原理,如圖8所示,運(yùn)用點云恢復(fù)算法、統(tǒng)計濾波方法,將輸入關(guān)鍵幀的2D像素坐標(biāo)轉(zhuǎn)換為3D點云坐標(biāo),從而形成像素點云隊列[31]。其計算過程為

圖8 針孔相機(jī)成像原理圖

(5)

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

qx、qy——像素坐標(biāo)與成像平面的平移量

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

(u,v)——圖像坐標(biāo)

d——深度相機(jī)測量出的像素點距離

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

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

(6)

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

twc——3×1平移向量

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

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

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

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

3 仿真與實驗

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

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

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

圖11 運(yùn)行效果對比

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

表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%;標(biāo)準(zhǔn)差0.021 032 m,降低46.68%。相對軌跡平均誤差0.018 938 m,降低7.93%;均方根誤差0.026 485 m,降低9.89%;標(biāo)準(zhǔn)差0.018 515 m,降低11.81%。數(shù)據(jù)集仿真結(jié)果表明:本文改進(jìn)算法的估計位姿更接近真實位姿,定位精度也較ORB-SLAM2算法有明顯提升。

3.2 果園噴藥機(jī)器人定位與建圖實驗

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

圖15 真實果園場景

圖16 實驗平臺

3.2.1算法性能測試

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

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

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

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

在晴天與陰雨天兩種工況下對同一場景進(jìn)行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果園噴藥機(jī)器人定位實驗

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

圖19 實驗平臺直線行駛

圖20 直線運(yùn)動過程

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

表5 直線運(yùn)動軌跡坐標(biāo)

圖21 直線運(yùn)動軌跡

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

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

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

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

3.2.3果園噴藥機(jī)器人稠密建圖實驗

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

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

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

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

4 結(jié)論

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

(2)基于ORB-SLAM2算法框架引入稠密建圖線程,構(gòu)建RGB-D相機(jī)運(yùn)動模型,利用點云恢復(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;此外,果園噴藥機(jī)器人可通過本文算法在直線、回環(huán)運(yùn)動中實現(xiàn)準(zhǔn)確定位,并能獲取機(jī)器人所處環(huán)境的三維場景信息,構(gòu)建出的稠密點云地圖可為果園噴藥機(jī)器人的自主導(dǎo)航提供基礎(chǔ)。