基于三維視覺的蘋果采摘姿態估計算法研究

胡凱瑩，高瑞龍，桂鵬輝，岳嘉良，周喬君

（中國計量大學 計量與測試工程學院，浙江杭州， 310018）

0 引言

果實采摘是其生產鏈中的一個最耗時最費力的環節，果實的采摘具有較高的技術要求，需要技術經驗豐富的采摘工人佩戴手套采摘，根據市場的需求決定是否需要裁剪果實的梗，保證果實在堆放時不會有碰撞刮傷，果實采摘的質量直接影響到之后果實能否長期存儲和運輸，影響其經濟價值[1~2]。由于果樹上果實與枝條位置關系復雜，在采摘過程中經常發生抓手被樹枝卡住或抓手將果實和枝條一并抓到的情況，可能會導致果實損傷、果樹損傷，甚至使機器人受損[2~3]。在現實生產中，經驗豐富的采摘工能夠根據果實的生長姿態采取相應的采摘方式，可以靈活地避開樹枝精準地抓取到果實，并將果實安全地放置在果實框內，為使機器人能夠模仿采摘工的采摘方式，就必須先要感知果實的生長姿態，因此研究果實在樹枝上的生長姿態具有重要意義。

目前視覺傳感技術已經有較高的提升，平面視覺和三維視覺對自動化帶來了更多的可能，有學者將視覺傳感技術用于果實的視覺和定位，已有部分算法可以進行果實的姿態估計，但多數是根據果實紋理信息估計的二維姿態，三維姿態的算法存在運算量大的缺陷，受限較多[4~6]。鑒于這些問題，本研究將先通過果實及其附近點云來獲取果實質心以及果實鄰域內枝條的擬合直線，以點與線的空間關系來估計果實在樹枝上的生長姿態。算法的主要流程如下：首先是果實的識別和分割，通過目前已經非常成熟的YOLOv3網絡模型在彩色圖像上識別和分割果實[7~9]；然后對目標果實進行定位，提取識別框內部的點云，使用RANSAC算法擬合果實球面；接著計算距離果實最近的枝條直線，用RANSAC算法在目標果實鄰域空間內擬合距離果實最近的樹枝直線；最后以球心到樹枝直線的垂線方向作為采摘果實的最佳采摘方向，這樣有利于降低抓手被樹枝卡住或誤抓樹枝的情況，可以提高采摘系統的自適應能力，從而提高采摘成功率。

1 系統概述

■1.1 蘋果采摘系統概述

由于要獲取較為精確的果實位置和姿態，所以需要使用RGB-D相機，同時也要機械臂能夠做出適應果實姿態的動作，所以需要使用一款六軸機械臂，蘋果采摘系統的示意圖如圖1(a)所示，此系統采用眼在手外的安裝方式，將相機固定在六軸機械手的底座位置，這樣可以使相機的空間與機械臂的可工作空間有較大的重疊，提高整個系統的作業空間；由于果實的葉子為最大程度地接收太陽光照，多為向上平鋪分布，而果實由于重力作用大多在樹葉下側，故相機采用向上傾斜的安裝方式，這樣可以有效降低樹葉對果實和樹枝的視野遮擋，提高識別成功率。機械臂選用六軸機械臂，可以根據相機定位信息準確到達目標點，也可以按照姿態估計算法計算出來的果實方向調整機械臂末端接近果實的方向，達到減少機械臂與樹枝碰撞的情況；末端執行機構為帶有硅膠墊的三指抓手，硅膠墊可以增加與果實的摩擦力，防止發生滑動，同時硅膠墊也起到保護果實的作用，三指抓手可以更加穩定地抓取果實。

圖1 采摘系統和采摘姿態示意圖

■1.2 樹上果實姿態定義

圖1(b)展示了本文設定的果實采摘姿態，紅色物體為蘋果的模型，棕色物體為距離目標果實最近的樹枝，此樹枝不一定是附著果實的枝條；其中點O為通過RANSAC算法擬合出來的果實的質心，直線AB為通過RANSAC算法擬合出來的距離目標果實最近的枝條直線，圖中向量OC垂直于直線AB，點C為垂足，定義抓手采摘果實的方向為向量OC的方向，讓六自由度機械臂的末端抓手以向量OC的方向去接近目標果實的位置O，閉合抓手執行采摘動作，這樣可以降低末端執行器在接近果實的過程中抓到樹枝的可能性。

2 采摘姿態估計算法

■2.1 算法流程介紹

如圖2所示，(1)先使用3D相機獲取果實區域的圖像，獲取的圖像包括彩色圖像和深度圖像，彩色圖像里有視野中的顏色信息，深度圖像中有視野中物體的距離信息，通過已經訓練好的YOLOv3圖像識別算法識別和分割果實區域圖像，將此區域的深度圖像轉換為點云，供后續步驟使用；(2)接著進行果實質心的計算，因為無法直接找到果實的質心，需要進行近似處理，將蘋果近似視為一個球體，以球體的球心為果實的質心，對果實區域內部的點云通過RANSAC算法進行球面擬合[10]，得到果實的近似球面；(3)找到果實的質心即可尋找距離果實最近的枝條，這時僅需考慮距離果實一定區域的點云即可，可以大幅降低運算量，使用RANSAC算法對果實鄰域空間內的點云進行直線擬合，得到距離果實最近的樹枝直線；(4)得到果實的質心以及距離果實最近的枝條直線，即可通過空間幾何關系，計算出過球面中心點且與樹枝直線垂直的直線，如圖1(b)中OC方向即為所求方向。

圖2 姿態估計算法流程圖

■2.2 基于RANSAC算法的球面擬合與直線擬合

RANSAC為（Random Sample Consensus）的縮寫，此算法是根據一組包含異常數據的樣本數據集，計算出有效數據的數學模型參數，得到有效樣本數據的算法。RANSAC算法經常用于計算機視覺中，如在三維視覺領域中同時解決一對相機的匹配點問題及基本矩陣的計算。

RANSAC算法的基本假設是樣本中包含正確數據(inliers，可以被模型描述的數據，可以適應數學模型的數據)，也包含異常數據(outliers，偏離正常范圍很遠、無法適應數學模型的數據)，這些異常數據為也就是噪聲，稱這個數據集中含有噪聲。這些異常數據可能是由于錯誤的測量、錯誤的假設、錯誤的計算等產生的。同時RANSAC也假設，給定一組正確的數據，存在可以計算出符合這些數據的模型參數的方法。RANSAC算法是最流行的穩健估計工具之一，特別適合用于處理含有噪聲和異常值的數據，三位相機獲取的點云中有一些波動和異常數據，也會存在分布在目標前方的樹葉等表面的點云，這些點云對我們的計算是有干擾的，對于計算來說是噪聲，可以用RANSAC算法來降噪，同時擬合相應的模型，圖3是RANSAC算法流程圖。

圖3 RANSAC算法流程圖

首先隨機抽取樣本點，樣本個數為所模型最小需求個數，球面擬合至少需要四個樣本點，直線擬合需要至少兩個樣本點。然后以樣本點進行目標模型的計算，確定一個樣本模型。然后計算其他點是否滿足設定的目標條件，對于球面目標條件為點到球面的最小距離是否滿足閾值條件，對于直線，目標條件為點到直線的距離是否滿足閾值。把滿足目標條件的點定義為內點，記錄這個樣本模型的內點數量，通過迭代計算出內點數最多的模型參數，也就是最優的模型參數。本文采用RANSAC法來處理所得點云，進行果實球面的擬合以及距離果實最近枝條的直線擬合，擬合過程如下。

對于果實球面擬合，目標模型為球面方程(1)，其中a，b，c為球心的x，y，z坐標，r為球面的半徑。空間中四個不在同一平面的點可以確定一個球面，先在所有點中隨機抽取四個點來擬合一個初始球面。然后篩選內點，內點的篩選條件為不等式(2)，即距離此球面的距離不超過球面半徑的五分之一的點歸類為內點，統計所有情況中內點數量最多的模型參數作為最終參數。

距離果實最近的枝條直線，是使用果實周圍的點云進行擬合的，目標模型為直線方程(3)，空間中兩個點即可確定一條直線，所以先在點云中隨機抽取兩個點擬合一條初始直線，內點篩選條件為d≥10mm，其中d為點到目標直線的距離，即距離直線的距離小于10mm，視此點為內點，10mm為一個固定閾值，是根據果實可能生長得最粗直徑設定的，見方程(4)。

■2.3 果實采摘姿態估計

果實的采摘姿態為從果實質心出發到與附近枝條最近點的連線方向OC，O點為球心，則僅需計算點C坐標。因為AB垂直于OC，且點C在直線AB上，同時也在直線OC上，所以可以通過方程組(5)計算點C坐標。

3 實驗與分析

本文通過對枝上果實進行重復性拍照實驗，并用本文算法進行果實采摘姿態的計算，來測試算法的穩定性。測試一共獲取了500組數據，將計算得到的所有方向向量繪制在圖4(b)中，圖4(a)是其中一組數據的點云圖，其中紅色部分為RANSAC算法提取出的果實表面的點云，除將部分樹枝點云誤判為果實表面的點云外，其他點都分布在果實表面，綠色部分為距離果實最近的枝條表面點云，辭職先的擬合較好，灰色部分為其他點云，主要是分布在果實周圍樹葉上的點云，可以排除部分樹葉對計算結果的影響，擬合效果較好。

圖4 估計結果分布圖

圖4(a)中箭頭方向是實驗結果圖4(b)的平均，可以看到果實質心位置基本不變，變化較大的是C點，這與枝條的擬合結果有關，因為實驗用的枝條截面半徑遠小于10mm，所以誤差較大，計算每次結果與平均值的角度差，繪制了角度誤差累積分布比例圖，如圖5所示。

圖5 角度誤差累計分布比例圖

從圖中可以看出角度偏差在10°以內的比例為62.66%，角度偏差在20°以內的比例為85.02%，此外所有角度誤差的平均值為11.81°，標準差為13.65°，此誤差在實際采摘工作中是允許的；另外經計算，此算法平均估計一個果實的采摘方向所用時間為0.54s，是能夠滿足實際采摘需求的。

4 結語

本文針對現有蘋果自動化采摘系統中自適應能力不足的缺點，提出了一種通過計算果樹與鄰近枝條的空間位置關系來估計果實采摘姿態的算法，可以利用3D相機采集的果實及其周圍空間的點云信息，擬合出果實球面和最近枝條直線，然后計算出直線上距離球心最近的點，將球心與此點的連線方向作為機械臂末端執行器采摘果實的方向，可以降低采摘過程中被發生碰撞的概率。最后設計實驗，通過實驗結果可以看出，若所允許的最大角度誤差為20°時，估計結果有效率為85.02%，角度誤差的平均值為11.81°，標準差為13.65°，平均每個果實所用時間為0.54s。算法的精度以及效率在實際果實采摘中是可以滿足要求的，具有實用價值。