黃瓜采摘機(jī)械手運動規(guī)劃與控制系統(tǒng)設(shè)計

王 燕

(洛陽理工學(xué)院 電氣工程與自動化學(xué)院，河南 洛陽 471023)

黃瓜采摘機(jī)械手運動規(guī)劃與控制系統(tǒng)設(shè)計

王 燕

(洛陽理工學(xué)院 電氣工程與自動化學(xué)院，河南 洛陽 471023)

針對現(xiàn)有采摘機(jī)器人存在機(jī)構(gòu)笨重、成本高、智能化水平低下等問題，研制了一套四自由度關(guān)節(jié)型采摘機(jī)械手。結(jié)合黃瓜采摘作業(yè)的特點，采用兩段擺線運動組合的水平抓取運動規(guī)劃算法，提出了基于DSP上位機(jī)運動控制器+CAN總線+DSP關(guān)節(jié)控制器的分布式控制系統(tǒng)。機(jī)械手定位精度測試實驗結(jié)果表明：機(jī)械手定位精度可達(dá)6.9 mm，基本可滿足黃瓜采摘作業(yè)的要求。

黃瓜采摘機(jī)械手；運動規(guī)劃；控制系統(tǒng)

農(nóng)業(yè)機(jī)器人是一類以農(nóng)產(chǎn)品為操作對象、兼有人類部分信息感知和四肢行動功能、可重復(fù)編程的柔性自動化或半自動化設(shè)備。采摘機(jī)器人作為農(nóng)業(yè)機(jī)器人的重要類型，具有很大的發(fā)展?jié)摿ΑＤ壳?，日本、荷蘭等國已研制出應(yīng)用于溫室和小型農(nóng)場的西紅柿、黃瓜、甘藍(lán)等多種采摘機(jī)器人[1-3]，我國在該領(lǐng)域中的研究雖然還處于起步階段，但也取得了一些可喜的成果，如中國農(nóng)業(yè)大學(xué)研制的草莓、茄子采摘機(jī)器人和浙江大學(xué)研制的番茄收獲機(jī)械手等[4-6]。綜合國內(nèi)外果蔬采摘機(jī)器人的研究文獻(xiàn)發(fā)現(xiàn)，對于采摘機(jī)器人的研究大多集中在視覺系統(tǒng)對目標(biāo)果實的識別和定位上，對機(jī)械手本體結(jié)構(gòu)與控制系統(tǒng)的研究較少。很多農(nóng)業(yè)采摘機(jī)器人大都直接購買現(xiàn)有的工業(yè)機(jī)械手，控制系統(tǒng)采用PC機(jī)+運動控制卡的結(jié)構(gòu)形式，這就存在機(jī)構(gòu)笨重、成本高、智能化水平不夠高等問題。針對這種情況，研制了一套四自由度關(guān)節(jié)型采摘機(jī)械手，并提出了基于DSP上位機(jī)運動控制器+CAN總線+DSP關(guān)節(jié)控制器的控制系統(tǒng)結(jié)構(gòu)，并通過實驗驗證了機(jī)械手及控制系統(tǒng)的性能。

1 黃瓜采摘機(jī)械手機(jī)械結(jié)構(gòu)設(shè)計

圖1 采摘機(jī)械手結(jié)構(gòu)簡圖

黃瓜采摘機(jī)械手機(jī)械本體主要由機(jī)械臂與末端執(zhí)行器組成。機(jī)械手實物如圖1所示，由腰關(guān)節(jié)、肩關(guān)節(jié)、肘關(guān)節(jié)和腕關(guān)節(jié)4個旋轉(zhuǎn)關(guān)節(jié)組成，屬于關(guān)節(jié)式機(jī)械手，它的一端固定在基座上，另一端自由并安裝末端執(zhí)行器。機(jī)械臂前3個自由度來確定目標(biāo)果實的位置，而為便于采摘，應(yīng)使末端執(zhí)行器在采摘過程中保持水平，在腕關(guān)節(jié)加一個旋轉(zhuǎn)自由度，故4個自由度的機(jī)械臂即可滿足采摘作業(yè)要求。

機(jī)械臂4個關(guān)節(jié)的機(jī)械結(jié)構(gòu)形式基本相同，均采用直流力矩電機(jī)串接諧波減速器的傳動方式，所設(shè)計的機(jī)械手利用CAN總線通信技術(shù)，所有關(guān)節(jié)只需4根連線(兩根電源線和兩根通信線)即可實現(xiàn)與上位機(jī)及關(guān)節(jié)之間的通信；各關(guān)節(jié)控制器具體執(zhí)行各關(guān)節(jié)的位置和速度控制算法。智能關(guān)節(jié)的設(shè)計簡化了系統(tǒng)布線，縮小了控制系統(tǒng)體積，可方便地實現(xiàn)機(jī)械臂關(guān)節(jié)的擴(kuò)展，這為機(jī)器人構(gòu)件的標(biāo)準(zhǔn)化、模塊化提供了有利條件。

2 黃瓜采摘機(jī)械手運動規(guī)劃算法

運動規(guī)劃是根據(jù)末端執(zhí)行器的作業(yè)要求來設(shè)計各關(guān)節(jié)位移、速度、加速度對時間的運動規(guī)律，規(guī)劃函數(shù)必須保證關(guān)節(jié)變量及其前兩階導(dǎo)數(shù)的連續(xù)性，使規(guī)劃的關(guān)節(jié)軌跡光滑，同時應(yīng)盡量較少額外的運動如游移和抖振[7-8]。擺線運動因其計算簡單，軌跡連續(xù)平滑，并能在有限區(qū)間的端點產(chǎn)生零速度和零加速度，特別適用于點到點的關(guān)節(jié)空間軌跡規(guī)劃。在正則形式下，其運動方程可描述為[9-10]：

(1)

容易得到它的各階導(dǎo)數(shù)為：

s′(τ)=1-cos2πτ，

(2)

s″(τ)=2πsin2πτ。

(3)

圖2 基于兩段擺線組合的平抓方式軌跡規(guī)劃

圖3 黃瓜采摘機(jī)械手控制系統(tǒng)總體框圖

從初始工作狀態(tài)位姿運動到目標(biāo)黃瓜過程中的這段軌跡，如果直接采用基于擺線運動的規(guī)劃，會碰撞其他果實，有時還會出現(xiàn)碰掉目標(biāo)果實的嚴(yán)重后果。因此，采用兩段擺線運動軌跡組合的平抓方式進(jìn)行規(guī)劃，保證機(jī)械手水平抓取的常態(tài)，如圖2所示。

當(dāng)采摘機(jī)器人由工作狀態(tài)初始位姿A運動到目標(biāo)黃瓜采摘點T時，首先控制機(jī)器人由A點運動到F點，在此段運動過程中保持腕關(guān)節(jié)水平；然后再由F點運動到T點。FT是平行于x軸的直線，即F點與T點的位置關(guān)系為：

xF=xT-Δx,yF=yT,zF=zT。

(4)

3 黃瓜采摘機(jī)械手控制系統(tǒng)設(shè)計

系統(tǒng)采用基于DSP的上位機(jī)運動控制器+下位機(jī)關(guān)節(jié)DSP控制器的多CPU、分布式控制結(jié)構(gòu)，并采用有效支持分布式控制和實時控制的CAN總線通訊，以驅(qū)動各個關(guān)節(jié)協(xié)調(diào)工作。機(jī)械手運動規(guī)劃控制系統(tǒng)框圖如圖3所示。

3.1 上位機(jī)控制系統(tǒng)硬件

上位機(jī)運動規(guī)劃控制系統(tǒng)主要由電源模塊、dsPIC30F4012最小系統(tǒng)電路、CAN總線通信模塊、串口模塊與固態(tài)繼電器控制模塊組成，用來完成逆運動學(xué)計算、軌跡規(guī)劃、插補(bǔ)運算、末端執(zhí)行器控制與視覺系統(tǒng)及下位機(jī)關(guān)節(jié)控制器的通信任務(wù)等。電源模塊實現(xiàn)對運動控制器的供電，dsPIC30F4012最小系統(tǒng)是處理器能夠正常工作的最低硬件要求，包括復(fù)位電路、時鐘電路、ICD2仿真下載電路、LED測試電路。

dsPIC30F4012提供一個異步串行模塊UART，可實現(xiàn)半雙工或全雙工通信，波特率范圍為38 bps～1.875 Mbps[11]。由于dsPIC30F4012的U1TX和U1RX引腳上是TTL電平，而RS232接口的標(biāo)準(zhǔn)電平采用負(fù)邏輯；因此，必須把TTL電平與RS232電平進(jìn)行相互轉(zhuǎn)換，本系統(tǒng)采用MAXIM公司的MAX3221E芯片來完成電平轉(zhuǎn)換。

圖4 CAN總線接口電路圖

CAN模塊用于和機(jī)械手各關(guān)節(jié)的通信，黃瓜采摘機(jī)械手采用一點對多點的通信方式，上位機(jī)運動控制器和4個關(guān)節(jié)控制器的CAN模塊均采用dsPIC30F4012內(nèi)嵌標(biāo)準(zhǔn)CAN控制器和CAN收發(fā)器MCP2551組成。MCP2551是CAN控制器和物理總線之間的接口，工作速率高達(dá)1 Mbps，它的TXD端和RXD端分別接dsPIC30F4012的CANTX端和CANRX端，CANH和CANL連接到總線上，完成與總線的通信，如圖4所示。

3.2 下位關(guān)節(jié)控制器硬件

下位關(guān)節(jié)控制器由dsPIC30F4012最小系統(tǒng)、H橋驅(qū)動電路、信號反饋電路、CAN總線通信模塊、保護(hù)與故障處理模塊、AS5045轉(zhuǎn)角反饋模塊組成。主要負(fù)責(zé)關(guān)節(jié)電機(jī)的位置控制、反饋信號處理、接收上位機(jī)控制命令并向上位機(jī)發(fā)送各關(guān)節(jié)位置信息等。

dsPIC30F4012中的電機(jī)控制PWM模塊產(chǎn)生PWM信號，作為電機(jī)驅(qū)動電路的基極信號，H橋電路由4只場效應(yīng)晶體管IRF3250組成，并由專用驅(qū)動控制芯片IR2104來驅(qū)動，從而控制直流電機(jī)的正反轉(zhuǎn)。如圖5所示。

圖5 H橋驅(qū)動電路

電機(jī)位置傳感器采用瑞士Austria Microsystems公司的AS5045無接觸式磁旋轉(zhuǎn)編碼器。它是一個片上系統(tǒng)，整合了集成式Hall元件，提供用戶可編程的零位置設(shè)定和診斷，內(nèi)嵌DSP引擎，能夠檢測磁場的方向并計算出12位的二進(jìn)制編碼，可通過同步串行接口(SSI)訪問絕對角度值，其分辨率達(dá)到0.087 9°，用于精確測量整個360°范圍內(nèi)的角度。測量角度時，只需簡單地在芯片中心的正上方放置一個雙極磁鐵即可。

3.3 上位機(jī)運動控制系統(tǒng)軟件

上位機(jī)運動控制系統(tǒng)主程序流程圖如圖6所示。

系統(tǒng)上電后，機(jī)械手首先從非工作狀態(tài)初始位姿運動到工作狀態(tài)初始位姿，然后檢查是否有串口中斷(視覺系統(tǒng)將目標(biāo)黃瓜抓取點位置獲取后發(fā)出)，如果有中斷，則可進(jìn)行采摘機(jī)械手的逆運動學(xué)運算，反之，繼續(xù)查詢；然后上位機(jī)運動控制器向關(guān)節(jié)控制器發(fā)送采集當(dāng)前位置信息的指令，并通過CAN接收中斷檢查是否將信息發(fā)出，當(dāng)上位機(jī)收到關(guān)節(jié)控制器的當(dāng)前位置信息后，進(jìn)行軌跡規(guī)劃和插值運算，并將規(guī)劃出的位置控制量通過CAN總線發(fā)到各關(guān)節(jié)控制器，驅(qū)動電機(jī)到達(dá)指定位置。當(dāng)機(jī)械手到達(dá)目標(biāo)采摘點后，上位機(jī)控制末端執(zhí)行器完成黃瓜抓持與切割操作，最后機(jī)械手運動到黃瓜放置點，松開手抓，黃瓜在重力作用下落入收集器皿中，完成一根黃瓜的采摘任務(wù)。

圖6 上位機(jī)控制系統(tǒng)主程序流程圖

3.4 關(guān)節(jié)控制器軟件

關(guān)節(jié)控制器整體流程圖如圖7所示，主要包括系統(tǒng)初始化及各功能模塊初始化子程序、電流環(huán)調(diào)節(jié)子程序、CAN通信子程序、A/D采樣中斷子程序、位置環(huán)調(diào)節(jié)子程序、AS5045電機(jī)轉(zhuǎn)角采集子程序等。

關(guān)節(jié)控制器上電進(jìn)行初始化后，便檢查是否有CAN接收中斷。如果沒有，則繼續(xù)查詢，反之，則開始接收上位機(jī)發(fā)送的位置指令；啟動AD并采樣流經(jīng)關(guān)節(jié)電機(jī)電流，判斷系統(tǒng)是否過流，如果是，則置位錯誤標(biāo)志，并使電機(jī)停轉(zhuǎn)；如系統(tǒng)處于正常工作狀態(tài)，則調(diào)用電流環(huán)控制模塊；然后采集AS5045編碼器產(chǎn)生的反饋位置信號，進(jìn)行PID調(diào)節(jié)，并將控制量通過PWM模塊輸出，從而控制關(guān)節(jié)電機(jī)達(dá)到期望位置。

4 黃瓜采摘機(jī)器人實驗

為驗證運動規(guī)劃算法及控制系統(tǒng)設(shè)計的合理性，對機(jī)械臂進(jìn)行了定位精度測試實驗。以目標(biāo)采摘區(qū)域中10個采摘點進(jìn)行實驗，實驗環(huán)境如圖8所示。

圖7 關(guān)節(jié)控制器流程圖 圖8 采摘機(jī)器人定位精度實驗平臺

具體試驗步驟如下：

(1)對于給定的采摘點，求其逆運動學(xué)方程，求解方法見文獻(xiàn)[12]，以其中1組為例：px=613.4 mm、py=267.6 mm、pz=425 mm，代入逆運動學(xué)方程，可得對應(yīng)的4個關(guān)節(jié)角度值：θ1=23.57°、θ2=76.49°、θ3=-64.14°、θ4=-12.36°。

(2)采用基于擺線運動的關(guān)節(jié)空間軌跡規(guī)劃方法，由上位機(jī)運動控制器對機(jī)械手進(jìn)行從工作狀態(tài)初始位姿到目標(biāo)采摘點的軌跡規(guī)劃和插值運算，并將規(guī)劃出的關(guān)節(jié)位置指令通過CAN總線發(fā)送到各關(guān)節(jié)控制器，從而驅(qū)動機(jī)械手到達(dá)指定位置。規(guī)劃時間取3 s，步長N取200，采用定時插補(bǔ)方式實現(xiàn)。

(3)機(jī)械手運動到指定位置后，利用美國法如公司生產(chǎn)的便攜式三坐標(biāo)測量儀Platinum FaroArm測量機(jī)械臂末端位置坐標(biāo)，其測量精度可達(dá)0.013 mm。測量結(jié)果如表1所示。

表1 機(jī)械手末端位置坐標(biāo)測量結(jié)果

由表1可知：x軸方向誤差較小，最大為4.3 mm；y軸和z軸方向較大，y軸最大誤差為6.9 mm，z軸最大誤差為-6.7 mm。盡管z軸誤差偏大，但由于黃瓜果實的抓取點是在果蒂的中心處，果蒂的長度一般為30 mm～50 mm；而z軸誤差方向又是垂直向下的，故z軸誤差不會影響到正常的采摘作業(yè)。但y軸誤差可能會造成末端手爪碰撞目標(biāo)黃瓜而導(dǎo)致不能抓持黃瓜，影響采摘作業(yè)。機(jī)械手位置誤差產(chǎn)生的原因主要有：(1)各組成連桿結(jié)構(gòu)參數(shù)的誤差，主要是由于加工裝配引起的誤差；(2)各關(guān)節(jié)的運動變量誤差引起的位姿誤差。

5 結(jié) 語

(1)設(shè)計了模塊化智能關(guān)節(jié)并構(gòu)建四自由度采摘機(jī)械手。該機(jī)械手結(jié)構(gòu)簡單緊湊，重量輕成本低，動作靈活平穩(wěn)，具有足夠的強(qiáng)度和承載能力。

(2)結(jié)合黃瓜采摘機(jī)器人自身結(jié)構(gòu)特點與果實采摘作業(yè)要求，提出了基于擺線運動的水平抓取軌跡規(guī)劃方法，將其應(yīng)用于采摘機(jī)器人關(guān)節(jié)空間的軌跡規(guī)劃，實現(xiàn)機(jī)器人快速平穩(wěn)運動。

(3)提出了采用基于DSP的上位機(jī)運動控制器+CAN總線+下位機(jī)DSP關(guān)節(jié)控制器的分布式控制方案，搭建了控制系統(tǒng)的硬件平臺，并進(jìn)行了軟件設(shè)計。

(4)機(jī)械手定位精度測試實驗表明：定位精度可達(dá)6.9 mm，基本能滿足黃瓜采摘作業(yè)的要求，只是定位誤差需要進(jìn)一步提高。

[1] Kondo N,Monta M.Basic study on chrysanthemum cutting sticking robot[C]//In proceedings of the international symposium on agricultural mechanization and automation,1997,1:93-98.

[2] Foglia M M,Reina G.Agricultural robot for radicchio harvesting[J].Journal of filed robotics,2006,23(6):363-377.

[3] Van Henten E J,Van Tuijl B J,Hemminget J,et al.An autonomous robot for harvesting cucumbers in greenhouses[J].Autonomous robots,2002,13(3):241-258.

[4] 徐麗明.草莓收獲機(jī)器人系統(tǒng)的研究[D].北京:中國農(nóng)業(yè)大學(xué),2006:40-46.

[5] 宋健,孫學(xué)巖,張鐵中等.開放式茄子采摘機(jī)器人設(shè)計與試驗[J].農(nóng)業(yè)機(jī)械學(xué)報,2009,40(1):143-147.

[6] 梁喜鳳,王永維.番茄收獲機(jī)械手奇異性分析與處理[J].農(nóng)業(yè)工程學(xué)報,2006,22(1):85-88.

[7] Neelam R P,Kamal T S.Intelligent planning of trajectories for pick-and-place operations[C]//In Proc of International Conference on System,Man and Cybernetics,2000:55-60.

[8] Gasparetto A,Zanotto V.A new method for smooth trajectory planning of robot manipulators[J].Mechanism and Machine Theory,2007(42):455-471.

[9] Jorge Angeles.機(jī)器人機(jī)械系統(tǒng)原理理論、方法和算法[M].宋偉剛譯.北京:機(jī)械工業(yè)出版社，2004:141-143.

[10] 莊鵬，姚正秋.基于擺線運動規(guī)律的懸索并聯(lián)機(jī)器人軌跡規(guī)劃[J].機(jī)械設(shè)計,2006(9):21-24.

[11] 何禮高.dsPIC30F電機(jī)與電源系列數(shù)字信號控制器原理與應(yīng)用[M].北京:北京航天航空大學(xué)出版社,2007:32-42.

[12] Zhang L B,Wang Y,Yang Q H, et al.Kinematics and trajectory planning of a cucumber harvesting robot manipulator[J].International Journal of Agricultural and Biological Engineering,2009,2(1):1-7.

Design on Motion Planning and Manipulator Control System of Cucumber Picking Robot

WANG Yan

(Luoyang Institute of Science and Technology, Luoyang 471023, China)

Aiming at the existing problems of picking robot such as ponderous mechanism, high price, low intelligence, an articulated manipulator with four degree-of-freedom is developed. The horizontal grasping motion planning algorithm of two combined cycloidal motion is proposed, considering the characteristics of cucumber picking operation. And, the distributed control structure of the picking manipulator with DSP upper monitor+CAN bus+DSP joint controllers is presented. In order to verify performances of the manipulator and its control system, the positioning accuracy experiments are carried out, and results show that position accuracy of the manipulator can reach to 6.9mm, which basically meets the requirements of the cucumber picking operation.

cucumber picking manipulator; motion planning; control system

2016-12-29

王燕(1981-)，女，河南駐馬店人，博士，講師，主要從事機(jī)器人技術(shù)及智能儀表方面的研究.

河南省高等學(xué)校重點科研項目(16A413007).

10.3969/i.issn.1674-5403.2017.02.016

TP273+5

A

1674-5403(2017)02-0056-05