CN110480639A - 一种工业机器人监控区域边界运动规划的方法 - Google Patents

Abstract

本发明公开了一种工业机器人监控区域边界运动规划的方法，根据用户设置的监控区域和机器人的当前速度，计算出虚拟监控区域，当机器人运行进入虚拟监控区域时，进行减速控制，使得当机器人运动到监控区域边界时的速度在一个比较低的范围，避免高速下紧急制动带来力矩过大的危害；对机器人进行减速控制，是通过规划控制器的速度倍率来调节已前瞻规划好的轨迹速度，机器人运动轨迹不受影响；同时考虑到机器人的运动趋势，动态刷新虚拟监控区域，当机器人实时位置不在刷新后的虚拟监控区域时，表明机器人运动轨迹可能朝远离监控区域边界的方向变化，此时再以一定的规律逐步恢复降速前的速度，在保证安全制动的同时，避免过度降低机器人的作业效率。

Description

一种工业机器人监控区域边界运动规划的方法

技术领域

本发明涉及一种工业机器人监控区域边界运动规划的方法。

背景技术

工业机器人以其大运动范围、灵活快速的运动能力，在工业领域得到越来越广泛的应用。通常工业机器人的作业空间是受限的，在其动作范围附近可能存在障碍物；一个机器人工作站也不仅仅是机器人单机工作，存在其他设备如焊机或其他机器人协同作业；用户在操作机器人时也需要相对安全的操作区域。因此机器人自身避障、保障协同工作的其它设备及人员安全，已成为需要解决的重要课题。

为了解决机器人工作区域的安全问题，中国发明专利案CN105555490A提出在机器人工作区域，根据作业人员与机器人接触的危险程度，利用不同颜色标示划分不同的危险程序的工作区域，再根据不同危险程度的作业区域，限制机器人不同的移动速度。另中国发明专利案CN108656103A在工作区域中以不共线的三点建置多个分隔界面，进而形成全分隔面，从而将工作区域划分为安全区域和禁止区域。在划分的安全区域内机器人可全速运行，当进入禁止区域则紧急制动。

在机器人高速运动的情况下，当从安全工作的区域进入禁止的区域时，如果直接紧急制动会导致机器人关节承受力矩过大，影响电机性能，甚至损坏机械结构，造成危害。因此，一般机器人在进入禁止区域前需要提前进行减速，以上专利均未提及机器人临近区域边界的运动减速方法。另外，机器人的位置、速度复杂多变，提前减速的判断条件有可能造成误判断，若减速过程不及时修复可能带来机器人不必要的停机。

发明内容

本发明的目的在于，克服现有技术存在的缺陷，提出一种工业机器人监控区域边界运动规划的方法，根据用户设置的监控区域和机器人的当前速度，计算出虚拟监控区域，当机器人运行进入虚拟监控区域时，进行减速控制，使得当机器人运动到监控区域边界时的速度在一个比较低的范围，避免高速下紧急制动带来力矩过大的危害；对机器人进行减速控制，是通过规划控制器的速度倍率来调节已前瞻规划好的轨迹速度，机器人运动轨迹不受影响；同时考虑到机器人的运动趋势，动态刷新虚拟监控区域，当机器人实时位置不在刷新后的虚拟监控区域时，表明机器人运动轨迹可能朝远离监控区域边界的方向变化，此时再以一定的规律逐步恢复降速前的速度，在保证安全制动的同时，避免过度降低机器人的作业效率。

本发明的目的是解决当机器人高速运行到监控区域边界，进入禁止的区域时，紧急制动造成的机器人本体损伤问题。基本思路如下：

机器人监控区域可为三维空间下的任意形状，机器人可以在区域内作业，也可以在区域外作业，由用户根据作业空间进行设置。如图1(机器人工作在监控区域内)和图2(机器人工作在监控区域外)所示，区域O为设定的监控区域，P为机器人TCP当前位置，V为机器人TCP当前速度。

根据机器人速度V和最大安全加速度a_max，由公式(1)，计算提前减速距离△L。

ΔL＝V²/2a_max (1)

若机器人在区域内作业，则将监控区域O的边界向内偏移△L，若机器人在区域外作业，则将监控区域O的边界向外偏移△L，以此获得虚拟监控区域O'，O'与O之间的阴影区域即为提前减速区。

速度V由机器人轨迹规划的速度V_plan与当前周期的速度倍率K计算得出，如下：

V＝K×V_plan (2)

其中未减速前速度倍率K等于原始用户设置的速度倍率K_s。

当机器人TCP位置P还未到达虚拟监控区域O'时，机器人保持原速度倍率K_s运动，当机器人TCP位置P进入虚拟监控区域O'时，设计速率倍率调节函数K_sub(t)进行提前减速。速度倍率调节函数K_sub(t)为时间的多项式，边界条件要求如下：

时间t的范围为0～T_sub，T_sub为用户设定的调节时间，其值越小，调节过程越短。K_cur是K_sub(t)的起始值，即当前周期的速度倍率，首次调节的K_cur等于原始倍率K_s。

速度调节期间，令K等于K_sub(t)，则机器人进行减速。

减速后速度V变化，根据公式(1)计算变化后的△L，监控区域O需进行对应的偏移,实时刷新得到虚拟监控区域O'。按照TCP当前点P与虚拟监控区域O'的相对位置变化，判断机器人的运动趋势，若当前点P已出虚拟监控区域，再重新调整速度倍率调节函数，逐步恢复至原速度倍率K_s，调整后的速度倍率调节函数为K'_sub(t)。整个过程实行动态加减速，避免过度降低机器人速度，尽可能地提高机器人工作效率。再次调整的多项式K'_sub(t)的边界条件要求如下：

其中T_sub，K_cur，K_s含义同上。

基于上述技术分析，本发明为实现发明目的的技术方案一种工业机器人监控区域边界运动规划的方法，其步骤如下：

步骤1.输入当前TCP速度V和最大安全加速度a_max，计算提前减速距离△L：

ΔL＝V²/2a_max

若用户设置机器人在区域内作业，则向内偏移设定的监控区域O，偏移距离△L，得到虚拟监控区域O'；若用户设置机器人在区域外作业，则向外偏移设定的监控区域O，偏移距离△L，得到虚拟安全区域O'。

步骤2.判断机器人当前TCP位置点P：

若机器人当前TCP位置点P在虚拟监控区域O'与监控区域O之间，则根据设计的速度倍率调节函数K_sub(t)降低TCP运动速度，提前减速。

若机器人当前TCP位置点P在虚拟监控区域O'之外，判断上次运动周期是否已提前减速：

若上次运动周期未提前减速，说明上个周期机器人也未进入虚拟监控区域，则不干涉运动规划，保持当前速度倍率。

若上次运动周期已提前减速，说明上个周期机器人是在虚拟监控区域内的，即当前机器人运动趋势是远离监控区域边界，或者其运动轨迹仅是靠近监控区域但不会越过边界，应按照重新设计的倍率调节函数K'_sub(t)逐步恢复至原始规划速度倍率，从而避免过度降低机器人的作业效率。

步骤3.机器人当前TCP位置点P已到达监控区域O边界，此时机器人运动速度V经过提前减速的过程，已在一个比较低的范围，故可实行紧急制动，机器人立即停止运动。

步骤4.每周期重新计算参数△L，刷新虚拟监控区域O'。重复步骤1-3，实现对TCP速度的动态加减速调整，直到运动至规划位置或到达监控区域边界为止。

本发明工业机器人监控区域边界运动规划的方法，根据运动速度计算虚拟监控区域，当越过虚拟监控区域则提前进行减速，在保证机器及人员安全的同时，预防紧急制动带来的大力矩损伤。

本发明工业机器人监控区域边界运动规划方法，对机器人进行减速控制，是通过规划控制器的速度倍率来调节已前瞻规划好的轨迹速度，无需重新对机器人的轨迹和速度进行规划，规划速度倍率算法更简单，同时机器人运动轨迹不受影响，不带来轨迹不可控问题。

本发明工业机器人监控区域边界运动规划的方法，实时动态刷新虚拟监控区域，结合机器人运动趋势，若趋势为远离监控区域边界则以一定的规律逐步恢复至原速度倍率，否则继续减速。通过动态加减速调整，最大程度的提高机器人作业效率。

本发明工业机器人监控区域边界运动规划的方法，无需附加任何的外置装置，全部通过软件实现，无硬件成本，用户操作简单。

附图说明

图1是内虚拟监控区域示意图。

图2是外虚拟监控区域示意图。

图3是机器人速度V的动态调整示意图；其中：图3(a)为没有任何速度调整的情况，图3(b)为临近监控区域的运动规划的情况，图3(c)为进行降速后又远离监控区域的运动规划的情况。

图4是监控区域边界的运动规划流程图。

具体实施方式

下面结合具体实施例，对本发明作进一步详细说明。

本发明所提出的一种工业机器人监控区域边界运动规划的方法，目的是解决当机器人高速运行到监控区域边界，进入禁止的区域时，紧急制动造成的机器人本体损伤问题。配合附图说明如下：

机器人监控区域可为三维空间下的任意形状，机器人可以在区域内作业，也可以在区域外作业，由用户根据作业空间进行设置。如图1(机器人工作在监控区域内)和图2(机器人工作在监控区域外)所示，区域O为设定的监控区域，P为机器人TCP当前位置，V为机器人TCP当前速度。

根据机器人速度V和最大安全加速度参数a_max，由公式(1)，计算提前减速距离△L。

ΔL＝V²/2a_max (1)

若机器人在区域内作业，则将监控区域O的边界向内偏移△L，若机器人在区域外作业，则将监控区域O的边界向外偏移△L，以此获得虚拟监控区域O′，O′与O之间的阴影区域即为提前减速区。

速度V由机器人轨迹规划的速度V_plan与当前周期的速度倍率K计算得出，如下：

V＝K×V_plan (2)

其中未减速前速度倍率K等于原始用户设置的速度倍率K_s。

当机器人TCP位置P还未到达虚拟监控区域O′时，机器人保持原速度倍率K_s运动，当机器人TCP位置P进入虚拟监控区域O′时，设计速率倍率调节函数K_sub(t)进行提前减速。速度倍率调节函数K_sub(t)为时间的多项式，边界条件要求如下：

时间t的范围为0～T_sub，T_sub为用户设定的调节时间，其值越小，调节过程越短。K_cur是K_sub(t)的起始值，即当前周期的速度倍率，首次调节的K_cur等于原始倍率K_s。

速度调节期间，令K等于K_sub(t)，则机器人进行减速。

减速后速度V变化，根据公式(1)计算变化后的△L，监控区域O需进行对应的偏移,实时刷新得到虚拟监控区域O′。按照TCP当前点P与虚拟监控区域O′的相对位置变化，判断机器人的运动趋势，若当前点P已出虚拟监控区域，再重新调整速度倍率调节函数，逐步恢复至原速度倍率K_s，调整后的速度倍率调节函数为K'_sub(t)。整个过程实行动态加减速，避免过度降低机器人速度，尽可能地提高机器人工作效率。再次调整的多项式K'_sub(t)的边界条件要求如下：

图3为机器人速度V的动态调整示意图，其中图(a)为没有任何速度调整的情况下，当机器人运行到监控区域边界O时紧急制动，输出速度立即变为0，本体承受巨大冲击；图(b)为临近监控区域的运动规划情况，即当机器人进入虚拟监控区域O′，如b′至b〞段所示，按照速率倍率调节函数K_sub(t)进行降速，当运行到监控区域边界O时，速度已在一个比较低的范围，此时再立即停止；图(c)为进行降速后，实时刷新虚拟监控区域O′，刷新后的虚拟监控区域为O〞，当机器人已远离虚拟监控区域O〞，如c′至c〞段所示，将按照调整后的速度倍率函数K'_sub(t)进行加速，直至恢复到降速前的速度。

本发明的具体实现流程如图4所示：

1.输入当前TCP速度V和参数a_max，根据公式(1)，计算得到虚拟监控区域参数△L。若用户设置机器人在区域内作业，则如附图1偏移设定的监控区域(实线框O)得到虚拟监控区域(虚线框O′)；若用户设置机器人在区域外作业，则如附图2偏移设定的监控区域(实线框O)得到虚拟安全区域(虚线框O′)。

2.判断当前TCP位置点P是否在虚拟监控区域O′与监控区域O之间(图1和图2阴影区内)，若在则如附图3(b)中b′至b〞段，根据设计的速度倍率调节函数K_sub(t)降低TCP运动速度，提前减速。

3.机器人当前TCP位置点P在虚拟监控区域O′之外，判断上次运动周期是否已提前减速，若未提前减速说明上个周期机器人也未进入虚拟监控区域，则不干涉运动规划，保持当前速度倍率。

4.机器人当前TCP位置点P在虚拟监控区域O′之外，判断上次运动周期已提前减速，说明上个周期机器人是在虚拟监控区域内的，即当前机器人运动趋势是远离监控区域边界，或者其运动轨迹仅是靠近监控区域但不会越过边界，此时如附图3(c)中c′至c〞段，应按照重新设计的倍率调节函数K'_sub(t)逐步恢复至原始规划速度倍率，从而避免过度降低机器人的作业效率。

5.机器人当前TCP位置点P已到达监控区域O边界，此时机器人运动速度V经过提前减速的过程，已在一个比较低的范围，故可实行紧急制动，机器人立即停止运动。

6.每周期重新计算参数△L，刷新虚拟监控区域O′。重复上述步骤，实现对TCP速度的动态加减速调整，直到运动至规划位置或到达监控区域边界为止。

Claims (1)

1.一种工业机器人监控区域边界运动规划的方法，其步骤如下：

步骤1.输入当前TCP速度V和最大安全加速度a_max，计算提前减速距离△L：

ΔL＝V²/2a_max；

若用户设置机器人在区域内作业，则向内偏移设定的监控区域O，偏移距离△L，得到虚拟监控区域O'；若用户设置机器人在区域外作业，则向外偏移设定的监控区域O，偏移距离△L，得到虚拟安全区域O'；

步骤2.判断机器人当前TCP位置点P：

若机器人当前TCP位置点P在虚拟监控区域O'与监控区域O之间，则根据设计的速度倍率调节函数K_sub(t)降低TCP运动速度，提前减速；

若机器人当前TCP位置点P在虚拟监控区域O'之外，判断上次运动周期是否已提前减速：

a.若上次运动周期未提前减速，说明上个周期机器人也未进入虚拟监控区域，则不干涉运动规划，保持当前速度倍率；

b.若上次运动周期已提前减速，说明上个周期机器人是在虚拟监控区域内的，按照重新设计的倍率调节函数K'_sub(t)逐步恢复至原始规划速度倍率，从而避免过度降低机器人的作业效率；

步骤3.机器人当前TCP位置点P已到达监控区域O边界，此时机器人运动速度V经过提前减速的过程，机器人立即停止运动；

步骤4.每周期重新计算参数△L，刷新虚拟监控区域O'；重复步骤1-3，实现对TCP速度的动态加减速调整，直到运动至规划位置或到达监控区域边界为止。