采摘機器人視覺定位及行為控制的硬件在環虛擬試驗系統設計

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

?

采摘機器人視覺定位及行為控制的硬件在環虛擬試驗系統設計

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

（1. 華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642； 2. 天津職業技術師范大學機械工程學院，天津 300222）

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

機器人；算法；設計；硬件在環仿真；雙目立體視覺；葡萄；虛擬現實

0 引 言

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

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

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

1 系統架構與原理

本研究設計的基于硬件在環仿真的采摘機器人虛擬試驗系統由雙目立體視覺模板和虛擬采摘機器人仿真模塊組成。系統架構如圖1所示。

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

2 采摘對象的視覺定位

2.1 視覺系統標定與圖像校正

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

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

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

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

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

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

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

注：實驗室環境下用雙目立體視覺系統采集葡萄所獲的左相機圖像即左圖像。

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 葡萄空間信息提取

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

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

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

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

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

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

3.1 采摘機器人三維建模與坐標系建立

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

a. 采摘機器人外形a. Picking robot shapeb. 采摘機器人原點及各關節坐標系b. Origin coordinate and joints coordinate systems of picking robot

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

表1 采摘機器人D-H參數

3.2 虛擬采摘機器人運動學求解

3.2.1 正向運動學求解

虛擬環境下采摘機器人各軸運動是通過正向運動學模型進行驅動的，即通過給定一組關節值來計算機器人末端執行器坐標系O相對于基座坐標系0的位置和姿態，每相鄰連桿之間的變換矩陣可由式（4）和表1中的D-H參數計算得到[25]。

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

3.2.2 逆運動學求解

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

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

因此可解出關節角1

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

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

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

式中為引力勢場常量。

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

式中正、負號對應3的2種可能解。

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

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

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

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

當5=0時，機械臂處于奇異形位。當時，可求得5。

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

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

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

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

3.3.1 基于人工勢場的防碰撞路徑規劃

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

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

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

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

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

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

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

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

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

4 系統開發與試驗

4.1 系統開發

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

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

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

a. 硬件在環系統

a. Hardware-in-loop system

b. 虛擬采摘機器人及虛擬環境

b. Virtual picking robot and virtual environment

c. 末端執行器靠近候選葡萄

c. End effector was closing to candidate grape

d. 夾剪葡萄果梗

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

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

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

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

表2 虛擬試驗情況統計

注：“路徑規劃”中“S”代表路徑規劃成功，即無碰撞；“F”代表失敗。“夾剪果梗”中“S”代表夾剪成功，“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次試驗中，成功的視覺定位次數為29次（85.29%），有5次定位失敗，通過分析，發現在5次失敗的定位中有2次因葡萄被葉子遮擋導致采摘點識別錯誤，3次是采摘點的立體匹配錯誤。當采摘點識別與立體匹配出現錯誤時，會導致大的定位誤差。如表2中第9、13、20、27和33次視覺定位失敗，出現了大誤差，且這種大誤差往往很難通過誤差補償方式來解決[32]，從而使得虛擬試驗時路徑規劃、采摘夾剪作業都失敗。由此可見，視覺精準定位是采摘機器人成功作業的核心關鍵環節。

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

4.2.2 路徑規劃試驗分析

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

4.2.3 夾剪果梗試驗分析

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

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

5 結 論

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

[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] 陳勇，常綠，夏晶晶. 基于硬件在環仿真試驗平臺的電動液壓助力系統能耗分析[J]. 農業工程學報，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] 苑嚴偉，李樹君，吳海華，等. 水稻田虛擬試驗場景建模與仿真[J]. 農業機械學報，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] 臧宇，朱忠祥，宋正河，等. 農業裝備虛擬試驗系統平臺的建立[J]. 農業機械學報，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] 楊方飛，閻楚良. 基于視景仿真的聯合收獲機虛擬試驗技術[J]. 農業機械學報，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] 謝斌，李靜靜，魯倩倩，等. 聯合收割機制動系統虛擬樣機仿真及試驗[J]. 農業工程學報，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]. 系統仿真學報，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]. 農業機械學報，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] 熊俊濤，鄒湘軍，鄒海鑫，等. 結合機器視覺的采摘機械手的定位仿真研究[J]. 計算機工程與應用，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] 羅陸鋒，鄒湘軍，熊俊濤，等. 自然環境下葡萄采摘機器人采摘點的自動定位[J]. 農業工程學報，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]. 農業工程學報，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] 羅陸鋒，鄒湘軍，劉天湖，等. 大數據量虛擬景觀的三維模型優化與漫游[J]. 系統仿真學報，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] 熊有倫. 機器人技術基礎[M]. 武漢：華中科技大學出版社，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] 朱大奇，顏明重. 移動機器人路徑規劃技術綜述[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] 朱元峰，孟軍，謝光華，等. 基于復合層次包圍盒的實時碰撞檢測研究[J]. 系統仿真學報，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

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

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

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

羅陸鋒，鄒湘軍，程堂燦，楊自尚，張 叢，莫宇達. 采摘機器人視覺定位及行為控制的硬件在環虛擬試驗系統設計[J]. 農業工程學報，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