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

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

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

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

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

1 視覺SLAM數(shù)學(xué)表示及相機(jī)模型構(gòu)建

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

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

相機(jī)的運(yùn)動(dòng)、觀測方程如下：

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

(1)

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

(2)

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

1.2 單目相機(jī)模型構(gòu)建

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

設(shè)物理成像平面到相機(jī)光心O的距離為焦距f，點(diǎn)P在相機(jī)坐標(biāo)系中坐標(biāo)為P(xc，yc，zc)，由△P′O′O與△PAO的相似關(guān)系得：

(3)

(4)

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

(5)

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

(6)

相機(jī)坐標(biāo)系與世界坐標(biāo)系間的轉(zhuǎn)換可由相機(jī)位姿矩陣R、平移向量T表示，即：

(7)

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

(8)

2 ORB-SLAM2算法

2.1 ORB-SLAM2算法整體流程

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

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

2.2 特征提取與追蹤

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

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

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

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

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

最后，使用漢明距離對(duì)2幅圖像的描述子進(jìn)行匹配，并計(jì)算點(diǎn)對(duì)的距離極值。當(dāng)描述子間漢明距離大于2倍上述極小值時(shí)，可認(rèn)為出現(xiàn)誤匹配，需利用運(yùn)動(dòng)估計(jì)算法將其剔除。

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

(9)

其次，基于像素塊的矩計(jì)算灰度質(zhì)心：

(10)

最后，計(jì)算特征點(diǎn)方向：

(11)

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

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

Pi=Down(Pi-1)

。

(12)

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

3 結(jié)果與分析

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

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

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

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

表1 絕對(duì)軌跡誤差平均值

3.2 果園定位模擬試驗(yàn)

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

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

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

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

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

表2 直線運(yùn)動(dòng)軌跡坐標(biāo)

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

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

4 結(jié)論

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

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