CN114619439B - 多轴机械臂防撞击保护控制系统 - Google Patents

Abstract

本发明公开了一种多轴机械臂防撞击保护控制系统，包括用于采集编码器输出的A、B相脉冲信号的编码器采集模块，用于接收A、B相脉冲信号的数值滤波模块，用于实时对比编码器A、B相脉冲信号时序的时序对比模块，用于接收来自时序对比模块发送的符合时序信号的时间对比模块，用于记录来自时序对比模块和时间对比模块发送的非正交时序信号和运行方向不一致信号的撞击预判模块，当非正交时序信号或运行方向不一致信号累计值达到预设值时，即判定机械臂堵转或者发生了撞击，输出报警信号给报警器，并输出停机信号给机械臂驱动器，控制机械臂执行机构停止工作。本发明及时判定性强，实现成本低、可靠性和集成度高，通用性强。

Description

多轴机械臂防撞击保护控制系统

技术领域

本发明涉及多轴机械臂控制系统，尤其是涉及多轴机械臂防撞击保护控制系统。

背景技术

工业自动化飞速发展的今天，机械臂广泛应用于工业生产和自动化加工领域，这就使得机械臂工作过程中，因其出现故障而造成人员伤亡及损坏设备事件时有发生。因此，各种用于工业自动化的防撞击保护应运而生。

目前，防撞击保护主要采取三种方式：人员巡视、硬件检测、程序判定。人员巡视需要配备相应人员日夜不间断工作，而且可靠性因人而已，往往起不到及时的保护效果。硬件检测法是添加外部硬件检测模块，一般为电流检测模，当机械臂在发生撞击后，机械臂因无法移动而持续输出大扭矩导致输出电流增大，当输出电流超过一定程度后硬件检测模块报警，停止机械臂运动；缺点是在发生撞击后不能及时停止运行，造成设备本身二次伤害，当发生机械臂撞击人事故时会持续一段更严重的挤压，无法保护被撞人员安全。程序判定法能在发生撞击的过程中输出报警信号让机械臂停止运行，因此程序判定防撞击保护具备更广泛的应用前景。

目前，实现轴系机械臂防撞击保护程序判定方法有：1，极限位检测法：利用光电开关或者微动开关设定左右极限，通过程序算法判定极限位状态，这种方法只能保证机械臂在运行时不会跑出正常区域范围，但并无法解决局域范围内的堵转或者撞击判断。2，运动超时判定法：利用预设时间超时保护，该方法虽然可以判断所有区域的堵转或因撞击产生的故障，但是会产生很大的延时，需要等到设定时间到了以后才能给出故障判断让电机停止，无法有效起到对人员和仪器保护作用。3，同步对比法：通过机械臂电机运动的步数和编码器或光栅尺采集的数据进行对比判定，当电机运动步数和编码器或者光栅尺的步数差超过一定范围后即判定为撞击或者发生堵转；但是这种方法不能监控撞击过程，在判定撞击的时候需要最少两次以上的对比判定，不能起到及时防撞击保护，而且在人机交互撞击和挤压伤害保护上无法避免深度伤害。

发明内容

本发明目的在于提供一种多轴机械臂防撞击保护控制系统。

为实现上述目的，本发明采取下述技术方案：

本发明所述多轴机械臂防撞击保护控制系统，包括下述模块：

编码器采集模块，用于采集多个编码器输出的A、B相脉冲信号，并输出给数值滤波模块；

所述数值滤波模块，用于接收所述编码器采集模块输出的所述A、B相脉冲信号，并将A、B相脉冲信号中的高频干扰信号剔除后输出给时序对比模块；

所述时序对比模块，用于实时对比来自数值滤波模块发送的各编码器A、B相脉冲信号时序，当编码器A、B相脉冲信号时序出现非正交时序时，发送非正交时序信号给撞击预判模块；若编码器A、B相脉冲信号时序预设的符合时序关系，发送符合时序信号给时间对比模块；

所述时间对比模块，用于接收来自时序对比模块发送的符合时序信号，实时对比A、B相脉冲信号是否符合预设的超前或者滞后范围值，用以判断机械臂的实际运行方向和设定方向是否一致；若符合所述超前或者滞后范围值，判断机械臂的实际运行方向和设定方向一致，发送运行方向一致信号给实时位置记录模块；否则，发送运行方向不一致信号给所述撞击预判模块；

所述撞击预判模块，用于记录来自时序对比模块和时间对比模块发送的所述非正交时序信号和运行方向不一致信号；当非正交时序信号或运行方向不一致信号累计值达到预设值时，即判定机械臂堵转或者发生了撞击，输出报警信号给报警器，并输出停机信号给机械臂驱动器，控制机械臂执行机构停止工作。

本发明优点体现在以下方面：

1、及时判定性强，由于算法实现采用FPGA处理芯片，算法模块单元均是同步进行，采集信号的每一个上升沿和下降沿以及电平持续时间均能实时监控，在每个信号变化时做出结果判定并执行；在发生撞击或者堵转的时候第一时间做出处理。

2、实现成本低、可靠性高，通过普通的编码器、最小资源的FPGA处理芯片，就能实现仪器撞击或者挤压保护，由于FPGA处理芯片为硬件逻辑门编程，其可靠性较高。

3、集成度高，FPGA处理芯片可以根据需求同时采集多个编码器信号并同时控制多个电机驱动器，编码器的采集和电机驱动器的控制均为同步，时序关系相互不牵连；可以方便的增加设备数量，具有良好的扩展性。

4、通用性强，目前大部分运动设备均配备有编码器，凡是配备编码器的设备均能在添加一块FPGA处理芯片实现上述控制，达到及时对设备和人员起到保护作用。

附图说明

图1是本发明的系统结构示意图。

图2是本发明的系统工作流程图。

具体实施方式

下面结合附图对本发明的实施例作详细说明，本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。

如图1所示，本发明的多轴机械臂防撞击保护控制系统可以同时同步控制多个电机，并采集多路编码器。在实时控制机械臂运行时候监控机械臂在任意时间的运动状态，达到实时监控防撞击的目的。本发明由一个FPGA处理器、多路编码器和电机以及对应的一个或者多个电机驱动器组成，多路电机的联动用以控制多轴向机械臂的自由动作。

如图2所示，本发明的多轴机械臂防撞击保护控制系统，包括下述模块：

编码器采集模块，用于采集多个编码器输出的A、B相脉冲信号，并输出给数值滤波模块；

所述数值滤波模块，用于接收所述编码器采集模块输出的所述A、B相脉冲信号，并将A、B相脉冲信号中的高频干扰信号剔除后输出给时序对比模块；

所述时序对比模块，用于实时对比来自数值滤波模块发送的各编码器A、B相脉冲信号时序，当编码器A、B相脉冲信号时序出现非正交时序时，发送非正交时序信号给撞击预判模块；若编码器A、B相脉冲信号时序预设的符合时序关系，发送符合时序信号给时间对比模块；

所述时间对比模块，用于接收来自时序对比模块发送的符合时序信号，实时对比A、B相脉冲信号是否符合预设的超前或者滞后范围值，用以判断机械臂的实际运行方向和设定方向是否一致；若符合所述超前或者滞后范围值，判断机械臂的实际运行方向和设定方向一致，发送运行方向一致信号给实时位置记录模块；否则，发送运行方向不一致信号给所述撞击预判模块；

所述撞击预判模块，用于记录来自时序对比模块和时间对比模块发送的所述非正交时序信号和运行方向不一致信号；当非正交时序信号或运行方向不一致信号累计值达到预设值时，即判定机械臂堵转或者发生了撞击，输出报警信号给报警器，并输出停机信号给机械臂驱动器，控制机械臂执行机构停止工作。

如图2所示，本发明的系统工作过程简述如下：

在步骤401中，控制端查询仪器设备状态，执行上电寻HOME初始化指令，控制多项电机驱动器移动三个轴向的电机初始化到HOME点，然后执行步骤402；

在步骤402中，FPAG控制器在未接受移动命令时如果检测到编码器变化重新执行步骤401；FPAG控制器收到移动控制指令后，开始执行步骤403；

在步骤403中，FPGA控制器的实时高通数值滤波开始工作，按照相应的转速动态调整高通数值滤波的阈值，实时的滤除干扰信号，然后进行步骤404；

在步骤404中，时序对比模块判断编码器的A相和B相脉冲信号的时序关系，若判定通过，进入步骤405中；否则，则进入步骤406，触发一次撞击预判信号并使得撞击预判累加计数器加一；

在步骤405中，时间对比模块判断编码器的A相和B相脉冲信号的电平持续时间内边缘变化的时间关系，若判定通过，进入步骤408中；否则，则进入步骤406，触发一次撞击预判并使得撞击预判累加计数器加一；

在步骤408中，位置记录模块实时记录每一项编码器的位置反馈，分别把每次记录的当前位置信息传递给步骤409和步骤410；

在步骤409中，位置信息在每连续更新5个最小单位会触发一次撞击清除信号，执行步骤406；

在步骤406中，撞击预判模块计数器累加值达到N次会触发一次撞击判定执行步骤407；

在步骤407中，撞击预判模块计数器累加值达到3次会触发一次撞击判定，整机发生故障停止运行，如果未达到累加值继续执行执行步骤404；

在步骤410中，目标位置比较器实时判定当前位置和预定位置是否相等，如果不等执行步骤408，如果相等机械臂正常运动完毕。

Claims (1)

1.一种多轴机械臂防撞击保护控制系统，其特征在于：包括下述模块：

编码器采集模块，用于采集多个编码器输出的A、B相脉冲信号，并输出给数值滤波模块；

所述数值滤波模块，用于接收所述编码器采集模块输出的所述A、B相脉冲信号，并将A、B相脉冲信号中的高频干扰信号剔除后输出给时序对比模块；

所述时序对比模块，用于实时对比来自数值滤波模块发送的各编码器A、B相脉冲信号时序，当编码器A、B相脉冲信号时序出现非正交时序时，发送非正交时序信号给撞击预判模块；若编码器A、B相脉冲信号时序预设的符合时序关系，发送符合时序信号给时间对比模块；

所述时间对比模块，用于接收来自时序对比模块发送的符合时序信号，实时对比A、B相脉冲信号是否符合预设的超前或者滞后范围值，用以判断机械臂的实际运行方向和设定方向是否一致；若符合所述超前或者滞后范围值，判断机械臂的实际运行方向和设定方向一致，发送运行方向一致信号给实时位置记录模块；否则，发送运行方向不一致信号给所述撞击预判模块；

所述撞击预判模块，用于记录来自时序对比模块和时间对比模块发送的所述非正交时序信号和运行方向不一致信号；当非正交时序信号或运行方向不一致信号累计值达到预设值时，即判定机械臂堵转或者发生了撞击，输出报警信号给报警器，并输出停机信号给机械臂驱动器，控制机械臂执行机构停止工作。

Images