基于PLC的擠奶機(jī)器人三自由度機(jī)械臂控制方法研究

郭洋　吳澤全　蔡曉華　侯云濤

摘要：為提高我國奶牛養(yǎng)殖自動(dòng)化水平，研究一種擠奶機(jī)器人機(jī)械臂控制系統(tǒng)。該系統(tǒng)采用PLC通過CANOpen通信控制伺服電機(jī)驅(qū)動(dòng)機(jī)械臂完成一系列擠奶動(dòng)作，整個(gè)過程無人化，實(shí)現(xiàn)自動(dòng)化擠奶的同時(shí)可以有效提高擠奶工作效率和產(chǎn)奶量、節(jié)省生產(chǎn)成本和勞動(dòng)時(shí)間，填補(bǔ)了國內(nèi)擠奶機(jī)器人產(chǎn)品的空白。

關(guān)鍵詞：擠奶機(jī)器人；伺服電機(jī)；PLC；三自由度機(jī)械臂；CANOpen通信；控制方法

中圖分類號：TP242 文獻(xiàn)標(biāo)識碼：A 文章編號：1674-1161（2017）06-0015-04

隨著人們對鮮奶及牛奶制品數(shù)量與品質(zhì)的需求與日俱增，標(biāo)準(zhǔn)化、規(guī)模化、集約化、現(xiàn)代化成為我國奶牛養(yǎng)殖業(yè)發(fā)展的趨勢和目標(biāo)。全自動(dòng)智能擠奶機(jī)器人可以最大程度地實(shí)現(xiàn)奶牛自然狀態(tài)下擠奶過程，根據(jù)奶牛需要隨時(shí)擠奶，在保證奶牛健康的同時(shí)也提高了牛奶產(chǎn)量與品質(zhì)，并有效提高了工作效率，節(jié)約了人力成本。目前，全自動(dòng)智能擠奶機(jī)器人技術(shù)主要掌握在歐美發(fā)達(dá)國家手中，為了擺脫國外技術(shù)的束縛，本研究開發(fā)出一套具有自主知識產(chǎn)權(quán)的全自動(dòng)智能擠奶機(jī)器人，通過CANOpen通信控制伺服電機(jī)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)，完成清洗、按摩、套杯、擠奶、脫杯、藥浴等一系列動(dòng)作。

1 擠奶機(jī)器人機(jī)械臂運(yùn)動(dòng)控制

1.1 擠奶機(jī)器人工作流程

擠奶機(jī)器人是集成了機(jī)械、自動(dòng)控制、傳感器、計(jì)算機(jī)視覺、軟件數(shù)據(jù)庫等的一體化系統(tǒng)。采用模塊化設(shè)計(jì)，以主控計(jì)算機(jī)為核心，分為管理、識別、控制、數(shù)據(jù)采集等功能模塊。各模塊間接口采用網(wǎng)絡(luò)、CAN總線、RS232，USB，RFID和Zigbee等多種通訊方式。控制系統(tǒng)原理框圖如圖1所示。

擠奶機(jī)器人的機(jī)械臂結(jié)構(gòu)采用三自由度控制方式，擠奶流程為：擠奶箱上方的攝像機(jī)檢測到奶牛進(jìn)入擠奶箱后，上位機(jī)發(fā)送指令將擠奶箱氣動(dòng)門關(guān)閉，并放下精飼料槽喂給奶牛精飼料，攝像機(jī)根據(jù)奶牛臀部位置初步確定奶牛乳房位置，上位機(jī)發(fā)出指令給PLC，PLC控制伺服電機(jī)驅(qū)動(dòng)機(jī)械臂以安全軌跡運(yùn)動(dòng)到奶牛乳房附近，機(jī)械臂末端安裝有精確定位攝像頭，將捕捉到的4個(gè)奶牛乳頭位置信息發(fā)送給上位機(jī)，PLC接收上位機(jī)信息驅(qū)動(dòng)機(jī)械臂分別對4個(gè)奶牛乳頭依次進(jìn)行清洗、按摩、套杯、擠奶、脫杯、藥浴等一系列程序，最后擠奶結(jié)束，機(jī)械臂返回。

1.2 PLC與伺服電機(jī)的通信

擠奶機(jī)器人靠機(jī)械臂來完成一系列擠奶動(dòng)作，機(jī)械臂能否精準(zhǔn)定位是擠奶的關(guān)鍵。采用費(fèi)斯托伺服電機(jī)驅(qū)動(dòng)機(jī)械臂，費(fèi)斯托伺服電機(jī)的控制器作為CANOpen通訊從站，通過ABX-CANOpen（AB7301-B）主站網(wǎng)關(guān)轉(zhuǎn)接到西門子Profibus-DP總線，將伺服驅(qū)動(dòng)器接入西門子S7-300PLC系統(tǒng)中，在西門子S7-300PLC上可以實(shí)現(xiàn)與費(fèi)斯托伺服電機(jī)控制器的數(shù)據(jù)交換和對伺服電機(jī)的控制。

采用西門子S7-300系列PLC，CPU為 315-2DP，AB7301網(wǎng)關(guān)作為315-2DP的ProfibusDP從站，與其他從站設(shè)置相同。首先安裝HMS提供的GSD文件，安裝完成后在“設(shè)備組態(tài)—網(wǎng)絡(luò)視圖”的硬件列表中將會(huì)出現(xiàn)網(wǎng)關(guān)的硬件選項(xiàng)，如圖2所示。

首先配置PLC，然后點(diǎn)擊“網(wǎng)絡(luò)視圖”，進(jìn)入網(wǎng)絡(luò)配置畫面。拖拽硬件目錄中的ABX-Gateway CANOpenMaster圖標(biāo)到網(wǎng)絡(luò)視圖空白處，添加DP從站，選擇“分配到新主站”，選擇主站，系統(tǒng)將建立PLC主站和網(wǎng)關(guān)從站的總線網(wǎng)絡(luò)。進(jìn)入網(wǎng)關(guān)硬件配置頁面，點(diǎn)擊網(wǎng)關(guān)接口圖標(biāo)，打開網(wǎng)關(guān)DP接口設(shè)置頁面。根據(jù)實(shí)際情況改變網(wǎng)關(guān)DP的地址，本研究網(wǎng)關(guān)的DP地址為72。網(wǎng)關(guān)的DP地址硬件設(shè)置在網(wǎng)關(guān)前指示燈下，有2個(gè)撥碼開關(guān)，上面的是十位，下面的是個(gè)位。DP地址=十位的數(shù)值加個(gè)位的數(shù)值。根據(jù)前面CANOpen網(wǎng)絡(luò)配置的實(shí)際輸入、輸出字節(jié)長度，配置網(wǎng)關(guān)在PLC上的輸入、輸出字節(jié)長度。本研究實(shí)際配置的輸入是26字節(jié)，而目錄里沒有對應(yīng)的長度，所以先配置1個(gè)字（2字節(jié)），再配置12個(gè)字（24字節(jié)），組合成26字節(jié)；本研究實(shí)際配置的輸出是26字節(jié)，而目錄里沒有對應(yīng)的長度，所以先配置1個(gè)字（2字節(jié)），再配置12個(gè)字（24字節(jié)），組合成26字節(jié)。PLC網(wǎng)關(guān)硬件配置如圖3所示。

INPUT地址為CANOpen總線側(cè)發(fā)送給PLC的數(shù)據(jù)地址，OUTPUT地址為PLC發(fā)送給CANOpen總線側(cè)的數(shù)據(jù)地址。

1.3 PLC控制伺服電機(jī)

PLC通過CANOpen協(xié)議與伺服電機(jī)建立通訊，根據(jù)伺服電機(jī)控制字的含義，PLC對應(yīng)控制單元發(fā)送數(shù)值，即可實(shí)現(xiàn)控制字定義的功能。以水平方向伺服電機(jī)為例，控制字從PIB258至PIB265?？刂谱諴IB258的功能見表1。

PLC控制伺服電機(jī)流程為：設(shè)備上電后首先初始化程序，3個(gè)方向的伺服電機(jī)將機(jī)械臂驅(qū)動(dòng)到原點(diǎn)位置（原點(diǎn)為機(jī)械結(jié)構(gòu)設(shè)置的零位置）；當(dāng)牛欄上方攝像頭檢測到有牛進(jìn)入擠奶機(jī)器人牛欄后關(guān)閉牛欄，根據(jù)牛欄上方攝像頭采集的奶牛位置信息初步確定奶牛乳頭大致位置，上位機(jī)將奶牛乳頭大致位置信息以三維坐標(biāo)的形式發(fā)送給PLC，PLC驅(qū)動(dòng)伺服電機(jī)將機(jī)械臂運(yùn)動(dòng)到奶牛乳頭前方，機(jī)械臂上安裝有用于精確定位奶牛乳頭的攝像頭，該攝像頭將奶牛乳頭的精確位置信息發(fā)送給上位機(jī)，上位機(jī)計(jì)算出奶牛乳頭精確的位置信息，以三維坐標(biāo)的形式（水平方向、垂直方向和旋轉(zhuǎn)方向）發(fā)送給PLC，PLC再次驅(qū)動(dòng)伺服電機(jī)將機(jī)械臂運(yùn)動(dòng)到奶牛乳頭下方；PLC控制打開真空閥門，按照上位機(jī)發(fā)送的奶牛乳頭精確位置信息將套杯依次套入4個(gè)奶牛乳頭，并通過脈動(dòng)器開始擠奶，脈動(dòng)器管路上安裝有真空流量計(jì)，當(dāng)真空流量計(jì)檢測到管路流量低于200 g/min時(shí)，關(guān)閉真空并脫杯停止擠奶，當(dāng)4個(gè)乳頭都停止擠奶后，PLC控制機(jī)械臂回到設(shè)定的原點(diǎn)，打開牛欄放牛出欄，并等待下一頭牛入欄擠奶。

2 機(jī)械臂行走作業(yè)定位測試

上位機(jī)采集到奶牛乳頭位置信息后，機(jī)械臂能否精準(zhǔn)套杯是本研究的關(guān)鍵點(diǎn)。根據(jù)奶牛乳頭生物特征，套杯精度在1 mm范圍內(nèi)可以實(shí)現(xiàn)套杯并擠奶。伺服電機(jī)運(yùn)行速度和加速度、減速度控制在伺服控制器軟件中設(shè)置，設(shè)定運(yùn)行速度為V=300 mm/s，加速度為a1=1.5 m/s2，減速度為a2=1.5 m/s2。根據(jù)加速度公式，加速時(shí)間為Δt=Δv/a，加速行程為S=0.5at2，在設(shè)定行程的情況下，計(jì)算出行程時(shí)間，再實(shí)際測量行程距離，結(jié)果見表2。

由表2可知：隨著定位距離的增加，定位精度始終保持在1 mm以內(nèi)，說明定位精度能夠滿足機(jī)械臂精確定位套杯要求。

3 結(jié)論

本研究設(shè)計(jì)的擠奶機(jī)器人機(jī)械臂控制系統(tǒng)，通過上位機(jī)采集奶牛乳頭位置信息，采用PLC與伺服電機(jī)CANOpen通信控制機(jī)械臂運(yùn)動(dòng)實(shí)現(xiàn)精準(zhǔn)定位，定位精度達(dá)到1 mm以內(nèi)，能達(dá)到自動(dòng)擠奶的要求。endprint