王 燕

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

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

王 燕

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

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

黃瓜采摘機(jī)械手；運(yùn)動規(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)果實(shí)的識別和定位上，對機(jī)械手本體結(jié)構(gòu)與控制系統(tǒng)的研究較少。很多農(nóng)業(yè)采摘機(jī)器人大都直接購買現(xiàn)有的工業(yè)機(jī)械手，控制系統(tǒng)采用PC機(jī)+運(yùn)動控制卡的結(jié)構(gòu)形式，這就存在機(jī)構(gòu)笨重、成本高、智能化水平不夠高等問題。針對這種情況，研制了一套四自由度關(guān)節(jié)型采摘機(jī)械手，并提出了基于DSP上位機(jī)運(yùn)動控制器+CAN總線+DSP關(guān)節(jié)控制器的控制系統(tǒng)結(jié)構(gòu)，并通過實(shí)驗(yàn)驗(yàn)證了機(jī)械手及控制系統(tǒng)的性能。

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

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

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

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

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

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

(1)

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

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

(2)

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

(3)

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

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

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

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

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

(4)

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

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

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

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

dsPIC30F4012提供一個(gè)異步串行模塊UART，可實(shí)現(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ī)械手采用一點(diǎn)對多點(diǎn)的通信方式，上位機(jī)運(yùn)動控制器和4個(gè)關(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)編碼器。它是一個(gè)片上系統(tǒng)，整合了集成式Hall元件，提供用戶可編程的零位置設(shè)定和診斷，內(nèi)嵌DSP引擎，能夠檢測磁場的方向并計(jì)算出12位的二進(jìn)制編碼，可通過同步串行接口(SSI)訪問絕對角度值，其分辨率達(dá)到0.087 9°，用于精確測量整個(gè)360°范圍內(nèi)的角度。測量角度時(shí)，只需簡單地在芯片中心的正上方放置一個(gè)雙極磁鐵即可。

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

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

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

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

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

具體試驗(yàn)步驟如下：

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

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

(3)機(jī)械手運(yùn)動到指定位置后，利用美國法如公司生產(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軸誤差偏大，但由于黃瓜果實(shí)的抓取點(diǎn)是在果蒂的中心處，果蒂的長度一般為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é)的運(yùn)動變量誤差引起的位姿誤差。

5 結(jié) 語

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

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

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

(4)機(jī)械手定位精度測試實(shí)驗(yàn)表明：定位精度可達(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ì)與試驗(yàn)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2009,40(1):143-147.

[6] 梁喜鳳,王永維.番茄收獲機(jī)械手奇異性分析與處理[J].農(nóng)業(yè)工程學(xué)報(bào),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] 莊鵬，姚正秋.基于擺線運(yùn)動規(guī)律的懸索并聯(lián)機(jī)器人軌跡規(guī)劃[J].機(jī)械設(shè)計(jì),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é)校重點(diǎn)科研項(xiàng)目(16A413007).

10.3969/i.issn.1674-5403.2017.02.016

TP273+5

A

1674-5403(2017)02-0056-05