CN108748144B - 一种人机协作机械臂的碰撞识别方法 - Google Patents

Abstract

本发明公开了一种人机协作机械臂的碰撞识别方法，该方法包括利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，并将关节力矩命令值发送至关节驱动器以控制机械臂执行期望轨迹规划动作；关节力矩命令值为关节力矩控制器在PD控制率作用下对期望关节力矩、期望关节转速和期望关节转角三个通道同时进行控制时所需要的力矩值；实时计算期望关节力矩与关节力矩命令值之间的关节力矩偏差值，并根据此差值计算碰撞能量值，且当碰撞能量值大于设定阈值时按照五阶多项式曲线规划机械臂的关节转角以控制机械臂执行避让动作。因此，采用本发明提供的碰撞识别方法，能够准确快速识别出发生在工业机械臂本体上任意位置处的碰撞。

Description

一种人机协作机械臂的碰撞识别方法

技术领域

本发明涉及机器人技术领域，特别涉及一种人机协作机械臂的碰撞识别方法。

背景技术

人机协作型机械臂是指能够与人在生产线上共享工作区域的机器人。人机协作机械臂除了具有一般工业机械臂的功能以外，还能够感知人与机械臂之间的物理接触，并对人机接触做出合适的响应，实现人与机械臂的协同作业，有助于扩大机械臂的应用范围，特别是应用于产品线更替周期短的应用场景中。由于人机协作工业机械臂与人共享工作区域，在机械臂与人之间可能发生碰撞。为了保障人的安全，机械臂必须对人与机械臂之间的碰撞作出快速、准确的识别。传统的工业机械臂的安全保障方法，是将机械臂与人的工作区域完全隔离，仅对机械臂末端的工具与工件之间的碰撞进行检测。这种碰撞检测方法，完全不具备识别发生在机械臂本体与人之间的碰撞，不能用于人机协作型机械臂。

目前公开了一种基于力矩差检测的碰撞识别方法，该方法是根据运动轨迹实时地预测出各个关节的理论力矩值，实时计算关节理论值与关节实际采样力矩值之间的差值，当力矩差值超出碰撞阈值时视为发生碰撞，但是该方法没有考虑机器人关节的摩擦力矩，对碰撞的检测敏感度很低，只能够检测出能量较大的碰撞，不能用于人机协作型机械臂。另外，还公开了一种利用弹性碰撞机构和导电机构识别机器人碰撞的方法，此方法只能够对发生在特定位置的碰撞进行识别。

发明内容

本发明的目的是提供一种人机协作机械臂的碰撞识别方法，能够准确快速识别出发生在工业机械臂本体上任意位置处的碰撞。

为实现上述目的，本发明提供了如下方案：

一种人机协作机械臂的碰撞识别方法，所述碰撞识别方法包括：

利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，并将所述关节力矩命令值发送至关节驱动器以控制机械臂执行期望轨迹规划动作；所述关节力矩命令值为所述关节力矩控制器在PD控制率作用下对期望关节力矩、期望关节转速和期望关节转角三个通道同时进行控制时所需要的力矩值；所述期望关节力矩为采用机械臂关节逆动力学模型计算机械臂沿期望轨迹运动时的力矩；所述机械臂关节逆动力学模型为结合惯性力、科氏力、重力、以及非线性摩擦力的力矩计算模型；所述非线性摩擦力包括静摩擦力、线性粘性摩擦力和非线性粘性摩擦力；

实时计算所述期望关节力矩与所述关节力矩命令值之间的关节力矩偏差值；

根据所述关节力矩偏差值计算碰撞能量值，并当所述碰撞能量值大于设定阈值时按照五阶多项式曲线规划机械臂的关节转角以控制机械臂执行避让动作；所述五阶多项式曲线为q(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵；其中，k_j,j＝0,...,5为关节轨迹规划参数；所述关节轨迹规划参数的值是根据虚拟反弹策略方程组计算得到；所述虚拟反弹策略方程组为

q⁺、

分别为机械臂在发生碰撞时的关节转角、关节转速和关节角加速度，T为机械臂执行避让动作所需要的时间。

可选的，所述碰撞识别方法还包括：

当所述碰撞能量值小于或者等于所述设定阈值且当所述碰撞能量值小于或者等于所述设定阈值的持续时间大于设定时间时，辨识非线性关节摩擦力模型参数，并根据辨识确定的所述非线性关节摩擦力模型参数对应的补偿方式对机械臂转动过程中的摩擦力进行补偿；所述非线性关节摩擦力模型参数包括：关节正转方向静摩擦力、关节反转方向静摩擦力、关节正转方向线性粘性摩擦系数、关节反转方向线性粘性摩擦系数、关节正转方向非线性粘性摩擦系数、关节反转方向非线性粘性摩擦系数。

可选的，所述辨识非线性关节摩擦力模型参数，并根据辨识确定的所述非线性关节摩擦力模型参数对应的补偿方式对机械臂转动过程中的摩擦力进行补偿，具体包括：，

当机械臂的关节转角位置从静止状态朝正转方向转动且关节转速小于关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节正转方向静摩擦力；所述关节正转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于正转关节力矩偏差阈值时，采用

对所述关节正转方向静摩擦力进行修正，并根据修正后的关节正转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿，其中，

修正前的值；

为

修正后的值；

为关节正转方向静摩擦力；

为所述关节力矩偏差值的平均值；

当机械臂的关节转角位置从静止状态朝反转方向转动且关节转速小于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节反转方向静摩擦力；所述关节反转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值，并计算所述关节力矩偏差值的绝对值的平均值，且所述关节力矩偏差值的绝对值的平均值大于反转关节力矩偏差阈值时，采用

对所述关节反转方向静摩擦力进行修正，并根据修正后的关节反转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿；其中

修正前的值；

修正后的值；

为关节反转方向静摩擦力；

为所述关节力矩偏差值的绝对值的平均值；

当机械臂的关节转角位置从静止状态朝正转方向转动且关节转速大于等于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节正转方向线性粘性摩擦系数和关节正转方向非线性粘性摩擦系数；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于所述正转关节力矩偏差阈值时，采用最小二乘方法计算所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数，并根据计算获取的所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数的计算公式为：

其中，

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数；

为关节转速；e_τ为所述关节力矩偏差值；^oldτ_f是由更新前的非线性关节摩擦力模型计算的克服机械臂非线性摩擦力的关节力矩分量；

为关节正转方向静摩擦力在修正前的值；

当机械臂的关节转角位置从静止状态朝反转方向转动且关节转速大于等于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节反转方向线性粘性摩擦系数和关节反转方向非线性粘性摩擦系数；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于所述反转关节力矩偏差阈值时，采用最小二乘方法计算所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数，并根据计算获取的所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数的计算公式为：

其中，

和

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数；

为关节反转方向静摩擦力在修正前的值。

可选的，所述机械臂关节逆动力学模型为：τ_d＝τ_I+τ_C+τ_G+τ_f；其中，τ_d为期望关节力矩；

为克服机械臂惯性力作用的关节力矩分量；M表示机械臂的质量矩阵；

是关节角加速度；

为克服机械臂科氏力作用的关节力矩分量；q为关节转角，由关节位置编码器测量得到；

为关节转速，由关节速度编码器测量得到；C表示机械臂的科氏力矩阵；τ_G为克服机械臂重力作用的关节力矩分量；τ_f为克服机械臂非线性摩擦力作用的关节力矩分量；τ_f是采用非线性关节摩擦力模型计算获取的。

可选的，所述非线性关节摩擦力模型为：

其中，

为关节正转方向静摩擦力；

为关节反转方向静摩擦力；

为关节正转方向线性粘性摩擦系数；

为关节反转方向线性粘性摩擦系数；

为关节正转方向非线性粘性摩擦系数；

为关节反转方向非线性粘性摩擦系数。

可选的，所述利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，具体包括：

依据PD控制率，按照下式计算关节力矩命令值；所述关节力矩命令值的计算公式为：

其中，e_q＝q-q_d为关节转角偏差反馈；q_d为关节转角命令值；

为关节转速偏差反馈；

为关节转速命令值；K_p为关节转角偏差控制增益；K_D为关节转角偏差微分控制增益。

可选的，所述根据所述关节力矩偏差值计算碰撞能量值，具体包括：

根据以下公式计算碰撞能量值；所述公式为：

其中，p_col为碰撞能量值；

为第i个关节的关节力矩偏差反馈；

为第i个关节的关节转速偏差反馈；i为关节标号。

可选的，在利用基于PD控制的通用关节力矩控制器计算关节力矩命令值之前，所述碰撞识别方法还包括：

获取机械臂启动信号，并在获取所述机械臂启动信号之后输出机械臂执行期望轨迹规划动作信号。

可选的，所述碰撞识别方法还包括：

当获取到机械臂执行完毕规划的避让动作信号后输出机械臂停机报警信号。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供了一种人机协作机械臂的碰撞识别方法，该方法包括利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，并将所述关节力矩命令值发送至关节驱动器以控制机械臂执行期望轨迹规划动作；所述关节力矩命令值为所述关节力矩控制器在PD控制率作用下对期望关节力矩、期望关节转速和期望关节转角三个通道同时进行控制时所需要的力矩值；实时计算所述期望关节力矩与所述关节力矩命令值之间的关节力矩偏差值，并根据所述关节力矩偏差值计算碰撞能量值，且当所述碰撞能量值大于设定阈值时按照五阶多项式曲线规划机械臂的关节转角以控制机械臂执行避让动作。因此，采用本发明提供的碰撞识别方法，能够准确快速识别出发生在工业机械臂本体上任意位置处的碰撞。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例人机协作机械臂碰撞识别方法的流程示意图；

图2为本发明实施例人机协作机械臂碰撞识别方法的控制器框架图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种人机协作机械臂的碰撞识别方法，能够准确快速识别出发生在工业机械臂本体上任意位置处的碰撞。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明实施例人机协作机械臂碰撞识别方法的流程示意图，图2为本发明实施例人机协作机械臂碰撞识别方法的控制器框架图。

参见图1和图2，本发明实施例提供的一种人机协作机械臂的碰撞识别方法，具体包括以下几个步骤：

步骤101：获取机械臂启动信号，并在获取所述机械臂启动信号之后输出机械臂执行期望轨迹规划动作信号。

步骤102：利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，并将所述关节力矩命令值发送至关节驱动器以控制机械臂执行期望轨迹规划动作；所述关节力矩命令值为所述关节力矩控制器在PD控制率作用下对期望关节力矩、期望关节转速和期望关节转角三个通道同时进行控制时所需要的力矩值；所述期望关节力矩为采用机械臂关节逆动力学模型计算机械臂沿期望轨迹运动时的力矩；所述机械臂关节逆动力学模型为结合惯性力、科氏力、重力、以及非线性摩擦力的力矩计算模型；所述非线性摩擦力包括静摩擦力、线性粘性摩擦力和非线性粘性摩擦力。

所述机械臂关节逆动力学模型为：τ_d＝τ_I+τ_C+τ_G+τ_f(1)；其中，τ_d为期望关节力矩；

为克服机械臂惯性力作用的关节力矩分量；M表示机械臂的质量矩阵；

是关节角加速度；

为克服机械臂科氏力作用的关节力矩分量；q为关节转角，由关节位置编码器测量得到；

为关节转速，由关节速度编码器测量得到；C表示机械臂的科氏力矩阵；τ_G为克服机械臂重力作用的关节力矩分量；τ_f为克服机械臂非线性摩擦力作用的关节力矩分量；τ_f是采用非线性关节摩擦力模型计算获取的。

非线性关节摩擦力模型为：

其中，

为关节正转方向静摩擦力；

为关节反转方向静摩擦力；

为关节正转方向线性粘性摩擦系数；

为关节反转方向线性粘性摩擦系数；

为关节正转方向非线性粘性摩擦系数；

为关节反转方向非线性粘性摩擦系数。

依据PD控制率，按照公式(3)计算关节力矩命令值；所述关节力矩命令值的计算公式为：

其中，e_q＝q-q_d为关节转角偏差反馈；q_d为关节转角命令值；

为关节转速偏差反馈；

为关节转速命令值；K_p为关节转角偏差控制增益；K_D为关节转角偏差微分控制增益。

步骤103：实时计算所述期望关节力矩与所述关节力矩命令值之间的关节力矩偏差值。此关节力矩偏差值对机械臂外力解算与碰撞识别。

步骤104：根据所述关节力矩偏差值计算碰撞能量值，并当所述碰撞能量值大于设定阈值时按照五阶多项式曲线规划机械臂的关节转角以控制机械臂执行避让动作；即当识别出碰撞发生时，按照虚拟反弹策略规划机械臂的避让动作，采用基于关节转角伺服控制的方法实现，避免在位置伺服和力矩伺服控制模式之间切换，避免引起机械臂震颤。

所述碰撞能量值的计算公式为：

其中，p_col为碰撞能量值；

为第i个关节的关节力矩偏差反馈；

为第i个关节的关节转速偏差反馈；i为关节标号。

所述五阶多项式曲线为q(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵(5)；其中，k_j,j＝0,...,5为关节轨迹规划参数；所述关节轨迹规划参数的值是根据虚拟反弹策略方程组计算得到；所述虚拟反弹策略方程组为：

q⁺、

分别为机械臂在发生碰撞时的关节转角、关节转速和关节角加速度，T为机械臂执行避让动作所需要的时间。

步骤105：当获取到机械臂执行完毕规划的避让动作信号后输出机械臂停机报警信号。机械臂发出报警，提示操作人员处理碰撞事件。

为了保证机械臂在温度、负载变化的情况下，始终保持良好的碰撞识别能力，所述碰撞识别方法还包括：

步骤106：当所述碰撞能量值小于或者等于所述设定阈值且当所述碰撞能量值小于或者等于所述设定阈值的持续时间大于设定时间时，辨识非线性关节摩擦力模型参数，并根据辨识确定的所述非线性关节摩擦力模型参数对应的补偿方式对机械臂转动过程中的摩擦力进行补偿。所述非线性关节摩擦力模型参数包括：关节正转方向静摩擦力、关节反转方向静摩擦力、关节正转方向线性粘性摩擦系数、关节反转方向线性粘性摩擦系数、关节正转方向非线性粘性摩擦系数、关节反转方向非线性粘性摩擦系数。辨识时所依据的机械臂运动状态参数包括：期望关节力矩τ_d和关节力矩命令值τ_in之间的关节力矩偏差值

关节转角q、关节转速

关节转角偏差反馈e_q、关节转速偏差反馈

即长时间未检测到碰撞时，利用最近一段时间内的机械臂运动状态辨识非线性关节摩擦力模型参数，并根据静摩擦力、线性粘性摩擦力、非线性粘性摩擦力不同的特征以及关节正转和关节反转的摩擦力特征不同，分别对关节正转方向静摩擦力、关节反转方向静摩擦力、关节正转方向粘性摩擦系数、关节反转方向粘性摩擦系数、关节正转方向非线性粘性摩擦系数、关节反转方向非线性粘性摩擦系数进行辨识。另外，考虑到机械臂关节温度对摩擦力模型具有显著影响，按照关节温度从低到高分为若干个等级分别进行保存。

将机械臂工作环境的温度范围划分为若干个等级，对每个等级内的参数均采用如下的辨识方法；所述辨识方法为：

当机械臂的关节转角位置q₀从静止状态朝正转方向转动且关节转速

小于关节转速阈值

时，确定所述非线性关节摩擦力模型参数为关节正转方向静摩擦力；所述关节正转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值e_τ，并计算所述关节力矩偏差值e_τ的平均值

且当所述关节力矩偏差值的平均值

大于正转关节力矩偏差阈值

时，采用

对所述关节正转方向静摩擦力进行修正，并根据修正后的关节正转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿；其中

修正前的值；

修正后的值；

为所述关节力矩偏差值的平均值。

当机械臂的关节转角位置q₀从静止状态朝反转方向转动且关节转速

小于关节转速阈值

时，确定所述非线性关节摩擦力模型参数为关节反转方向静摩擦力；所述关节反转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值e_τ，并计算所述关节力矩偏差值e_τ的绝对值的平均值

且所述关节力矩偏差值的绝对值的平均值

大于反转关节力矩偏差阈值

时，采用

对所述关节反转方向静摩擦力进行修正，并根据修正后的关节反转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿；其中

为

修正前的值；

为

修正后的值；

为所述关节力矩偏差值的绝对值的平均值。

当机械臂的关节转角位置q₀从静止状态朝正转方向转动且关节转速

大于关节转速阈值

时，确定所述非线性关节摩擦力模型参数为关节正转方向线性粘性摩擦系数和关节正转方向非线性粘性摩擦系数；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值e_τ，并计算所述关节力矩偏差值的平均值

且当所述关节力矩偏差值的平均值

大于所述正转关节力矩偏差阈值

时，采用最小二乘方法计算所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数，并根据计算获取的所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数的计算公式为：

其中，

和

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数，^oldτ_f是由更新前的非线性关节摩擦力模型计算的克服机械臂非线性摩擦力的关节力矩分量；

为关节正转方向静摩擦力在修正前的值。

当机械臂的关节转角位置q₀从静止状态朝反转方向转动且关节转速

大于关节转速阈值

时，确定所述非线性关节摩擦力模型参数为关节反转方向线性粘性摩擦系数和关节反转方向非线性粘性摩擦系数；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值e_τ，并计算所述关节力矩偏差值e_τ的平均值

且当所述关节力矩偏差值的平均值大于所述反转关节力矩偏差阈值

时，采用最小二乘方法计算所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数，并根据计算获取的所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数的计算公式为：

其中，

和

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数；

为关节反转方向静摩擦力在修正前的值。

上述辨识方法能够实现对非线性关节摩擦力模型参数变化的自适应控制，可以避免机械臂本体、环境温度变化导致的摩擦力欠补偿或者过补偿问题。

与现有技术相比，本发明实现的有益效果为：

(1)采用本发明提供的碰撞识别方法，能够准确快速的识别在机械臂本体任意位置处的碰撞，且无需配置力矩传感器，成本低，可靠性高。

(2)采用本发明提供的的摩擦力在线辨识方法，能够自动辨识机械臂非线性关节摩擦力模型参数，保证机械臂在温度、负载变化的情况下，始终具有良好的碰撞识别能力。

(3)采用关节摩擦力模型能够描述关节的静摩擦力、粘性摩擦力的一阶和二阶项，并根据关节温度的不同进行等级划分和分别存储，有利于在机械臂温度变化、关节磨损等情况发生时，仍然能够保持足够高的碰撞识别灵敏度，有利于识别低能量的碰撞，充分保障人机协作机械臂不会对操作人员构成伤害。

(4)本发明提供的碰撞识别方法是一种通用的碰撞识别方法，能够应用于各种类型机械臂，包括六轴关节机械臂、四轴关节机械臂、SCARA机械臂、直角坐标机械本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种人机协作机械臂的碰撞识别方法，其特征在于，所述碰撞识别方法包括：

利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，并将所述关节力矩命令值发送至关节驱动器以控制机械臂执行期望轨迹规划动作；所述关节力矩命令值为所述关节力矩控制器在PD控制率作用下对期望关节力矩、期望关节转速和期望关节转角三个通道同时进行控制时所需要的力矩值；所述期望关节力矩为采用机械臂关节逆动力学模型计算机械臂沿期望轨迹运动时的力矩；所述机械臂关节逆动力学模型为结合惯性力、科氏力、重力、以及非线性摩擦力的力矩计算模型；所述非线性摩擦力包括静摩擦力、线性粘性摩擦力和非线性粘性摩擦力；

实时计算所述期望关节力矩与所述关节力矩命令值之间的关节力矩偏差值；

根据所述关节力矩偏差值计算碰撞能量值，并当所述碰撞能量值大于设定阈值时按照五阶多项式曲线规划机械臂的关节转角以控制机械臂执行避让动作；所述五阶多项式曲线为q(t)＝k₀+k₁t+k₂t²+k₃t³+k₄t⁴+k₅t⁵；其中，k_j,j＝0,...,5为关节轨迹规划参数；所述关节轨迹规划参数的值是根据虚拟反弹策略方程组计算得到；所述虚拟反弹策略方程组为

q(0)为机械臂在发生碰撞时的关节转角，

为机械臂在发生碰撞时的关节转速，

为机械臂在发生碰撞时的关节角加速度，q⁺、

分别为机械臂在发生碰撞时的关节转角、关节转速和关节角加速度，T为机械臂执行避让动作所需要的时间，q(T)为机械臂执行避让动作时的关节转角，

为机械臂执行避让动作时的关节转速，

为机械臂执行避让动作时的关节角加速度。

2.根据权利要求1所述的碰撞识别方法，其特征在于，所述碰撞识别方法还包括：

当所述碰撞能量值小于或者等于所述设定阈值且当所述碰撞能量值小于或者等于所述设定阈值的持续时间大于设定时间时，辨识非线性关节摩擦力模型参数，并根据辨识确定的所述非线性关节摩擦力模型参数对应的补偿方式对机械臂转动过程中的摩擦力进行补偿；所述非线性关节摩擦力模型参数包括：关节正转方向静摩擦力、关节反转方向静摩擦力、关节正转方向线性粘性摩擦系数、关节反转方向线性粘性摩擦系数、关节正转方向非线性粘性摩擦系数、关节反转方向非线性粘性摩擦系数。

3.根据权利要求2所述的碰撞识别方法，其特征在于，所述辨识非线性关节摩擦力模型参数，并根据辨识确定的所述非线性关节摩擦力模型参数对应的补偿方式对机械臂转动过程中的摩擦力进行补偿，具体包括：

当机械臂的关节转角位置从静止状态朝正转方向转动且关节转速小于关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节正转方向静摩擦力；所述关节正转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于正转关节力矩偏差阈值时，采用

对所述关节正转方向静摩擦力进行修正，并根据修正后的关节正转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿，其中，

为

修正前的值；

为

修正后的值；

为关节正转方向静摩擦力；

为所述关节力矩偏差值的平均值；

当机械臂的关节转角位置从静止状态朝反转方向转动且关节转速小于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节反转方向静摩擦力；所述关节反转方向静摩擦力对应的补偿方式为采集所述关节力矩偏差值，并计算所述关节力矩偏差值的绝对值的平均值，且所述关节力矩偏差值的绝对值的平均值大于反转关节力矩偏差阈值时，采用

对所述关节反转方向静摩擦力进行修正，并根据修正后的关节反转方向静摩擦力对机械臂转动过程中的摩擦力进行补偿；其中

为

修正前的值；

为

修正后的值；

为关节反转方向静摩擦力；

为所述关节力矩偏差值的绝对值的平均值；

当机械臂的关节转角位置从静止状态朝正转方向转动且关节转速大于等于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节正转方向线性粘性摩擦系数和关节正转方向非线性粘性摩擦系数；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于所述正转关节力矩偏差阈值时，采用最小二乘方法计算所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数，并根据计算获取的所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节正转方向线性粘性摩擦系数和所述关节正转方向非线性粘性摩擦系数的计算公式为：

其中，^newf₁ ⁺和

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数；

为关节转速；e_τ为所述关节力矩偏差值；^oldτ_f是由更新前的非线性关节摩擦力模型计算的克服机械臂非线性摩擦力的关节力矩分量；

为关节正转方向静摩擦力在修正前的值；

当机械臂的关节转角位置从静止状态朝反转方向转动且关节转速大于等于所述关节转速阈值时，确定所述非线性关节摩擦力模型参数为关节反转方向线性粘性摩擦系数和关节反转方向非线性粘性摩擦系数；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对应的补偿方式为：采集所述关节力矩偏差值，并计算所述关节力矩偏差值的平均值，且当所述关节力矩偏差值的平均值大于所述反转关节力矩偏差阈值时，采用最小二乘方法计算所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数，并根据计算获取的所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数对机械臂转动过程中的摩擦力进行补偿；所述关节反转方向线性粘性摩擦系数和所述关节反转方向非线性粘性摩擦系数的计算公式为：

其中，^newf₁ ^-和

是更新后的关节正转方向线性粘性摩擦系数和更新后的关节正转方向非线性粘性摩擦系数；

为关节反转方向静摩擦力在修正前的值。

4.根据权利要求1所述的碰撞识别方法，其特征在于，所述机械臂关节逆动力学模型为：τ_d＝τ_I+τ_C+τ_G+τ_f；其中，τ_d为期望关节力矩；

为克服机械臂惯性力作用的关节力矩分量；M表示机械臂的质量矩阵；

是关节角加速度；

为克服机械臂科氏力作用的关节力矩分量；q为关节转角，由关节位置编码器测量得到；

为关节转速，由关节速度编码器测量得到；C表示机械臂的科氏力矩阵；τ_G为克服机械臂重力作用的关节力矩分量；τ_f为克服机械臂非线性摩擦力作用的关节力矩分量；τ_f是采用非线性关节摩擦力模型计算获取的。

5.根据权利要求4所述的碰撞识别方法，其特征在于，所述非线性关节摩擦力模型为：

其中，

为关节正转方向静摩擦力；

为关节反转方向静摩擦力；f₁ ⁺为关节正转方向线性粘性摩擦系数；f₁ ^-为关节反转方向线性粘性摩擦系数；

为关节正转方向非线性粘性摩擦系数；

为关节反转方向非线性粘性摩擦系数。

6.根据权利要求5所述的碰撞识别方法，其特征在于，所述利用基于PD控制的通用关节力矩控制器计算关节力矩命令值，具体包括：

依据PD控制率，按照下式计算关节力矩命令值；所述关节力矩命令值的计算公式为：

其中，e_q＝q-q_d为关节转角偏差反馈；q_d为关节转角命令值；

为关节转速偏差反馈；

为关节转速命令值；K_p为关节转角偏差控制增益；K_D为关节转角偏差微分控制增益。

7.根据权利要求6所述的碰撞识别方法，其特征在于，所述根据所述关节力矩偏差值计算碰撞能量值，具体包括：

根据以下公式计算碰撞能量值；所述公式为：

其中，p_col为碰撞能量值；

为第i个关节的关节力矩偏差反馈；

为第i个关节的关节转速偏差反馈；i为关节标号。

8.根据权利要求1所述的碰撞识别方法，其特征在于，在利用基于PD控制的通用关节力矩控制器计算关节力矩命令值之前，所述碰撞识别方法还包括：

获取机械臂启动信号，并在获取所述机械臂启动信号之后输出机械臂执行期望轨迹规划动作信号。

9.根据权利要求1所述的碰撞识别方法，其特征在于，所述碰撞识别方法还包括：

当获取到机械臂执行完毕规划的避让动作信号后输出机械臂停机报警信号。

Images