啤酒生产线六自由度机械手抓瓶控制系统设计

啤酒生产线介绍：

机械手抓瓶视频：

啤酒生产线六自由度机械手抓瓶控制系统设计：
任务要求

装箱工艺主要分为2个部分：视觉检测系统和机械手分部。

视觉检测系统工作目的是：通过装置于合适位置的摄像头，对摆放在装箱工作台上的空箱体进行拍摄，并对所拍摄到的图像进行处理，获取装瓶位的实时坐标，并将坐标传送至控制机械手的PLC。

机械手部分的主要工作是：根据视觉检测系统所获得的装瓶位坐标，按照一定的轨迹，将通过贴标分部的瓶体依次装入箱体的装瓶位。

项目技能点

1). 熟悉三菱Q系列PLC；

2). 熟悉三菱编程软件GXDeveloper的使用方法；

3). 熟悉三菱六自由度机械手的使用

硬件介绍

本啤酒生产线装箱分部所采用的是Q系列PLC的CPU为Q03UDCPU和Q172DRCPU。其中，Q03UDCPU为主CPU，Q172DRCPU为从CPU，即机器人CPU。

本次设计机器人分部中使用控制器为CR1Q-700系列，此控制器是与设计中使用的机械手专门匹配的控制器，为小型控制器，CPU设置类型为外部设置，在PLC基板上插入机器人控制器，省配线，不需要I/O模块，使系统成本降低。

机械臂为RV-3SQ系列六自由度机械臂，为执行动作的单元，具体型号为RV-3SQ-S11。

机械臂共六个关节，每个关节内有一个伺服电动机，由绝对编码器来检测位置。控制器与机械手间也是独立的，通过两条线缆即马达动力线和编码器信号线来连接它们，其中马达线用来提供动力来驱动电机，信号线提供信号来控制电机。

硬件连接图

软件介绍

本分部用到软件：

1、 RT ToolBox2

用于编写机器人程序

2、 MELFA-Vision

视觉软件，从摄像头拍摄的相片中识别圆孔位置

3、 GX Developer

用于与前后分部的通信，程序已写好（不到10行），不需要管

RT ToolBox2使用方法：

软件的运行状态分为三种：离线、在线、模拟。

1、模拟状态

选择工作区-新建，通信设置如下：

IP Address=192.168.0.5

Port=10001

Send Timeout=5000msec

Recieve Timeout=30000msec

之后在菜单栏在线中选择模拟，在左侧工作区展开在线-程序，右键选择新建，编写程序，例如：（以下为本例中所用程序）

编辑完成按CTRL+S保存，将鼠标置于工作区编写的程序文件名处，右键选择调试状态下打开，开始模拟。

2、在线状态

在线模式时，菜单栏在线中选择在线，展开左侧工作区在线-程序，编写程序完毕后，在程序编辑区右键选择全写入，程序即可下载到控制器中。若在程序文件名处，右键选择调试状态下打开，则可进行实时调试。

程序的运行：

手控器背面白色按钮应弹开，并将钥匙旋转至自动模式，按下SVO ON打开伺服电源，按下START运行程序。

另外还有示教模式可以手动控制机械臂，位置坐标可以实时读入。使用时先将钥匙旋转至手动模式，再将手控器背面白色按钮应按下，拉住手控器背面黄色按钮（注意不要拉到头），按下SERVO键，4秒后伺服指示灯亮，再按下JOG进入示教模式。可以选择XYZ模式或JOINT模式。其中ＸＹＺ模式下，X、Y、Z控制机械臂末端位移，A、B、C控制姿态。JOINT模式下，控制各个关节单独运动。按下HAND键控制手爪，-C抓合，+C松开。

MELFA-Vision使用方法：

建立作业，点击下图绿色按钮联机

然后在RT TOOLBOX中运行示例程序，程序中调用运行作业（本例中使用的作业为“2013.job”）并使用函数读取识别的圆孔坐标

所做工作：

1、 程序流程图设计

注意，这里只是开环控制，没有引入反馈，系统不知道瓶子是否放到位。作为改进，我

们设计了一种闭环控制系统，系统实时监测机械臂抓手位置并与放瓶位进行比较，通过偏差消除偏差。闭环系统框图如下：（紫色区域为增加的闭环部分）

2、 程序代码

Ovrd 30                                                '速度30%

Mov P0                                                 '开始移动到安全位置

HOpen 1                                               '打开手抓

If M_NvOpen(1)<>1 Then                       '如果端口没有打开

NVOpen "COM2:" As #1                          '打开2号端口作为1号

EndIf

Wait M_NvOpen(1)=1                              '等待打开

NVLoad #1,"2013"                                   '读入视觉里的程序“2013”

NVPst #1,"2013","E76","J81","M100",0     '调用程序“2013”存入单元E76到J81中的数据

For M1=1 To M_NvNum(1)                       '数据编号从1开始循环

Mov P1                                                  '抓瓶位置前方

Mvs P2                                                  '抓瓶位置

Dly 0.1                                                   '延时

HClose 1                                                '闭合手抓

Dly 0.2

Mov P6                                                '抓瓶位置上方

Mov P4                                                 '姿态调整

Mov P5                                                '放瓶位置上方

P10.X = P_NvS1(M1).X                            '调取视觉输出X坐标

P10.Y = P_NvS1(M1).Y                             '调取视觉输出Y坐标

P10.Z = 450

P11=P10

P11.Z=370                                       

Mov P10                                                 '移动至放瓶位置

Mvs P11                                                '垂直下放瓶

Dly 0.1

HOpen 1

Dly 0.2

P11.X=P10.X-20                     '以下程序前后左右晃动是为了使瓶子更容易落入孔中

Mvs P11

P11.X=P10.X+20

Mvs P11

P11.Y=P10.Y-20

Mvs P11

P11.Y=P10.Y+20

Mvs P11

P11.X=P10.X-20

Mvs P11

P11.X=P10.X+20

Mvs P11

P11.Y=P10.Y-20

Mvs P11

P11.Y=P10.Y+20

Mvs P11

P10.Y=P10.Y+50

Mvs P10

Next M1                                                 '下一个放瓶编号

Mov P0

Hlt                                                         ' 暂停

End

各点坐标：

P0=(463.910,-75.340,680.230,-120.360,75.850,-46.420)(6,0)

P1=(459.520,-147.240,328.670,-121.440,76.300,-46.810)(6,0)

P2=(453.870,-70.340,329.430,-120.450,75.850,-46.490)(6,0)

P3=(57.620,-500.830,562.160,-125.830,85.470,152.050,0.000,0.000)(6,0)

P4=(403.440,-178.560,679.430,-97.990,84.750,-117.210)(6,0)

P5=(8.460,-433.000,696.350,-106.650,84.670,170.590)(6,0)

P6=(463.910,-75.340,680.230,-120.360,75.850,-46.420)(6,0)

P7=(8.460,-433.000,696.350,-106.650,84.670,170.590)(6,0)

P10=(270.060,-647.260,450.740,-125.830,85.470,152.050,0.000,0.000)(6,0)

P11=(270.060,-647.260,362.160,-125.830,85.470,152.050,0.000,0.000)(6,0)

3、 最终效果

本程序最终效果，12个瓶中有两个未能成功落入孔中，抓瓶和下落过程还有偏差，有待完善！

4、 感想：

这次实习通过对三菱六自由度机械手的学习，让我学到了很多东西。刚开始的时候感觉困难重重，不知道该从何下手。通过对使用文档的阅读和向老师请教，慢慢熟悉了机械手的示教方法和编程方法，后来又学会了视觉系统的使用以及校正。最后通过努力，将视觉系统与机械手结合在一起，达到了预期的效果。在这短短几天的实习中，每天都有新的问题，同时也有新的进展。这让我明白了无论学习还是做事，只要迎难而上并持之以恒，没有解决不了的问题。