基于NJ的两自由度并联械手控制系统.docVIP

“Omron杯”Sysmac自动化控制应用设计科目作品申报表 作品名称 基于NJ的两自由度并联机械手控制系统 作品简介 一、开发背景 随着工业自动化水平提高，机器人应用越来越广泛。OMRON公司推出的NJ系列的自动化控制器除了具有PLC的基本功能外，还具有高速、复杂、精密的运动控制功能；具有丰富的通信与网络功能，提供功能强大的自动化平台软件Sysmac Studio。OMRON公司与教育部合作举办了第九届“Omron杯”Sysmac自动化控制应用设计大赛。本次大赛以模拟足球点球赛形式对机械手进行控制，如图1所示，通过对NJ系列控制器，我校团队热烈响应 图3 HMI_Control：触摸屏控制机械手解除抱闸和磁铁使能的程序。Motion_Err：处理错误的程序； Action_Control：控制机械手运动的程序； HMI_Robot：是在触摸屏上显示机械手运动模型和运动学反推算法的程序； Robot_Range：运动过程中检测机械手是否超出范围的程序。 （2）FH视觉设备实时采集小球的位置坐标，并输出坐标给NJ控制器。视觉数据采集程序说明如下。 图4程序为触发视觉相机每隔20ms进行一次拍照； 图4 图5程序为数据类型转换，将视觉发送的XY坐标的DINT数据，转成LREAL的数据格式。 图5 图6程序为将数据格式转换之后的视觉XY坐标与机械手的实际XY坐标匹配。 图6 图7程序为用堆栈收集视觉通道给出的数值； 图7 （3）NS触摸屏作为人机操作界面，可直接控制各设备动作。NS触摸屏程序包括运行界面和参数界面，如图8（运行界面）和图9（参数界面）所示。 图8 运行界面用于完成机械手运动前的准备工作：包括解除电机左右抱闸、伺服锁定、定义原点、开启算法；可实时显示机械手运动模型轨迹，将机械手旋转角度，实时坐标显示出来；可通过（回原按钮）使机械手返回原点；可通过（复位按钮）清除错误。 图9 参数界面用于修改硬件的参数，包括小球半径、偏移量、球场宽度左、球场宽度右、机械手前端距离、击打板宽度、视觉零点距离、Lt、Lb、Rt、Rb、机械手X区间、机械手Y上限和机械手Y下限。 三、使用说明 在运行界面按下，进行参数界面进行硬件参数的设定；设定完后按下，返回运行界面，然后按下，锁定伺服轴；依次按下，，解除电机左右抱闸；按下，设置机械手初始位置即原点；按下，启动Robot运动学反推算法，可按住点动按钮上、下、左、右使机械手上下左右的点动来确认机械手动作正常，按下可以使机械手回到原点位置，按下，启动Robot自动防守功能。 设计说明 （含原理图、设计原理、设计方案等，字数不超过3000字） 四、设计原理 根据大赛提供的程序模版，内含伺服锁定、轴组使能、定义原点、机械手运动正解和反解、视觉数据采集程序、点动、超限报错等程序，此处不再赘述，本次设计主要针对机械手的防守功能进行编程。根据小球运动轨迹分析如图10所示： 图10 图10内红色虚线框为机械手活动范围，①至⑦为小球运动轨迹各情况。如图建立XY坐标系,分析如下： ①球往右边发射，并发生了反射； ②球往右边发射，没发生发射，但防守点坐标超过了机械手的运动范围； ③球往右边发射，没发生发射，且防守点坐标在机械手的运动范围内； ④球垂直发射； ⑤、⑥、⑦情况类似于①、②、③。 五、设计方案 根据分析情况编写功能块（ST语言）。机械手防守时，机械手在Y=（机械手前端距离-机械手击打板宽度）的直线上左右运动，故只需求出相应X坐标即可。功能块编程思路： 1.首先判断小球是否垂直射出，当垂直射出时，机械手防守点的X坐标为小球轨迹的X坐标； 2.当非垂直射出时，根据两点式直线方程： （Y-y1）/(y2-y1)=(X-x1)/(x2-x1)， 已知Y=（机械手前端距离-机械手击打板宽度），代入求出交点的X坐标。 (1)当x坐标在球场宽度左右范围内时，即小球运动不发生反射，运动轨迹为②、③、⑤、⑥的某一种情况，再判断小球是否可运动到机械手运动限位位置，当不能到限位时，根据两点式直线方程（Y-y1）/(y2-y1)=(X-x1)/(x2-x1),代入Y=（机械手前端距离-机械手击打板宽度），求出交点的X坐标，当X坐标不能到限位时，机械手防守点X坐标为求出的X坐标；当能到限位时，机械手防守点X坐标为机械手限位点X坐标。 (2)当X超出球场宽度左右范围时，小球运动轨迹为①、⑦的某种情况，以轨迹①为例说明：先根据两点式直线方程（Y-y1）/(y2-y1)=(X-x1)/(x2-x1)，代入X=球场宽度左，求出小球与球场宽度左的交点的Y坐标，再求出关于Y坐标所在直线的反射线上的两个对称点，再根据两点式直线方程公式求出反射轨迹的直线方程，代入Y=（机械手前端距离-机械手击打板宽度），求出交点的X坐标，当X坐标不能到限位时