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

王亮 趙德安 劉曉洋

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

關鍵詞：蘋果采摘;ToF;Hu不變矩;A*算法;路徑規劃

DOI：10. 11907/rjdk. 182257

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

0 引言

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

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

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

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

1 三維信息獲取

1.1 Balser ToF工作原理

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

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

1.2 蘋果形狀特征獲取

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

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

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

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

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

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

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

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

1.3.1 果實三維位置獲取

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

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

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

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

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

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

2 采摘路徑最優化方法

2.1 障礙柵格法模型建立

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

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

2.2 基于A*算法的路徑啟發式搜索

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

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

A*算法的估價函數是F（n）=G（n）+H（n）。這里，F（n）是估價函數，G（n）是起點到當前指定方格的移動耗費，H（n）是當前點到指定目標方格的預估耗費。

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

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