基于深度相機的蘋果采摘機器人路徑規(guī)劃研究

王亮 趙德安 劉曉洋

摘 要：為了優(yōu)化蘋果采摘機器人采摘路徑，在獲得蘋果樹場景三維位置信息的基礎(chǔ)上，提出一種具有多種地形損耗的A*算法。結(jié)合ToF（Time-of-Flight）深度相機和Hu不變矩獲取蘋果和不同障礙物的三維位置信息，建立存在果實和多種障礙物的二維地圖。在二維地圖上，利用具有不同地形損耗函數(shù)的A*算法進行仿真實驗。改進后的A*算法將障礙物分為可通過的障礙物（樹葉）和不可通過的障礙物（樹枝），且障礙物存在位置處的自帶移動耗費向周圍以線性遞減，避免了基本的A*算法中只具有單種障礙物問題，從而增加了不同種類障礙物對路徑選擇的影響程度，優(yōu)化了路徑質(zhì)量。對比實驗表明：改進后的算法提高了對于復(fù)雜地圖的處理能力，產(chǎn)生的路徑長度更短，轉(zhuǎn)折次數(shù)更少。

關(guān)鍵詞：蘋果采摘;ToF;Hu不變矩;A*算法;路徑規(guī)劃

DOI：10. 11907/rjdk. 182257

中圖分類號：TP301文獻標(biāo)識碼：A文章編號：1672-7800（2019）003-0001-06

0 引言

中國是世界上最大的蘋果生產(chǎn)國。隨著我國城鎮(zhèn)化步伐的加快，大量青年進城務(wù)工，導(dǎo)致農(nóng)村勞動力短缺，不能滿足蘋果采摘對勞動力的需求，蘋果人工采摘還存在作業(yè)量大和效率低下問題[1]，農(nóng)業(yè)生產(chǎn)方式向自動化智能化轉(zhuǎn)變迫在眉睫。

使用機器人采摘果實最先由Schertz & Brown[2]在20世紀(jì)60年代提出，之后采摘機器人經(jīng)歷了50多年的發(fā)展，從最早期的機器人采摘果實需要數(shù)十秒到如今采摘果實在10秒以內(nèi)。國內(nèi)采摘機器人發(fā)展起步較晚，采摘果實的快速性與實時性不能滿足要求。為提高機器人采摘蘋果效率，必須解決蘋果圖像處理和采摘路徑規(guī)劃兩大難題。江蘇大學(xué)2011年研制的蘋果采摘機器人，采用單目USB 攝像頭，圖像處理程序用C++編寫，圖像處理算法基于蘋果在RGB空間的顏色特征，平均采摘一個蘋果的時間為15s[3-4]。由于沒有使用路徑規(guī)劃算法，導(dǎo)致采摘路徑出現(xiàn)重疊，采摘時間過長，不能勝任野外采摘工作。

相關(guān)研究主要有：呂繼東等[5]首先對蘋果果實進行OTSU動態(tài)閾值分割，之后利用采集圖像之間的信息關(guān)聯(lián)性縮小目標(biāo)果實處理區(qū)域，并采用快速去均值歸一化積相關(guān)算法跟蹤識別目標(biāo)果實。司永勝等[6]先使用紅綠色差法分割出蘋果并提取圓心和半徑，通過建立基于面積特征與極線幾何相結(jié)合的匹配策略，實現(xiàn)雙目視覺下的果實匹配與定位。以上兩個研究僅對普通的RGB圖像進行算法處理，復(fù)雜度較高。苑嚴(yán)偉、張小超等[7]將蘋果采摘的路徑規(guī)劃問題轉(zhuǎn)化為三維旅行商問題進行求解，結(jié)合雙目攝像頭獲得的蘋果位置信息提出了改進的蟻群算法，解決了局部收斂問題，提高了采摘效率。但該算法沒有考慮到實際采摘情況下的障礙物問題，對于現(xiàn)實情況下果樹生長的復(fù)雜環(huán)境適用性不是很高。比利時學(xué)者 Tien Thanh Nguyen利用 Kinect 相機提供的顏色信息和三維形狀信息，基于點云PCL（PointCloud Library），蘋果識別率高，定位精度在10mm以下，同步識別20個蘋果的圖像處理時間在1s以內(nèi)[8]。該研究利用深度相機識別和定位蘋果，速度很快，但沒有從采摘路徑規(guī)劃方面提高蘋果采摘效率。

針對以上研究存在的問題，本文選擇ToF深度相機作為采摘攝像頭，快速獲取整個蘋果樹的多種類型圖像，從三維點云圖像和深度圖像中準(zhǔn)確定位多個成熟蘋果的三維位置，并且篩選蘋果所處范圍內(nèi)的障礙物，將三維視圖轉(zhuǎn)化為二維圖像，形成存在多種障礙物的二維地圖。根據(jù)蘋果采摘的實際情況，改進A*算法中的地形移動損耗，在具有多種障礙物的二維地圖上進行路徑規(guī)劃。編寫路徑規(guī)劃的算法代碼，對改進后的A*算法在路徑規(guī)劃中的效果進行驗證，從而在有限時間內(nèi)達(dá)到路徑距離最短、規(guī)劃速度最快的采摘要求。本文從圖像處理和采摘路徑規(guī)劃兩個方面提高了蘋果采摘效率。

1 三維信息獲取

1.1 Balser ToF工作原理

Balser ToF（Time-of-Flight）相機是德國Balser公司生產(chǎn)的一種工業(yè)3D相機。相機自帶的傳感器發(fā)出經(jīng)調(diào)制的近紅外光，遇物體后反射，傳感器通過計算光線發(fā)射和反射時間差或相位差計算被拍攝景物的距離，產(chǎn)生深度信息。再結(jié)合傳統(tǒng)的相機拍攝，就能將物體的三維輪廓以不同顏色代表不同距離的地形圖方式呈現(xiàn)。原理如圖1所示，實物如圖2所示。

本文先利用ToF相機對蘋果樹整體場景進行采集，同時獲得整個場景的強度圖、置信圖、深度圖、三維點云圖像。強度圖代表整個場景接收到的反射光強弱，如圖3（a）所示。置信圖代表整個場景的每一個點深度的可信度，如圖3（b）所示。圖中點的亮度越大，則該點的深度距離準(zhǔn)確度越高。深度圖表示了場景中的深度信息，如圖3（c）所示，越靠近相機，顏色顯示越深，若顯示黑色，表示該點距離不可測。三維點云圖像是場景中點云數(shù)據(jù)的集合，如圖3（d）所示。

1.2 蘋果形狀特征獲取

由于Basler的ToF相機獲得圖像中并沒有RGB圖像，通過在強度圖和三維圖中分析蘋果和障礙物可以發(fā)現(xiàn)，蘋果和障礙物在圖像中顏色上是相近的，因此利用顏色特征確定蘋果位置是不準(zhǔn)確的，但是蘋果反射的光強和障礙物樹枝樹葉返回的光強不一樣，導(dǎo)致在圖像中有著明顯區(qū)別。因此本文先對場景圖像在像素方面進行預(yù)處理，RGB值大于150設(shè)置為白色，小于150設(shè)置為黑色（如圖4所示），然后在預(yù)處理的圖像上對蘋果形狀特征進行提取。場景的形狀特征提取采用平移不變形、旋轉(zhuǎn)不變性、縮放不變性的Hu不變矩方法[8]。

1.2.1 Hu不變矩提取蘋果圖像特征

矩特性主要表征了圖像區(qū)域的幾何特征，又稱為幾何矩。因為其存在旋轉(zhuǎn)、平移等多個不變特征，所以又稱其為不變矩。在圖像處置中，不變矩能夠描述圖像的整體性質(zhì)，在邊緣提取、圖像匹配及目標(biāo)識別中得到廣泛應(yīng)用[9]。

Hu不變矩概念由Hu[10]提出，闡述了Hu不變矩中連續(xù)函數(shù)矩的定義和矩的基本性質(zhì)，并給出了由二階和三階歸一化中心矩的非線性組合構(gòu)成的7個量值，并證明了這些矩具有平移、比例、旋轉(zhuǎn)不變性，后被人們稱為Hu不變矩。

Hu不變矩對分割后的圖像中的目標(biāo)連通域進行形狀特征提取。設(shè)圖像中的目標(biāo)連通域上的某一像素點位置為[（x，y）]，則整個連通域的（[p+q]）階矩為：

在對圖片中物體的實際識別過程中發(fā)現(xiàn)，只有一階矩到四階矩保持較好，其它幾個不變矩帶來的誤差較大，所以只選擇前4個矩進行計算。表1是任意選取的3幅蘋果目標(biāo)區(qū)域和從圖中截取的葉子的不變矩特征值。從表中可以看出不變矩值整體呈現(xiàn)遞減趨勢，蘋果和葉子的不變矩值差異明顯，即不同目標(biāo)區(qū)域的Hu不變矩值是不同的，因此采用不變矩值獲取目標(biāo)物的形狀特征可行。

因為蘋果本身紋理特征并不復(fù)雜，并且形狀特征和周邊障礙物區(qū)別較大，所以利用Hu不變矩識別蘋果的輪廓速度很快，效果很好。識別結(jié)果如圖5所示。

1.3 果實與障礙物三維位置獲取

1.3.1 果實三維位置獲取

在確定了蘋果在圖像中的二維位置后，再利用Tof相機的深度圖獲取蘋果的深度信息。為了得到蘋果的準(zhǔn)確位置，在深度圖上對應(yīng)蘋果區(qū)域以蘋果為中心隨機選取5個點作為預(yù)選的蘋果三維數(shù)據(jù)，然后對這5個點求平均值，得到蘋果中心點的最終三維位置。

在Tof顯示圖像中，圖像中心點的二維位置是（0，0），x軸坐標(biāo)值隨著方向向右增大，y軸坐標(biāo)值隨著方向向下增大。表2展示從左到右蘋果的三維位置信息（蘋果3：從左往右時，圖像上偏下部的蘋果。蘋果4：從左往右時，圖像上偏上部的蘋果，坐標(biāo)單位為dm）。

1.3.2 附近障礙物三維位置獲取

以上節(jié)得到的蘋果位置作為中心，選取邊長13cm的正方形作為蘋果和障礙物共存的區(qū)域位置，選取邊長8cm的正方形區(qū)域作為蘋果存在區(qū)域（實驗室所用塑料蘋果的寬度在8～10cm左右），在三重構(gòu)圖和強度圖上找尋預(yù)測的障礙物區(qū)域內(nèi)亮度和蘋果亮度有明顯區(qū)別的點，利用找尋到的點坐標(biāo)，在深度圖上找到對應(yīng)的深度位置，若深度位置與蘋果的深度位置在±8cm范圍內(nèi)，就認(rèn)定以該點為蘋果附近障礙物的位置。

對于找到的障礙物，針對樹枝和樹葉的亮度不一樣設(shè)置閾值，當(dāng)亮度閾值大于85小于100時視為樹葉，其余的障礙物視為樹枝。圖6為障礙物樹葉的位置。

由于樹枝的亮度和環(huán)境背景的亮度相似，所以并不能識別出樹枝，只要認(rèn)定蘋果附近存在障礙物，通過亮度判斷是否為可通過的樹葉，若不為樹葉，則直接判定為不可通過的障礙物樹枝。表3表示不同障礙物的位置信息。不同障礙物的位置如圖7所示，單位為dm。

2 采摘路徑最優(yōu)化方法

2.1 障礙柵格法模型建立

本文在二維空間進行路徑規(guī)劃，用柵格法表示存在障礙物的地圖，該方法起源于美國CMU大學(xué)。柵格法的基本思想是：將規(guī)劃空間劃分成等面積的小區(qū)，每個小區(qū)稱為一個柵格，柵格面積大小一般由所描述的問題及所要求的搜索精度決定[12]。本文選取8cm長度作為柵格的邊長，因為本實驗所選用的塑料蘋果橫向平均長度在8cm左右。圖8顯示一個規(guī)劃空間的柵格劃分情況。每個柵格上的運動信息規(guī)定了機器人在這個柵格上的運動擴展方向，它們分別是前、后、左、右、右前、右后、左后、左前，如圖9所示[13]。

通過柵格法得到的地圖可把搜索區(qū)域簡化成一個二維數(shù)組，數(shù)組的每一個元素是網(wǎng)絡(luò)的一個方塊，方塊被標(biāo)記成完全可通過（無任何障礙）、可通過障礙（樹葉）、不可通過障礙（樹枝）和蘋果。利用蘋果和不同障礙物的相對位置，初始化圖中的方格，A代表蘋果在圖中所處位置，B代表不可通過的障礙物樹枝的位置，L代表可通過障礙物樹葉的位置，如圖10所示。

2.2 基于A*算法的路徑啟發(fā)式搜索

對規(guī)劃空間建立必要的空間障礙物，以及采用合理的數(shù)據(jù)結(jié)構(gòu)對規(guī)劃空間中的自由區(qū)域和障礙物區(qū)域進行描述，無碰撞路徑規(guī)劃就是選取某種有效的搜索方法，搜索出一條經(jīng)過所有蘋果且移動耗費總和最小的無碰撞路徑[14]。

路徑規(guī)劃常見的啟發(fā)式優(yōu)先搜索包括局部擇優(yōu)搜索、全局擇優(yōu)搜索、A*算法以及近幾年發(fā)展起來的人工神經(jīng)網(wǎng)絡(luò)搜索、遺傳算法搜索等[15]，這些算法都使用了啟發(fā)函數(shù)，但在具體選取最佳搜索節(jié)點時策略不同。局部擇優(yōu)搜索法是在搜索過程中選取“最佳節(jié)點”后，舍棄其它的兄弟、父親節(jié)點，然后一直搜索下去。這種搜索結(jié)果由于舍棄了其它節(jié)點，可能也把最好的節(jié)點舍棄了，求解的最佳節(jié)點只是在該階段的最佳并不一定是全局最佳。最好優(yōu)先搜索沒有舍棄節(jié)點（除非該節(jié)點是死節(jié)點），在每一步的估價中都把當(dāng)前的節(jié)點和以前的節(jié)點估價值比較，得到一個“最佳的節(jié)點”，這樣可有效防止“最佳節(jié)點”丟失。在最好優(yōu)先搜索算法基礎(chǔ)加上一些約束條件，就是本文采用的A*算法[16-20]。

A*算法的估價函數(shù)是F（n）=G（n）+H（n）。這里，F(xiàn)（n）是估價函數(shù)，G（n）是起點到當(dāng)前指定方格的移動耗費，H（n）是當(dāng)前點到指定目標(biāo)方格的預(yù)估耗費。

在A*算法中，令水平或垂直方向移動一格的耗費為方格的邊長，沿對角線移動耗費是沿水平或垂直移動的1.414倍。之所以選取近似小數(shù)，是因為比例基本正確，避免了求根運算，從而加快代碼的運算速度[20]。對于完全可通過方格，方格自帶的移動耗費是0，對于存在可通過的障礙物（樹葉）方格，方格的自帶移動耗費為方格邊長的一半。然后以該節(jié)點為中心，向水平和垂直方向依次遞減1/2。而對于不可通過的障礙物（樹枝）方格，并不能夠到達(dá)該節(jié)點，所以不會計算到該節(jié)點的移動耗費，并且鄰近節(jié)點進行移動時不能通過其頂點。圖11顯示了各個方格自帶的移動耗費。

事先是沒有辦法知道路徑的長度和路徑上是否具有障礙物的，所以使用曼哈頓方法直接計算H值[21]。曼哈頓方法是一種計算城市中一個地方到另一個地方的街區(qū)數(shù)的方法。該方法只計算沿水平方向和豎直方向的方格數(shù)，不能沿對角線穿過方格。并且在計算H值時忽略一切障礙物，僅僅對剩余的距離進行一個估算而非實際值。