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

权利要求书

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

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

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

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

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

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

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

2.根据权利要求1所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的主控算法进行检查的循环周期为1ms。

3.根据权利要求1或2所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤二中设计的机械臂关节位姿超限判断算法，具体为：

从机械臂电机驱动器中读取各关节的角位移，之后与所设定的各关节的角位移的限制范围进行比较，若超出限定范围则返回错误原因的编号；其中，所述的从机械臂电机驱动器中读取各关节的角位移为检查机械臂关节变量，包括：角位移上限、角位移下限、最大角速度、最大角加速度。

4.根据权利要求3所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤二中设计的机械臂末端位姿超限判断算法，具体为：

从机械臂电机驱动器中读取各关节的角位移，之后由机械臂的正运动学计算出末端的位置姿态信息，再与所设定的末端姿态的限制范围进行比较，若超出限制范围则返回错误原因的编号；其中，所述的从机械臂电机驱动器中读取各关节的角位移为机械臂末端在以机器人基座建立的坐标系中x轴，y轴，z轴方向上的位置。

5.根据权利要求1、2或4所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤二中设计的机械臂停止算法，具体为：

调用机械臂关节位姿超限判断算法和机械臂末端位姿超限判断算法，当两种算法之一返回错误时，则控制机械臂按设定方式减速停止；在减速过程中，机械臂停止算法读取各关节转轴处电机的实际角速度，将所述的实际角速度减去预设的角加速度后的值，作为期望的角速度指令输出到关节电机驱动器；当所有电机的实际角速度低于设定的阈值时，判断机械臂已完全停止，输出停机指令。

6.根据权利要求5所述的一种基于EtherCAT的六轴机械臂安全限位方法，其特征在于：所述的步骤三中判断运行情况是否超限的过程具体为：

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

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