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

叢佩超 崔利營 萬現全 李佳星 劉俊杰 張 欣

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

0 引言

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

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

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

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

1 算法框架及視覺SLAM理論基礎

1.1 算法框架

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

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

1.2 視覺SLAM理論基礎

為實現果園噴藥機器人的高精度定位與三維稠密地圖構建,需有豐富的圖像信息作為保證。基于經濟性考慮,本文采用Astra RGB-D相機。果園噴藥機器人通過RGB-D相機獲取環境信息,如圖2所示,機器人在pose1、pose2、…、poseN處觀測果園環境中路標點y1、y2、…、yn。通過視覺SLAM算法處理圖像數據,根據相機的運動方程、觀測方程估計相機位姿,再借助坐標系轉換關系確定機器人的位置與姿態,以實現其自身的精準定位[27]。

圖2 果園場景下的相機運動狀態

RGB-D相機的運動方程為

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

(1)

其觀測方程為

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

(2)

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

pt——測量t時刻數據

qt——測量t時刻噪聲

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

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

zt,i——相機在xt處對路標點yi的觀測數據

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

圖3 Astra RGB-D結構

點云坐標系變換關系式為

Wo,j=To,ciWci,j

(3)

式中To,ci——第i個關鍵幀位姿

Wci,j——第i個關鍵幀的坐標系點云

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

2 同時定位與三維稠密地圖構建

2.1 ORB特征點提取優化

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

圖4 特征點提取流程圖

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

圖5 ORB特征點提取

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

圖6 高斯圖像金字塔

圖7 稠密建圖流程圖

FK=pyrDown(FK+1)

(4)

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

pyrDown——下采樣函數

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

2.2 三維稠密地圖構建

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

圖8 針孔相機成像原理圖

(5)

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

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

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

(u,v)——圖像坐標

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

n——實際距離和測得距離d的比例系數

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

(6)

式中Rwc——3×3旋轉矩陣

twc——3×1平移向量

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

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

圖9 基于ROS的ORB-SLAM2系統節點框架

圖10 關鍵幀選取示意圖

3 仿真與實驗

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

3.1 數據集仿真分析

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

圖11 運行效果對比

借助測評工具EVO對圖11得到的運行結果進行處理,以評估上述兩種SLAM算法的定位精度。首先,將兩種SLAM算法運行后的估計位姿KeyFrame Trajectory與數據集真實位姿(groundtruth)進行對比,結果如圖12所示;其次,分別獲取兩種SLAM算法對應的絕對軌跡誤差(APE,圖13)、相對軌跡誤差(RPE,圖14);最后,運行兩種算法各15次,求取相應的絕對軌跡誤差與相對軌跡誤差平均值,結果如表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%。數據集仿真結果表明:本文改進算法的估計位姿更接近真實位姿,定位精度也較ORB-SLAM2算法有明顯提升。

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

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

圖15 真實果園場景

圖16 實驗平臺

3.2.1算法性能測試

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

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

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

圖18 特征點匹配數量結果

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

表3 初始化時間平均值

表4 關鍵幀匹配平均數量

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

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

圖19 實驗平臺直線行駛

圖20 直線運動過程

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

表5 直線運動軌跡坐標

圖21 直線運動軌跡

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

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

圖22 回環運動過程

圖23 回環運動軌跡

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

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

圖24 稠密地圖構建

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

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

4 結論

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

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