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

郭洋　吳澤全　蔡曉華　侯云濤

摘要：為提高我國奶牛養殖自動化水平，研究一種擠奶機器人機械臂控制系統。該系統采用PLC通過CANOpen通信控制伺服電機驅動機械臂完成一系列擠奶動作，整個過程無人化，實現自動化擠奶的同時可以有效提高擠奶工作效率和產奶量、節省生產成本和勞動時間，填補了國內擠奶機器人產品的空白。

關鍵詞：擠奶機器人；伺服電機；PLC；三自由度機械臂；CANOpen通信；控制方法

中圖分類號：TP242 文獻標識碼：A 文章編號：1674-1161（2017）06-0015-04

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

1 擠奶機器人機械臂運動控制

1.1 擠奶機器人工作流程

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

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

1.2 PLC與伺服電機的通信

擠奶機器人靠機械臂來完成一系列擠奶動作，機械臂能否精準定位是擠奶的關鍵。采用費斯托伺服電機驅動機械臂，費斯托伺服電機的控制器作為CANOpen通訊從站，通過ABX-CANOpen（AB7301-B）主站網關轉接到西門子Profibus-DP總線，將伺服驅動器接入西門子S7-300PLC系統中，在西門子S7-300PLC上可以實現與費斯托伺服電機控制器的數據交換和對伺服電機的控制。

采用西門子S7-300系列PLC，CPU為 315-2DP，AB7301網關作為315-2DP的ProfibusDP從站，與其他從站設置相同。首先安裝HMS提供的GSD文件，安裝完成后在“設備組態—網絡視圖”的硬件列表中將會出現網關的硬件選項，如圖2所示。

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

INPUT地址為CANOpen總線側發送給PLC的數據地址，OUTPUT地址為PLC發送給CANOpen總線側的數據地址。

1.3 PLC控制伺服電機

PLC通過CANOpen協議與伺服電機建立通訊，根據伺服電機控制字的含義，PLC對應控制單元發送數值，即可實現控制字定義的功能。以水平方向伺服電機為例，控制字從PIB258至PIB265。控制字PIB258的功能見表1。

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

2 機械臂行走作業定位測試

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

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

3 結論

本研究設計的擠奶機器人機械臂控制系統，通過上位機采集奶牛乳頭位置信息，采用PLC與伺服電機CANOpen通信控制機械臂運動實現精準定位，定位精度達到1 mm以內，能達到自動擠奶的要求。endprint