工业机器人非正常停机后自动返回原位的方法和系统

技术领域

本发明涉及工业机器人技术领域，尤其涉及一种工业机器人非正常停机后自动返回原位的方法和系统。

背景技术

随着工业4.0的逐步推进，工业机器人开始越来越多地应用于工业各个领域的生产制造中。在实际应用中，车间的供电或网络不稳定、工组人员误操作等问题均会导致机器人非正常停机，而工业机器人在日常生产制造中的各种问题日益受到工程师以及工厂管理人员的重视，由于操作工业机器人有一定的专业性和复杂性，且国内大部分工厂的安全控制规范都要求机器人在非正常停机后都必须让机器人回到事先设定好的安全位置，简称原位，才能够再次启动。

目前，工业机器人在发生报警、碰撞以及人为介入等造成的停机后，通常有两种方法让机器人返回原位。其中，一种方法是操作人员通过机器人示教手动移动机器人回原位，这种方法对机器人编程要求相对较低，但受到环境和设备等因素的影响，编程效率相对较低；此外，由于机器人非正常停机的位置不可预料且不固定，因此每次非正常停机都需要重新人工示教回原位，可重复性差。另一种方法是对机器人工作路径进行二次开发，当机器人出现非正常停机时，控制机器人按照当前工作路径继续行动以到达终点工位，以可以实现机器人自动返回原位。然而机器人执行工作路径的过程中，其末端执行器的状态会发生变化，例如，搬运机器人移动到抓件位置后，夹爪会抓紧工件，该方法没有考虑机器人末端执行器的往返状态变化，在控制机器人自动返回原位的过程中可能因机器人末端执行器的状态发生变化而导致撞击事故。

发明内容

针对现有技术的以上缺陷或改进需求，本发明的目的在于提供一种工业机器人非正常停机后返回原位的方法和系统，在机器人非正常停机时，根据中断点位相对于工作路径的偏离值和机器人末端执行器的状态规划机器人回原位的路径，以避免由于人工调节子任务程序轨迹和机器人末端执行器状态改变导致撞击事故。

为达此目的，本发明采用以下技术方案：

本发明提供一种工业机器人非正常停机后自动返回原位的方法，包括：

S100、将机器人的各工作路径储存为多个子任务程序，对分别所述多个子任务程序及所述各子任务程序中的点位进行编号，在基本运动指令的基础上增加运动类型、程序编号以及点位编号，形成自定义运动指令；

S200、收到回原位指令后，将机器人当前位置作为中断点位，获取所述中断点位的第一位置坐标数据；

S300、基于最近一次执行的自定义运动指令和所述第一位置坐标数据，计算确定所述中断点位与当前工作路径的偏离值；

S400、若所述偏离值符合允许偏差范围，则根据机器人末端执行器状态控制所述机器人返回原位。

进一步地，对于所述各子任务程序中的任一点位，其对应的所述自定义运动指令包括程序编号、点位编号、目标点位以及运动类型，其中：

位置型变量数组，用于存储所述各子任务程序中所有点位的位置坐标数据；字符型变量数组，用于存储运动类型；数值型变量数组，用于存储所述各子任务程序中所有点位的参考工具坐标系号和工件坐标系号。

进一步地，步骤S300包括：

S301、根据最近一次执行的自定义运动指令，确定出发点位和目标点位；

S302、根据所述出发点位、所述目标定位以及所述中断点位的相对位置关系，确定所述中断点位与当前工作路径的偏离值。

进一步地，步骤S302包括：

读取所述出发点位的第二位置坐标数据和所述目标定位的第三位置坐标数据；

根据所述第一位置坐标数据、所述第二位置坐标数据和所述第三位置坐标数据，计算所述中断点位与所述出发点位的第一距离、所述中断点位与所述目标点位的第二距离以及所述出发点位与所述目标点位的第三距离；

若所述第一距离和所述第二距离均小于所述第三距离，则确定所述中断点位处于所述出发点位与所述目标点位之间的轨迹。

进一步地，步骤S302还包括：

根据所述第一位置坐标数据、所述第二位置坐标数据和所述第三位置坐标数据，计算所述中断点位到所述出发点位与所述目标点位所构成的直线的第四距离，将所述第四距离作为所述偏离值。

进一步地，步骤S400包括：

根据最近一次执行的自定义运动指令，读取所述出发点位与所述目标点位之间的运动类型；

当机器人处于关节运动，若所述偏差值小于关节运动允许偏差值，则根据机器人末端执行器状态控制所述机器人返回原位；

当机器人处于直线运动，若所述偏差值小于直线运动允许偏差值，则根据机器人末端执行器状态控制所述机器人返回原位。

进一步地，所述自定义运动指令还包括通过布尔变量记录机器人末端执行器状态，其中，所述机器人末端执行器状态包括机器人末端执行器未执行动作和机器人末端执行器执行动作。

进一步地，所述根据机器人末端执行器状态控制所述机器人返回原位包括：

若机器人末端执行器未执行动作，则基于当前工作路径控制所述机器人从所述中断点位逆向走位回到原位；

若机器人末端执行器执行动作，则基于当前工作路径控制所述机器人从所述中断点位正向走位回到原位。

进一步地，所述方法还包括设置原位安全区，步骤S300之前还包括：

若根据所述第一位置坐标数据判定所述中断点位落于所述原位安全区内，则控制所述机器人从所述中断点位移动走位回到原位。

本发明还提供一种工业机器人非正常停机后自动返回原位的系统，包括：

自定义运动指令形成模块，用于将机器人的各工作路径储存为多个子任务程序，对分别所述多个子任务程序及所述各子任务程序中的点位进行编号，在基本运动指令的基础上增加运动类型、程序编号以及点位编号，形成自定义运动指令；

中断点位偏离值计算模块，用于收到回原位指令后获取所述中断点位的第一位置坐标数据，并基于最近一次执行的自定义运动指令和所述第一位置坐标数据，计算确定所述中断点位与当前工作路径的偏离值；

机器人控制模块，用于根据所述偏离值和机器人末端执行器状态控制所述机器人返回原位的路径。

本发明的有益效果：

本发明提供了一种本发明提供一种工业机器人非正常停机后自动返回原位的方法和系统，其中，将机器人的各工作路径储存为多个子任务程序，对分别多个子任务程序及各子任务程序中的点位进行编号，在基本运动指令的基础上增加运动类型、程序编号以及点位编号，形成自定义运动指令；收到回原位指令后，将机器人当前位置作为中断点位，获取中断点位的第一位置坐标数据；基于最近一次执行的自定义运动指令和第一位置坐标数据，计算确定中断点位与当前工作路径的偏离值；若偏离值符合允许偏差范围，则根据机器人末端执行器状态控制机器人返回原位。

1.本发明中将机器人的各工作路径储存为多个子任务程序，可以根据实际工作需求更新和修改子任务程序，提高本发明的适应性；对基本运动指令进行二次开发形成自定义运动指令，进而利用数组存储和检索所需的数据，提高本发明的工作效率。

2.本发明校核了机器人中断点位与当前工作路径的偏差值，非正常停机后，在人工调节子任务程序轨迹导致机器人位置偏离当前工作路径时，有效避免执行自动返回原位发生撞击事故。

3.本发明基于机器人末端执行器状态控制机器人正向或逆向走位回到原位，避免由于机器人末端执行器状态发生改变导致撞击事故。

本申请附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明实施例中工业机器人非正常停机后自动返回原位的方法的流程图；

图2是本发明实施例子任务程序中储存的工作路径的示意图；

图3是本发明实施例中自定义运动指令的示意图；

图4是本发明实施例中机器人非正常停机的示意图；

图5是本发明实施例中回原位中断程序的流程图；

图6是本发明实施例的中断点位与工作路径的偏离程度的示意图；

图7是本发明实施例中工业机器人非正常停机后自动返回原位的系统的示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

在本发明的描述中，除非另有明确的规定和限定，术语“相连”、“连接”、“固定”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像本申请实施例中一样被特定定义，否则不会用理想化或过于正式的含义来解释。

本申请实施例提供一种工业机器人非正常停机后自动返回原位的方法和系统，在工业机器人非正常停机时，根据中断点位相对于工作路径的偏离值和机器人末端执行器的状态规划机器人回原位的路径，有效避免由于人工调节子任务程序轨迹和机器人末端执行器状态改变导致撞击事故。

本申请实施例提供一种工业机器人非正常停机后自动返回原位的方法，该方法的流程图如图1所示，本实施例中的工业机器人非正常停机后自动返回原位的方法包括步骤S100-S400。

S100、将机器人的各工作路径储存为多个子任务程序，对分别多个子任务程序及各子任务程序中的点位进行编号，在基本运动指令的基础上增加运动类型、程序编号以及点位编号，形成自定义运动指令。

本实施例子任务程序中储存的工作路径的示意图如图2所示，将机器人从原位和一个工作设备之间的往返路径记为一条工作路径，每条工作路径中包括多个点位。其中，在原位附近划分出原位安全区，该原位安全区中机器人可以自由移动并且不会与周围设备发生干涉，通常将原位安全区设置为矩形或圆柱形。

进一步地，按照上述工作路径设计机器人正常运行的子任务程序。需要说明的是，通常子任务程序记录的工作路径包括：从原位出发、运动到设备外、进入夹具、末端执行器执行动作、退出夹具、运动到设备外、回原位的工作流程，但基于各工业机器人实际应用场景，子任务程序的起始点和结束点也可以不是原位，但一定要求在原位安全区内。

继续参见图2，首先分别对多个子任务程序进行编号，例如，将机器人从原点到设备1的工作路径对应的子任务程序作为程序1，将机器人从原点到设备2的工作路径对应的子任务程序作为程序2，以此类推。而后对各子程序任务中的点位进行编号，原位的点位编号固定为1，一般而言各子任务程序的开头和结尾均为原位的点位编号1，按照机器人完成一次工作路径的顺序依次对该工作路径中的其余点位进行编号。

本实施例中，在通过子任务程序编号和点位编号将每条工作路径固定后，在基本运动指令的基础上开发多个运动指令函数集——自定义运动指令。

以ABB机器人为例，ABB机器人的基础运动指令包括关节运动MoveJ、直线运动MoveL、圆弧运动MoveC以及绝对位置运动MoveAbsJ。其中，以关节运动MoveJ为例，若以关节运动指令规划机器人在两点位的运动路径，其对应机器人编程语句形式为：“MoveJp

可以看出，机器人的基础运动指令对应的机器人编程语句定义了机器人从出发点位运动到目标点位的过程，包含了机器人从出发点位运动到目标点位时的运动类型、目标点位、运行速度、机器人工具中心点以及参考用户坐标系。在此基础上，自定义运动指令增加程序编号和点位编号，并将这些变量全部参数化，形成含有形参的指令函数集。为ABB机器人为例，在基础运动指令进行二次开发后形成的自定义运动指令参见图3，其中，自定义运动中的运动类型中，将基础运动指令中关节运动MoveJ、直线运动MoveL、圆弧运动MoveC这三个指令合并为一个含有形参的自定义指令MOVELCJ，将绝对位置运动MoveAbsJ指令改变为MoveAbsJ。进一步地，自定义运动指令中的形参包括运动类型、程序编号、点位编号、目标点位(及位置变量)、运动速度、转弯半径以及目标点位的参考工具坐标系和工件坐标系。可以理解的是，自定义运动指令中记录的点位编号即是出发点位的编号。

进一步地，本实施例中，通过数组来储存自定义运动指令的相关数据：定义多个不同类型的数组来存储自定义运动指令中的形参。其中，位置型变量数组用于存储各个子任务程序中所有点位的位置坐标数据，字符型变量数组用于存储运动类型，数值型变量数组用于存储各个子任务程序中所有点位的参考工具坐标系号和工件坐标系号。

具体而言，按照自定义运动指令编写所有子运动程序，若共有N1个子任务程序，每个子任务程序中最多有N2个点位，即最大点位编号N2。则，通过数组来储存自定义运动指令中的形参包括：

(1)定义二维位置型数组PG，该二维位置型数组PG用于存储每个子任务程序中各个点位的位置坐标数据，则二维位置型数组的容量为PG{N

(2)定义二维字符串型数组SG，该二维字符串型数组SG用于存储每个子任务程序中各个点位的运动类型，则二维字符串型数组SG容量为SG{N

(3)定义二维数值型数组NG

(4)定义二维数值型数组NG

本实施例中，自定义运动指令的数据输入包括：机器人执行子任务程序时，机器人系统按照自定义运动指令自动记录当前的程序编号和点位编号，并根据程序编号找到该程序编号对应的数组变量，按照点位编号依次将每个点位的位置坐标数据记录在对应的位置型变量数组中，将每个点位的运动类型记录在对应的字符串型变量数组中，将每个点位的参考工具坐标系和工件坐标系分别记录在对应的数值型变量数组中。此外，通过布尔变量记录机器人末端执行器是否执行动作，例如，搬运机器人的末端执行器为夹爪，通过布尔变量记录夹爪是否抓取工件(或者记录夹爪是否放置工件)。将所有子程序任务都执行一次后，机器人系统便可以以数组的形式存储各子任务程序中所有点位对应的形参，完成自定义运动指令的数据初始化。需要说明的是，本实施例中，采用数组进行存储便于数据存储和数据检索，同时可以大大简化程序运动指令行数。

机器人在执行子任务程序时，机器人系统实时记录和更新机器人当前的程序编号、点位编号以及机器人末端执行器状态。本实施例中，当前程序编号记为NowProNum，当前目标点位编号记为NowPointNum，当前机器人末端执行器状态为BoolGripACT。进一步地，将机器人当前的程序编号和点位编号与自定义语言指令进行匹配，可以获取机器人当前运动路径的相关参数。参见图4，机器人出现非正常停机时，根据机器人系统记录和机器人程序的预读功能，可以确定机器人正在执行程序编号为NowProNum的子程序任务，并处于出发点位PX

S200、收到回原位指令后，将机器人当前位置作为中断点位，获取中断点位的第一位置坐标数据。

本实施例中，当操作者接触机器人停机警报、按下回原点按钮后，机器人系统收到回原位指令并触发回原位中断程序。可以理解的是，当机器人出现非正常停机时，由于人工调控子任务程序轨迹或碰撞可能导致机器人当前位置偏离工作路径，因此本实施例中，回原位中断程序用于校核机器人是否具备自动回原位的条件，并用于根据机器人末端执行器状态规划机器人自动回原位的路径，该回原位中断程序的流程图参见图5。

参见图5，触发回原位中断程序后，将机器人当前位置作为中断点位PX，机器人系统获取中断点位的第一位置坐标数据。其中，若根据第一位置坐标数据判定中断点位落于原位安全区内，则控制机器人从中断点位移动走位回到原位。需要说明的是，本实施例中原位安全区配置有区域监控功能，当机器人工具中心点进入原位安全区后DO

进一步地，若机器人的中断点位在原位安全区外，则需要判定该中断点位是否偏离工作路径，可以理解的是，这里所说的工作路径是指当前子任务程序对应的工作路径。若该中断点位未偏离工作路径或偏离程度较小，则控制机器人按照当前工作路径正向或反向走位回到原点；否则，由操作者决定是否手动回原位。

S300、基于最近一次执行的自定义运动指令和第一位置坐标数据，计算确定中断点位与当前工作路径的偏离值。

S301、根据最近一次执行的自定义运动指令，确定出发点位和目标点位；

S302、根据出发点位、目标定位以及中断点位的相对位置关系，确定中断点位与当前工作路径的偏离值。

具体地，读取出发点位的第二位置坐标数据和目标定位的第三位置坐标数据，根据第一位置坐标数据、第二位置坐标数据和第三位置坐标数据，计算中断点位与出发点位的第一距离、中断点位与目标点位的第二距离以及出发点位与目标点位的第三距离。本实施例的中断点位与工作路径的偏离程度的示意图如图6所示，根据机器人运动的特点，如果机器人的中断点位处于出发点位与目标点位的轨迹上时，下列关系成立：

其中，ΔX

换而言之，若第一距离和第二距离均小于第三距离，则确定中断点位处于出发点位与目标点位之间的轨迹。但仅仅确定当前位置的中断点位处于出发点位与目标点位之间的轨迹上还不够，此时仍然不能判断出机器人当前位置PX有没有被人为移动，且脱离开机器人系统自动规划的PX

进一步地，本实施例的中断点位与当前工作路径的偏离值的计算包括：根据第一位置坐标数据、第二位置坐标数据和第三位置坐标数据，计算中断点位到出发点位与目标点位所构成的直线的第四距离，其中，参见图6，第四距离为D

S400、若偏离值符合允许偏差范围，则根据机器人末端执行器状态控制机器人返回原位。

具体地，本实施例中允许偏差值为允许偏差系数与第三距离的乘积，其中，根据机器人运动类型确定允许偏差系数，例如，PX

进一步地，根据最近一次执行的自定义运动指令，读取出发点位与目标点位之间的运动类型。当机器人处于关节运动时，偏差值小于关节运动允许偏差值，则根据机器人末端执行器状态控制机器人返回原位；当机器人处于直线运动时，偏差值小于直线运动允许偏差值，则根据机器人末端执行器状态控制机器人返回原位。可以理解，若偏离值不符合允许偏差范围，则说明机器人当前位置偏离当前工作路径较多，可能是人工移动了机器人，如果坚持自动回原位可能发生碰撞，此时，让用户自主决定是否依然采用自动回原位。如果用户确认则从当前位置执行回原轨迹。如果用户拒绝从当前位置执行回原轨迹，则程序终止，需要用户手动回原位。

前文中提到自定义运动指令还包括通过布尔变量记录机器人末端执行器状态，其中，机器人末端执行器状态包括机器人末端执行器未执行动作和机器人末端执行器执行动作。需要说明的是这里根据机器人末端执行器状态来确定机器人回原位路径是因为在机器人末端执行器执行动作时，机器人末端执行器状态变化较为复杂，因此必须分情况考虑回原路径，否则会撞机

本实施例中，若机器人末端执行器未执行动作，则基于当前工作路径控制机器人从中断点位逆向走位回到原位。具体地，调用布尔变量确定机器人末端执行器没有执行动作时，在数据存储区检索存储的关于程序编号为NowProNum的相关数据，以PX为起点朝PX

进一步地，若机器人末端执行器执行动作，则基于当前工作路径控制机器人从中断点位正向走位回到原位。具体地，调用布尔变量确定机器人末端执行器执行动作时，在数据存储区检索存储的关于程序号为NowProNum的相关数据，以PX为起点朝PX

至此回原位完成，检测原位OK，回原位中断程序结束。

综上所述，本申请实施例中将机器人的各工作路径储存为多个子任务程序，可以根据实际工作需求更新和修改子任务程序，提高本发明的适应性；对基本运动指令进行二次开发形成自定义运动指令，进而利用数组存储和检索所需的数据，提高本发明的工作效率。同时，本申请校核了机器人中断点位与当前工作路径的偏差值，非正常停机后，在人工调节子任务程序轨迹导致机器人位置偏离当前工作路径时，有效避免执行自动返回原位发生撞击事故。此外，基于机器人末端执行器状态控制机器人正向或逆向走位回到原位，避免由于机器人末端执行器状态发生改变导致撞击事故。

本申请实施例还提供一种工业机器人非正常停机后自动返回原位的系统，该系统的示意图如图7所示，本实施例中的工业机器人非正常停机后自动返回原位的系统包括自定义运动指令形成模块、中断点位偏离值计算模块以及机器人控制模块。

自定义运动指令形成模块用于将机器人的各工作路径储存为多个子任务程序，对分别多个子任务程序及各子任务程序中的点位进行编号，在基本运动指令的基础上增加运动类型、程序编号以及点位编号，形成自定义运动指令；

中断点位偏离值计算模块用于收到回原位指令后获取中断点位的第一位置坐标数据，并基于最近一次执行的自定义运动指令和第一位置坐标数据，计算确定中断点位与当前工作路径的偏离值；

机器人控制模块用于根据偏离值和机器人末端执行器状态控制机器人返回原位的路径。

应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述仅是本申请的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。