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

（中國(guó)計(jì)量大學(xué) 計(jì)量與測(cè)試工程學(xué)院，浙江杭州， 310018）

0 引言

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

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

1 系統(tǒng)概述

■1.1 蘋果采摘系統(tǒng)概述

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

圖1 采摘系統(tǒng)和采摘姿態(tài)示意圖

■1.2 樹上果實(shí)姿態(tài)定義

圖1(b)展示了本文設(shè)定的果實(shí)采摘姿態(tài)，紅色物體為蘋果的模型，棕色物體為距離目標(biāo)果實(shí)最近的樹枝，此樹枝不一定是附著果實(shí)的枝條；其中點(diǎn)O為通過RANSAC算法擬合出來的果實(shí)的質(zhì)心，直線AB為通過RANSAC算法擬合出來的距離目標(biāo)果實(shí)最近的枝條直線，圖中向量OC垂直于直線AB，點(diǎn)C為垂足，定義抓手采摘果實(shí)的方向?yàn)橄蛄縊C的方向，讓六自由度機(jī)械臂的末端抓手以向量OC的方向去接近目標(biāo)果實(shí)的位置O，閉合抓手執(zhí)行采摘?jiǎng)幼鳎@樣可以降低末端執(zhí)行器在接近果實(shí)的過程中抓到樹枝的可能性。

2 采摘姿態(tài)估計(jì)算法

■2.1 算法流程介紹

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

圖2 姿態(tài)估計(jì)算法流程圖

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

RANSAC為（Random Sample Consensus）的縮寫，此算法是根據(jù)一組包含異常數(shù)據(jù)的樣本數(shù)據(jù)集，計(jì)算出有效數(shù)據(jù)的數(shù)學(xué)模型參數(shù)，得到有效樣本數(shù)據(jù)的算法。RANSAC算法經(jīng)常用于計(jì)算機(jī)視覺中，如在三維視覺領(lǐng)域中同時(shí)解決一對(duì)相機(jī)的匹配點(diǎn)問題及基本矩陣的計(jì)算。

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

圖3 RANSAC算法流程圖

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

對(duì)于果實(shí)球面擬合，目標(biāo)模型為球面方程(1)，其中a，b，c為球心的x，y，z坐標(biāo)，r為球面的半徑?？臻g中四個(gè)不在同一平面的點(diǎn)可以確定一個(gè)球面，先在所有點(diǎn)中隨機(jī)抽取四個(gè)點(diǎn)來擬合一個(gè)初始球面。然后篩選內(nèi)點(diǎn)，內(nèi)點(diǎn)的篩選條件為不等式(2)，即距離此球面的距離不超過球面半徑的五分之一的點(diǎn)歸類為內(nèi)點(diǎn)，統(tǒng)計(jì)所有情況中內(nèi)點(diǎn)數(shù)量最多的模型參數(shù)作為最終參數(shù)。

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

■2.3 果實(shí)采摘姿態(tài)估計(jì)

果實(shí)的采摘姿態(tài)為從果實(shí)質(zhì)心出發(fā)到與附近枝條最近點(diǎn)的連線方向OC，O點(diǎn)為球心，則僅需計(jì)算點(diǎn)C坐標(biāo)。因?yàn)锳B垂直于OC，且點(diǎn)C在直線AB上，同時(shí)也在直線OC上，所以可以通過方程組(5)計(jì)算點(diǎn)C坐標(biāo)。

3 實(shí)驗(yàn)與分析

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

圖4 估計(jì)結(jié)果分布圖

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

圖5 角度誤差累計(jì)分布比例圖

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

4 結(jié)語

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