一种注塑机械手堆叠算法及其运动控制策略

余杰先，张中华，陈凌玲，李久林，温舒然

（珠海格力智能装备有限公司，广东珠海 519000）

0 引言

注塑机机械手一般包括单轴手臂机械手、三轴机械手、五轴机械手，由控制系统、驱动系统、执行机构等组成[1-3]。执行机构有机械手臂或气缸以及桁架机械臂等[4-7]。传统的机械手控制系统和驱动系统是分开的，两者之间通过脉冲模式连接。随着总线技术、芯片技术的不断发展，在一个MCU 里面集成了运动控制轨迹规划和伺服驱动算法，大大减少了线缆的连接，节省了人工成本，提高了工作效率[8-15]。

注塑机械手如图1 所示，其基本的功能就是按照预先编好的程序执行工作流程：在自动模式下到达注塑机模内上方待机点，等待注塑机开模完成信号，到达模内取物，取物完成离开模内，达到型外放物到流水线上，放物完成机械手抬上升，到达模内上方等待下一个开模完信号。如此周期往返执行[16-21]。随着工艺需求的不断完善，机械手要记忆每一个周期的放物位置，为下一个周期提供基本的位置偏移，这样就需要一个XYZ三维的堆叠算法，通过在示教器界面配置好堆叠的属性参数，自动运行时自动判断每一个周期的XYZ坐标。注塑机机械手一般的运动包括：单轴点对点定长运动、多轴点对点联动、多轴组合运动、多轴提前变速变位运动[21-27]。

图1 注塑机械手

本文着重介绍机械手控制系统基本构成、堆叠工艺算法和提前变速运动算法。

1 机械手控制系统基本构成

本文控制系统为集成控制和驱动为一体，主要包括上位机示教器、控制模块、伺服功率模块。总体框图如图2所示。

图2 总体流程

示教器的MCU 使用了TI 公司的AM335X。AM335x微处理器基于ARM Cortex-A8 处理器，在图像、图形处理、外设以及EtherCAT 和PROFIBUS 等工业接口选项方面得到了增强。该器件支持高级操作系统（HLOS）。Linux®和Android ™可从德州仪器（TI）免费获取。AM335x 微处理器包含功能框图中显示的子系统和以下简要说明：微处理器单元（MPU）子系统基于ARM Cortex-A8 处理器，PowerVR SGX™图形加速器子系统提供3D 图形加速功能以支持显示和游戏特效。可编程实时单元子系统和工业通信子系统（PRU-ICSS）与ARM内核彼此独立，允许单独操作和计时，以实现更高的效率和灵活性。PRU-ICSS 支持更多外设接口和EtherCAT、PROFINET、EtherNet/IP、PROFIBUS、Ethernet Powerlink、Sercos 等实时协议。此外，凭借PRU-ICSS 的可编程特性及其对引脚、事件和所有片上系统（SoC）资源的访问权限，该子系统可以灵活地实现快速实时响应、专用数据处理操作以及自定义外设接口，并减轻SoC 其他处理器内核的任务负载。

驱控一体的主MCU 使用了TI 公司的高性能处理器TMS320F28377D。TMS320F28377D基本参数：双核32位浮点MCU，主频200 MHz；两个支持ASRAM 和SDRAM的外部存储器接口（EMIF）；双6 通道直接存储器存取（DMA）控制器；多达169 个支持输入滤波的独立可编程、复用通用输入/输出（GPIO）引脚；扩展外设中断控制器（ePIE）；多个支持外部唤醒的低功耗模式（LPM）；USB 2.0（MAC+PHY）；支持12 引脚3.3 V 兼容通用并行端口（uPP）接口；2 个控制器局域网（CAN）模块；3个高速（最高50 MHz）SPI端口；2个多通道缓存串行端口（McBSP）；4 个串行通信接口（SCI/UART）；2 个I²C接口；4 个模数转换器（ADC）；12 个16 位模式外部通道；24个12位模式外部通道；8个具有12位数模转换器（DAC）参考的窗口化比较器；3 个12 位缓存DAC 输出；24条具有增强功能的脉冲调制器（PWM）通道；16条高分辨率脉宽调制器（HRPWM）通道；6 个增强型捕捉（eCAP）模块；3 个增强型正交编码器脉冲（eQEP）模块；8 条Δ-Σ 滤波器模块（SDFM）输入通道；外设模块主要包括PWM 模块，QEP 模块，SDFM 模块，SPI 模块，SCI 模块，IO 模块，CNT 计数模块；中断优先级包括OC中断XINT1，PWM中断，IRQ中断XINT2，PWM同步SPI中断，SCI中断采用FIFO模式，中断优先级从高到低。

2 注塑机机械手堆叠算法

当前的机械手从注塑机中取出产品都是按固定的点位投放，这就还需要工人手动将产品按指定排列方式进行堆放。本文算法可以使机械手自动以某种设定的排列方式将成品整齐排列，从而提高生产效率，并且使生产更加安全，更加智能化。

堆叠算法的具体流程为：当机械臂到达模外的堆叠起点位置，先将Y轴升至原点；然后按顺序分别求出第三方向上的目标位置、第二方向的目标位置、第一方向的目标位置；最后按Z-X-Y的顺序分3 次单轴运动到各轴的目标位置。堆叠流程如图3所示。

图3 堆叠流程

先将Y轴上升至原点，是为了在Z轴和X轴平面上移动产品时不碰撞到之前已经排列好的产品。同理，最后步骤中先移动Z和X轴，最后才移动Y轴也是为了在堆叠本产品时不会碰撞到之前已经堆叠好的产品。

本文中开始和结束时的轴运动步骤是固定的，关键步骤是求出当前序号产品在各轴上的目标位置。具体算法为：将产品序号减1并对3个方向上的堆叠个数的乘积求余得到结果temp值，用temp值除以第一和第二方向上堆叠个数的乘积并向下取整得到该产品在第三方向上的偏移个数，用第三方向的起点位置加上偏移个数与堆叠间隔的乘积得到该产品在第三方向上的目标位置；将temp 值对第一方向和第二方向上堆叠个数的乘积求余得到新的temp值，用temp值除以第一方向上的堆叠个数并向下取整得到该产品在第二方向上的偏移个数，用第二方向的起点位置加上偏移个数与堆叠间隔的乘积得到该产品在第二方向上的目标位置；用temp 值对第一方向的堆叠个数求余得到该产品在第一方向上的偏移个数，用第一方向的起点位置加上偏移个数与堆叠间隔的乘积得到该产品在第一方向上的目标位置。

假设产品的堆叠顺序为X-Z-Y，起点位置为x0，y0，z0，在第一方向X轴上堆叠个数为nx，堆叠间隔为ix，在第二方向Z轴上堆叠个数为nz，堆叠个数为iz，在第三方向Y轴上堆叠个数为ny，堆叠间隔为iy，产品序号为n。

首先求得该产品在第三方向Y轴上的目标位置Sy：

然后，求得该产品在第二方向Z轴上的目标位置Sz：

最后，求得该产品在第一方向X轴上的目标位置Sx：

图4 所示是产品按XZ-Y顺序排列的xz平面图，起点位置为产品序号1 的位置，在第一方向X轴上的堆叠个数为3 个，堆叠间距为50，第二方向Z轴上的堆叠个数为4个，堆叠间距为40，第三方向Y轴上的堆叠个数为5个，堆叠间隔为60。

图4 xz平面图

假设要求序号为26 的产品堆放位置。首先，求得该产品在第三方向Y轴上的目标位置Sy：

所以，该产品在Y轴的目标位置为当前XZ平面高2个Y堆叠间隔距离的XZ平面位置上。

然后，求得该产品在第二方向Z轴上的目标位置Sz：

所以该产品在Z轴上的目标位置为当前XZ平面Z方向上第一列的位置，即Z轴起点位置。

最后，求得该产品在第一方向X轴上的目标位置Sx：

所以该产品在X轴上的目标位置为当前XZ平面上X方向上第二列的位置。所以序号为26的产品堆放位置为2号产品位置往Y轴方向偏移2个Y轴堆叠间距的位置。

3 提前变速运动算法

传统的注塑机行业，注塑机锁模产品成型后，由人工伸手进模内抓取产品，这不仅效率低，而且风险高。随着工业机器人的发展，一种可应用于注塑机自动抓取的机械臂控制系统应运而生，其快速的加减速速度曲线规划可以将注塑模内产品的抓取周期缩短至2 s，大量应用于手机壳、塑料片的抓取。然而随着行业不断的发展，注塑机产品工艺流程复杂度不断提高，比如，快餐盒的生产，要求堆叠轻放，则机械手从注塑机模内抓取产品后，横行出型外后，Y轴要先快速下降，下降到接近堆放区域，速度要平滑下降到一个低速率段，缓慢到达堆放点，这就需要提前减速的功能。同样，机械手在放完产品后，由于像快餐盒这种特殊的产品特性，机械手Y轴上升的初始阶段需要比较低的速率，同时也要兼顾效率，Y轴上升到一定的区域后，需要平滑加速到高速率段，这就要引入提前加速的功能。

3.1 提前减速

提前减速的速度曲线如图5 所示。由图可知，假设第n个采样周期末的瞬时速度为V（n），第n个采样周期的位置增量为ΔS（n），运动的总长度为S，加速度为a，提前减速距离为Sd，第一阶段速度为Vobj1，第二阶段速度为Vobj2，插补周期为Ts。注塑机机械手Y轴从上往下堆叠轻放产品由两部分组成：加速到匀速段和提前减速到低速段。

图5 提前减速的速度曲线

（1）第一段速度规划

此阶段的位移为S1=S-Sd，在加速度期间v（n）=a·Ts·n，匀速期间v（n）=Vobj1=v0，在整个过程中：

当实时S=S1时，此刻将进入提前减速点，时间点为t0，速度为v0，进入第二阶段。

（2）第二阶段速度规划

此阶段初速度为v0，位移为S2=S-S1，由减速到低速段、低速匀速段、减速到0 段组成。减速到低速段：t1=（v0-Vobj2）/a；v（n）=v0-a·Ts·n。减速到0 段：t3=Vobj2/a；v（n）=Vobj2-a·（Ts·n-t2）。低速匀速段：t2=S2-0.5×（v0-Vobj2）×（v0-Vobj2）/a；v（n）=Vobj2。

当实时S=S2，第二阶段运动完成。

3.2 提前加速

提前加速的速度曲线如图6 所示，其实现方法和提前减速是一样的。

图6 提前加速的速度曲线

4 实验验证

本算法成功应用在自主研发的注塑机机械手控制系统上，在终端客户处，该系统已经能成熟稳定运行，且能胜任更加复杂的注塑产品的抓取、堆叠轻放、堆叠轻走等任务。其中提前减速、提前加速功能，从伺服端抓取的波形图如图7～8所示。采用了本文提出的堆叠算法，并采取相应的软硬件控制策略，可以使得注塑模内产品的抓取周期缩短至2 s，大量应用于手机壳、塑料片的抓取，极大改善了上述的安全和效率问题。

图7 提前减速速度曲线

图8 提前加速速度曲线

5 结束语

随着现代工业自动化技术的不断发展，不少自动化设备都应用到了工业自动化大生产中，其中就包括将机械臂应用于注塑机的作业中。本文从注塑机作业中产品在模内的取放问题出发，以提高生产效率和提高作业安全为目标，分析了传统作业和应用机械臂进行作业的发展历程，提出了一种更加高效的方法。通过调研注塑行业中的自动化流程技术应用发展历程，较为明确了解注塑机械手在注塑自动化应用的实际性价效能，进而考虑进一步优化当前注塑机械手的工艺及相关算法，以提高注塑工艺的自动化程度，降低生产成本，提高生产效率，从而达到提高生产效益的目的。