羅陸鋒，鄒湘軍，程堂燦，楊自尚，張 叢，莫宇達(dá)

?

采摘機器人視覺定位及行為控制的硬件在環(huán)虛擬試驗系統(tǒng)設(shè)計

羅陸鋒1,2，鄒湘軍1※，程堂燦2，楊自尚2，張 叢2，莫宇達(dá)1

（1. 華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機械與裝備關(guān)鍵技術(shù)教育部重點實驗室，廣州 510642； 2. 天津職業(yè)技術(shù)師范大學(xué)機械工程學(xué)院，天津 300222）

因采摘機器人野外試驗易受收獲季節(jié)、氣候和場地等諸多因素的限制，為輔助試驗采摘機器人視覺定位及其行為控制算法，設(shè)計了基于硬件在環(huán)仿真的葡萄采摘機器人虛擬試驗。該文先利用雙目立體視覺提取葡萄串采摘點及防碰包圍體等空間信息；然后以實驗室已有的6自由度采摘機器人樣機為原型，建立三維虛擬仿真模型，運用D-H法建立機器人坐標(biāo)變換，求解虛擬環(huán)境下機器人運動學(xué)正解和逆解；再以實物視覺提取的葡萄串空間信息為基礎(chǔ)，運用VC++、Javascript等編程語言在虛擬現(xiàn)實平臺EON上對采摘機器人視覺定位及其采摘行為進(jìn)行仿真設(shè)計和編程實現(xiàn)，設(shè)計出一套以實物視覺與虛擬采摘機器人相結(jié)合的硬件在環(huán)仿真平臺。最后，在該平臺上對葡萄采摘機器人進(jìn)行了34次虛擬試驗，試驗中視覺定位、路徑規(guī)劃、夾剪果梗3個環(huán)節(jié)的成功率依次為85.29%、82.35%、82.35%。結(jié)果表明，該方法可很好地運用于驗證和試驗采摘機器人視覺定位及其行為算法。

機器人；算法；設(shè)計；硬件在環(huán)仿真；雙目立體視覺；葡萄；虛擬現(xiàn)實

0 引 言

因采摘機器人視覺及控制系統(tǒng)復(fù)雜，使得樣機試驗成本高，周期長。在采摘機器人樣機開發(fā)過程中，傳統(tǒng)采摘試驗通常是在果園進(jìn)行，而果園試驗易受采摘對象的收獲季節(jié)、天氣和場地等諸多因素限制，這使得研發(fā)和設(shè)計的采摘機器人視覺及控制算法難以得到及時有效的試驗與驗證，從而延長樣機開發(fā)周期。

近年來，隨著虛擬現(xiàn)實技術(shù)的不斷發(fā)展，硬件在環(huán)虛擬仿真作為一種低成本、低風(fēng)險的試驗輔助手段被廣泛應(yīng)用于各個領(lǐng)域[1-6]。在采摘機器人方面，Van Henten等[7-8]為研究黃瓜采摘機器人免碰撞路徑規(guī)劃算法，通過在Matlab軟件環(huán)境下建立6自由度三維機械臂模型來進(jìn)行相關(guān)算法的驗證；Zou等[9-10]針對采摘機器人提出了基于虛擬機械手的視覺定位誤差建模方法和基于多領(lǐng)域統(tǒng)一仿真的運動建模方法；劉繼展等[11]為研究采摘機器人摘取及移送過程中導(dǎo)致的果穗振動與果粒脫落問題，提出了一種果穗振動仿真與試驗?zāi)Ｐ停恍芸萚12]運用虛擬機械手和CCD視覺硬件構(gòu)建的仿真系統(tǒng)來對采摘機器人定位誤差及其補償機制進(jìn)行研究。通過綜述國內(nèi)外文獻(xiàn)發(fā)現(xiàn)：目前虛擬仿真技術(shù)在采摘機器人領(lǐng)域的應(yīng)用研究大多只涉及單個問題，而將硬件在環(huán)仿真與虛擬現(xiàn)實相結(jié)合進(jìn)行采摘機器人視覺定位、路徑規(guī)劃、采摘夾切等連續(xù)過程的系統(tǒng)性虛擬仿真試驗的研究報道還非常少見。

為輔助進(jìn)行采摘機器人視覺定位及智能行為試驗，降低樣機開發(fā)成本，本文設(shè)計了一種基于硬件在環(huán)仿真的采摘機器人虛擬試驗方法。以葡萄為采摘對象，構(gòu)建6自由度虛擬采摘機器人。先利用雙目立體視覺系統(tǒng)對葡萄進(jìn)行識別與定位，求解出葡萄采摘點及其空間包圍體。然后將葡萄的空間信息發(fā)送給虛擬采摘機器人，在虛擬場景下進(jìn)行采摘機器人路徑規(guī)劃、采摘夾剪等行為的三維虛擬仿真，以實現(xiàn)對采摘機器人視覺及行為控制算法的系統(tǒng)性試驗與驗證。

1 系統(tǒng)架構(gòu)與原理

本研究設(shè)計的基于硬件在環(huán)仿真的采摘機器人虛擬試驗系統(tǒng)由雙目立體視覺模板和虛擬采摘機器人仿真模塊組成。系統(tǒng)架構(gòu)如圖1所示。

系統(tǒng)先通過雙目相機對葡萄目標(biāo)進(jìn)行左右圖像采集，然后對左圖像進(jìn)行葡萄分割、采摘點和果粒點識別，再通過立體匹配和三維反求提取葡萄采摘點空間坐標(biāo)及葡萄防碰包圍體。虛擬機器人模塊根據(jù)雙目視覺模塊采集的葡萄空間信息在虛擬環(huán)境中繪制出虛擬葡萄及其包圍體的三維模型，虛擬機器人根據(jù)采摘點坐標(biāo)和防碰包圍體進(jìn)行路徑規(guī)劃、逆運動學(xué)求解，最后控制末端執(zhí)行器運動至采摘點執(zhí)行采摘夾剪作業(yè)。

2 采摘對象的視覺定位

2.1 視覺系統(tǒng)標(biāo)定與圖像校正

本研究中的雙目立體視覺系統(tǒng)由2個USB接口相機MV-VD120SC（中國維視圖像公司生產(chǎn)）組成，該型號相機的幀率為15 Hz，雙目相機基線距離為50mm。為提取葡萄串空間信息，采用德國Halcon公司生產(chǎn)的圓形標(biāo)定板對雙目立體視覺系統(tǒng)進(jìn)行標(biāo)定，使用張正友標(biāo)定法[13]求解相機的內(nèi)部參數(shù)（焦距、鏡頭徑向失真系數(shù)等）和外部參數(shù)（攝像機位置和方向、掃視角和傾斜角）。在完成相機標(biāo)定后，利用Bouguet極線校正算法[14]對左右圖像進(jìn)行校正，極線校正的目的是讓兩個相機的光軸平行，使極點位于無窮遠(yuǎn)處，從而使得校正后像點在左右圖像上的高度一致，當(dāng)進(jìn)行立體匹配時，只需在同一行上搜索左右像平面的匹配點，大大提高立體匹配效率。

2.2 葡萄采摘點及防碰包圍體計算

2.2.1 葡萄果梗采摘點與果粒識別

葡萄采摘點的精準(zhǔn)定位是采摘機器人無損夾剪的重要前提。圖2a為實驗室環(huán)境下用雙目立體視覺系統(tǒng)采集紅提葡萄所獲的左相機圖像（即左圖像），通過提取該圖像的H顏色分量，對其進(jìn)行自適應(yīng)閾值分割[15-16]可得到圖2b，對圖2b進(jìn)行像素區(qū)域閾值和形態(tài)學(xué)降噪處理后得到葡萄串輪廓（圖2c）。再通過求解葡萄輪廓區(qū)域的極值點來獲得輪廓頂點、外接矩形。利用重心距[17]求解葡萄圖像重心。根據(jù)葡萄輪廓信息設(shè)置果梗感興趣區(qū)域，在區(qū)域內(nèi)進(jìn)行累計概率霍夫直線檢測[18]。利用作者前期研究中提出的葡萄重心到果梗區(qū)域內(nèi)的直線最小距離約束求解左圖像中的葡萄采摘點[19]，如圖2d中白色點所示。

在提取出葡萄果梗上的采摘點后，再對左圖像中葡萄外接矩形區(qū)域進(jìn)行霍夫圓檢測[20]，獲得果粒的圓心坐標(biāo)和半徑，檢測結(jié)果如圖2d所示。在對果粒進(jìn)行圓檢測時，因果粒之間存在相互遮擋、粘連等復(fù)雜情況，使得檢測出來的圓中常存在冗余圓。為提高果粒檢測性能，建立如下3條規(guī)則：1）兩任意果粒圓心之間距離大于或等于最小果粒直徑(2min)；2）果粒圓心位于葡萄圖像域內(nèi)；3）果粒半徑在限定范圍之內(nèi)，范圍中最大和最小半徑通過作者前期研究中提出的果粒半徑預(yù)測模型[21]計算獲得。規(guī)則用如下式子表達(dá)：

式中為任意2個被檢測出來的果粒圓心之間的距離，pixels；min和max分別為最小和最大果粒半徑，pixels；(center,center)為果粒圓心坐標(biāo)，regiongrape為葡萄圖像域，即圖2c中白色區(qū)域。

a. 原始圖像a. Original imageb. 分割圖像b. Segmentation image c. 分割結(jié)果c. Segmentation imaged. 左圖像檢測結(jié)果d. Detected result in left image e.右圖像中立體匹配結(jié)果e. Matching results in right imagef. 3D葡萄模型f. Three-dimensional model of grape

注：實驗室環(huán)境下用雙目立體視覺系統(tǒng)采集葡萄所獲的左相機圖像即左圖像。

Note: Left-images are the images that are captured by the left camera of binocular stereo vision system under the lab.

圖2 葡萄串三維空間信息提取過程

Fig.2 Extracting process of grape clusters space information

2.2.2 葡萄空間信息提取

通過上述步驟對左圖像進(jìn)行采摘點和果粒圓檢測后，得到這些點的在左圖像中像點坐標(biāo)。為求解這些點的空間坐標(biāo)，還需通過立體匹配計算出這些點在左右圖像中的視差。本研究采用具備線性光照不變的歸一化互相關(guān)測度[22]進(jìn)行立體匹配（如圖2e所示），再依據(jù)三角測量原理[23]求得采摘點和果粒的實際空間坐標(biāo)。在求得采摘點和果粒的三維空間坐標(biāo)后，基于作者前期研究中構(gòu)建的葡萄空間坐標(biāo)系[21]，如圖3a所示，本文將葡萄防碰包圍體簡化為一個圓柱容器，則可通過求解葡萄串高度和最大直徑來獲得圓柱包圍體。

葡萄串高度通過最上方果粒與最下方果粒的坐標(biāo)和半徑值計算得到（見式（2））。葡萄串最大直徑可通過在平面內(nèi)計算全部被檢測出的果粒到采摘點距離最大值來求得（見式（3））。

式中和分別為葡萄包圍圓柱體的高度和直徑；up和down分別為最上方果粒和最下方果粒的坐標(biāo)值；up和down分別表示最上方果粒和最下方果粒的半徑值；l為果粒到軸的距離；為果粒序號，x和z分別表示第個果粒的，坐標(biāo)。

圖3b為求解獲得葡萄空間包圍體。在提取出葡萄采摘點及包圍體信息后，將這些信息通過數(shù)據(jù)接口發(fā)送給虛擬采摘機器人進(jìn)行采摘行為仿真。

a. 葡萄坐標(biāo)系a. Coordinate system of grapeb. 葡萄包圍體圓柱b. Bounding cylinder of grape

3 采摘機器人建模及行為虛擬仿真

3.1 采摘機器人三維建模與坐標(biāo)系建立

采摘機器人三維虛擬模型的逼真性與精確性對硬件在環(huán)仿真試驗結(jié)果的可信度有著重要影響，本研究中所構(gòu)建虛擬采摘機器人嚴(yán)格按著課題組已有六自由度機器人的實際尺寸及作業(yè)空間來進(jìn)行創(chuàng)建。機器人外形如圖4a所示，該機器人由6個轉(zhuǎn)動副構(gòu)成，它們負(fù)責(zé)調(diào)整末端執(zhí)行器至合適的采摘姿位，以便執(zhí)行器能夾住葡萄并剪斷果梗。夾剪式末端執(zhí)行器由夾指、托盤和剪刀3個功能部分組成。其中夾指機構(gòu)主要負(fù)責(zé)夾緊葡萄果梗。托盤則從后下方托住葡萄串，承載一定的葡萄重力以防止葡萄抖動和滑落。當(dāng)夾指和托盤將葡萄串鎖住后，由剪刀機構(gòu)將果梗剪斷。

a. 采摘機器人外形a. Picking robot shapeb. 采摘機器人原點及各關(guān)節(jié)坐標(biāo)系b. Origin coordinate and joints coordinate systems of picking robot

本研究使用Solidworks軟件對采摘機器人進(jìn)行三維CAD建模與裝配，而后通過數(shù)據(jù)轉(zhuǎn)化將裝配體導(dǎo)入虛擬現(xiàn)實平臺Eon Studio中進(jìn)行渲染與行為仿真[24]。采摘機械臂是由剛性連桿和剛性關(guān)節(jié)組成，其運動是通過控制機械手各關(guān)節(jié)來實現(xiàn)。為便于采摘機器人正向、逆向運動學(xué)求解，根據(jù)機器人結(jié)構(gòu)與參數(shù)構(gòu)建D-H表（見表1），建立采摘機器人坐標(biāo)系，其中基準(zhǔn)坐標(biāo)系為0、各關(guān)節(jié)局部坐標(biāo)系為O（=1,2,…,6）、末端執(zhí)行器坐標(biāo)系為O。采摘機器人各關(guān)節(jié)坐標(biāo)系4b所示。

表1 采摘機器人D-H參數(shù)

3.2 虛擬采摘機器人運動學(xué)求解

3.2.1 正向運動學(xué)求解

虛擬環(huán)境下采摘機器人各軸運動是通過正向運動學(xué)模型進(jìn)行驅(qū)動的，即通過給定一組關(guān)節(jié)值來計算機器人末端執(zhí)行器坐標(biāo)系O相對于基座坐標(biāo)系0的位置和姿態(tài)，每相鄰連桿之間的變換矩陣可由式（4）和表1中的D-H參數(shù)計算得到[25]。

式中s表示sinθ；c表示cosθ(=1, 2, 3, …, 6)；α-1為扭角，(°)；a-1為連桿長度，mm；d為連桿偏置，mm。

3.2.2 逆運動學(xué)求解

通過雙目立體視覺獲取葡萄采摘點的三維坐標(biāo)之后，就可計算出末端執(zhí)行器坐標(biāo)系O需到達(dá)的目標(biāo)位置。再結(jié)合末端執(zhí)行器夾剪果梗的姿態(tài)可推導(dǎo)出機械臂末端連桿的位姿（,,,）。逆運動學(xué)求解是在已知機器人末端執(zhí)行器相對于基坐標(biāo)系位姿的情況下，計算出滿足條件的各機械臂關(guān)節(jié)變量，從而建立末端連桿的姿位與機械臂關(guān)節(jié)變量之間的運動關(guān)系。本研究采用反變換法[26]，用連桿逆變換左乘方程（5）的兩邊，把關(guān)節(jié)變量分離出來。首先求解1，用逆變換左乘方程（5）的兩邊得

令矩陣方程（6）兩端的元素（2,4）對應(yīng)相等，可得

因此可解出關(guān)節(jié)角1

然后令矩陣方程（6）兩端的元素（1, 4）和（3, 4）分別對應(yīng)相等，得方程（9）和（10）。

式中23=cos(2+3)，23=sin(2+3)。

將方程（7）、（9）、（10）左右兩邊先平方再相加可得

式中為引力勢場常量。

再利用三角代換可求解出3

式中正、負(fù)號對應(yīng)3的2種可能解。

令矩陣方程（13）兩邊的元素（1,4）和（2,4）分別對應(yīng)相等，可得

由方程組（14）可解得23和23

因23=2+3，則可由方程組（15）解得

再令兩邊元素（1,3）和（3,3）分別對應(yīng)相等，可得

當(dāng)5=0時，機械臂處于奇異形位。當(dāng)時，可求得5。

令矩陣方程（19）兩邊元素（1,3）和（3,3）分別對應(yīng)相等，可解得到5。

再令矩陣方程（21）兩邊元素（3,1）和（1,1）分別對應(yīng)相等可求得6。

從求得的（1,2,3,4,5,6）可知，3存在2種可能解。為獲得一組機械臂唯一的逆解，經(jīng)過數(shù)值計算與驗證，當(dāng)表達(dá)式3的解中取負(fù)號時，后續(xù)求得的其他關(guān)節(jié)角會出現(xiàn)超出了D-H參數(shù)表中角度范圍的情況，故舍去該解，從而獲得唯一逆解。

3.3 采摘機器人路徑規(guī)劃與虛擬行為控制

3.3.1 基于人工勢場的防碰撞路徑規(guī)劃

在葡萄采摘過程中，為避免機器人末端執(zhí)行器碰傷葡萄果粒，防碰撞路徑規(guī)劃是其關(guān)鍵所在。目前主要的路徑規(guī)劃方法有兩大類：基于模型的全局路徑規(guī)劃和基于傳感器的局部路徑規(guī)劃[27]。本研究使用基于人工勢場的局部路徑規(guī)劃方法[28]，基本思想是將采摘機械手的運動看成是其在虛擬力場的受力，采摘點對其產(chǎn)生吸引力，葡萄包圍體及其他障礙物對其產(chǎn)生排斥力，通過吸引力和排斥力的相互作用進(jìn)行路徑規(guī)劃。定義人工勢場sum()為[28]

式中att()為目標(biāo)位姿吸引力場，rep()為葡萄包圍體及其他障礙物的排斥力場，=(,)T為末端執(zhí)行器在工作空間中的位置。

令目標(biāo)位姿位為g，因att()與目標(biāo)位姿有關(guān)，于是可定義目標(biāo)點的吸引力場為

則根據(jù)引力att()為引力場的負(fù)梯度可得

對于排斥力場，選取時應(yīng)符合以下2個條件[29]：1）需滿足人工勢場sum()連續(xù)可微，且在=g時為0（取最小值）；2）在人工勢場sum()的作用下，系統(tǒng)是穩(wěn)定的。設(shè)0位障礙物空間位置，定義排斥力場為

式中為排斥力勢場常量；為障礙物影響的最大距離范圍。當(dāng)時，斥力場將不再對機器人運動產(chǎn)業(yè)作用。根據(jù)排斥力rep()為排斥力勢函數(shù)的負(fù)梯度可得

3.3.2 碰撞檢測及虛擬機器人運動控制

在規(guī)劃完路徑后，機器人按著已規(guī)劃好的路徑進(jìn)行途徑點插補，并進(jìn)行逆運動學(xué)求解，解出各關(guān)節(jié)變量（1,2,3,4,5,6），再用正運動學(xué)方法控制機器人末端執(zhí)行器運動至目標(biāo)位姿執(zhí)行夾剪作業(yè)。為檢驗路徑規(guī)劃是否合理，本研究采用層次包圍盒算法[30]在虛擬環(huán)境中進(jìn)行碰撞檢測，實時監(jiān)控機器人與葡萄防碰包圍體是否發(fā)生碰撞。如果有碰撞發(fā)生，則說明防碰撞路徑規(guī)劃失敗，還需進(jìn)一步改進(jìn)算法。

本研究利用EON平臺中的模塊化編程與路由通信機制相結(jié)合對虛擬采摘機器人各運動行為進(jìn)行控制。圖5為虛擬環(huán)境下采摘機器人運動行為的控制過程。主要涉及的模塊有：移動、旋轉(zhuǎn)、位置傳感器、角度傳感器、時間傳感器、開關(guān)節(jié)點、路由等。移動模塊主要用于末端執(zhí)行器的夾持等，通過控制、、坐標(biāo)實現(xiàn)對末端執(zhí)行器夾指的控制；旋轉(zhuǎn)模塊用于控制機械臂6個關(guān)節(jié)的旋轉(zhuǎn)和末端執(zhí)行器剪刀的旋轉(zhuǎn)切割運動等，主要是控制繞、、旋轉(zhuǎn)的、、3個控制量；位置傳感器和角度傳感器用于感知個關(guān)節(jié)運動的行程位置，當(dāng)達(dá)到預(yù)定值時及時觸發(fā)反饋信息，形成控制回路；時間傳感器用于控制機械手各關(guān)節(jié)的運動速度與加速度；開關(guān)節(jié)點用于銜接各模塊之間的相互通信。

4 系統(tǒng)開發(fā)與試驗

4.1 系統(tǒng)開發(fā)

基于硬件在環(huán)仿真的葡萄采摘機器人虛擬試驗系統(tǒng)包括硬件部分和軟件部分。其中硬件部分由雙目相機、葡萄串、葡萄支架及導(dǎo)軌、仿真葡萄莖葉、雙目相機及支架、圖像處理及虛擬仿真平臺、標(biāo)定板等組成，兩部相機被平行安裝于帶導(dǎo)軌的支架上，如圖6a所示。圖像處理及虛擬仿真平臺配置為：Intel(R)Core(TM)i5-3230M CPU@2.60 GHz，4G內(nèi)存，Window 7操作系統(tǒng)。軟件部分由視覺定位和虛擬仿真兩部分組成。視覺定位軟件使用Opencv2.3.1和Visual C++ 2008進(jìn)行開發(fā)。先通過雙目相機采集左右圖像，利用標(biāo)定好的參數(shù)對左右圖像進(jìn)行校正，再提取葡萄串采摘點和防碰空間包圍體等信息，將這些信息傳遞給虛擬仿真模塊。

1.葡萄 2.相機3.圖像處理及虛擬仿真平臺4.支架 5.標(biāo)定板

1.Grape 2.Cameras 3.Image processing and virtual simulation platform 4.Support 5.Calibration board

a. 硬件在環(huán)系統(tǒng)

a. Hardware-in-loop system

b. 虛擬采摘機器人及虛擬環(huán)境

b. Virtual picking robot and virtual environment

c. 末端執(zhí)行器靠近候選葡萄

c. End effector was closing to candidate grape

d. 夾剪葡萄果梗

虛擬仿真部分使用Visual C++ 2008、Eon Studio、Eon SDK、EonX、Javascript進(jìn)行聯(lián)合編程開發(fā)。先構(gòu)建6自由度虛擬采摘機器人及虛擬仿真環(huán)境模型。然后將模型經(jīng)過數(shù)據(jù)轉(zhuǎn)化導(dǎo)入虛擬仿真平臺Eon Studio，在虛擬平臺中設(shè)置采摘機器人基坐標(biāo)系和雙目立體視覺系統(tǒng)的世界坐標(biāo)系與虛擬采摘機器人基準(zhǔn)坐標(biāo)之間的關(guān)系（相當(dāng)于樣機試驗中的手眼標(biāo)定）。再在EON平臺上使用模塊化編程對采摘機器人運動控制、路徑規(guī)劃、夾剪行為等過程進(jìn)行編程。軟硬件之間數(shù)據(jù)接口使用EonX控件進(jìn)行設(shè)計，接口包括輸入EventIn和輸出EventOut兩種接口，用于實現(xiàn)雙目視覺系統(tǒng)與虛擬采摘機器人仿真系統(tǒng)之間的數(shù)據(jù)通信。

圖6b為虛擬環(huán)境下6自由度采摘機器人，圖6c為路徑規(guī)劃后機器人正逐步靠近待采摘葡萄串，6d為末端執(zhí)行器達(dá)到采摘點后，正執(zhí)行夾剪作業(yè)。

4.2 采摘機器人虛擬試驗與分析

為試驗本系統(tǒng)的實用性，從市場購買帶果梗的紅提葡萄串，將其以自然生長的形式懸掛于架上，如圖6a所示。然后使用雙目立體相機采集葡萄圖像對，深度距離（相機與葡萄之間距離）控制在500～1 000 mm之間。試驗中攝像機位置保持不變，通過改變葡萄位置進(jìn)行試驗。在實驗室內(nèi)進(jìn)行34次虛擬試驗。運用前期研究中提出的視覺定位誤差計算方法[31]對采摘點在（水平方向）和（深度方向）方向的定位誤差進(jìn)行測算。試驗數(shù)據(jù)統(tǒng)計見表2。

表2 虛擬試驗情況統(tǒng)計

注：“路徑規(guī)劃”中“S”代表路徑規(guī)劃成功，即無碰撞；“F”代表失敗?！皧A剪果?！敝小癝”代表夾剪成功，“F”代表失敗。

Note: “S” in “path planning” represented a success path planning, which indicated that the path planning had no collision, and the “F” represented the failure. “S” in “clamping and cutting” represented a success clamping and cutting operation and the “F” represents the failure.

4.2.1 視覺定位試驗分析

在34次試驗中，成功的視覺定位次數(shù)為29次（85.29%），有5次定位失敗，通過分析，發(fā)現(xiàn)在5次失敗的定位中有2次因葡萄被葉子遮擋導(dǎo)致采摘點識別錯誤，3次是采摘點的立體匹配錯誤。當(dāng)采摘點識別與立體匹配出現(xiàn)錯誤時，會導(dǎo)致大的定位誤差。如表2中第9、13、20、27和33次視覺定位失敗，出現(xiàn)了大誤差，且這種大誤差往往很難通過誤差補償方式來解決[32]，從而使得虛擬試驗時路徑規(guī)劃、采摘夾剪作業(yè)都失敗。由此可見，視覺精準(zhǔn)定位是采摘機器人成功作業(yè)的核心關(guān)鍵環(huán)節(jié)。

為了驗證視覺定位算法的實時性，對34次試驗中視覺定位的消耗時間進(jìn)行計算，通過運用C++編程語言中的程序運行時間函數(shù)對消耗時間進(jìn)行統(tǒng)計[21]。每次視覺定位所耗時間在0.39～0.79 s之間。

4.2.2 路徑規(guī)劃試驗分析

在29次成功定位中，有1次路徑規(guī)劃時出現(xiàn)末端執(zhí)行器碰撞葡萄包圍體邊界的情況，通過分析，發(fā)現(xiàn)其原因是求得的采摘點與實際包圍體上方間距離過小，且求得的采摘點在方向誤差為負(fù)（即求得采摘點到相機距離比實際采摘點遠(yuǎn)），且誤差達(dá)到9.83 mm。因而，當(dāng)末端執(zhí)行器在靠近果梗采摘點時執(zhí)行器底部與葡萄包圍體上邊緣發(fā)生碰撞，導(dǎo)致碰撞報錯，最終出現(xiàn)路徑規(guī)劃失敗。其余28次規(guī)劃均獲得成功，總成功率為82.35%。圖7為其中一次虛擬試驗中的運動路徑，白色點為該次試驗中末端執(zhí)行器夾指中點處所經(jīng)過的空間軌跡。

4.2.3 夾剪果梗試驗分析

由表2中可知，在29次成功的視覺定位中，方向的視覺定位誤差最大為7.55 mm，方向最大值為9.83 mm。當(dāng)路徑規(guī)劃成功后，在28次夾剪果梗作業(yè)中，尚未出現(xiàn)有末端執(zhí)行器夾持果梗失敗的情況，成功率82.35%，說明所設(shè)計末端執(zhí)行器夾指能容忍視覺定位系統(tǒng)所產(chǎn)生計算誤差，證明執(zhí)行器容錯設(shè)計滿足視覺定位誤差的要求。

從上述試驗情況來看，采摘機器人在采摘點識別、立體匹配、路徑規(guī)劃等3個環(huán)節(jié)均出現(xiàn)了失敗的情況，說明這些環(huán)節(jié)的算法存在一定缺陷，還有待改進(jìn)，也證明了本文所設(shè)計的硬件在環(huán)仿真平臺對幫助試驗和改進(jìn)機器人視覺與控制算法、機器人容錯結(jié)構(gòu)設(shè)計有很好的實用價值。

5 結(jié) 論

為輔助試驗采摘機器人視覺定位及其行為控制算法，設(shè)計了一種基于硬件在環(huán)仿真的采摘機器人虛擬試驗系統(tǒng)。從試驗系統(tǒng)結(jié)構(gòu)、雙目立體視覺定位、采摘機器人三維建模及運動學(xué)求解、路徑規(guī)劃和采摘機器人虛擬行為控制等方面對系統(tǒng)進(jìn)行了詳細(xì)闡述，并完成了基于硬件在環(huán)仿真的采摘機器人視覺定位試驗系統(tǒng)開發(fā)。該系統(tǒng)先使用雙目立體視覺對葡萄果梗上的采摘點和葡萄包圍體進(jìn)行三維定位。再將獲得葡萄定位信息發(fā)送給虛擬采摘機器人，虛擬采摘機器人依據(jù)視覺定位信息規(guī)劃防碰撞路徑和夾剪作業(yè)。最后在該平臺上對葡萄進(jìn)行了34次視覺定位及行為控制虛擬試驗，試驗中視覺定位成功次數(shù)29次(85.29%)、路徑規(guī)劃成功28次(82.35%)、夾剪成功28次(82.35%)。結(jié)果表明該虛擬試驗系統(tǒng)能對采摘機器人的視覺定位、路徑規(guī)劃、夾剪行為等環(huán)節(jié)算法進(jìn)行試驗與驗證，可為采摘機器人視覺系統(tǒng)算法與末端執(zhí)行器結(jié)構(gòu)的改進(jìn)提供參考。

[1] Kang D K, Kim M S. Hardware-in-the-loop simulation to evaluate the drive performance of the electric two- wheelers on a motor dynamometer[J]. International Journal of Automotive Technology, 2015, 16(6): 1031－1040.

[2] 陳勇，常綠，夏晶晶. 基于硬件在環(huán)仿真試驗平臺的電動液壓助力系統(tǒng)能耗分析[J]. 農(nóng)業(yè)工程學(xué)報，2010，26(8)：117－122.

Chen Yong, Chang Lü, Xia Jingjing. Energy consumption analysis of electro-hydraulic power steering system based on hardware-in-loop simulation test[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2010, 26(8): 117－122. (in Chinese with English abstract)

[3] 苑嚴(yán)偉，李樹君，吳海華，等. 水稻田虛擬試驗場景建模與仿真[J]. 農(nóng)業(yè)機械學(xué)報，2013，44(增刊1)：224－229. Yuan Yanwei, Li Shujun, Wu Haihua, et al. Modeling and simulation of rice paddy field virtual scene[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(Supp.1): 224－229. (in Chinese with English abstract)

[4] 臧宇，朱忠祥，宋正河，等. 農(nóng)業(yè)裝備虛擬試驗系統(tǒng)平臺的建立[J]. 農(nóng)業(yè)機械學(xué)報，2010，41(9)：70－74. Zang Yu, Zhu Zhongxiang, Song Zhenghe, et al. Virtual experiment system platform for agricultural equipment[J]. Transactions of the Chinese Society for Agricultural Machinery, 2010, 41(9): 70－74. (in Chinese with English abstract)

[5] 楊方飛，閻楚良. 基于視景仿真的聯(lián)合收獲機虛擬試驗技術(shù)[J]. 農(nóng)業(yè)機械學(xué)報，2011，42(1)：79－83. Yang Fangfei, Yan Chuliang. Virtual test of combine harvester based on visual simulation[J]. Transactions of the Chinese Society for Agricultural Machinery, 2011, 42(1): 79－83. (in Chinese with English abstract)

[6] 謝斌，李靜靜，魯倩倩，等. 聯(lián)合收割機制動系統(tǒng)虛擬樣機仿真及試驗[J]. 農(nóng)業(yè)工程學(xué)報，2014，30(4)：18－24.

Xie Bin, Li Jingjing, Lu Qianqian, et al. Simulation and experiment of virtual prototype braking system of combine harvester[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2014, 30(4): 18－24. (in Chinese with English abstract)

[7] van Henten E J, Hemming J, van Tuijl B A J. Collision-free motion planning for a cucumber picking robot[J]. Biosystems Engineering, 2003, 86(2): 135－144.

[8] van Henten E J, Schenk E J, van Willigenburg L G. Collision-free inverse kinematics of the redundant seven-link manipulator used in a cucumber picking robot[J]. Biosystems Engineering, 2010, 106: 112－124.

[9] Zou Xiangjun, Zou Haixin, Lu Jun. Virtual manipulator-based binocular stereo vision positioning system and errors modeling[J]. Machine Vision and Applications, 2012, 23(1): 43－63.

[10] 鄒湘軍，金雙，陳燕，等. 基于Modelica 的采摘機械手運動控制建模[J]. 系統(tǒng)仿真學(xué)報，2009，21(14)：5882－5885.

Zou Xiangjun, Jin Shuang, Chen Yan, et al. Modelica- based modeling research of picking manipulator motion control process[J]. Journal of System Simulation, 2009, 21(14): 5882－5885. (in Chinese with English abstract)

[11] 劉繼展，唐善奇，單帥，等. 機器人采摘葡萄果穗振動仿真與試驗[J]. 農(nóng)業(yè)機械學(xué)報，2016，47(5)：1－8. Liu Jizhan, Tang Shanqi, Shan Shuai, et al. Simulation and test of grape fruit cluster vibration for robotic harvesting[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(5): 1－8. (in Chinese with English abstract)

[12] 熊俊濤，鄒湘軍，鄒海鑫，等. 結(jié)合機器視覺的采摘機械手的定位仿真研究[J]. 計算機工程與應(yīng)用，2011，47(24)：139－142.

Xiong Juntao, Zou Xiangjun, Zou Haixin, et al. Study of picking manipulator location simulation based on machine vision[J]. Computer Engineering and Applications, 2011, 47(24): 139－142. (in Chinese with English abstract)

[13] Zhang Z. A flexible new technique for camera calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(11): 1330－1334.

[14] Bouguet J Y. Visual Methods for Three-Dimensional Modeling[D]. California: California Institute of Technology, 1999.

[15] Sezgin M, Sankur B. Survey over image thresholding techniques and quantitative performance evaluation[J]. Journal of Electronic Imaging, 2004, 13(1): 146－165.

[16] Otsu N. A threshold selection method from gray-level histograms[J]. IEEE Transactions on Systems Man and Cybernetics, 1979, 9(1): 62－66.

[17] Rafael C G, Richard E W. Digital Image Processing[M]. Beijing: Publish House of Electronics Industry, 2012: 542－543.

[18] Galambos M C, Kittler J. Robust detection of lines using the progressive probabilistic hough transform[J]. Computer Vision and Image Understanding, 2000, 78(1): 119－137.

[19] 羅陸鋒，鄒湘軍，熊俊濤，等. 自然環(huán)境下葡萄采摘機器人采摘點的自動定位[J]. 農(nóng)業(yè)工程學(xué)報，2015，31(2)：14－21.

Luo Lufeng, Zou Xiangjun, Xiong Juntao, et al. Automatic positioning for picking point of grape picking robot in natural environment[J].Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(2): 14－21. (in Chinese with English abstract)

[20] Galambos C, Kittler J, Matas J. Gradient-based progressive probabilistic hough transform[J]. IEEE Vision Image and Signal Processing, 2002, 148(3): 158－165.

[21] 羅陸鋒，鄒湘軍，葉敏，等. 基于雙目立體視覺的葡萄采摘防碰空間包圍體求解與定位[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(8)：41－47.

Luo Lufeng, Zou Xiangjun, Ye Min, et al. Calculation and localization of bounding volume of grape for undamaged fruit picking based on binocular stereo vision[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(8): 41－47. (in Chinese with English abstract)

[22] Steger C, Ulrich M. Wiedemann C. Machine Vision Algorithms and Applications[M]. Weinheim: Wiley-VCH, 2007.

[23] Rodriguez J J, Aggarwal J K. Stochastic analysis of stereo quantization error[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1990, 12: 467－470.

[24] 羅陸鋒，鄒湘軍，劉天湖，等. 大數(shù)據(jù)量虛擬景觀的三維模型優(yōu)化與漫游[J]. 系統(tǒng)仿真學(xué)報，2009，21(6)：1654－1657.

Luo Lufeng, Zou Xiangjun, Liu Tianhu et al. 3D model optimization and navigation of large-scale virtual landscape[J]. Journal of System Simulation, 2009, 21(6): 1654－1657. (in Chinese with English abstract)

[25] 熊有倫. 機器人技術(shù)基礎(chǔ)[M]. 武漢：華中科技大學(xué)出版社，2011：34－35.

[26] Paul R P, Shimano B E, Mayer G. Kinematic control equations for simple manipulators[J]. IEEE Transactions Systems, Man and Cybernetics, 1981, 11(6): 449－455.

[27] 朱大奇，顏明重. 移動機器人路徑規(guī)劃技術(shù)綜述[J]. 控制與決策，2010，25(7)：961－967.

Zhu Daqi, Yan Mingzhong. Survey on technology of mobile robot path planning[J]. Control and Decision, 2010, 25(7): 961－967. (in Chinese with English abstract)

[28] Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. International Journal of Robotics and Research, 1986, 5(1): 90－98.

[29] Ge S S, Cui Y J. New potential functions for mobile robot path planning[J]. IEEE Transactions on Robots and Automation, 2000, 16(5): 615－620.

[30] 朱元峰，孟軍，謝光華，等. 基于復(fù)合層次包圍盒的實時碰撞檢測研究[J]. 系統(tǒng)仿真學(xué)報，2008，20(2)：372－377.

Zhu Yuanfeng, Meng Jun, Xie Guanghua et al. Research on real-time collision detection based on hybrid hierarchical bounding volume[J]. Journal of System Simulation, 2008, 20(2): 372－377. (in Chinese with English abstract)

[31] Luo Lufeng, Tang Yunchao, Zou Xiangjun, et al. Vision-based extraction of spatial information in grape clusters for harvesting robots[J]. Biosystems Engineering, 2016, 151: 90－104.

[32] Zou Xiangjun, Ye Min, Luo Chengyu, et al. Fault-tolerant design of a limited universal fruit-picking end-effector based on visoin positioning error[J]. Applied Engineering in Agriculture, 2016, 32(1): 5－18.

Design of virtual test system based on hardware-in-loop for picking robot vision localization and behavior control

Luo Lufeng1,2, Zou Xiangjun1※, Cheng Tangcan2, Yang Zishang2, Zhang Cong2, Mo Yuda1

(1.510642; 2.300222)

In the process of developing picking robot prototype, the traditional picking tests are usually performed in orchard, which are limited by certain factors such as the harvesting season, weather condition and venue. So, the investigated and designed algorithm for the vision and control system of picking robots can’t be verified effectively and timely, and the prototype development cycle has to last longer. To test the vision and control algorithm of picking robot, a hardware-in-the-loop virtual experimental system based on binocular stereo vision for grape-picking robot was designed in this paper, which was composed of hardware and software units. The hardware units consisted of binocular camera, grape clusters, grape imitative leaf and stems, support structure of grape clusters and its guide rail, calibration board, and so on. The software units included vision processing part and virtual picking robot. Firstly, the spatial information such as the picking point and the anti-collision bounding volume of the grape cluster was extracted by binocular stereo vision. The picking point on the peduncle of the grape cluster was detected by using a minimum distance restraint between the barycentre of the pixel region of grape cluster and the detected lines in the ROI (region of interest) of peduncle. The anti-collision bounding volume of the grape cluster was calculated by transforming the spatial coordinates of the picking point and all detected grape berries into the coordinate system of grape clusters. Secondly, the three-dimensional models of the picking robot were constructed according to the picking robot prototype with 6 degrees of freedom which already existed in our laboratory. The Denavit-Hartenberg (D-H) method was adopted to establish the robot coordinate transformation. The direct and inverse solutions of the robot kinematics were solved by using the inverse transformation method, and then the only inverse solution was obtained. Thirdly, the moving path of picking robot was planned based on the artificial potential field theory. The collision between the robot manipulator and the grape clusters in the virtual environment was detected by using the hierarchical bounding box algorithm which can validate the reasonability of path planning. The motion simulation of the virtual picking robot was programmed by combining the modular programming and the routing communication mechanism. Finally, the spatial information of the grape clusters was extracted by programming the application code using Visual C++ and OpenCV (open source computer vision library), and the path planning and the motion simulation of the virtual picking robot were performed based on the virtual reality platform EON, Visual C++ and JavaScript. The hardware-in-the-loop virtual experimental platform was established by combining the binocular stereo vision and virtual picking robot. On this platform, 34 tests were performed by changing the position of the grape clusters under laboratory environment while the binocular cameras kept still. And every test included 3 steps, the first step was vision locating, the second was path planning and the last was clamping and cutting operation. In all the tests, 29 tests were successful in vision locating, and 5 tests were failed in vision locating. Among those 5 failed tests, 2 tests were wrong in picking point detection and 3 tests were failed in stereo matching on the picking point. There was one test failed in path planning when the grape clusters were located correctly, and all of the clamping and cutting operation for the grape clusters ran smoothly when the anti-collusion path was planned successfully. In general, the success rates of the tests on visual localization, path planning, clamping and cutting operation were 85.29%, 82.35%, 82.35%, respectively. The results showed that the method developed in this study can be used to verify and test the visual location and behavior algorithm of the picking robot, and then provide the support to the harvesting robot development, test and continuous improvement.

robots; algorithms; design; hardware-in-the-loop; binocular stereo vision; grape; virtual reality

10.11975/j.issn.1002-6819.2017.04.006

TP391

A

1002-6819(2017)-04-0039-08

2016-05-25

2017-01-20

國家自然科學(xué)基金資助項目（31571568），廣東省科技計劃項目（2015A020209111，2015A020209120，2014A020208091）

羅陸鋒，男（漢族），湖南新化人，博士生，講師，主要從事機器視覺與虛擬現(xiàn)實、農(nóng)業(yè)采摘機器人研究。廣州華南農(nóng)業(yè)大學(xué)工程學(xué)院，510642。Email：luolufeng617@163.com

鄒湘軍，女（漢族），湖南衡陽人，教授，博士生導(dǎo)師，主要從事農(nóng)業(yè)采摘機器人、智能設(shè)計與制造、虛擬現(xiàn)實等研究。廣州華南農(nóng)業(yè)大學(xué)工程學(xué)院，510642。Email：xjzou1@163.com

羅陸鋒，鄒湘軍，程堂燦，楊自尚，張 叢，莫宇達(dá). 采摘機器人視覺定位及行為控制的硬件在環(huán)虛擬試驗系統(tǒng)設(shè)計[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(4)：39－46. doi：10.11975/j.issn.1002-6819.2017.04.006 http://www.tcsae.org

Luo Lufeng, Zou Xiangjun, Cheng Tangcan, Yang Zishang, Zhang Cong, Mo Yuda. Design of virtual test system based on hardware-in-loop for picking robot vision localization and behavior control[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 39－46. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.04.006 http://www.tcsae.org