CN112123333A - 一种多关节机器人碰撞检测方法及其装置 - Google Patents

Abstract

本发明公开了一种多关节机器人碰撞检测方法及其装置，该方法包括：A、采集机器人正常工作状态下，规划路径的全程运动过程中关节电机电流变化数据，计算得到标准电流数据；B、在所述机器人运行时，将机器人关节电机的实时电流数据、所述标准电流数据输入碰撞检测模块进行对比，若输出结果为第一结果，则代表机器人发生碰撞；若输出结果为第二结果，则代表机器人工作正常。采用本发明，能够解决传统无碰撞检测功能的机器人需要安装辅助装置导致占地过大和实现碰撞检测功能带来的硬件成本过高的问题。

Description

一种多关节机器人碰撞检测方法及其装置

技术领域

本发明涉及工业机器人技术，尤其涉及一种多关节机器人碰撞检测方法及其装置。

背景技术

目前，工业机器人在生产中已经得到了广泛的应用，随着其应用范围的不断扩大，人们对机器人的安全性要求日益提高，机器人碰撞检测功能是保证机器人与操作员安全的一项重要功能。

现有的不具备碰撞检测功能的机器人，一般要通过设置安全围栏或者安全光幕等辅助安全装置的方式阻止人员接近，设置这些辅助安全装置需要占用较大的场地面积或空间。具有碰撞检测功能的机器人，通常使用力矩传感器或者复杂算法计算当前关节力矩的方式进行检测，会耗费较大的计算资源，且需要使用高性能处理器，还会导致成本上升。也有的机器人，是通过在机械结构上添加大量传感器的方式判断是否有碰撞发生，该种方式下额外增加的传感器也会导致成本的增加。

发明内容

有鉴于此，本发明的主要目的在于提供一种多关节机器人碰撞检测方法及其装置，以解决传统无碰撞检测功能的机器人需要安装辅助装置导致占地过大和实现碰撞检测功能带来的硬件成本过高的问题。

为实现上述发明目的，本发明的技术方案如下：

一种多关节机器人碰撞检测方法，包括如下步骤：

A、采集机器人正常工作状态下，规划路径的全程运动过程中关节电机电流变化数据，计算得到标准电流数据；

B、在所述机器人运行时，将机器人关节电机的实时电流数据、所述标准电流数据输入碰撞检测模块进行对比，若输出结果为第一结果，则代表机器人发生碰撞；若输出结果为第二结果，则代表机器人工作正常。

其中：步骤A中所述计算得到标准电流数据的过程，包括：

A1、利用主控制器发送控制指令，启动现场可编程门阵列FPGA打开对机器人关节电机的采样电流数据上传功能；

A2、在所述机器人运动过程中，通过主控制器将每次路径规划点位及FPGA回传的关节电机的电流数据进行匹配，并储存在内存中；

A3、重复步骤A1～步骤A2采集多组所述关节电机的电流数据，并对同一时刻的所述多组电流数据取平均，生成电流数据平均值，即得到标准电流数据。

步骤B中所述将机器人关节电机的实时电流数据、所述标准电流数据输入碰撞检测模块进行对比的过程，包括：

B1、将所述标准电流数据与预设的上限系数通过上限乘法器相乘、将所述标准电流数据与预设的下限系数通过下限乘法器相乘，分别得到上限电流偏差值和下限电流偏差值；所述上限系数为大于1的常数，所述下限系数为小于1的常数；

B2、将实时关节电机的电流数据并行输入上限比较器、下限比较器，再分别与所述上限电流偏差值、下限电流偏差值进行比较，然后将上述比较输出结果再输入“或”门进行“或”运算。

进一步地，步骤B2所述将实时关节电机的电流数据并行输入上限比较器、下限比较器，分别与所述上限电流偏差值、下限电流偏差值进行比较的比较输出结果，具体为：

若实时电流数据大于上限电流偏差值时上限比较器输出逻辑真“1”，若实时电流数据小于上限电流偏差值时上限比较器输出逻辑假“0”；或，

若实时电流数据小于下限电流偏差值时下限比较器输出逻辑真“1”，若实时电流数据大于下限电流偏差值时下限比较器输出逻辑假“0”；

相应地，所述比较输出结果为：

(1)上限比较器输出逻辑真“1”，下限比较器输出逻辑假“0”；或

(2)上限比较器输出逻辑假“0”，下限比较器输出逻辑真“1”；或

(3)上限比较器输出逻辑假“0”，下限比较器输出逻辑假“0”。

进一步地，将所述比较输出结果输入“或”门进行“或”运算，分别得到：

(1')“或”门输出逻辑真“1”，代表发生碰撞；或

(2')“或”门输出逻辑假“0”，代表未发生碰撞。

步骤B所述输出结果为第一结果，即逻辑真“1”；所述输出结果为第二结果，即逻辑假“0”。

所述电流变化数据、标准电流数据、实时电流数据，分别为电流变化值、标准电流值、实时电流值。

一种多关节机器人碰撞检测装置，包括：

碰撞检测模块，用于根据机器人关节电机的标准电流数据与机器人关节电机的实时电流数据进行比较，根据输出的比较结果判断所述机器人是否发生碰撞。

所述碰撞检测模块进一步包括：

上限乘法器，用于将所述标准电流数据与预设的上限系数相乘，得到上限电流偏差值；

下限乘法器，用于将所述标准电流数据与预设的下限系数相乘，得到下限电流偏差值；

上限比较器、下限比较器，分别用于将所述上限电流偏差值、下限电流偏差值与所述实时电流数据比较，得到比较输出结果；

“或”门，用于将所述比较输出结果进行“或”运算，输出碰撞检测结果。

一种包含所述多关节机器人碰撞检测装置的机器人。

本发明的多关节机器人碰撞检测方法及其装置，具有如下有益效果：

1)采用该多关节机器人的碰撞检测方法，只需要对关节电机工作电流进行采样，通过对关节电机电流进行采样对比，即可实现在不新增加硬件的情况下，使机器人具有碰撞检测功能。

2)进一步地，应用本发明的方法，还可以利用现场可编程门阵列(FieldProgrammable Gate Array，FPGA)能够并行运算的特征，同时对所有关节电机的工作电流进行检测，只需利用FPGA进行简单的运算即可满足运算需求，不会占用大量的逻辑资源，因而具有反应快速的特点。

3)将本发明应用于一些从事重复运动工作的机器人，如从事搬运码垛等重复运动的机器人，不但能够使其获得碰撞检测功能，能够提高安全性，还能够应用于一些与人协作的场景，进一步增加了用途，提高了产品价值。

附图说明

图1为本发明实施例多关节机器人碰撞检测装置硬件结构示意图；

图2为本发明实施例多关节机器人碰撞数据采集过程流程图；

图3为本发明实施例多关节机器人运行时碰撞检测处理过程流程图；

图4为本发明实施例FPGA碰撞检测模块的判断逻辑示意图。

具体实施方式

下面结合附图及本发明的实施例对本发明作进一步详细的说明。

图1为本发明实施例多关节机器人碰撞检测装置硬件结构示意图。

如图1所示，该多关节机器人碰撞检测装置，主要包括示教器1、内存2、主控制器3、存储器4、第一电流采样电路5、第二电流采样电路6、现场可编程门阵列(FPGA)7、第一电机11、第二电机12。其中，所述的电流采样电路、电机(即关节电机)，均可设有多组。

所述示教器1，用于对机器人进行编程控制，实现人机交互和设置碰撞检测功能。

所述内存2，用于存储与当前机器人程序模块对应的一路或多路关节电机的电流采样数据。

在本实施例中，所述关节电机的电流采样数据，具体为关节电机(如第一电机11、第二电机12等)的电流值。

所述主控制器3，用于对机器人进行路径规划、关节电机的电流采样数据处理与传输，以及对所述FPGA7进行工作设置。

所述存储器4，用于记录机器人工作程序模块对应的历史电流采样数据。在本实施例中，所述的存储器4，包括但不限于硬盘、固态硬盘、闪存等外存储器。

所述第一电流采样电路5、第二电流采样电路6等采样电路，用于分别采集第一电机11、第二电机12等多组电机运行时机器人关节电机的工作电流数据。所述电流数据，具体为关节电机的电流值。

所述FPGA7，分别与第一电流采样电路5、第二电流采样电路6等多组电流采样电路电性连接，用于对一路或多路机器人关节电机的工作电流进行采集，并对第一电机11、第二电机12等多路电机进行控制。

在本实施例中，所述第一电流采样电路5、第二电流采样电路6等多组电流采样电路，也可统称为电流采样模块。

所述第一电机11、第二电机12等多个/组关节电机，为机器人动作执行机构，用于控制机器人执行各种动作。

这里，所述的第一电机11、第二电机12等多个/组关节电机，也可统称为关节电机组。

本发明的多关节机器人碰撞检测装置的检测原理是：当机器人在运动中受外力时，会依据电机控制算法调整电机的转矩以抵消外力干扰，机器人调整电机转矩，则意味着关节电机的电流会发生变化；在机器人重复运动的过程中，任意两次重复运动过程的的电机转矩变化是相同的，如果此时电机转矩产生差异，则意味着受到了外力干扰，即机器人的关节有可能发生了碰撞。

图2为本发明实施例多关节机器人碰撞数据采集过程流程图。

如图2所示，采集机器人正常工作状态下，机器人完整运动过程的工作电流变化数据的过程，包括如下步骤：

步骤21：利用主控制器3通过并行总线发送指令，启动FPGA7打开对机器人关节电机的采样电流数据上传功能。

这里，由于FPGA7的电流数据采集速度非常高，每次采集电流数据均上传会浪费总线资源，一般主控制器3的路径规划信息设为几百微秒至数毫秒内向FPGA7发送一次，FPGA7可以在接收此次路径规划信息后，回传上一路径规划点位运动的电流平均值。

本实施例中，所述主控制器3，可采用ARM架构的TI AM5728处理器，利用GPMC总线上传电流数据的方式实现。也可以采用Zynq 7000ARM+FPGA SoC，通过内部AXI总线上传电流数据的方式实现。还可以利用X86处理器，通过PCIE总线上传电流数据的方式实现。

步骤22：设置机器人按照设定的程序开始运动(即按照规划路径进行全程运动)，在机器人运动过程中，通过主控制器3将每次路径规划点位及FPGA回传的电流数据进行匹配，并储存在内存2中。

步骤23：对所述机器人设定同一程序按照步骤22重复运行多次，采集多组电流数据。

步骤24：对同一时刻的所述多组电流数据取平均，生成电流数据平均值。具体为：将多次运行采集到的电流数据取平均，生成最终的一组标准电流数据，将该组标准电流数据将作为参照标准。

步骤25：将所述平均后的电流数据，即标准电流数据保存至存储器中，以供图3所示的后续步骤中使用。

图3为本发明实施例多关节机器人运行时碰撞检测处理过程流程图。

如图3所示，该多关节机器人运行时实时地对比机器人关节电机的电流数据，所述的碰撞检测处理过程，包括如下步骤：

步骤31：通过主控制器3下发点位数据时，将上述标准电流数据一同下发。

步骤32：计算实时电流数据与主控制器3下发的所述标准电流数据的差异。

步骤33：判断所述实时电流数据与所述下发的标准电流数据差异是否超过预设的阈值，如果是，则执行步骤34；否则，返回步骤31。

步骤34：停止执行，然后执行步骤35。

步骤35：利用FPGA7通过并行总线上报异常状态。并结束本次碰撞检测过程。

上述步骤31～步骤35中，采用如图4所示的FPGA碰撞检测模块，按相应的碰撞检测数值逻辑进行判断。本实施例中，以单路电流碰撞检测为例进行说明，具体过程如下：

首先，加载机器人程序模块，利用主控制器3同时从存储器4中将对应的标准电流数据读取至内存2中，每次在主控制器3下发路径规划点位数据时，同时通过并行总线下发对应的关节电机的标准电流数据。

其次，设置两个乘法单元，即上限乘法器、下限乘法器，用于分别计算允许的电流偏差范围，将标准电流数据并行乘以预设的上、下限系数，分别得到上限电流偏差值、下限电流偏差值。在本实施例中，允许偏差±5％的电流大小，相应地，所述上限系数就设为1.05，下限系数就设为0.95。例如，若标准电流值为1A，两个乘法单元将在一个计算周期内并行计算出上限电流偏差值为1.05A与下限电流偏差值为0.95A。

再次，FPGA的电流采集电路/模块，原本就存在且持续工作，用于控制机器人的关节电机。

为了利用FPGA实现本发明的碰撞检测功能，需要将电流数据同时传送到碰撞检测模块的比较器中，将本次碰撞检测运行中的关节电机的实时电流数据分别与上、下限比较器进行比较，实时电流数据大于上限时上限比较器输出第一结果，如逻辑真“1”，实时电流数据小于上限时上限比较器输出第二结果，如逻辑假“0”；实时电流数据小于下限时下限比较器输出逻辑真“1”，实时电流数据大于下限时下限比较器输出逻辑假“0”。然后，通过两个比较器，即上限比较器、下限比较器，比较输出的结果分三种情况：

(1)上限比较器输出逻辑真“1”，下限比较器输出逻辑假“0”，此情况下“或”门输出逻辑真“1”，代表发生碰撞；或，

(2)上限比较器输出0，下限比较器输出1，此情况下或门输出1，代表发生碰撞；或，

(3)上限比较器输出0，下限比较器输出0，此情况下或门输出0，代表机器人没有碰撞，正常工作。

经过上述比较过程可知，如果实时关节电机的电流数据超出了上限或者下限比较器，或门输出异常信号，需进行异常处理，如停机，并且通过并行总线向主控报告异常状态。

考虑到机器人实际工作运行时，关节电机发热可能会引起工作电流变化，机器人目标操作物重量偏差等一些物理因素上的变化，机器人每次工作时的电流也不可能是完全一致的，但是会在一个范围内，因此，通过设置上限系数、下限系数确定了这个范围的上界与下界。比如，机器人正常工作时电流是1A，上限系数为1.05，下限系数为0.95，那么电机在0.95-1.05A的电流都算是工作正常，如果与物品相撞，电流会增得很大例如2A，这时超出范围就会判断为机器人撞到东西。当物品与机器人发生同向碰撞，例如机器人从右向左运动，传送带的物品也从右向左运动，但是传送带的速度更快一些就会发生同向碰撞，此时机器人关节电机电流是变小的，当然此时的异常处理就可能不是停机了，而是加速向左或者向上抬机械臂。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。

Claims (10)

1.一种多关节机器人碰撞检测方法，其特征在于，包括如下步骤：

A、采集机器人正常工作状态下，规划路径的全程运动过程中关节电机电流变化数据，计算得到标准电流数据；

B、在所述机器人运行时，将机器人关节电机的实时电流数据、所述标准电流数据输入碰撞检测模块进行对比，若输出结果为第一结果，则代表机器人发生碰撞；若输出结果为第二结果，则代表机器人工作正常。

2.根据权利要求1所述多关节机器人碰撞检测方法，其特征在于，步骤A中所述计算得到标准电流数据的过程，包括：

A1、利用主控制器发送控制指令，启动现场可编程门阵列FPGA打开对机器人关节电机的采样电流数据上传功能；

A2、在所述机器人运动过程中，通过主控制器将每次路径规划点位及FPGA回传的关节电机的电流数据进行匹配，并储存在内存中；

A3、重复步骤A1～步骤A2采集多组所述关节电机的电流数据，并对同一时刻的所述多组电流数据取平均，生成电流数据平均值，即得到标准电流数据。

3.根据权利要求1所述多关节机器人碰撞检测方法，其特征在于，步骤B中所述将机器人关节电机的实时电流数据、所述标准电流数据输入碰撞检测模块进行对比的过程，包括：

B1、将所述标准电流数据与预设的上限系数通过上限乘法器相乘、将所述标准电流数据与预设的下限系数通过下限乘法器相乘，分别得到上限电流偏差值和下限电流偏差值；所述上限系数为大于1的常数，所述下限系数为小于1的常数；

B2、将实时关节电机的电流数据并行输入上限比较器、下限比较器，再分别与所述上限电流偏差值、下限电流偏差值进行比较，然后将上述比较输出结果再输入“或”门进行“或”运算。

4.根据权利要求3所述多关节机器人碰撞检测方法，其特征在于，步骤B2所述将实时关节电机的电流数据并行输入上限比较器、下限比较器，分别与所述上限电流偏差值、下限电流偏差值进行比较的比较输出结果，具体为：

若实时电流数据大于上限电流偏差值时上限比较器输出逻辑真“1”，若实时电流数据小于上限电流偏差值时上限比较器输出逻辑假“0”；或，

若实时电流数据小于下限电流偏差值时下限比较器输出逻辑真“1”，若实时电流数据大于下限电流偏差值时下限比较器输出逻辑假“0”；

相应地，所述比较输出结果为：

(1)上限比较器输出逻辑真“1”，下限比较器输出逻辑假“0”；或

(2)上限比较器输出逻辑假“0”，下限比较器输出逻辑真“1”；或

(3)上限比较器输出逻辑假“0”，下限比较器输出逻辑假“0”。

5.根据权利要求4所述多关节机器人碰撞检测方法，其特征在于，将所述比较输出结果输入“或”门进行“或”运算，分别得到：

(1')“或”门输出逻辑真“1”，代表发生碰撞；或

(2')“或”门输出逻辑假“0”，代表未发生碰撞。

6.根据权利要求1所述多关节机器人碰撞检测方法，其特征在于，步骤B所述输出结果为第一结果，即逻辑真“1”；所述输出结果为第二结果，即逻辑假“0”。

7.根据权利要求1所述多关节机器人碰撞检测方法，其特征在于，所述电流变化数据、标准电流数据、实时电流数据，分别为电流变化值、标准电流值、实时电流值。

8.一种多关节机器人碰撞检测装置，其特征在于，包括：

碰撞检测模块，用于根据机器人关节电机的标准电流数据与机器人关节电机的实时电流数据进行比较，根据输出的比较结果判断所述机器人是否发生碰撞。

9.根据权利要求8所述的多关节机器人碰撞检测装置，其特征在于，所述碰撞检测模块进一步包括：

上限乘法器，用于将所述标准电流数据与预设的上限系数相乘，得到上限电流偏差值；

下限乘法器，用于将所述标准电流数据与预设的下限系数相乘，得到下限电流偏差值；

上限比较器、下限比较器，分别用于将所述上限电流偏差值、下限电流偏差值与所述实时电流数据比较，得到比较输出结果；

“或”门，用于将所述比较输出结果进行“或”运算，输出碰撞检测结果。

10.一种包含权利要求8～9任一项所述多关节机器人碰撞检测装置的机器人。

Images