一种基于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的六轴机械臂安全限位方法，所述的步骤三中判断运行情况是否超限的过程具体为：

安全算法通过主控算法的接口读取关节转轴处电机驱动器的数据，获得机械臂转轴的关节变量，并通过正运动学计算获得机械臂末端的位置姿态信息，同时调用机械臂关节位姿超限判断算法、机械臂末端位姿超限判断算法，根据两个算法返回值判断机械臂运行情况是否超限，若超限，则调用机械臂停止算法，该算法向电机驱动器发送指令，使机械臂减速。

具体实施方式七：

与具体实施方式六不同的是，本实施方式的一种基于ethercat的六轴机械臂安全限位方法，所述的步骤三中若超限返回的运行信息为运行错误原因的错误编号；其中，错误编号包括：0、1、2；如果运行顺利，程序返回0，表示运行期间没有出现问题；如果是机械臂末端位姿超限判断算法判定出超限结果，则返回1；如果是机械臂关节位姿超限判断算法判定出超限结果，则返回2。从而，当机械臂自己因为超限停了，根据返回的数字就可以判断是出了什么问题。

实施例1：

本发明通过读取电机驱动器的数据获得机械臂六个轴的关节变量，经正运动学计算可以获得末端的位置姿态信息。之后将其与设定的范围比较，判断机械臂是否超限，若超限则向电机驱动器发出减速指令，使机械臂停止。

本安全模块算法包含三个部分，分别是机械臂关节位姿超限判断函数、机械臂末端位姿超限判断函数、机械臂停止函数。上述三者与机械臂主控算法以及关节电机驱动器的交互关系如图1所示。安全模块的框图如图2所示。

机械臂主控算法为基于ethercat的实时循环任务，循环周期为1ms。在每个循环周期内，机械臂主控算法都调用一次安全模块函数securitymodule()，检查机械臂是否有危险并做出相应处理。安全模块通过主控算法的接口读取电机驱动器的数据，获得机械臂六个轴的关节变量，经正运动学计算获得末端的位置姿态信息，同时调用机械臂关节位姿超限判断函数、机械臂末端位姿超限判断函数，根据两个函数返回值判断机械臂是否超限，若超限则向电机驱动器发送指令，使机械臂减速。在减速过程中，机械臂停止算法读取各关节电机的实际角速度，将其减去预设的角加速度，作为期望的角速度指令输出到关节电机驱动器。当所有电机的实际角速度低于一个阈值时，机械臂停止算法判断机械已完全停止，输出停机指令。

图3至图6为操作单轴(实验中为六轴机械臂的1轴)使机械臂末端超过所设范围的实验结果。其中，图3为机械臂停止算法对机械臂关节电机发出的期望角速度指令，图4为机械臂关节电机实际角速度，图5为电机的实际角位移，图6为机械臂末端在超出范围之后停止过程中在x轴，y轴，z轴方向的位移情况。

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

可以通过改变内设的加速度数值以改变实际停止时间。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。