面向柑橘采摘的機械手采摘程序設計

湯 旸, 楊光友,2,王焱清,2

(1 湖北工業大學農機工程研究設計院, 湖北 武漢 430068;2 湖北省農機裝備智能化工程技術研究中心, 湖北 武漢 430068)

國內外對于果蔬機械采摘進行了許多研究,并取得了一系列研究成果[1-7]。從這些研究成果可知,由于機械臂的擬人特性能夠實現幾乎所有的抓取作業,同時還能夠適應諸如灰塵、農藥等惡劣環境,因此采摘機械普遍采用機械臂作為執行機構移動裝置。如熊俊濤等[8]在其設計的多類型水果采摘機器人中采用了四自由度關節機械臂;Onishi等[9]在其設計的自動化水果收獲機器人中采用了六自由度的UR3機械臂;尹吉才[10]為其設計的蘋果采摘機器人設計了一個六自由度關節機械臂。然而只有機械臂還無法執行采摘作業,還需要末端執行器來配合機械臂共同完成果蔬采摘。柔性執行器作為一種末端執行器具有一定的彈性形變能力,能夠降低對采摘對象造成的損傷,因而受到了研究者的廣泛關注[11-15]。如鮑官軍等[16]設計了一種由3個作為手指部分的氣動柔性彎曲關節和一個作為腕部的氣動柔性扭轉關節組成的柔性末端執行器;盧偉等[17]設計了一種三指四指節的柔性手爪,用于褐菇的無損采摘。本文面向柑橘采摘,基于機械臂和柔性手爪所組成的采摘機械手平臺,研究了機械手控制程序的設計。

1 采摘硬件平臺

如圖1所示,采摘硬件平臺由機械臂、機械臂控制器、柔性手爪、手爪控制器、視覺裝置以及上位機組成。在柑橘采摘作業中,視覺裝置根據Intel RealSense R200深度相機提供的實時彩色圖像和包含場景深度信息的深度圖像,獲取柑橘的位置信息,機械臂帶動末端執行器到達指定的柑橘位置,柔性手爪與柑橘直接接觸抓取柑橘,并使其與果梗分離實現采摘。上位機在其中負責控制、監測上述硬件平臺設備。

圖1 采摘平臺硬件組成

上位機采用硬件為NVIDIA GTX950M 2G DDR3獨立顯卡、Intel Core i5-4210M雙核處理器、4GB DDR3L內存,系統為Windows10系統的神舟K650D-I5 D3筆記本電腦。

1.1 機械臂

采摘機械臂采用Denso VS-6556G六關節機械臂(圖2a),各項技術參數如表1所示。

表1 機械臂技術參數

圖2 機械臂及其控制器

機械臂控制器為Denso RC7M控制器如圖2b所示,控制器系統版本為2.330,接線情況如圖2c所示,通過以太網與上位機進行連接、數據通信。機械臂控制器的主要功能是根據上位機的控制程序驅動機械臂。

1.2 柔性手爪

柔性手爪如圖3a所示,三根通過柔性材料制成的手指間隔120°分布在圓周底座上。由于內部氣囊結構和外部氣動網格結構,手指會根據氣壓的變化發生彎曲,而正負氣壓能改變手指的彎曲方向,因此采用氣壓驅動的方式控制柔性手爪的張開、閉合動作。柔性手爪具有一定的彈性形變能力且手指表面比較粗糙,作為末端執行器能夠有效降低采摘過程對采摘對象造成的損傷,提高采摘的容錯誤差,同時能讓抓取也更加牢固。

圖3 柔性手爪及其控制器

柔性手爪控制器如圖3b所示,控制器由Arduino控制板、驅動元件和氣壓回路組成,接線如圖3c所示。通過USB串口與上位機進行連接、數據通信。手爪控制器的主要功能是根據上位機的控制程序調節回路的氣壓,切換回路的正壓、負壓工況,從而控制手爪的張開、閉合動作。

2 采摘控制程序設計

采摘控制程序的主要功能是根據獲取到的柑橘位置信息,控制機械臂運動到指定位置,配合柔性手爪的抓取動作,協作執行果梗分離動作,完成柑橘采摘,并將采摘后的柑橘放置到指定位置。

2.1 機械臂控制程序設計

機械臂控制程序由通信功能模塊和運動控制功能模塊組成。在程序的實現過程中,采用Denso的ORIN2軟件開發工具包,ORIN2提供了CAO應用程序編程接口和NetwoRC設備驅動組件,其中NetwoRC是專門用于RC7M控制器的通信、驅動組件,與CAO所提供的編程接口結合,能夠對機械臂進行二次開發,實現上位機與機械臂控制器的數據通信以及對機械臂的控制功能。

2.1.1通信功能模塊通信功能模塊用于實現上位機與機械臂控制器之間的數據通信,其中用到了NetwoRC提供的ROBOTalk通信包。該通信包提供了2種與機械臂控制器通信的方式,分別為RS232串口通信和以太網通信。這里采用以太網通信。

上位機與機械臂控制器通過創建的ArmConn()函數實現通信連接,具體實現方式如圖4所示。

圖4 ArmConn()程序流程

上位機與機械臂控制器之間通過傳遞變量的形式進行數據通信,而變量又以位姿變量和狀態變量為主。位姿變量包含機械臂在空間中的位置和姿態等信息,可以分為P型變量和J型變量。P型變量的值對應機械臂末端的位置和姿態,變量形式為P(X,Y,Z,RX,RY,RZ,FIG)。其中:X、Y、Z表示機械臂末端位置,RX、RY、RZ表示機械臂末端姿態,FIG是機械臂的關節形態。J型變量的值對應機械臂各關節的角度,變量形式為J(J1,J2,J3,J4,J5,J6)。狀態變量包含機械臂的工作狀態等信息,比如@BUSY_STATUS的值對應機械臂的工作狀態,@CURRENT_POSITION的值對應機械臂當前的末端位姿。

上位機與控制器之間通過創建的ArmGetVar()和ArmSaveVar()函數傳遞位姿變量。其中,ArmGetVar()用于讀取機械臂控制器中的位姿變量,ArmSaveVar()用于將位姿變量存入機械臂控制器。ArmGetVar()通過pICtrl->AddVariable(VarName, “”, &pIVar)創建位姿變量名為VarName的CaoVariable類pIVar對象,最后通過pIVar->get_Value(&Value)讀取到指定變量的值。ArmSaveVar()則在創建位姿變量名為VarName的pIVar對象后,通過pIVar->put_Value(&Value)將值存入到指定變量中。

上位機通過創建的ArmStatusMonitor()函數從控制器中獲取當前的機械臂狀態變量,實現機械臂狀態監測功能。ArmStatusMonitor()通過pIRobot->AddVariable(VarName, “”, &pIVar)創建狀態變量名為VarName的pIVar對象,最后通過pIVar->get_Value(&Value)讀取到指定變量的值。

2.1.2運動控制功能模塊上位機通過創建的ArmMove()函數向機械臂控制器發送控制指令,控制機械臂運動到指定位姿,具體實現方式如圖5所示。

圖5 ArmMove()程序流程

其他機械臂運動控制函數如ArmGoHome()函數用于控制機械臂返回初始位姿,通過向ArmMove()中的vntPose輸入機械臂的初始位姿實現,ArmStop()函數用于中止機械臂運動,通過調用Halt(“”)函數實現,ArmRotate()函數用于控制機械臂某關節旋轉,ArmApproach()函數用于控制機械臂沿X、Y、Z軸方向接近等等。

機械臂控制器中包含NetwoRC提供的PAC驅動程序RobSlave。RobSlave在運行狀態下能根據上位機發送的控制指令,執行對應的PAC程序控制機械臂運動,PAC為Denso機械臂所使用的控制編程語言,因此創建的機械臂運動控制函數都需要調用IsRobSlaveRunning()函數來判斷機械臂控制器中的RobSlave是否運行。

2.2 柔性手爪控制程序設計

柔性手爪控制程序由通信功能模塊和手爪動作控制功能模塊組成,除上位機程序以外,還包括手爪控制器的Arduino程序。在程序的實現過程中,通過SerialPort串口通信工具包,實現上位機與手爪控制器中Arduino的USB串口通信。上位機程序主要用于發送控制指令和監測手爪控制器,而Arduino程序則負責監測引腳以及通過引腳控制各氣壓回路元件,在程序的協作下共同實現柔性手爪的動作控制功能。

2.2.1通信功能模塊上位機與手爪控制器通過在上位機程序中創建的GripperConn()函數和在Arduino程序中創建的Setup()函數實現數據通信。GripperConn()通過調用InitPort(COM3, 9600, NONE, 8, 1)初始化串口程序,然后通過StartMonitoring()啟動串口,其中COM3為串口號、9600為波特率、NONE為檢驗位、8為數據位、1為停止位。Setup()通過調用Serial.begin(9600)與上位機建立串口通信連接,其中9600為波特率與上位機程序的波特率對應。

手爪控制器通過在Arduino程序中創建的ReceiveData()函數接收上位機發送的信息。ReceiveData()通過Serial.available()判斷串口的連接狀態,通過Serial.read()進行單字節信息的讀取,每4個字節就將信息存入到data[4]字符數組中,data[4]即為接收到的上位機信息。

2.2.2動作控制功能模塊

上位機信息以控制指令為主,通過在上位機程序中創建的GripperOpen()和GripperClose()函數發送控制指令,控制柔性手爪動作。其中,GripperOpen()控制手爪張開,GripperClose()控制手爪閉合。GripperOpen()通過調用WriteToPort(data)向手爪控制器發送手爪張開指令,GripperClose()通過調用WriteToPort(data)向手爪控制器發送手爪閉合指令,其中data即為控制指令,指令內容如表2所示。根據接收到的控制指令,手爪控制器通過在Arduino程序中創建的GripperAction()函數控制柔性手爪的張開和閉合動作,具體實現方式如圖6所示。

表2 控制指令

圖6 GripperAction()程序流程

2.3 機械手采摘控制程序設計

機械手柑橘采摘流程如圖7所示。

圖7 采摘流程

根據柑橘采摘流程,基于機械臂和柔性手爪的控制程序進行機械手采摘程序設計。

2.3.1軌跡規劃軌跡規劃功能的主要目的就是根據采摘點的位置信息,計算出機械臂在采摘過程中所要經過的軌跡位姿點,通過創建的TrajectoriesPlanning()函數實現。

考慮到機械手的果梗分離動作、采摘過程中的枝葉干涉以及采摘對象的放置,分別根據計算結果預設5個關鍵的位姿點。第一個點為采摘準備點,位于柑橘正前方一定距離的位置;第二個點為采摘點,即柑橘的位置點;第三個點為采摘完成點,位于采摘點前方和下方一定距離的位置,同時這個點還是采摘動作中P42的X,Y,Z的值;第四個點為采摘放置點,用于放置采摘的柑橘,放置點的位置取決于收集筐的位置;第五個點為放置完成點,用于確保放置完成,在離放置點正上方一定距離的位置,最后將這5個位姿點分別存放到機械臂控制器的P40、P41、P42、P43和P44變量中。

2.3.2機械手果梗分離動作控制由于末端執行器采用柔性手爪,因此機械臂將在手爪抓取到柑橘后,通過分別旋轉第4個和第六個關節180°扭動柑橘,同時向后移動拉扯柑橘的方式,分離柑橘和果梗。為實現此功能,創建了ArmPickAction()函數。

ArmPickAction()通過調用GripperClose()控制柔性手爪閉合抓取柑橘,再調用ArmMove(1L, “@E P42”, “”)控制機械臂完成扭動、彎折、拉扯柑橘果梗的動作。其中P42的X,Y,Z取決于采摘完成點的位置,而RX,RY,RZ則通過計算機械臂關節旋轉的姿態變化獲得,FIG取1。

2.3.3采摘程序實現機械手的采摘程序通過創建ArmPick()函數實現,具體實現方式如圖8所示。

圖8 ArmPick()程序流程

3 驗證試驗

為了驗證本文所設計的機械臂控制程序、柔性手爪控制程序以及機械手采摘程序的可行性,編寫了測試程序界面。

3.1 測試程序界面

測試程序界面采用MFC微軟基礎類庫提供的界面控件和應用程序編程接口(圖9),按照功能可以劃分為柔性手爪功能區域、機械臂功能區域、消息區域以及采摘功能區域。

圖9 測試程序界面

柔性手爪功能區域對應圖9的區域1方框。區域內包括串口通信參數的輸入框、柔性手爪的通信連接與斷開按鈕、柔性手爪的張開和閉合動作控制按鈕、動作控制指令的輸入框與發送指令按鈕;機械臂功能區域對應圖9的區域2方框,區域內包括機械臂控制器IP地址的輸入框、機械臂的通信連接與斷開按鈕、機械臂當前工作狀態與位姿顯示框、機械臂狀態監測功能按鈕、機械臂運動方式和運動目標位姿變量的輸入框、機械臂運動與急?？刂瓢粹o、位姿變量的輸入框、位姿變量保存與讀取按鈕;消息區域對應圖9的區域3方框,區域內包括采摘程序運行狀態等數據信息的顯示框;采摘功能區域對應圖9的區域4方框,區域內包括機械手復位、采摘和停止采摘控制按鈕。

3.2 測試過程及結果分析

在實驗室環境下,以所布置的仿真柑橘樹上的柑橘作為采摘對象,以采摘平臺及其各項控制程序作為測試對象。

3.2.1測試前準備工作柔性手爪的工作參數設置,通過上位機測試程序界面將柔性手爪氣壓回路中的氣壓值設置為柔性手爪的最大承載氣壓100 kPa;上位機與機械臂通信設置,打開機械臂控制器,通過小型示教器將通信方式設置為以太網,通信權限設置為允許讀寫,啟動模式設置為外部啟動模式;機械臂的其他設置,通過小型示教器將機械臂的速度值設置為36%,并以循環運行的方式運行機械臂控制器中的RobSlave程序。

3.2.2程序測試

1)機械臂控制程序測試試驗。通過上位機測試程序界面輸入機械臂控制器的IP地址192.168.0.2,再通過連接功能按鈕啟動通信模塊程序建立與機械臂的數據通信連接,將P型的機械臂末端位姿坐標變量P(312,-197.62,463.34,90,30,90,1)和J型的機械臂各關節角度變量J(30,60,90,15,90,45)分別傳遞并保存到機械臂控制器中,選擇機械臂的運動插補方式為曲線插補運動,選擇機械臂的運動目標位姿變量為已經存入控制器中的P型和J型變量,最后通過運動控制功能按鈕啟動運動控制程序,控制機械臂從初始位姿運動到指定位姿。

2)柔性手爪控制程序測試試驗。通過上位機測試程序界面輸入串口號COM3、波特率9600、檢驗位NONE、數據位8、停止位1,再通過連接功能按鈕啟動通信模塊程序與手爪控制器建立數據通信連接,通過動作控制功能按鈕啟動控制程序,控制柔性手爪執行張開、閉合動作。

3)機械手柑橘采摘試驗。在已經與機械臂和柔性手爪建立通信連接的情況下,根據獲得的柑橘位置信息,通過采摘功能按鈕啟動機械手采摘控制程序,為機械手的采摘軌跡進行規劃后,控制機械手按照軌跡運動到柑橘采摘點,柔性手爪同步抓取柑橘,執行柑橘與果梗的分離動作,并最終將采摘后的柑橘放置到指定位置,完成柑橘采摘。

4)測試結果。如圖10所示,圖10a為機械臂的初始位姿,10b、10c為機械臂控制程序的測試結果,圖10d、10e為柔性手爪控制程序的測試結果,圖10f、10g、10h、10i、10j為機械手采摘控制程序的測試結果。

3.2.3試驗結果分析分析整個驗證試驗,影響機械手采摘作業的因素主要有4個。

1)柔性手爪內部的氣壓值,測試過程中設置為100 kPa。氣壓值過大導致超出手爪的氣壓承載能力,損壞柔性手爪;氣壓值過小導致手爪無法提供足夠大的抓取力,無法抓牢柑橘,從而導致在果梗分離或放置的過程中柑橘脫落,導致采摘失敗。

2)機械臂的運動速度,測試過程中設置為36%。運動速度過慢增加機械臂在運動過程中所消耗的時間,減慢采摘速度;運動速度過快導致人員發生危險,測試材料發生損壞。

3)采摘軌跡規劃功能程序,測試過程中規劃了5個關鍵位姿點。位姿點過少無法滿足機械手在采摘過程中所要執行的必要動作,導致機械手沿采摘軌跡運動時與柑橘及周圍的枝干、葉片發生擦碰,柑橘相對于采摘點發生位移,使手爪抓取不到柑橘,最終采摘失敗,或是機械手、柑橘樹、其他柑橘的損傷,位姿點過多,機械臂在運動過程中所消耗的時間增加,減慢采摘速度。

4)分離動作控制功能程序,測試過程中控制機械手的第四、六關節同方向各旋轉180°旋扭柑橘果梗,同時控制機械手向后拉扯柑橘果梗,180°的旋轉角度和只有旋扭、拉扯兩個部分的果梗分離動作不會因為旋轉角度過大、動作過多增加果梗分離消耗的時間,減慢采摘速度,又不會因為旋轉角度過小、動作過少導致無法分離柑橘和果梗,導致采摘失敗。

4 結束語

本文以Denso VS-6556G六關節機械臂作為采摘機械臂、氣動柔性手爪作為末端執行器組成了采摘機械手平臺,研究了機械手柑橘采摘的控制程序設計,并基于測試程序界面進行了控制程序測試試驗和柑橘采摘試驗,驗證了機械臂運動控制程序和柔性手爪動作控制程序,以及根據已知的柑橘位置信息,規劃采摘軌跡,控制機械手運動到采摘位置抓取柑橘,成功實現柑橘與果梗的分離,并將其帶到指定的位置進行放置的可行性,對水果采摘機器人的控制程序設計具有參考價值。