[发明专利]一种基于EtherCAT的六轴机械臂安全限位方法

说明书

一种基于EtherCAT的六轴机械臂安全限位方法，属于自动控制领域。现有的机械臂在不限制加速度的情况下自行停止时，存在对产生的抖动的控制效果差的问题。一种基于EtherCAT的六轴机械臂安全限位方法，设计基于EtherCAT的控制机械臂运作的主控算法；设计机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法，且在主控算法的每个检测并执行的周期内执行一次；机械臂主控算法的每一个循环周期内调用一次安全算法，检查机械臂的运行情况，并判断运行情况是否超限；若超限，则返回运行信息，并调用机械臂停止算法，控制机械臂减速。本发明可以安全、稳定地使机械臂停止，从而为机械臂的使用安全提供保障。

技术领域

本发明涉及一种自动控制方法，特别涉及一种基于EtherCAT的六轴机械臂安全限位方法。

背景技术

在机械臂进行运动作业时需要限制末端的位置不能超出给定的范围，且应保证各关节的姿态、位置不相互冲突。一旦发生上述情况，需要机械臂停止运动。如果不限制机械臂的加速度，在停止时机械臂会产生很大的加速度，机身发生剧烈抖动。如果该问题不解决，会导致关节与电机负载过大，减少寿命。而抖动也可能引起机体重心大幅度移动，如果固定不够牢固，甚至会使整台机器倾倒，引发人身安全问题、财产损失。因此需要设计一种算法，使得机械臂在超出限制区域时以不超过上限的加速度减速直至完全停止。

发明内容

本发明的目的是为了解决现有的机械臂在不限制加速度的情况下自行停止时，存在对产生的抖动的控制效果差的问题，而提出一种基于EtherCAT的六轴机械臂安全限位方法。

一种基于EtherCAT的六轴机械臂安全限位方法，所述方法包括以下步骤：

步骤一、设计基于EtherCAT的控制机械臂运作的主控算法；所述的主控算法实时循环任务，在每一个循环周期检测一遍接收到的指令并进行指令的执行；

步骤二、设计安全判断算法，且在主控算法的每个检测并执行的周期内执行一次；其中，所述的安全判断算法包括：

机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法；

步骤三、机械臂主控算法的每一个循环周期内调用一次安全算法，检查机械臂的运行情况，并判断运行情况是否超限；

若超限，则返回运行信息，并调用机械臂停止算法，控制机械臂减速；

若未超限，不进行停机操作，机械臂主控算法继续运行。

本发明的有益效果为：

本发明在机械臂控制过程中，额外设计安全算法限制机械臂运行加速度，保护算法包括机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法；主控算法在机械臂每次循环周期的正常工作过程中调用安全算法，分别通过角位移上限、角位移下限、最大角速度、最大角加速度的量来实现机械臂关节位姿超限判断算法，通过末端在以机器人基座建立的坐标系中x轴，y轴，z轴方向上的位置的范围来实现量机械臂末端位姿超限判断算法的判定，判断出超限后再调用停止算法控制电机驱动器停止；即可保证机械臂在超出设定范围时减速停机。本发明方法可以安全、稳定地使机械臂停止，从而为机械臂的使用安全提供保障。具体效果如图3至图6所示。图3至图6为操作单轴(实验中为六轴机械臂的1轴)使机械臂末端超过所设范围的实验结果。其中，图3为程序对机械臂关节电机发出的期望角速度指令，图4为机械臂关节电机实际角速度，图5为电机的实际角位移，图6为机械臂末端在超出范围之后停止过程中在x轴，y轴，z轴方向的位移情况。

机械臂1轴以常值角速度0.1rad/s持续运行，直至机械臂末端超出范围，启动安全算法。可以从结果中看到，在判断超出范围后，安全模块控制机械臂以0.0008rad/s2的角加速度减速。实际角速度符合预期，可以在125ms内从0.1rad/s减至0，末端各方向的停止轨迹不超过3mm。并可以通过改变内设的加速度数值以改变实际停止时间。