果園噴藥機器人的單目視覺導航定位算法研究

萬現(xiàn)全， 王虎奇， 叢佩超， 肖宜軒， 趙曰煒， 陳熙來， 劉俊杰

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

柑橘作為我國兩廣地區(qū)的重要水果品種，種植面積廣、產(chǎn)量大。傳統(tǒng)柑橘噴藥作業(yè)以人工為主，存在噴藥效率低、成本高、環(huán)境污染大等缺點，因此，引入自主移動式機器人進行噴藥作業(yè)意義重大[1]。機器人噴藥的前提是能夠實現(xiàn)自主導航，而其定位精度尤為關鍵，因此，該問題已成為當前農業(yè)機器人領域的熱點問題[2]。

目前，果園作業(yè)機器人常用定位方式有衛(wèi)星定位、激光雷達定位、視覺定位等。其中衛(wèi)星定位技術最為成熟，已得到廣泛應用。熊斌等基于北斗衛(wèi)星導航系統(tǒng)實現(xiàn)施藥機在果園的定位，平均跟蹤誤差不超過0.13 m[3]。劉兆朋等依靠GNSS實現(xiàn)噴霧機的自動定位導航，可滿足水田、旱田環(huán)境的作業(yè)要求[4]。但柑橘果園中樹葉遮擋嚴重，衛(wèi)星信號易丟失，導致其定位失敗。激光雷達抗干擾能力強，掃描速度快。李秋潔等采用二維激光雷達獲取果園樹干測量數(shù)據(jù)，擬合樹行直線，將樹行中心線作為導航路徑[5]。劉偉洪等針對二維激光雷達無法應對復雜三維果園場景的問題，提出基于3D LiDAR的果園定位導航方法，具有可靠的穩(wěn)定性[6]。然而，激光雷達價格昂貴，不能滿足農業(yè)機器人經(jīng)濟性要求。視覺導航使用的設備簡單、成本較低，在GNSS信號弱的地方也可以進行定位，是果園機器人的熱點方向，Ji等最早提出基于Hough變換的圖像處理方法實現(xiàn)農業(yè)機器人的導航定位[7]。孟慶寬等提出一種基于人工蜂群算法的視覺導航路徑提取方法，解決了農業(yè)機器人導航基準線提取速度慢、檢測精度低等問題[8]。張雄楚等提出一種紅棗收獲機在灰棗棗園進行視覺導航的路徑檢測算法，通過圖像處理擬合導航路徑[9]。畢松等利用實例分割神經(jīng)網(wǎng)絡估計果園機器人視覺導航行間位姿和果樹位置[10]。上述研究應用場景不包含復雜柑橘果園場景，且均利用Hough變換或者最小二乘法通過作物或道路檢測擬合導航路徑，其導航線精度依賴于圖像分割效果，但往往因為遮擋造成圖像分割失敗。視覺同時定位與建圖(simultaneous localization and mapping，SLAM)技術可較好地處理復雜和動態(tài)場景，具有良好的應用前景，但針對果園場景的該方面研究較少，且定位精度不足[2]。

本研究針對果園噴藥機器人視覺導航過程中定位精度低的問題，優(yōu)化ORB-SLAM2系統(tǒng)特征點選取策略，設置描述子閾值，并優(yōu)化圖像金字塔層級，增強其尺度及旋轉不變性，以保證農藥噴灑機器人定位精度，通過TUM數(shù)據(jù)集和搭建模擬柑橘園環(huán)境驗證不同工況下的算法性能，為實現(xiàn)噴藥機器人自主定位提供算法基礎。

1 視覺SLAM數(shù)學表示及相機模型構建

1.1 視覺SLAM數(shù)學表示

噴藥機器人通過單目相機獲取周圍果園環(huán)境信息的過程為：離散時刻t=t1，t2，…，ti下，機器人在位置x1，x2，…，xi處觀測果園環(huán)境中路標點q1，q2，…，qi(圖1)。視覺SLAM算法處理圖像數(shù)據(jù)最終實現(xiàn)機器人狀態(tài)估計及周圍環(huán)境重建[11]。

相機的運動、觀測方程如下：

xi=f(xi-1,pi,wi)[12]；

(1)

zi,t=g(qt,xi,vi,t)[12]。

(2)

式中：xi表示機器人i時刻所在位置；pi為相機i時刻的測量數(shù)據(jù)；wi為測量噪聲；zi，t為相機在xi處對路標點qt的觀測數(shù)據(jù)；qt為相機觀測到的路標點；vi，t為觀測噪聲。

1.2 單目相機模型構建

基于經(jīng)濟性考慮，本研究果園噴藥機器人采用單目相機進行定位。單目相機可簡化為針孔模型，用于描述世界坐標系下三維點映射到像素坐標系的過程(圖2)。世界坐標系中點P(xw，yw，zw)經(jīng)過相機光心投影后，在物理成像平面產(chǎn)生成像點P(x′，y′，z′)，坐標系轉換涉及世界坐標系、像素坐標系、歸一化坐標系及相機坐標系[13](圖3)。

設物理成像平面到相機光心O的距離為焦距f，點P在相機坐標系中坐標為P(xc，yc，zc)，由△P′O′O與△PAO的相似關系得：

(3)

(4)

設像素坐標系在軸u、v分別縮放α、β倍，原點平移[cx，cy]后得歸一化坐標系，則點P歸一化坐標(x，y)與像素坐標(u，v)關系如公式(5)所示：

(5)

分別代入公式(3)、公式(4)，整理為矩陣形式，并令gx=αf，gy=βf，得

(6)

相機坐標系與世界坐標系間的轉換可由相機位姿矩陣R、平移向量T表示，即：

(7)

聯(lián)立(6)(7)方程，得到針孔模型中像素坐標系與世界坐標系的轉換關系：

(8)

2 ORB-SLAM2算法

2.1 ORB-SLAM2算法整體流程

ORB-SLAM2是基于特征點法的實時SLAM系統(tǒng)，支持單目、雙目、RGB-D相機3種模式[14]，采用3個并行線程：追蹤、局部建圖、回環(huán)檢測實現(xiàn)移動式機器人的定位及建圖功能，其算法流程見圖4[11]。

其中，跟蹤線程從圖像中提取、追蹤ORB特征后，根據(jù)幀間匹配初始化相機位姿，并跟蹤、優(yōu)化重建的局部地圖；局部建圖線程完成關鍵幀的插入，并剔除劣質關鍵幀，生成新地圖點，進一步優(yōu)化位姿；回環(huán)檢測模塊檢測到回環(huán)時，根據(jù)該回環(huán)計算、消除漂移誤差，并將相同地圖點合并，最后進行位姿圖優(yōu)化，實現(xiàn)全局一致性[15]。

2.2 特征提取與追蹤

特征提取、追蹤是ORB-SLAM2系統(tǒng)的重要組成部分，其結果將直接影響算法定位的精度及建圖效果[14]。算法以ORB為圖像特征，由Oriented FAST關鍵點、BRIEF描述子組成(圖5)[16]。算法僅比較局部像素的亮度大小，實現(xiàn)關鍵點提取，保證算法實時性。描述子采用二進制描述向量，利用漢明距離表示兩特征點間的差異，提高圖像匹配的實時性[17]。

ORB-SLAM2算法計算旋轉前后特征點附近的圖像灰度質心及特征點方向，實現(xiàn)特征的旋轉不變性[18]。另外，算法構建圖像金字塔，得到不同分辨率的圖像，進行不同層的角點匹配，保證尺度不變性。但描述子數(shù)量較少時，特征點提取質量較差，會導致旋轉不變性降低；圖像金字塔層數(shù)有限，只能保證一定范圍內的尺度不變性。

為提高特征提取質量，保證果園噴藥機器人定位精度， 本研究基于ORB-SLAM2算法， 優(yōu)化描述子提取閾值，剔除劣質特征點，保證特征匹配精度及關鍵幀質量，增強其旋轉不變性，并且優(yōu)化圖像金字塔構造算法，增加下采樣產(chǎn)生的圖像層級，對每層圖像進行高斯濾波，提高不同距離下特征匹配的精度。本文算法具體實現(xiàn)過程如下。

2.2.1 特征匹配 首先，利用opencv庫imread函數(shù)檢測圖像中Oriented FAST角點的位置，實現(xiàn)過程為：(1)在圖像中選取亮度為Ia的像素塊A；(2)在以像素塊A為中心、半徑為r的圓上選取16個像素點，設置亮度閾值T0。若選取的圓上存在連續(xù)11個點的亮度值不在區(qū)間[Ia+T0，Ia-T0]之內，則像素塊A為特征點；(3)對圖像中所有像素均執(zhí)行上述步驟，以篩選出優(yōu)質特征點。

其次，依據(jù)角點位置計算BRIEF描述子，其提取數(shù)量閾值范圍設置為[T1，T2]，剔除不足T1個描述子的關鍵點，提高特征點質量，且限制描述子數(shù)量低于T2，以降低SLAM系統(tǒng)所占算力。

最后，使用漢明距離對2幅圖像的描述子進行匹配，并計算點對的距離極值。當描述子間漢明距離大于2倍上述極小值時，可認為出現(xiàn)誤匹配，需利用運動估計算法將其剔除。

2.2.2 旋轉不變性 利用灰度質心法計算圖像灰度質心及其特征點方向。首先，計算特征點附近像素塊A的矩：

(9)

其次，基于像素塊的矩計算灰度質心：

(10)

最后，計算特征點方向：

(11)

式中：m為像素塊的矩；u，v為像素塊坐標；I(u，v)為像素坐標的灰度值；φ為特征點方向角。

2.2.3 尺度不變性 優(yōu)化圖像金字塔層級，提高果園環(huán)境下特征點的尺度不變性，增強特征匹配精度。算法將圖像進行縮放，并利用opencv的copyMakeBorder復制圖像并填充邊緣。繼而，對圖像進行高斯濾波，利用opencv的pyrDown函數(shù)下采樣形成圖像金字塔(圖6)，高斯金字塔計算過程如下：

Pi=Down(Pi-1)

。

(12)

式中：Pi、Pi-1分別為第i、i-1次下采樣圖像，Down表示下采樣函數(shù)。

3 結果與分析

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

本試驗利用TUM數(shù)據(jù)集分別運行原ORB-SLAM2系統(tǒng)及本研究優(yōu)化算法，并使用測評工具EVO，對比數(shù)據(jù)集中真實位姿文件groundtruth.txt與2個SLAM算法的估計位姿文件KeyFrame.txt(圖7)，得到絕對軌跡誤差，以此評測上述2個SLAM算法的定位精度。

試驗平臺采用Ubuntu18.04系統(tǒng)、Inteli5-9600KF 3.70GHZ處理器、16 GB內存。在TUM數(shù)據(jù)集freiburg1_xyz(圖8)上分別運行改進前后的算法各10次，得出相應的絕對軌跡誤差，求取平均值，具體結果如表1所示。

從表1可知，本研究算法得到的絕對軌跡誤差平均值各項指標均有所提高。其中：最大誤差 0.092 610 m，較原ORB-SLAM2算法降低28.62%；平均誤差0.067 311 m，降低15.06%；均方根誤差0.068 630 m，降低16.44%。仿真結果表明，本研究優(yōu)化算法定位精度較原ORB-SLAM2算法有較大優(yōu)勢。

表1 絕對軌跡誤差平均值

3.2 果園定位模擬試驗

本研究搭建虛擬柑橘園環(huán)境(圖9)，在光照明、暗等多種工況條件下，將原ORB-SLAM2算法及本研究優(yōu)化的算法分別應用于Spark移動機器人平臺進行實物驗證。Spark移動機器人搭載HIKVISION DS-E11 單目攝像頭，IBM ThinkPad T460P筆記本(Intel core i7-6700HQ CPU)(圖10)。

3.2.1 算法性能試驗 本研究選取特征匹配數(shù)、初始化時間作為算法性能測試指標，在同一場景下對不同工況進行試驗。在不同距離、角度條件下共進行20次特征點匹配試驗，試驗過程及結果如圖11、圖12所示。

由試驗結果可知，在相同場景下本研究算法匹配到的特征點數(shù)量普遍高于原ORB-SLAM2算法，數(shù)量平均提升18.18%。

在光照明、暗條件下對同一場景進行40次初始化時間測試，統(tǒng)計其平均值。試驗結果(圖13)表明，2種算法在光照充足條件下表現(xiàn)更為優(yōu)異，且2類算法在同一種工況下的平均初始化時間相差不到0.5 s。

3.2.2 噴藥機器人果園定位試驗 選取長度3.6 m的直線運行軌跡，在光照明、暗2種工況下分別對優(yōu)化前后ORB-SLAM2算法進行20次試驗。直線運動中部分關鍵幀坐標信息見表2。其中，tx、ty、tz為關鍵幀當前位置坐標，qx、qy、qz、qw為位姿四元數(shù)。由圖14可知，本研究算法產(chǎn)生的關鍵幀略少，這是由于描述子優(yōu)化閾值后，局部建圖線程濾除特征不足的圖像幀，圖像關鍵幀選取的可靠性大大增強。分析20組試驗數(shù)據(jù)，本研究算法在模擬果園環(huán)境下定位精度良好(圖15)。由于單目視覺定位具有尺度不確定性，無法確定真實尺度[19]，在初始化時系統(tǒng)會固定尺度將單位設置為1，因此圖中軌跡坐標值均為相對值。

表2 直線運動軌跡坐標

回環(huán)檢測可消除果園噴藥機器人運行中的累計誤差，提高其定位精度[20]，因此本研究設計回環(huán)軌跡進行試驗，試驗結果(圖16)表明， 2種工況下，多次回環(huán)軌跡的坐標基本重合，表明回環(huán)時定位效果良好。光照不足時，由于環(huán)境亮度較低且初始化時間過長，導致部分初始位姿產(chǎn)生漂移，但仍可有效繪制噴藥機器人的位姿及軌跡。

綜上可知，相較原ORB-SLAM2系統(tǒng)，本研究所提算法的絕對誤差下降、定位精度提升。在模擬柑橘果園環(huán)境下，本研究算法特征匹配數(shù)量增多，初始化時間及關鍵幀數(shù)量接近原算法，并且在直線、回環(huán)運動中均可實現(xiàn)有效定位。

4 結論

本研究提出一種基于ORB-SLAM2的單目視覺定位算法，構建噴藥機器人單目相機模型，優(yōu)化圖像特征點的提取、匹配策略，增強其尺度及旋轉不變性，并通過TUM數(shù)據(jù)集和搭建模擬果園環(huán)境驗證了算法在果園噴藥機器人定位方面的有效性。

下一步工作將嘗試解決SLAM系統(tǒng)初始化時間過長的問題，克服在光照不足工況下的位姿漂移問題。為解決單目相機的尺度不確定性，將嘗試引入其他感知元件，借助多傳感器融合思想，進一步提升果園噴藥機器人的定位精度。