基于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