CN115213899A - 一种小型机械手控制器及其控制方法 - Google Patents

Abstract

本发明提出了一种小型机械手控制器及其控制方法，所述控制器包括主控模块，与所述主控模块连接的通信模块、光耦隔离模块、检流电路、限位开关和按钮开关，所述通信模块与人机界面相连，用于收发数据，所述光耦隔离模块输出端通过驱动模块与步进电机相连，所述检流电路连接于主控模块和驱动模块之间，所述限位开关连接有开关电磁阀，用于控制开关电磁阀的开合。本发明将转动臂的运动轨迹分解为直线片段和圆弧片段，分别利用直线插补和圆弧插补方式进行路线拟合，基于SPTA算法和脉冲数计算获得步进电机在各频段的运动步数，实现了步进电机的精确控制，平稳性好。

Description

一种小型机械手控制器及其控制方法

技术领域

本发明涉及机械手控制技术领域，具体而言，涉及一种小型机械手控制器及其控制方法。

背景技术

随着生产自动化水平的不断提高，越来越多的工业机械应用了机械手。机械手可代替人的繁重劳动，易实现生产的机械化和自动化，能在有害环境下操作以保护人身安全，因而广泛应用于机械制造、冶金、电子、轻工和原子能等部门。机械手控制器可以控制机械手，自动拾取产品，放置产品到指定位置。机械手的应用降低了工人劳动强度，提高了机器生产效率。

目前，传统的机械手控制器通常采用7段式或5段式S形算法，该算法中每个定时器只能控制一个步进电机，当CPU算力资源紧缺时，需要配合扩展芯片协同处理，制造成本高。而且，传统的机械手控制器响应速度慢，控制步进电机时运动轨迹不够平滑，平稳性低。

发明内容

鉴于上述问题，本发明提供了一种小型机械手控制器及其控制方法。

为解决上述技术问题，本发明采用的技术方案是：一种小型机械手控制器，用于控制机械手工作，所述控制器包括主控模块，与所述主控模块连接的通信模块、光耦隔离模块、检流电路、限位开关和按钮开关，所述通信模块与人机界面相连，用于收发数据，所述光耦隔离模块输出端通过驱动模块与步进电机相连，所述检流电路连接于主控模块和驱动模块之间，所述限位开关连接有开关电磁阀，用于控制开关电磁阀的开合。

作为优选方案，所述机械手包括基座，设于所述基座上的多个转动臂，以及设于所述转动臂末端的手爪，所述多个转动臂串联铰接，且相邻所述转动臂端部连接处设置有步进电机，用于驱动转动臂旋转，所述手爪上设有气缸，用于驱动手爪松开或夹紧，且所述气缸的气管上安装有开关电磁阀，所述人机界面和按钮开关设置于基座上。

作为优选方案，所述主控模块为单片机最小系统，所述单片机最小系统包括单片机、锁存器、外部RAM、外部EEPROM和晶振电路，所述单片机分别与锁存器、外部RAM、外部EEPROM和晶振电路的对应信号端相连。

本发明还提出了一种小型机械手控制器的控制方法，运行于如上任一项所述的小型机械手控制器上，包括：获取机械手从初始位置到目标位置的示教数据，并将所述示教数据发送至控制器，所述示教数据包括多个转动臂的运动轨迹和开关电磁阀的状态数据；建立直角坐标系，获取所述运动轨迹的直角坐标，将所述运动轨迹分解为直线片段和圆弧片段，当为直线片段时，则通过直线插补方式从片段起点移动至片段终点，当为圆弧片段时，则通过圆弧插补方式从片段起点移动至片段终点；将所述片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值；计算所述角度差值需要的脉冲数，基于SPTA算法并根据所述脉冲数驱使机械手转动达到目标位置；根据所述开关电磁阀的状态数据控制气缸动作，实现手爪的松开或夹紧。

作为优选方案，将片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值，包括：设片段起点的直角坐标为(X_i,Y_i)，片段终点的直角坐标为(X_i+1,Y_i+1)，则片段角度R(i)的公式如下所示：

角度差值为：R(i+1)-R(i)；

上式中，R1为第一转动壁的臂长，R2为第二转动壁的臂长。

作为优选方案，设从片段起点移动至片段终点需要的脉冲数为p(i，i+1)，则：

p(i,i+1)＝{R(i+1)-R(i)}/π。

作为优选方案，基于SPTA算法并根据所述脉冲数驱使机械手转动达到目标位置，包括：步骤1，设置最大定时器中断时间；步骤2，采用变量interruptCount计算发出一个脉冲所需中断个数；步骤3，运动步数加1，且当前频率下总步数减1；步骤4，判断当前频率下总步数是否等于0，若是，则进行下一步，反之，返回步骤3；步骤5，判断运动步数是否等于总步数，若是，则进行停止计算，输出各频段运动步数，反之，返回步骤2。

作为优选方案，所述机械手转动包括：空闲状态、加速状态、匀速状态和减速状态。

与现有技术相比，本发明的有益效果包括：采用单片机最小系统作为控制器的主控模块，其内部存储有控制程序，可根据实际生产需求进行适应调整。在主控模块和驱动模块之间设置光耦隔离模块，对输出信号进行电气隔离，避免电机工作时产生的感应电压将主控模块击穿烧毁，提高了控制器的可靠性和抗干扰能力。将转动臂的运动轨迹分解为直线片段和圆弧片段，分别利用直线插补和圆弧插补方式进行路线拟合，基于SPTA算法和脉冲数计算获得步进电机在各频段的运动步数，实现了步进电机的精确控制，平稳性好。

附图说明

参照附图来说明本发明的公开内容。应当了解，附图仅仅用于说明目的，而并非意在对本发明的保护范围构成限制。在附图中，相同的附图标记用于指代相同的部件。其中：

图1为本发明实施例小型机械手控制器的结构示意图；

图2为本发明实施例小型机械手的结构示意图；

图3为本发明实施例小型机械手控制器的控制方法流程示意图；

图4为本发明实施例SPTA算法的流程示意图；

图5为本发明实施例转动臂在一个片段内的状态切换图。

图中标号：1基座、2步进电机、3转动臂、4手爪、5气缸、6人机界面、7按钮开关。

具体实施方式

容易理解，根据本发明的技术方案，在不变更本发明实质精神下，本领域的一般技术人员可以提出可相互替换的多种结构方式以及实现方式。因此，以下具体实施方式以及附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限定或限制。

根据本发明的一实施方式结合图1示出。一种小型机械手控制器，用于控制机械手工作，该控制器包括主控模块，与主控模块连接的通信模块、光耦隔离模块、检流电路、限位开关和按钮开关7，通信模块与人机界面6相连，用于收发数据，光耦隔离模块输出端通过驱动模块与步进电机2相连，检流电路连接于主控模块和驱动模块之间，限位开关连接有开关电磁阀，用于控制开关电磁阀的开合。

本发明实施例中，光耦隔离模块用于对输出信号进行电气隔离，避免电机工作时产生的感应电压将主控模块击穿烧毁，提高了控制器的可靠性和抗干扰能力。检流电路用于对驱动模块上的电流进行监测并反馈至主控模块。限位开关用于控制开关电磁阀的开合，当主控模块输出高电平时，限位开关控制开关电磁阀打开，与气缸5相连的气管通入压缩气，驱动气缸5伸长，实现手爪4的夹紧；当主控模块输出低电平时，限位开关控制开关电磁阀关闭，与气缸5相连的气管断开压缩气，驱动气缸5收缩，实现手爪4的松开。

具体的，该主控模块为单片机最小系统，单片机最小系统包括单片机、锁存器、外部RAM、外部EEPROM和晶振电路，单片机分别与锁存器、外部RAM、外部EEPROM和晶振电路的对应信号端相连。

参见图2，上述机械手包括基座1，设于基座1上的多个转动臂3，以及设于转动臂3末端的手爪4，多个转动臂3串联铰接，且相邻转动臂3端部连接处设置有步进电机2，用于驱动转动臂3旋转，手爪4上设有气缸5，用于驱动手爪4松开或夹紧，且气缸5的气管上安装有开关电磁阀，人机界面6和按钮开关7设置于基座1上。

参见图3，本发明还提出了一种小型机械手控制器的控制方法，运行于如上任一项的小型机械手控制器上，包括如下步骤：

S101，获取机械手从初始位置到目标位置的示教数据，并将示教数据发送至控制器，示教数据为教导机械手运动的操作过程中产生的数据，包括多个转动臂的运动轨迹和开关电磁阀的状态数据。

S102，建立直角坐标系，获取运动轨迹的直角坐标，将运动轨迹分解为直线片段和圆弧片段，当为直线片段时，则通过直线插补方式从片段起点移动至片段终点，当为圆弧片段时，则通过圆弧插补方式从片段起点移动至片段终点。

S103，将片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值。

将片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值，包括：设片段起点的直角坐标为(X_i,Y_i)，片段终点的直角坐标为(X_i+1,Y_i+1)，则片段角度R(i)的公式如下所示：

角度差值为：R(i+1)-R(i)；

上式中，R1为第一转动壁的臂长，R2为第二转动壁的臂长。

S104，计算角度差值需要的脉冲数，基于SPTA算法并根据脉冲数驱使机械手转动达到目标位置。

设从片段起点移动至片段终点需要的脉冲数为p(i，i+1)，则：

p(i,i+1)＝{R(i+1)-R(i)}/π。

本发明实施例中，基于SPTA算法并根据脉冲数驱使机械手转动达到目标位置，包括：

步骤1，设置最大定时器中断时间。

步骤2，采用变量interruptCount计算发出一个脉冲所需中断个数。

步骤3，运动步数加1，且当前频率下总步数减1。

步骤4，判断当前频率下总步数是否等于0，若是，则进行下一步，反之，返回步骤3。

步骤5，判断运动步数是否等于总步数，若是，则进行停止计算，输出各频段运动步数，反之，返回步骤2。

函数PulseCountAlgorithm()为中断内处理函数，用于计算当前电机各频率段运动步数，从而实现对步进电机的加减速控制。机械手转动包括：空闲状态、加速状态、匀速状态和减速状态，如图5所示。

可以理解，SPTA算法是将运动时间分割成若干个合适的小时间片，在每个时间片内它都将速度参数加到位置参数上，如果位置参数溢出，它就会输出一个脉冲。速度参数溢出频率越高，电机运行的频率也就越高。

S105，根据开关电磁阀的状态数据控制气缸动作，实现手爪的松开或夹紧。

综上所述，本发明的有益效果包括：采用单片机最小系统作为控制器的主控模块，其内部存储有控制程序，可根据实际生产需求进行适应调整。在主控模块和驱动模块之间设置光耦隔离模块，对输出信号进行电气隔离，避免电机工作时产生的感应电压将主控模块击穿烧毁，提高了控制器的可靠性和抗干扰能力。将转动臂的运动轨迹分解为直线片段和圆弧片段，分别利用直线插补和圆弧插补方式进行路线拟合，基于SPTA算法和脉冲数计算获得步进电机在各频段的运动步数，实现了步进电机的精确控制，平稳性好。

应理解，所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

本发明的技术范围不仅仅局限于上述说明中的内容，本领域技术人员可以在不脱离本发明技术思想的前提下，对上述实施例进行多种变形和修改，而这些变形和修改均应当属于本发明的保护范围内。

Claims (8)

1.一种小型机械手控制器，用于控制机械手工作，其特征在于，所述控制器包括主控模块，与所述主控模块连接的通信模块、光耦隔离模块、检流电路、限位开关和按钮开关，所述通信模块与人机界面相连，用于收发数据，所述光耦隔离模块输出端通过驱动模块与步进电机相连，所述检流电路连接于主控模块和驱动模块之间，所述限位开关连接有开关电磁阀，用于控制开关电磁阀的开合。

2.根据权利要求1所述的小型机械手控制器，其特征在于，所述机械手包括基座，设于所述基座上的多个转动臂，以及设于所述转动臂末端的手爪，所述多个转动臂串联铰接，且相邻所述转动臂端部连接处设置有步进电机，用于驱动转动臂旋转，所述手爪上设有气缸，用于驱动手爪松开或夹紧，且所述气缸的气管上安装有开关电磁阀，所述人机界面和按钮开关设置于基座上。

3.根据权利要求1所述的小型机械手控制器，其特征在于，所述主控模块为单片机最小系统，所述单片机最小系统包括单片机、锁存器、外部RAM、外部EEPROM和晶振电路，所述单片机分别与锁存器、外部RAM、外部EEPROM和晶振电路的对应信号端相连。

4.一种小型机械手控制器的控制方法，运行于如权利要求1至3任一项所述的小型机械手控制器上，其特征在于，包括：

获取机械手从初始位置到目标位置的示教数据，并将所述示教数据发送至控制器，所述示教数据包括多个转动臂的运动轨迹和开关电磁阀的状态数据；

建立直角坐标系，获取所述运动轨迹的直角坐标，将所述运动轨迹分解为直线片段和圆弧片段，当为直线片段时，则通过直线插补方式从片段起点移动至片段终点，当为圆弧片段时，则通过圆弧插补方式从片段起点移动至片段终点；

将所述片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值；

计算所述角度差值需要的脉冲数，基于SPTA算法并根据所述脉冲数驱使机械手转动达到目标位置；

根据所述开关电磁阀的状态数据控制气缸动作，实现手爪的松开或夹紧。

5.根据权利要求4所述的小型机械手控制器的控制方法，其特征在于，将片段起点的直角坐标与片段终点的直角坐标进行比较，获得角度差值，包括：

设片段起点的直角坐标为(X_i,Y_i)，片段终点的直角坐标为(X_i+1,Y_i+1)，则片段角度R(i)的公式如下所示：

角度差值为：R(i+1)-R(i)；

上式中，R1为第一转动壁的臂长，R2为第二转动壁的臂长。

6.根据权利要求5所述的小型机械手控制器的控制方法，其特征在于，设从片段起点移动至片段终点需要的脉冲数为p(i，i+1)，则：

p(i,i+1)＝{R(i+1)-R(i)}/π。

7.根据权利要求4所述的小型机械手控制器的控制方法，其特征在于，基于SPTA算法并根据所述脉冲数驱使机械手转动达到目标位置，包括：

步骤1，设置最大定时器中断时间；

步骤2，采用变量interruptCount计算发出一个脉冲所需中断个数；

步骤3，运动步数加1，且当前频率下总步数减1；

步骤4，判断当前频率下总步数是否等于0，若是，则进行下一步，反之，返回步骤3；

步骤5，判断运动步数是否等于总步数，若是，则进行停止计算，输出各频段运动步数，反之，返回步骤2。

8.根据权利要求4所述的小型机械手控制器的控制方法，其特征在于，所述机械手转动包括：空闲状态、加速状态、匀速状态和减速状态。

Images