一种基于EtherCAT的六轴机械臂安全限位方法技术

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

A Safety Limitation Method for Six-Axis Manipulator Based on EtherCAT

A safety limit method of six-axis manipulator based on EtherCAT belongs to the field of automatic control. When the existing manipulator stops by itself without restricting acceleration, there is a problem that the control effect of jitter is poor. An EtherCAT-based method for six-axis manipulator safety limit is proposed, and the main control algorithm for manipulator operation based on EtherCAT is designed. The algorithm for joint position and attitude over-limit judgment, the algorithm for end position and attitude over-limit judgment of manipulator and the algorithm for stop of manipulator are designed. The algorithm is executed once in each cycle of detection and execution of the main control algorithm. A safety algorithm is called to check the operation of the manipulator and determine whether the operation is beyond the limit. If it exceeds the limit, the operation information is returned, and the stop algorithm of the manipulator is called to control the deceleration of the manipulator. The invention can stop the manipulator safely and stably, thus providing guarantee for the use safety of the manipulator.

【技术实现步骤摘要】
一种基于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。并可以通过改变内设的加速度数值以改变实际停止时间。附图说明图1为本专利技术的流程图；图2为本专利技术安全模块交互示意图；图3为本专利技术的机械臂停止算法对机械臂关节电机发出的期望角速度指令示意图；图4为本专利技术的机械臂关节电机实际角速度示意图；图5为本专利技术的电机的实际角位移示意图；图6为本专利技术的机械臂末端在超出范围之后停止过程中在x轴，y轴，z轴方向的位移情况示意图；具体实施方式具体实施方式一：本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述方法包括以下步骤：步骤一、设计基于EtherCAT的控制机械臂运作的主控算法；所述的主控算法实时循环任务，在每一个循环周期检测一遍接收到的指令并进行指令的执行，其中，EtherCAT是指以太网控制自动化技术；步骤二、设计安全判断算法，且在主控算法的每个检测并执行的周期内执行一次；其中，所述的安全判断算法包括：机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法；步骤三、机械臂主控算法的每一个循环周期内调用一次安全算法，检查机械臂的运行情况，并判断运行情况是否超限；若超限，则返回运行信息，并调用机械臂停止算法，控制机械臂减速；若未超限，不进行停机操作，机械臂主控算法继续运行。具体实施方式二：与具体实施方式一不同的是，本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述的主控算法进行检查的循环周期为1ms。具体实施方式三：与具体实施方式一或二不同的是，本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述的步骤二中设计的机械臂关节位姿超限判断算法，具体为：从机械臂电机驱动器中读取各关节的角位移，之后与所设定的各关节的角位移的限制范围进行比较，若超出限定范围则返回错误原因的编号；其中，所述的从机械臂电机驱动器中读取各关节的角位移为检查机械臂关节变量，包括：角位移上限、角位移下限、最大角速度、最大角加速度。具体实施方式四：与具体实施方式三不同的是，本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述的步骤二中设计的机械臂末端位姿超限判断算法，具体为：从机械臂电机驱动器中读取各关节的角位移，之后由机械臂的正运动学计算出末端的位置姿态信息，再与所设定的末端姿态的限制范围进行比较，若超出限制范围则返回错误原因的编号；其中，所述的从机械臂电机驱动器中读取各关节的角位移为检查机械臂末端变量，包括：末端在以机器人基座建立的坐标系中x轴，y轴，z轴方向上的位置的范围；x轴，y轴，z轴是指以机器人基座建立的坐标系的三个方向，主控算法计算出机械臂末端在这个坐标系中的位置坐标，并且，主控算法的运行调用机械臂关节位姿超限判断算法和机械臂末端位姿超限判断算法，并通过步骤三的机械臂关节位姿超限判断算法和机械臂末端位姿超限判断算法检查这个坐标的x，y，z的值有没有超出给定的范围。检查后给出结果，之后主控算法再根据结果来判断是否超限，若超限则执行机械臂停止算法，若没有超限不进行处理。具体实施方式五：与具体实施方式一、二或四不同的是，本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述的步骤二中设计的机械臂停止算法，具体为：调用机械臂关节位姿超限判断算法和机械臂末端位姿超限判断算法，当两种算法之一返回错误时，则控制机械臂按设定方式减速停止；在减速过程中，机械臂停止算法读取各关节转轴处电机的实际角速度，将所述的实际角速度减去预设的角加速度后的值，作为期望的角速度指令输出到关节电机驱动器；当所有电机的实际角速度低于设定的阈值时，判断机械臂已完全停止，输出停机指令。在机械臂每次循环周期的正常工作过程中调用安全算法，即可保证机械臂在超出设定范围时减速停机。具体实施方式六：与具体实施方式五同的是，本实施方式的一种基于EtherCAT的六轴机械臂安全限位方法，所述的步骤三中判断运行情况是否超限的过程具体为：安全算法通过主控算法的接口读取关节转轴处电机驱动器的数据，获得机械臂转轴的关节变量，并通过正运动学计算获得机械臂末端的位置姿态信息，同时调用机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法，根据两个算法返回值判断机械臂运行本文档来自技高网...

【技术保护点】
1.一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述方法包括以下步骤：步骤一、设计基于EtherCAT的控制机械臂运作的主控算法；所述的主控算法实时循环任务，在每一个循环周期检测一遍接收到的指令并进行指令的执行；步骤二、设计安全判断算法，且在主控算法的每个检测并执行的周期内执行一次；其中，所述的安全判断算法包括：机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法；步骤三、机械臂主控算法的每一个循环周期内调用一次安全算法，检查机械臂的运行情况，并判断运行情况是否超限；若超限，则返回运行信息，并调用机械臂停止算法，控制机械臂减速；若未超限，不进行停机操作，机械臂主控算法继续运行。

【技术特征摘要】
1.一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述方法包括以下步骤：步骤一、设计基于EtherCAT的控制机械臂运作的主控算法；所述的主控算法实时循环任务，在每一个循环周期检测一遍接收到的指令并进行指令的执行；步骤二、设计安全判断算法，且在主控算法的每个检测并执行的周期内执行一次；其中，所述的安全判断算法包括：机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法、机械臂停止算法；步骤三、机械臂主控算法的每一个循环周期内调用一次安全算法，检查机械臂的运行情况，并判断运行情况是否超限；若超限，则返回运行信息，并调用机械臂停止算法，控制机械臂减速；若未超限，不进行停机操作，机械臂主控算法继续运行。2.根据权利要求1所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的主控算法进行检查的循环周期为1ms。3.根据权利要求1或2所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤二中设计的机械臂关节位姿超限判断算法，具体为：从机械臂电机驱动器中读取各关节的角位移，之后与所设定的各关节的角位移的限制范围进行比较，若超出限定范围则返回错误原因的编号；其中，所述的从机械臂电机驱动器中读取各关节的角位移为检查机械臂关节变量，包括：角位移上限、角位移下限、最大角速度、最大角加速度。4.根据权利要求3所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤二中设计的机械臂末端位姿超限判断算法，具体为：从机械臂电机驱动器中读取各关节的角位移，之后由机械臂的正运动学计算出末端的位置姿态信息，再与所设定的末端姿态的限制范围进行比较，若超出限制范围则返回错误原因的编号；其中，...

【专利技术属性】
技术研发人员：林伟阳，李圣，刘雨，李湛，佟明斯，高会军，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江,23