賴(lài)文雅，王心海，凌永發(fā)，李碧青

(賀州學(xué)院，廣西 賀州 542899)

0 引言

用機(jī)器人和物聯(lián)網(wǎng)遠(yuǎn)程操控，代替?zhèn)鹘y(tǒng)的林間采摘作業(yè)，提高農(nóng)業(yè)生產(chǎn)的標(biāo)準(zhǔn)化及效率，是未來(lái)現(xiàn)代農(nóng)業(yè)的發(fā)展發(fā)向。機(jī)械手是機(jī)器人的重要部件，控制系統(tǒng)通過(guò)編程實(shí)現(xiàn)預(yù)期的操作，結(jié)合了人和機(jī)器的優(yōu)勢(shì)，體現(xiàn)了很強(qiáng)的人工智能。本文根據(jù)采摘機(jī)器人作業(yè)的特點(diǎn)，結(jié)合機(jī)械手的運(yùn)作方式，對(duì)機(jī)械手運(yùn)動(dòng)控制及避障進(jìn)行設(shè)計(jì)和驗(yàn)證，實(shí)現(xiàn)了機(jī)械手的避障及路徑優(yōu)化功能。

1 采摘機(jī)械手遠(yuǎn)程運(yùn)動(dòng)控制系統(tǒng)框架

為了實(shí)現(xiàn)對(duì)采摘機(jī)器人機(jī)械手的遠(yuǎn)程控制，本文提出了一種基于PLC且包括客戶(hù)端和服務(wù)端的遠(yuǎn)程運(yùn)動(dòng)控制系統(tǒng)。其中，服務(wù)端分為視頻采集和機(jī)械手控制兩部分，客戶(hù)端則包括對(duì)機(jī)械手的實(shí)時(shí)監(jiān)測(cè)、操控及仿真等。系統(tǒng)整體框架結(jié)構(gòu)如圖1所示。

采摘機(jī)械手遠(yuǎn)程運(yùn)動(dòng)控制系統(tǒng)由4部分構(gòu)成，具體敘述如下。

1)通訊協(xié)議模塊：①客戶(hù)端和服務(wù)端采用網(wǎng)絡(luò)協(xié)議通信，在客戶(hù)端指定數(shù)據(jù)包格式，并帶有確認(rèn)機(jī)制，服務(wù)端根據(jù)數(shù)據(jù)包協(xié)議進(jìn)行解析；②確定視覺(jué)服務(wù)器的數(shù)據(jù)傳輸協(xié)議包格式。

2)機(jī)器人控制服務(wù)器模塊：①客戶(hù)端操作機(jī)械手的界面；②機(jī)械手運(yùn)動(dòng)控制仿真與避障；③機(jī)器人仿真系統(tǒng)。

3)機(jī)械手模塊：機(jī)械手是用來(lái)進(jìn)行采摘抓取的部件，主要是負(fù)責(zé)目標(biāo)物體的采摘抓取。

4)視頻服務(wù)器模塊：機(jī)械手前端搭載CCD視覺(jué)傳感器，用來(lái)采集獲取前方實(shí)時(shí)信息，對(duì)采集的視頻進(jìn)行數(shù)據(jù)分析處理后反饋給客戶(hù)端服務(wù)器。

圖1 系統(tǒng)整體框架結(jié)構(gòu)圖Fig.1 The overall framework of the system structure

在整個(gè)架構(gòu)中，機(jī)械手是最重要的模塊，其可根據(jù)機(jī)器人控制服務(wù)器編程電磁閥動(dòng)作，實(shí)現(xiàn)手爪的抓取動(dòng)作，其仿真模型設(shè)計(jì)如圖2所示。

2 軟硬件設(shè)計(jì)

2.1 采摘機(jī)械手系統(tǒng)硬件平臺(tái)

2.1.1 PLC控制系統(tǒng)

本設(shè)計(jì)采用西門(mén)s-200PLC為系統(tǒng)核心控制器，包含中央微處理器、存儲(chǔ)器、手持編程器、電源，以及各種輸入及輸出接口。PLC控制器通常采用軟件編程實(shí)現(xiàn)繼電器、定時(shí)計(jì)數(shù)器，以及數(shù)模、模數(shù)轉(zhuǎn)換的功能。另外，PLC采用內(nèi)部總線(xiàn)實(shí)現(xiàn)數(shù)據(jù)信息的傳輸。PLC控制系統(tǒng)框架如圖3所示。

圖2 采摘機(jī)械手模塊仿真模型Fig.2 The simulation model of picking manipulator module

圖3 PLC控制系統(tǒng)框架圖Fig.3 The frame diagram of PLC control system

2.1.2 機(jī)械手運(yùn)動(dòng)控制參數(shù)轉(zhuǎn)換

在機(jī)械手運(yùn)動(dòng)控制過(guò)程中，系統(tǒng)對(duì)機(jī)械手進(jìn)行控制時(shí)，需要實(shí)時(shí)了解其位置、運(yùn)行速度及加速度等具體參數(shù)。本研究中，采用GT-400-SV運(yùn)動(dòng)控制卡控制機(jī)械手的控制，各參數(shù)的單位分別為：位置(pulse)、速度(pulse /ST)和加速度(pulse /ST2)，ST為控制其產(chǎn)生1個(gè)脈沖的周期時(shí)間。

1)設(shè)定運(yùn)動(dòng)位置。在閉環(huán)伺服系統(tǒng)運(yùn)動(dòng)位置設(shè)定中，機(jī)械手以速度和位置為參數(shù)的表達(dá)式分別為

(1)

(2)

其中，postion為機(jī)械手的運(yùn)動(dòng)位置；L為機(jī)械手關(guān)節(jié)長(zhǎng)度。

2)速度設(shè)置。在閉環(huán)伺服系統(tǒng)中，機(jī)械手速度設(shè)置以速度為控制模式中，絲杠轉(zhuǎn)速表達(dá)式為

(3)

電機(jī)轉(zhuǎn)速表達(dá)式為

(4)

根據(jù)PLC控制器4倍頻工作特性，伺服控制器設(shè)定速度的表達(dá)式為

(5)

其中，V為電機(jī)速度；Vel為伺服控制器的設(shè)定速度。

3)加速度設(shè)置。和前面兩個(gè)參數(shù)一樣，在閉環(huán)伺服系統(tǒng)中，驅(qū)動(dòng)器以速度為控制模式中，絲杠加速度表達(dá)式為

(6)

而電機(jī)轉(zhuǎn)速計(jì)算表達(dá)式為

(7)

根據(jù)PLC控制器4倍頻工作特性，控制器加速度表達(dá)式為

(8)

其中，acc為控制器設(shè)定的加速度值；a為機(jī)械手的運(yùn)動(dòng)加速度；m為脈沖倍頻。

2.2 采摘機(jī)械手系統(tǒng)軟件平臺(tái)

采摘機(jī)械手系統(tǒng)的軟件設(shè)計(jì)主要包括控制服務(wù)器(下位機(jī))和本地客戶(hù)端(上位機(jī))兩個(gè)方面?？刂品?wù)器主要由PLC為控制器為核心，負(fù)責(zé)采集機(jī)械手的實(shí)時(shí)運(yùn)行狀況和解析視頻服務(wù)器的圖像信息，并分析本地服務(wù)器的數(shù)據(jù)包，實(shí)時(shí)控制機(jī)械手；另外，其通過(guò)網(wǎng)絡(luò)通信的方式將數(shù)據(jù)信息發(fā)送給本地服務(wù)器端，完成二者之間信息的交互。本地客戶(hù)端主要由PC機(jī)機(jī)上位機(jī)控制軟件構(gòu)成，可以發(fā)送控制指令、接收和顯示采摘機(jī)器人的位置、速度，以及加速度信息。采摘機(jī)械手系統(tǒng)的軟件整體架構(gòu)如圖4所示。

圖4 采摘機(jī)械手系統(tǒng)的軟件設(shè)計(jì)架構(gòu)Fig.4 Software design framework of picking manipulator system

在整個(gè)軟件中設(shè)計(jì)中，控制服務(wù)器中的PLC控制程序最為復(fù)雜，包括公用、自動(dòng)、手動(dòng)和初始化4部分。PLC軟件主程序如圖5所示。

圖5 PLC軟件主程序圖Fig.5 PLC software main program diagram

圖5中，CJ為條件跳轉(zhuǎn)指令，主要是實(shí)現(xiàn)程序的跳轉(zhuǎn)，減少程序不必要的執(zhí)行時(shí)間，提高系統(tǒng)效率。X0和X1為模式選擇端口，X0閉合，X1斷開(kāi)時(shí)，程序?yàn)樽詣?dòng)控制模式；而當(dāng)X0斷開(kāi)，X1閉合時(shí)，程序?yàn)槭謩?dòng)程序和回原位程序。

3 采摘機(jī)械手運(yùn)動(dòng)學(xué)及避障原理

3.1 采摘機(jī)械手動(dòng)力學(xué)運(yùn)動(dòng)分析

為了實(shí)現(xiàn)對(duì)采摘機(jī)械手的實(shí)時(shí)控制，首先需要搭建機(jī)械手的平面模型，其是跟蹤控制機(jī)械手運(yùn)行軌跡的重要前提。該平面主要通過(guò)力學(xué)原理對(duì)機(jī)械手支撐結(jié)構(gòu)進(jìn)行動(dòng)力學(xué)分析，根據(jù)機(jī)械手各種前行參數(shù)實(shí)現(xiàn)對(duì)其的控制，達(dá)到路徑規(guī)劃和避障的目的。采摘機(jī)械手動(dòng)力學(xué)平面和空間結(jié)構(gòu)如圖6所示。

圖6 采摘機(jī)械手動(dòng)力學(xué)平面結(jié)構(gòu)圖Fig.6 The manual mechanical plane diagram of picking machine

如圖7所示，l1為關(guān)節(jié)連桿1，其長(zhǎng)度為250mm，可變角度為0°～180°；l2為關(guān)節(jié)連桿2，其長(zhǎng)度為250mm，可變角度為0°～100°。另外，由于機(jī)械手的關(guān)節(jié)編碼器采取的是編碼器工作模式，因此在每次調(diào)用運(yùn)動(dòng)程序時(shí)需對(duì)機(jī)械臂進(jìn)行初始化操作。關(guān)節(jié)坐標(biāo)系和平面直角坐標(biāo)系之間的坐標(biāo)切換表達(dá)式為

(9)

其中，x、y分別是關(guān)節(jié)2在直接坐標(biāo)系中的坐標(biāo)；q1、q2是關(guān)節(jié)1在空間坐標(biāo)系中轉(zhuǎn)過(guò)的角度。

圖7 采摘機(jī)械手動(dòng)力學(xué)空間結(jié)構(gòu)圖Fig.7 The manual mechanical space structure diagram of picking machine

根據(jù)式(9)可以計(jì)算出關(guān)節(jié)2末端在X-Y平面的位置，然后便可以實(shí)現(xiàn)對(duì)機(jī)械手的運(yùn)動(dòng)控制，計(jì)算表達(dá)式為

(10)

結(jié)合式(9)和式(10)，可得運(yùn)動(dòng)學(xué)逆運(yùn)算的表達(dá)式為

(11)

控制服務(wù)器是對(duì)機(jī)械臂關(guān)節(jié)進(jìn)行直接的處理，對(duì)機(jī)械手進(jìn)行控制時(shí)，需要計(jì)算機(jī)械臂已經(jīng)走過(guò)的距離，及其當(dāng)前位置，而該距離的計(jì)算通常采用脈沖進(jìn)行計(jì)算，該計(jì)算表達(dá)式為

(12)

其中，s為關(guān)節(jié)運(yùn)動(dòng)到目標(biāo)位置的絕對(duì)距離。

距離計(jì)算公式為

(13)

其中，β為機(jī)械手關(guān)節(jié)2轉(zhuǎn)過(guò)的角度。

3.2 采摘機(jī)械手避障原理

在機(jī)械手和障礙物目標(biāo)相切或者重疊時(shí)，表明有發(fā)生碰撞的可能，否則碰撞可能性為0。因此，在機(jī)械手運(yùn)動(dòng)控制的過(guò)程中，只需要考慮機(jī)械手和障礙物目標(biāo)相切或者重疊的情形，便可以實(shí)現(xiàn)避障。為了判斷機(jī)械手和障礙物發(fā)生碰撞的可能性，本文采用人工勢(shì)場(chǎng)法對(duì)機(jī)械手和障礙物表面排斥力進(jìn)行分析，并用dsafe和dmin分別表示機(jī)械手靠近障礙物的安全距離和極限距離；d為機(jī)械手和障礙物表面的最小距離。機(jī)械手和障礙物表面的距離關(guān)系如圖8所示。

圖8 機(jī)械手和障礙物表面的距離Fig.8 The distance between the robot and the surface of the obstacle

如圖8所示：根據(jù)d、dlim、dsafe3點(diǎn)與機(jī)械手平面直接的距離計(jì)算排斥力，即

(14)

其中，K為比例系數(shù)；fmax為最大排斥力。K和fmax之間的關(guān)系為

(15)

當(dāng)機(jī)械手向障礙物靠近時(shí)，d

4 實(shí)驗(yàn)與結(jié)果分析

為了驗(yàn)證采摘機(jī)械手運(yùn)動(dòng)控制及避障系統(tǒng)的實(shí)時(shí)性與可靠性，本文利用MatLab進(jìn)行了仿真試驗(yàn)。在試驗(yàn)中，設(shè)定步長(zhǎng)對(duì)0.5°，實(shí)現(xiàn)目的是測(cè)試機(jī)械手是否具備對(duì)空間障礙物進(jìn)行有效避障及重新規(guī)劃路徑的能力。仿真結(jié)果如圖9所示。

圖9 機(jī)械手避障仿真圖Fig.9 The obstacle avoidance simulation diagram of manipulator

如圖9所示：機(jī)械手沿著x軸正方向前進(jìn)，在步進(jìn)180次時(shí)達(dá)到A點(diǎn)，此時(shí)，與一障礙物達(dá)到了最大排斥力fmax，因此系統(tǒng)采取了避障措施，成功繞過(guò)了該障礙物。實(shí)驗(yàn)結(jié)果表明：該系統(tǒng)碰到障礙物時(shí)，能夠以最優(yōu)的圓弧路徑避開(kāi)障礙物向目標(biāo)點(diǎn)靠近，優(yōu)化效果明顯，具有很強(qiáng)的避障及路徑規(guī)劃能力。

5 結(jié)論

首先對(duì)采摘機(jī)械手遠(yuǎn)程運(yùn)動(dòng)控制系統(tǒng)框架進(jìn)行闡述；然后著手對(duì)系統(tǒng)整個(gè)軟硬件進(jìn)行了有效設(shè)計(jì)；最后通過(guò)對(duì)機(jī)械手的運(yùn)動(dòng)過(guò)程進(jìn)行建模，計(jì)算和分析了運(yùn)動(dòng)控制及避障的原理，實(shí)現(xiàn)了采摘機(jī)械手運(yùn)動(dòng)控制及避障系統(tǒng)。實(shí)驗(yàn)結(jié)果表明：該系統(tǒng)碰到障礙物時(shí)，能夠以以最優(yōu)的圓弧路徑避開(kāi)障礙物向目標(biāo)點(diǎn)靠近，優(yōu)化效果明顯，具有很強(qiáng)的避障及路徑優(yōu)化能力。