CN110340885B - 一种基于能量偏差观测器的工业机器人碰撞检测方法 - Google Patents

Abstract

本发明提出一种基于能量偏差观测器的工业机器人碰撞检测方法，计算工业机器人运行过程中各关节的总能量，进而设定各关节的碰撞检测算子和调整函数，得到能量偏差观测器，再通过能量偏差观测器求得各关节的碰撞扭矩和碰撞力，通过比较碰撞扭矩与扭矩阈值的大小、或比较碰撞力与碰撞力阈值的大小来判断工业机器人是否发生碰撞。本发明的方法无需添加额外的传感器，就能实现实时检测工业机器人与外部的碰撞并驱动工业机器人做出快速反应，避免了可能造成的损害，同时通过前馈补偿来提高***对碰撞力检测的快速性和准确性。

Description

一种基于能量偏差观测器的工业机器人碰撞检测方法

技术领域

本发明属于工业机器人技术领域，尤其是一种基于能量偏差观测器的工业机器人碰撞检测方法。

背景技术

工业机器人是面向工业领域的多自由度机械手或机器装置。工业机器人在焊接、磨抛、喷涂、搬运、装配等工业生产中的应用非常广泛。近些年来，随着工业机器人的应用范围越来越广，尤其是多机器人协作和人机协作的应用越来越多，人们对机器人安全性的要求越来越高。最初研制的工业机器人都是按预先编制好的运动轨迹进行工作，为了防止机器人在生产过程中与外界环境发生碰撞，往往配备防护栏，用于隔离机器人使其在密闭的空间内工作。然而随着机器人应用范围越来越广，尤其是多机器人协作、人机协作应用的增多，很多生产任务要求机器人与工作人员或其他机器人协作完成。因而，如何在开放的环境内保证机器人生产过程中的安全性，成为至关重要的问题。

为保证安全，控制***需要实时检测机器人与外界环境之间的接触力，当发生碰撞时，控制***需要快速地检测到碰撞，并通过相应的控制策略保证做出及时、合理的反应，不至于造成严重的损害。当前，通常采用添加外部传感器的方法来检测机器人是否与外界环境发生碰撞。比如：(1)采用视觉检测；(2)安装腕力传感器来检测碰撞；(3)安装关节扭矩传感器来检测碰撞；(4)安装感知皮肤来检测碰撞。

对于安装传感器检测机器人与外界环境碰撞的方法，都无一例外地增加了控制***的复杂性，提高了机器人的成本，且有些传感器的安装必须在机器人的设计之初就要考虑进去。

发明内容

本发明所解决的技术问题在于提供一种基于能量偏差观测器的工业机器人碰撞检测方法，实时检测外部碰撞并做出快速反应，实现了控制***不需要额外添加传感器就能检测出机器人与外界环境发生的碰撞，以避免可能造成的损害，并通过前馈补偿来提高***对碰撞力检测的快速性和准确性。

实现本发明目的的技术解决方案为：

一种基于能量偏差观测器的工业机器人碰撞检测方法，包括以下步骤：

步骤1：计算工业机器人运行过程中的关节i的总能量E_i；

步骤2：根据工业机器人关节i的总能量E_i设定关节i处的碰撞检测算子σ_Ei：

其中，K_E1＞0为增益，K_E2＞0为增益，τ_toli为碰撞发生后机器人关节i处电机输出的输出力矩，τ_ei为关节i处的碰撞扭矩，

为关节i的转动角度的偏导数；

设定调整函数u_e作为前馈调节，用于减小

在工业机器人与外界环境刚发生碰撞时的震荡；

步骤3：根据碰撞检测算子和调整函数，得到能量偏差观测器σ_Ei：

其中，K_E3＞0为增益；

步骤4：根据能量偏差观测器求得关节i处的碰撞扭矩τ_ei：

且τ_e∈R^6×1，

进而求得工业机器人在笛卡尔空间内的碰撞力F_e：

且F_e∈R^6×1；

步骤5：设置碰撞力阈值F_thon＞0，且F_thon∈R^6×1，设置碰撞扭矩阈值τ_thon＞0，且τ_thon∈R^6×1；

当实时计算得到的|F_e|＞F_thon时，则认为工业机器人发生了碰撞；若碰撞发生在工业机器人奇异位置，当实时计算得到的|τ_e|＞τ_thon时，则认为工业机器人发生了碰撞，否则认为工业机器人未发生碰撞。

进一步的，本发明的基于能量偏差观测器的工业机器人碰撞检测方法，步骤1中计算工业机器人运行过程中的关节i的总能量E_i具体步骤包括：

步骤1-1：计算工业机器人运行过程中的关节i的动能E_ki；

步骤1-2：计算工业机器人运行过程中的关节i的势能E_pi；

步骤1-3：关节i的总能量E_i为：E_i＝E_ki+E_pi。

进一步的，本发明的基于能量偏差观测器的工业机器人碰撞检测方法，步骤1-1中计算关节i的动能E_ki具体为：

1)关节i上的一点在基坐标系中的坐标为：

其中，

ⁱr为该点在坐标系{i}中的坐标，

为坐标系{i}与基坐标系的坐标变化矩阵；

2)计算得到该点的速度为：

其中，

点r在坐标系{i}中的速度；

3)对速度平方并求迹得：

4)关节i的ⁱr处、质量为dm的质点动能为：

5)关节i的动能为：

其中，J_i为关节i的伪惯性矩阵。

进一步的，本发明的基于能量偏差观测器的工业机器人碰撞检测方法，步骤1-2中计算关节i的势能E_pi具体为：

其中，m_i为关节i的总质量；g为重力加速度。

进一步的，本发明的基于能量偏差观测器的工业机器人碰撞检测方法，步骤2中调整函数u_e具体为：

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明的基于能量偏差观测器的工业机器人碰撞检测方法无需添加额外的传感器，就能实现实时检测工业机器人与外部的碰撞并驱动工业机器人做出快速反应，降低了工业机器人控制***的复杂性，降低了机器人的成本；

2、本发明的基于能量偏差观测器的工业机器人碰撞检测方法能够在开放的环境内有效保证机器人在运行过程中的安全性；

3、本发明的基于能量偏差观测器的工业机器人碰撞检测方法适用于各个阶段的工业机器人，无需在机器人设计之初就加入传感器的设计，已被设计出来的机器人也能使用本碰撞检测方法。

附图说明

图1是本发明的工业机器人坐标示意图；

图2是本发明的基于能量偏差观测器的碰撞检测控制框图；

图3是本发明的主动碰撞检测实验示意图；

图4是本发明的主动碰撞检测实验流程图；

图5是本发明的实际碰撞力与检测碰撞力曲线。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

一种基于能量偏差观测器的工业机器人碰撞检测方法，包括以下步骤：

步骤1：计算工业机器人运行过程中的关节i的总能量E_i，具体包括：

步骤1-1：计算工业机器人运行过程中的关节i的动能E_ki：

1)假定机器人关节i上的一点对坐标系{i}中的齐次坐标为ⁱr，对基坐标系{0}的齐次坐标为⁰r，则由图1可知关节i上的一点在基坐标系中的坐标为：

其中，

ⁱr为该点在坐标系{i}中的坐标，

为坐标系{i}与基坐标系的坐标变化矩阵；

2)计算得到该点的速度为：

其中，

点r在坐标系{i}中的速度；

3)对速度平方并求迹得：

4)关节i的ⁱr处、质量为dm的质点动能为：

5)关节i的动能为：

其中，J_i为关节i的伪惯性矩阵。

步骤1-2：计算工业机器人运行过程中的关节i的势能E_pi：

其中，m_i为关节i的总质量；g为重力加速度。

步骤1-3：关节i的总能量E_i为：

步骤2：根据工业机器人关节i的总能量E_i设定关节i处的碰撞检测算子σ_Ei：

其中，K_E1＞0为增益，K_E2＞0为增益，τ_toli为碰撞发生后机器人关节i处电机输出的输出力矩，τ_ei为关节i处的碰撞扭矩，

为关节i的转动角度的偏导数；

对上式求导展开得：

再进行拉普拉斯变化可得传递函数：

由上式可知，σ_Ei相当于对

引入了一个二阶***，观测值σ_Ei能够跟随

变化，在稳定后

将K_E1，K_E2调整为合适的增益值，就能抑制

的震荡影响。但是，仅采用碰撞检测算子σ_Ei来检测碰撞力，会产生较大的延迟，影响检测的快速性，因此设定调整函数u_e作为前馈调节，用于减小

在工业机器人与外界环境刚发生碰撞时的震荡：

步骤3：根据碰撞检测算子和调整函数，得到能量偏差观测器σ_Ei：

其中，K_E3＞0为增益；

对上式进行拉普拉斯变化可得传递函数：

为了使***具有快速性，又能减小

在机器人与外界环境发生碰撞刚开始时候的震荡，在控制***中加入调整函数u_e，调整函数u_e的作用是对二阶***进行前馈调节，改善***的性能。调整函数u_e相当于一个PD调节器，能够减小二阶***的超调量，提高快速性，被调节量的动态偏差较小，稳定偏差也不大。适当选取增益值可以使碰撞检测算法达到快速性和准确性要求。

基于能量偏差观测器的碰撞检测控制框图如图2所示，其输入为机器人各关节电机的实际输出扭矩与各关节的位置与速度信息，输出为各关节受到的碰撞扭矩与关节速度的乘积。由此可知，基于能量偏差观测器的碰撞检测相当于对

引入了一个二阶***，并在***的前向通道内加入了一个PD调节器。碰撞检测的观测值σ_Ei将跟随

变化，在稳态时

将K_E1，K_E2，K_E3调整为合适的增益值，不仅能抑制

的震荡，而且可以有效提高对碰撞检测的快速性。

步骤4：根据能量偏差观测器求得关节i处的碰撞扭矩τ_ei：

且τ_e∈R^6×1，

进而求得工业机器人在笛卡尔空间内的碰撞力F_e：

且F_e∈R^6×1；

步骤5：设置碰撞力阈值F_thon＞0，且F_thon∈R^6×1，设置碰撞扭矩阈值τ_thon＞0，且τ_thon∈R^6×1；

当实时计算得到的|F_e|＞F_thon时，则认为工业机器人发生了碰撞；若碰撞发生在工业机器人奇异位置，当实时计算得到的|τ_e|＞τ_thon时，则认为工业机器人发生了碰撞，否则认为工业机器人未发生碰撞。

实施例1

S1.本实施例以埃斯顿机器人工程有限公司的ER30型号工业机器人，进行详细说明。首先结合ER30工业机器人具体DH参数，计算机器人各关节运动时的能量：

ER30工业机器人的DH参数如下表：

S2.设计能量偏差观测器。

首先，定义ER30工业机器人关节i处的碰撞检测算子σ_Ei：

式中：τ_ei为碰撞在关节i处的引起的碰撞扭矩；σ_Ei为

的观测值；K_E1＞0，K_E2＞0为增益；τ_tol为碰撞发生后机器人关节i处电机输出的输出力矩。

其次，设计ER30工业机器人的碰撞检测算法调整函数为：

将调整函数作为ER30工业机器人碰撞检测算法的前馈调节，可得如下能量偏差观测器：

S3.求解ER30工业机器人的笛卡尔空间碰撞力。

碰撞力、力矩矢量F_e∈R^6×1：

当ER30工业机器人在奇异点发生碰撞时，由于

不是一个满秩矩阵，无法直接求得碰撞力F_e。这时，改判断

是否大于碰撞扭矩的临界值，来判断机器人是否发生碰撞。

S4.设置碰撞力阈值、碰撞扭矩阈值，检测碰撞。

设置一碰撞力阈值F_thon＞0，(F_thon∈R^6×1)，当|F_e|＞F_thon时，则可认为机器人发生了碰撞。当碰撞发生在机器人奇异位置时，设置一碰撞扭矩阈值τ_thon＞0，(τ_thon∈R^6×1)，当|τ_e|＞τ_thon时，则可认为机器人发生了碰撞。

如图3所示，让机器人以1m/s的末端速度在笛卡尔空间内沿y轴方向运动。在机器人运动的轨迹中放置一用于碰撞的物体。末端力传感器虽然可以直接检测到碰撞力，但仅仅只是用来对比由碰撞检测算法检测的碰撞力。

主动碰撞检测实验的实验流程如图4所示，包括：

(1)在机器人运动的轨迹中放置一用于碰撞的物体；

(2)操作机器人向y轴方向运动；

(3)机器人与物体发生碰撞；

(4)当上位机控制***检测到的碰撞力大于阈值时，视为发生碰撞，停止机器人的运动；

(5)采集处理实验过程中的碰撞力信号和传感器力信号，做对比分析。

碰撞检测实验结果如下：

由于碰撞发生在y轴方向，所以六维力传感器反馈的力信号和控制***检测的碰撞力都为y轴方向的分量。从图5中可以看出，在t≈1.3s时力传感器反馈的F_y有突变，说明机器人与物体发生了碰撞。在大约0.05s的延时后，碰撞检测算法检测到的碰撞力迅速上升，当其超过碰撞阈值时，控制***控制机器人停止运动。由于机器人的急停，机器人本体产生了一定震荡，使得碰撞力在1.5～1.7s期间有一定的波动。在机器人稳定后，碰撞部位与被撞物体的接触力维持在200N左右。

分析可得：通过碰撞检测算法检测的碰撞力约有0.05s的延时，满足主动碰撞检测的快速性要求。当机器人停止运行，并趋于稳定后，通过碰撞检测算法解算的碰撞力的稳态误差约为10N，满足主动碰撞检测的准确性要求。图中的碰撞阈值还有较大余量，可进一步减小，以减小碰撞力，缩短检测时间。由此可以说明该碰撞检测算法是有效的，使用该碰撞检测算法能够较好地保证安全性。

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

Claims (5)

1.一种基于能量偏差观测器的工业机器人碰撞检测方法，其特征在于，包括以下步骤：

步骤1：计算工业机器人运行过程中的关节i的总能量E_i；

步骤2：根据工业机器人关节i的总能量E_i设定关节i处的碰撞检测算子σ_Ei：

其中，K_E1＞0为增益，K_E2＞0为增益，τ_toli为碰撞发生后机器人关节i处电机输出的输出力矩，τ_ei为关节i处的碰撞扭矩，

为关节i的转动角度的偏导数；

设定调整函数u_e作为前馈调节，用于减小

在工业机器人与外界环境刚发生碰撞时的震荡；

步骤3：根据碰撞检测算子和调整函数，得到能量偏差观测器σ_Ei：

其中，K_E3＞0为增益；

步骤4：根据能量偏差观测器求得关节i处的碰撞扭矩τ_ei：

且τ_e∈R^6×1，

进而求得工业机器人在笛卡尔空间内的碰撞力F_e：

且F_e∈R^6×1；

步骤5：设置碰撞力阈值F_thon＞0，且F_thon∈R^6×1，设置碰撞扭矩阈值τ_thon＞0，且τ_thon∈R^{6 ×1}；

当实时计算得到的|F_e|＞F_thon时，则认为工业机器人发生了碰撞；若碰撞发生在工业机器人奇异位置，当实时计算得到的|τ_e|＞τ_thon时，则认为工业机器人发生了碰撞，否则认为工业机器人未发生碰撞。

2.根据权利要求1所述的基于能量偏差观测器的工业机器人碰撞检测方法，其特征在于，步骤1中计算工业机器人运行过程中的关节i的总能量E_i具体步骤包括：

步骤1-1：计算工业机器人运行过程中的关节i的动能E_ki；

步骤1-2：计算工业机器人运行过程中的关节i的势能E_pi；

步骤1-3：关节i的总能量E_i为：E_i＝E_ki+E_pi。

3.根据权利要求2所述的基于能量偏差观测器的工业机器人碰撞检测方法，其特征在于，步骤1-1中计算关节i的动能E_ki具体为：

1)关节i上的一点在基坐标系中的坐标为：

其中，

ⁱr为该点在坐标系{i}中的坐标，

为坐标系{i}与基坐标系的坐标变化矩阵；

2)计算得到该点的速度为：

其中，

点r在坐标系{i}中的速度；

3)对速度平方并求迹得：

4)关节i的ⁱr处、质量为dm的质点动能为：

5)关节i的动能为：

其中，J_i为关节i的伪惯性矩阵。

4.根据权利要求2所述的基于能量偏差观测器的工业机器人碰撞检测方法，其特征在于，步骤1-2中计算关节i的势能E_pi具体为：

其中，m_i为关节i的总质量；g为重力加速度。

5.根据权利要求1所述的基于能量偏差观测器的工业机器人碰撞检测方法，其特征在于，步骤2中调整函数u_e具体为：

Images