(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210661380.8 (22)申请日 2022.06.13 (71)申请人 南京东道电子装备技 术研究院有限 公司 地址 210000 江苏省南京市江宁区景祐路 33号 (72)发明人 曹海霞　孙俊杰　 (74)专利代理 机构 南京瑞华腾知识产权代理事 务所(普通 合伙) 32368 专利代理师 胡海 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种小型机 械手控制器及其控制方法 (57)摘要 本发明提出了一种小型机械手控制器及其 控制方法， 所述控制器包括主控模块， 与所述主 控模块连接的通信模块、 光耦隔离模块、 检流电 路、 限位开关和按钮开关， 所述通信模块与人机 界面相连， 用于收发数据， 所述光耦隔离模块输 出端通过驱动模块与步进电机相连， 所述检流电 路连接于主控模块和驱动模块之间， 所述限位开 关连接有开 关电磁阀， 用于控制开关电磁阀的开 合。 本发明将转动臂的运动轨迹分解为直线片段 和圆弧片段， 分别利用直线插补和圆弧插补方式 进行路线拟合， 基于SP TA算法和脉冲数计算获得 步进电机在各频段的运动步数， 实现了步进电机 的精确控制， 平稳性 好。 权利要求书2页 说明书4页 附图3页 CN 115213899 A 2022.10.21 CN 115213899 A 1.一种小型机械手控制器， 用于控制机械手工作， 其特征在于， 所述控制器包括主控模 块， 与所述主控模块连接的通信模块、 光耦隔离模块、 检流电路、 限位开关和按钮开关， 所述 通信模块与人机界面相连， 用于收发数据， 所述光耦隔离模块输出端通过驱动模块与步进 电机相连， 所述检流电路连接于主控模块和驱动模块之间， 所述限位开关连接有开关电磁 阀， 用于控制开关电磁阀的开 合。 2.根据权利要求1所述的小型机械手控制器， 其特征在于， 所述机械手包括基座， 设于 所述基座上的多个转动臂， 以及设于所述转动臂末端的手爪， 所述多个转动臂串联铰接， 且 相邻所述转动臂端部连接处设置有步进电机， 用于驱动转动臂旋转， 所述手爪 上设有气缸， 用于驱动手爪松开或夹紧， 且所述气缸 的气管上安装有开关电磁阀， 所述人机界面和按钮 开关设置于基座上。 3.根据权利要求1所述的小型机械手控制器， 其特征在于， 所述主控模块为单片机最小 系统， 所述单片机最小系统包括单片机、 锁存器、 外部RAM、 外部EEPROM和晶振电路， 所述单 片机分别与锁存器、 外 部RAM、 外 部EEPROM和晶振电路的对应信号端相连。 4.一种小型机械手控制器的控制方法， 运行于如权利要求1至3任一项所述的小型机械 手控制器上， 其特 征在于， 包括： 获取机械手从初始位置到目标位置的示教数据， 并将所述示教数据发送至控制器， 所 述示教数据包括多个转动臂的运动轨 迹和开关电磁阀的状态数据； 建立直角坐标系， 获取所述运动轨迹的直角坐标， 将所述运动轨迹分解为直线片段和 圆弧片段， 当为直线片段时， 则通过直线插补方式从片段起点移动至片段终点， 当为圆弧片 段时， 则通过圆弧插补方式从片段起 点移动至片段终点； 将所述片段起 点的直角坐标与片段终点的直角坐标进行比较， 获得角度差值； 计算所述角度差值需要的脉冲数， 基于SPTA算法并根据所述脉冲数驱使机械手转动达 到目标位置； 根据所述 开关电磁阀的状态数据控制气缸动作， 实现手爪的松开或夹紧。 5.根据权利要求4所述的小型机械手控制器的控制方法， 其特征在于， 将片段起点的直 角坐标与片段终点的直角坐标进行比较， 获得角度差值， 包括： 设片段起点的直角坐标为(Xi,Yi)， 片段终点的直角坐标为(Xi+1,Yi+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， 设置最大定时器中断时间；权　利　要　求　书 1/2 页 2 CN 115213899 A 2步骤2， 采用变量 interruptCount计算发出一个脉冲所需 中断个数； 步骤3， 运动步数加1， 且当前 频率下总步数减1； 步骤4， 判断当前 频率下总步数 是否等于 0， 若是， 则进行 下一步， 反之， 返回步骤3； 步骤5， 判断运动步数是否等于总步数， 若是， 则进行停止计算， 输出各频段运动步数， 反之， 返回步骤2。 8.根据权利要求4所述的小型机械手控制器的控制方法， 其特征在于， 所述机械手转动 包括： 空闲状态、 加速状态、 匀速状态和减速状态。权　利　要　求　书 2/2 页 3 CN 115213899 A 3