一种提高机器人碰撞力检测精度及保护灵敏度的方法

摘要

本发明公开了一种提高机器人碰撞力检测精度及保护灵敏度的方法，本发明通过动态调整滤波器截止频率，同时要求速度和位置以及反馈力矩需要同步滤波保证采样时刻的同步性，提高了检测碰撞力的精度。通过在编程时采样指令切换工具号以及碰撞力灵敏度，提高了碰撞力的检测的灵敏度。针对检测到碰撞后的动作方式提出了根据机器人类型来决定各个关节不同的动作方式，首先以减速机或者电机能承受的最大力矩减速停止，停止后受重力影响的关节保持力矩模式以及当前力矩给定不变，不受重力影响的关节打开刹车同时接通动态刹车短接电机绕组，在提供一定阻力的同时能方便操作人员手动将机器人移除到碰撞点之外，解除碰撞。

说明书

技术领域

本发明涉及工业机器人技术领域，尤其涉及一种提高机器人碰撞力检测精度及保护灵敏度的方法。

背景技术

在实际应用中工业机器人会因为人为误操作，周围工件的移动等各种各样的原因导致机器人与周围环境发生干涉碰撞。发生碰撞后如果不及时停止机器人的运行就会导致机器人本体特别是减速机、电机受到一个较大的外力从而导致减速机、电机等相对脆弱的部件损坏。传统的检测机器人是否发生碰撞的方法有以下几种：

1、通过检测电机电流是否超过了电机能承载的最大电流来检测是否发生了碰撞。

2、通过机器人末端安装检测装置(相机，雷达等)来主动探测机器人行进路径中的障碍物，从而探知是否会发生碰撞，提前动作。

3、通过动力学实时计算出当前时刻各个关节理论上的出力力矩，并检测电机实际出力力矩是否超过理论力矩一定的阀值从而判断是否发生碰撞。

上述第1种方案中往往按照电机最大值来设置保护门限，保护阀值很高，无法保护减速机。第2种方案需要外加传感器，增加了整个系统的成本不利于推广应用。第3种实时计算出理论力矩，并与实际力矩进行对比能及时有效的保护减速机、电机等部件而且不增加成本。

但第3种方案需要计算理论力的大小会存在下面几个问题：

1)、计算理论力时需要用到机器人的位置，速度，加速度等信息，但在计算加速度信息时有很大的毛刺(误差)，会给理论力的计算带来很大的干扰。

2)、机器人在实际运行过程中特别是应用于搬运，码垛时末端负载是不断变化的。末端负载变化要求计算理论力的动力学模型也随之变化，否则此时计算出来的理论力仍然是错误的。

3)、在实际应用中机器人运行有空行程和工作行程，这两种行程对碰撞力的检测灵敏度的要求是不同的。

4)、在系统检测到碰撞后动作的方式也是不同：位置模式立即停止，力矩模式，位置模式下回退。位置模式下立即停止，在停止过程中也会对减速机造成很大的冲击，有可能碰撞本身不会导致损坏反而停止过程损坏减速机，位置模式下回退方案可以完全消除减速机等部件的外部受力，但对回退路径的规划要求比较高。

发明内容

本发明提供了一种提高机器人碰撞力检测精度及保护灵敏度的方法，以解决背景技术中第3种方案存在的问题。

本发明采用的技术方案是：一种提高机器人碰撞力检测精度及保护灵敏度的方法，其特征在于包括如下步骤：

步骤1：实时采集工业机器人各关节的位置q

步骤2：根据各关节位置计算各关节速度q

步骤3：根据各关节速度计算各关节加速度q

步骤4：确定各关节的滤波器截止频率f

步骤5：确定最终生效的滤波器截止频率为各关节中最高截止频率，将各关节的滤波器截止频率f

f

上式中f

步骤6：将各关节的位置、速度、加速度输入步骤5中截止频率为f

步骤7：从CAD模型中得到机器人本体各个关节、连杆以及末端负载的动力学参数，所述动力学参数包括质量、重心、转动惯量；

步骤8：根据末端负载工件的参数预置几组不同的工具质量、重心、转动惯量，并根据实际的末端负载工况在编程指令中切换不同的工具号；

步骤9：根据不同的防碰撞应用要求预置几组不同的碰撞灵敏度预设值，根据不同的需求在编程指令中切换不同的碰撞灵敏度预设值组号；

步骤10：系统根据编程指令中设置的不同的工具号得到不同的最小惯性参数集，包括惯性参数集M(q)，离心力和科氏力参数集

步骤11：计算各关节理论力矩；

步骤12：实时采集各关节电机的反馈电流I

τ＝GK

上式中τ为关节力矩，G为减速比，K

步骤13：将各关节反馈力矩

步骤14：将理论力矩

τ

上式中τ

步骤15：根据步骤9中预设的各关节的碰撞力灵敏度r

τ

上式中τ

步骤16：分别比较各轴的碰撞力绝对值τ

步骤17：步骤16中只要有一个轴检测到发生了碰撞就进入碰撞后处理流程将各轴都切换到力矩模式，并给定减速机和电机能承受的最大力矩，给定的减速机和电机能承受的最大力矩小于实际的所能承受的最大力矩。

步骤18：实时检测各轴的速度，当速度接近0时候，采用不同的处理方式：不受重力影响的轴直接打开电机刹车并接通动态刹车短接电机三相绕组，受重力影响的轴保持力矩模式以及力矩给定不变。

优选地，步骤4中，确定各关节的滤波器截止频率的方法为：

将各关节速度带入下式得到各关节低通滤波器截止频率f

f

上式中f

优选地，步骤11中，计算各关节理论力矩的方法为：

将滤波后的位置q′

上式中M(q)为惯性参数集，

本发明的有益效果是：本发明通过动态调整滤波器截止频率，同时要求速度和位置以及反馈力矩需要同步滤波保证采样时刻的同步性，从而解决了计算理论力矩时由于加速度干扰比较大的问题，提高了检测碰撞力的精度。针对在实际应用中末端负载不停变换以及工作行程不同对碰撞力大小的要求不同，提出了在编程时采样指令切换工具号以及碰撞力灵敏度，提高了碰撞力的检测的灵敏度。针对检测到碰撞后的动作方式提出了根据机器人类型来决定各个关节不同的动作方式，首先以减速机或者电机能承受的最大力矩减速停止，停止后受重力影响的关节保持力矩模式以及当前力矩给定不变，不受重力影响的关节打开刹车同时接通动态刹车短接电机绕组，在提供一定阻力的同时能方便操作人员手动将机器人移除到碰撞点之外，解除碰撞。

附图说明

图1为本发明公开的一种提高机器人碰撞力检测精度及保护灵敏度的方法流程图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步详细描述，但本发明的实施方式不限于此。

实施例1：

参见图1，一种提高机器人碰撞力检测精度及保护灵敏度的方法，本发明以工业机器人常见的6关节机器人举例说明，其余类型机器人以此类推。

1、实时采集6个关节的位置q

2、根据6个关节位置计算各个关节速度q

3、根据6个关节速度计算各个关节加速度q

4、确定6个关节的滤波器截止频率：将6个关节速度q

f

式1中f

在这一步中，在计算各个关节加速度信息时，加入低通滤波，并且低通滤波的滤波器截止频率随着转速变化而变化，从而兼顾加速采样的光滑性和及时性。

5、确定最终生效的滤波器截止频率为6个关节中最高截止频率，将6个关节的滤波器截止频率f

f

式2中f

6、将6个关节的位置、速度、加速度输入步骤5中截止频率为f

为保证各信号采样时刻的一致性，在加速度滤波的同时，位置信号和速度信号也进行滤波延迟相应的时间，从伺服反馈的反馈力矩也进行相应的滤波延迟相同的时间。

7、从CAD模型中得到机器人本体各个关节、连杆以及末端负载的动力学参数，所述动力学参数包括质量、重心、转动惯量；

8、根据末端负载工件的参数预置几组不同的工具质量、重心、转动惯量，并根据实际的末端负载工况在编程指令中切换不同的工具号。

针对末端负载变化的问题，有的发明提出了实时辨识末端负载的方法，但实时辨识很难保证辨识的准确度以及实时性，往往辨识结束时候末端负载已经发生了变化，从而造成误报警或者报警延迟。本发明采用编程指令切换末端负载的工具号，无需辨识，保证了实际末端负载与动力学模型是一一对应的。

9、根据不同的防碰撞应用要求预置几组不同的碰撞灵敏度预设值，实际应用中根据不同的需求在编程指令中切换不同的碰撞灵敏度预设值组号。

10、系统根据编程指令中设置的不同的工具号得到不同的最小惯性参数集，包括惯性参数集M(q)，离心力和科氏力参数集

11、将滤波后的位置q′

式3中M(q)为惯性参数集，

12、实时采集6个关节电机的反馈电流I

τ＝GK

式4中τ为关节力矩，G为减速比，K

13、将6个关节反馈力矩τ

14、将理论力矩τ

τ

式5中τ

15、根据步骤9中预设的6个关节的碰撞力灵敏度r

τ

式6中τ

16、分别比较6个轴的碰撞力绝对值τ

17、步骤16中只要有一个轴检测到发生了碰撞就进入碰撞后处理流程，该举例中1、4、6轴为不受重力影响的轴，2、3、5轴为受重力影响的轴。将6个轴都切换到力矩模式，并给定减速机和电机能承受的最大力矩，给定的减速机和电机能承受的最大力矩小于实际的所能承受的最大力矩。

18、实时检测6个轴的速度，当速度接近0时候，采用不同的处理方式：1、4、6轴直接打开电机刹车并接通动态刹车短接电机三相绕组，2、3、5保持力矩模式以及力矩给定不变。

具体的，针对碰撞后的动作方式，本发明提出根据机器人的结构不同采用不同的处理方式。具体过程如下：

先让各个关节工作在力矩模式，并按照减速机或者电机的最大受力减速停机并实时检测关节速度。检测到当前关节停止后根据机器人的结构不同采用不同的处理方式，受重力影响的关节继续保持工作在力矩模式并保证给定力矩只需要克服重力，不受重力影响的关节直接停止使能并松开刹车，接通动态刹车短接电机三相绕组，保证不受重力影响的关节在发生碰撞后受到的阻力随着转速增大而阻力变大，使得发生碰撞后能够产生一定的回弹，而且操作人员能很容易手动移动这些关节到不发生碰撞的区域完全解除碰撞。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。