CN108058188B - 机器人健康监测和故障诊断***的控制方法 - Google Patents

Abstract

本发明属于机器人技术领域，涉及一种机器人健康监测和故障诊断***的控制方法，***包括位于运动关节处的数据获取部，用于判断机器人当前运行状态的运行状态监测部，用于判断机器人当前健康状态的健康状态监测部，根据当前机器人运行状态和健康状态做出对应处理的状态处理部，记录机器人状态参数的状态记录部。本发明通过建立机器人关节转矩预测模型和机器人机械参数辨识模型，实时判断机器人的运行状态和健康状态，并对于各种异常状态进行及时处理，可以有效地在早期发现机器人可能会发生的机械故障，减少经济损失。

Description

机器人健康监测和故障诊断***的控制方法

技术领域

本发明涉及机器人技术领域，特别涉及一种机器人健康监测和故障诊断***的控制方法。

背景技术

机器人已广泛应用于自动化生产各个领域，目前而言，缺乏有效的手段监控机器人的健康状态。机器人突然损坏有可能会使整个生产线停滞，造成重大经济损失。因此对机器人的健康监控和故障诊断对于避免损失有很重大的意义。

因此，有必要提供一种新的控制方法来解决上述问题。

发明内容

本发明的主要目的在于提供一种机器人健康监测和故障诊断***的控制方法。

本发明通过如下技术方案实现上述目的：一种机器人健康监测和故障诊断***的控制方法，

包括如下步骤：

S1、获取机器人的出厂时的机械参数向量P，维数为m×1，机械参数包括负载和各关节的质量、质心、惯量以及摩擦系数；

S2、获取机器人各关节运动量：关节角度q，关节速度

关节加速度，三者都是N×1向量；

S3、获取机器人各关节实际转矩T_r；

S4、建立机器人关节转矩的预测模型T_p＝B*P，B为已知参数矩阵；

S5、将S1的机械参数P和S2获取的关节运动量代入S4建立的模型，求解关节转矩的预测值T_p；

S6、比较S3所得关节实际转矩T_r和S5所得关节预测转矩T_p，判断当前时刻机器人各关节运动状态；

S7、建立机器人机械参数的辨识模型P_k＝(B^TB)^-1B^TT_r；

S8、将S2所得运动数据和S3所得关节实际转矩T_r代入S7建立的模型求解当前时刻机器人机械参数P_k；

S9、比较S8所得当前时刻机器人机械参数P_k和机器人出厂参数P₀，判断当前时刻机器人本体健康状态；

S10、记录S6所得机器人各关节运动状态和S9所得机器人本体健康状态；

S11、根据S6所得判断结果和S9所得判断结果，判断机器人的工作状态；

S12、根据S11所得结果，如果机器人状态异常，则进行异常处理；

S13、根据S11所得结果，如果机器人状态正常，按照公式P_k+1＝(1-λ)P_k+λP更新机械参数得到S1新值P_k+1，λ为更新速率，取值区间为[0.01,0.1]；

***包括：

位于运动关节处的数据获取部，用于实时获取当前机器人的状态参数，包括机械参数、各关节运动量和各关节转矩实测值；

运行状态监测部，用于建立机器人关节转矩预测模型，代入关节运动量和所述数据获取部获得的机械参数计算出当前关节转矩预测值，比较关节转矩预测值和实际值计算转矩偏差，判断机器人当前运行状态；

健康状态监测部，用于建立机器人机械参数辨识模型，代入关节运动量和所述数据获取部获得的关节转矩计算出当前机器人机械参数，与出厂机械参数进行比较即可判断机器人当前健康状态；

状态处理部，根据当前机器人运行状态和健康状态做出对应处理，如果状态正常，则更新机器人机械参数；如果状态异常，进行异常处理，异常处理方式包括减速运动和运动停止；

状态记录部，记录机器人状态参数；

其中S1～S3由数据获取部完成，S4～S6由运行状态检测部完成，S7～S9由健康状态检测部完成，S10由状态记录部完成，S11～S13由状态处理部完成。

具体的，所述机器人各关节实际转矩T_r由以下公式计算得到

其中τ_mi为第i轴电机输出转矩，G_i为第i关节的减速比，

为第i轴电机角速度，

为第i轴的角加速度，J_mi为第i轴电机转子的转动惯量，c_mi为第i轴电机粘性阻尼系数，f_mi为第i轴电机转子的摩擦转矩。电机输出转矩等于电机电流乘以转矩常数，机器人实际转矩T_r是一个N×1维的列向量，N代表机器人自由度数，第i个元素为第i轴关节转矩τ_i，即有T_r＝[τ₁，τ₂，…，τ_N]^T。

具体的，所述步骤S6包括：实际关节转矩减去预测关节转矩得到转矩偏差，根据转矩偏差判断机器人当前运行状态。

进一步的，将转矩偏差信号进行高通滤波，如果滤波后信号的绝对值大于第二阈值T₂，则认定机器人本体受到强烈的外部高频扰动；如果滤波后信号的绝对值大于第一阈值T₁且小于第二阈值T₂，则认定机器人本体收到一般外部高频扰动；第二阈值T₂大于第一阈值T₁。

进一步的，将转矩偏差信号进行低通滤波，如果滤波后信号的绝对值大于第二阈值T₂，则认定机器人本体受到强烈的外部低频扰动；如果滤波后信号的绝对值大于第一阈值T₁且小于第二阈值T₂，则认定机器人本体收到一般外部低频扰动；第二阈值T₂大于第一阈值T₁。

具体的，所述步骤S12中的异常处理方式包括减速运动和运动停止。

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

本发明通过建立机器人关节转矩预测模型和机器人机械参数辨识模型，实时判断机器人的运行状态和健康状态，并对于各种异常状态进行及时处理，可以有效地在早期发现机器人可能会发生的机械故障，减少经济损失。

附图说明

图1为实施例机器人健康监测和故障诊断***的结构示意图和流程图对应关系图；

图2为实施例机器人健康监测和故障诊断***工作过程中的逻辑框图。

图3为正常运行状态下Scara机器人第一轴关节转矩实际值和预测值的对比图。

具体实施方式

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

实施例：

如图1所示，本发明的一种机器人健康监测和故障诊断***的控制方法，***包括：位于运动关节处的数据获取部，用于实时获取当前机器人的状态参数，如机械参数、各关节运动量和各关节转矩实测值；

运行状态监测部，用于建立机器人关节转矩预测模型，代入关节运动量和所述数据获取部获得的机械参数计算出当前关节转矩预测值，比较关节转矩预测值和实际值计算转矩偏差，判断机器人当前运行状态；

健康状态监测部，用于建立机器人机械参数辨识模型，代入关节运动量和所述数据获取部获得的关节转矩计算出当前机器人机械参数，与出厂机械参数进行比较即可判断机器人当前健康状态；

状态处理部，根据当前机器人运行状态和健康状态做出对应处理，如果状态正常，则更新机器人机械参数；如果状态异常，进行异常处理，异常处理方式包括减速运动和运动停止；

状态记录部，记录机器人状态参数。

如图2所示，应用机器人健康监测和故障诊断***的控制方法，包括如下步骤：

S1、获取机器人的出厂时的机械参数向量P，维数为m×1，机械参数包括负载和各关节的质量、质心、惯量以及摩擦系数；

S2、获取机器人各关节运动量：关节角度q，关节速度

，关节加速度

，三者都是N×1向量；

S3、获取机器人各关节实际转矩T_r；

S4、建立机器人关节转矩的预测模型T_p＝B*P，B为已知参数矩阵；

S5、将S1的机械参数P和S2获取的关节运动量代入S4建立的模型，求解关节转矩的预测值T_p；

S6、比较S3所得关节实际转矩T_r和S5所得关节预测转矩T_p，判断当前时刻机器人各关节运动状态；

S7、建立机器人机械参数的辨识模型P_k＝(B^TB)^-1B^TT_r；

S8、将S2所得运动数据和S3所得关节实际转矩T_r代入S7建立的模型求解当前时刻机器人机械参数P_k；

S9、比较S8所得当前时刻机器人机械参数P_k和机器人出厂参数P₀，判断当前时刻机器人本体健康状态；

S10、记录S6所得机器人各关节运动状态和S9所得机器人本体健康状态；

S11、根据S6所得判断结果和S9所得判断结果，判断机器人的工作状态；

S12、根据S11所得结果，如果机器人状态异常，则进行异常处理；

S13、根据S11所得结果，如果机器人状态正常，按照公式P_k+1＝(1-λ)P_k+λP更新机械参数得到S1新值P_k+1，λ为更新速率，取值区间为[0.01,0.1]；

其中S1～S3由数据获取部完成，S4～S6由运行状态检测部完成，S7～S9由健康状态检测部完成，S10由状态记录部完成，S11～S13由状态处理部完成。

对于S1，机器人本体机械参数包括负载和各关节的质量、质心、惯量以及摩擦系数等，S1所指机械参数初始值在出厂前确定。

S2所指关节运动量包含关节角度q，关节速度

关节加速度

三者都是N×1向量，q＝[q₁，q₂，...，q_N]^T，

q_i、

和

分别为第i关节的角度、角速度以及角加速度。

S3所指关节实际转矩为驱动机构的输出转矩，由以下公式计算

下标i代表第机器人第i轴，其中λ_i为电机转矩常数，I_i为电机电流，τ_mi为电机输出转矩，G_i为关节减速比，

为电机角速度，

为电机角加速度，J_mi为电机转子的转动惯量，c_mi为电机粘性阻尼系数，f_mi为电机转子的摩擦转矩。电机输出转矩等于电机电流乘以转矩常数，机器人实际转矩T_r是一个N×1维的列向量，N代表机器人自由度数，第i个元素为第i轴关节转矩τ_i，即有T_r＝[τ₁,τ₂,…,τ_N]^T。

S4所指机器人实际转矩预测模型通过以下方法获取：关节转矩预测模型即机器人逆动力学模型，运用牛顿欧拉法建立单个关节的力和转矩平衡方程，通过运动学正向迭代求出杆件角速度、角加速度、质心速度以及质心加速度，通过动力学逆向迭代即可求出机器人关节受到的力和转矩。

下面以Scara机器人前两关节为例，详细说明上述推到过程，其中正向运动学迭代公式和逆向动力学迭代公式为：

各参数定义如下：

Scara前两关节运动学正向推导方程为

其中b₁＝[0，0，1]^T，b₂＝[0，0，1]^T，r_0，1＝[l₁，0，0]，r_0，c1＝[l_1c，0，0]，r_1，2＝[l₂，0，0]，r_1，c2＝[l_2c，0，0]，几何参数l₁，l_ic均为已知量，旋转矩阵R₁ ⁰和R₁ ²分别为：

Scara前两关节动力学逆向推导公式为：

f₂＝m₂a_c，2-m₂g₂

τ₂＝-f₂×r_1，c2+ω₂×(J₂ω₂)+J₂α₂

其中g₁＝[0，0，-9.8]，g₂＝[0，0，-9.8]，m₁为第一杆件质量，m₂位第二杆件质量，J₁为第一杆件相对于第一转轴的转动惯量，是一个3×3的矩阵；J₂为第二杆件相对第二转轴的转动惯量，同样是一个3×3的矩阵。由于Scara一轴二轴平行，不影响计算结果，可以假设J₁和J₂有以下形式：

根据转动惯量的平行轴定理求导J_1zz和J_2zz的值。已知第一杆件相对其质心转动惯量为H₁，第二杆件相对其质心转动惯量为H₂，则有J_1zz＝H₁+m₁*l_1c*l_1c，J_2zz＝H₂+m₂*l_2c*l_2c。推到结果为

考虑关节摩擦，令f₁和f₂分别为Scara机器人前两关节的摩擦转矩，则关节转矩方程修正为

摩擦转矩包含库伦摩擦、线性粘性阻尼、平方阻尼三部分，Scara机器人前两关节摩擦转矩表达式如下

其中f_di为干摩擦阻尼转矩系数，c_i1为粘性阻尼转矩系数，c_i2为平方阻尼转矩系数，整理Scara机器人逆动力学模型表达式，可以写成以下矩阵形式。

F＝B*P

其中矩阵B，向量P，向量F表达式分别为

F＝[τ₁，τ₂]^T

c₂＝cos(q₂) s₂＝sin(q₂)

对于单次采样，B矩阵的维数为2×9，P矩阵维数为9×1，F矩阵维数为2×1。而对于Y组采样数据，B矩阵的维数则为2Y×9，P矩阵的维数为9×1，F矩阵维数为2Y×1。

令关节转矩预测值为T_p，将F替换为T_p则可以得到关节转矩预测模型的表达式：

T_p＝B*P

S5所指关节转矩预测值可以通过S4所指预测模型算出，此时机器人机械参数向量P已知，且由S1指定，模型输入为S2所指的关节运动变量q，

将关节运动量带入B矩阵，通过矩阵点乘运算求出关节转矩预测值T_p。

S6所指运动状态判断方法如下：定义T_di为第i轴关节转矩偏差

T_di＝T_ri-T_pi

其中T_ri为T_r的第i个元素，为S3所计算第i轴关节转矩的实际值，T_pi为T_p的第i个元素，为S5所计算第i轴关节转矩的预测值。

定义第一判定阈值T₁，第二判定阈值T₂，且有T₂>T₁>0。

将T_di进行高通滤波，得到滤波后的信号T_hd。如果T₁<|T_hd|<T₂，则认定机器人受到一般外部高频扰动，如果|T_hd|>T₂则认定机器人受到强烈外部高频扰动，其中|·|为绝对值符号。扰动可能由以下因素引起：

1)减速器，电机等传动机构出现故障。

2)机器人本体受到撞击。

3)负载、夹具和机器人法兰之间连接出现松动，甚至出现部件脱落情况。

4)其他因素。

将T_di进行低通滤波，得到滤波后的信号T_sd。如果T₁<|T_sd|<T₂，则认定机器人受到一般外部高频扰动，如果|T_sd|>T₂则认定机器人受到强烈外部低频扰动，其中|·|为绝对值符号。扰动可能由以下因素引起：

1)负载力学参数填写的不正确。

2)传动机构缺少润滑。

3)传动机构老化。

4)机器人本体受到附加作用力。

5)外部环境过冷或过热。

6)其他因素。

S7所指机械参数辨识模型可以运用最小二乘法可得到，求解公式如下

P_k＝(B^TB)^-1B^TT_r

S8所指当前时刻机械参数可以通过S7所示模型计算，模型输入为S2所获取的关节运动量和S3获取的关节实际转矩，其中关节运动量可以初始化B阵，关节实际转矩可以初始化向量T_r，模型输出为机器人当前时刻的机械参数P_k。

S9所指机器人健康状态判断方法如下：即P₀为机器人出厂时机械参数向量，P_k为S8计算所得当前时刻机械参数向量，定义新变量ΔP＝||P_k-P₀||，其中符号||P_k-P₀||代表向量P_k-P₀的二范数。

如果ΔP>P₁则认为机器人健康状态异常，否则认为机器人健康状态正常。其中P₁>0为判断阈值，异常状态可能由以下因素引起：

1)电机出现问题。

2)传动机构磨损或老化。

此处同样以Scara机器人为例进行详细说明，将参数向量P写成两部分，P_k＝[P₁，P₂]，同样将P₀写成两部分，P₀＝[P₁₀，P₂₀]，其中P₁₀是和机器人质量参数有关的向量子集，P₂₀是和机器人摩擦转矩相关的向量子集，其中

P₁＝[c₁₁，c₁₂，c₂₁，c₂₂，f_d1，f_d2]，ΔP₁＝||P₁-P₁₀||，ΔP₂＝||P₂-P₂₀||，假若||ΔP₁||值出现异常，则有两种可能，一是电机出现故障，二是机器人本体有附加质量。假若||ΔP₂||值出现异常，则传动机构出现问题，例如减速机润滑不够理想或者减速机损坏。

S10记录S6所得机器人各关节运动状态和S9所得机器人本体健康状态。

S11针对S6和S9判断结果进行决策，如果机器人运行状态和健康状态都正常，则认为机器人状态正常，否则认为机器人状态异常。

S12针对S11所得机器人异常工作状态进行响应动作，当机器人受到强烈的外部扰动时，将机器人切换到停止模式，否则将机器人切换到低速运行模式。

S13针对S11所得机器人正常工作状态进行响应动作，具体动作是更新机器人机械参数，假设当前时刻机器人机械参数S1为P，当前时刻S8计算所得机器人机械参数为P_k，则下一时刻机器人机械参数S1为P_k+1:

P_k+1＝(1-λ)P_k+λP

式中λ为更新速率，取值区间为[0.01,0.1]，λ取值越大，S1所指机器人机械参数更新越快。

为验证本发明的正确性，通过采集Scara机器人前两关节运动量数据和电流数据，计算第一关节实际转矩，利用S7机械参数辨识模型，计算机器人当前机械参数，而后将机械参数以及重新采样的运动量带入S4关节转矩预测模型，计算第一关节转矩预测值，相关结果见图3，其中实线Prediction代表关节转矩预测值，虚线Real代表关节转矩实际值，因而可以看出在正常工作状态下，关节转矩预测值和实际值高度一致，验证了S4关节转矩预测模型的准确性。

以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (6)

1.一种机器人健康监测和故障诊断***的控制方法，其特征在于包括如下步骤：

S1、获取机器人的出厂时的机械参数向量P，维数为m×1，机械参数包括负载和各关节的质量、质心、惯量以及摩擦系数；

S2、获取机器人各关节运动量：关节角度q，关节速度

关节加速度

三者都是N×1向量；

S3、获取机器人各关节实际转矩T_r；

S4、建立机器人关节转矩的预测模型T_p＝B*P，B为已知参数矩阵；

S5、将S1的机械参数P和S2获取的关节运动量代入S4建立的模型，求解关节转矩的预测值T_p；

S6、比较S3所得关节实际转矩T_r和S5所得关节预测转矩T_p，判断当前时刻机器人各关节运动状态；

S7、建立机器人机械参数的辨识模型P_k＝(B^TB)^-1B^TT_r；

S8、将S2所得运动数据和S3所得关节实际转矩T_r代入S7建立的模型求解当前时刻机器人机械参数P_k；

S9、比较S8所得当前时刻机器人机械参数P_k和机器人出厂参数P₀，判断当前时刻机器人本体健康状态；

S10、记录S6所得机器人各关节运动状态和S9所得机器人本体健康状态；

S11、根据S6所得判断结果和S9所得判断结果，判断机器人的工作状态；

S12、根据S11所得结果，如果机器人状态异常，则进行异常处理；

S13、根据S11所得结果，如果机器人状态正常，按照公式P_k+1＝(1-λ)P_k+λP更新机械参数得到S1新值P_k+1，λ为更新速率，取值区间为[0.01,0.1]；

***包括：

位于运动关节处的数据获取部，用于实时获取当前机器人的状态参数，包括机械参数、各关节运动量和各关节转矩实测值；

运行状态监测部，用于建立机器人关节转矩预测模型，代入关节运动量和所述数据获取部获得的机械参数计算出当前关节转矩预测值，比较关节转矩预测值和实际值计算转矩偏差，判断机器人当前运行状态；

健康状态监测部，用于建立机器人机械参数辨识模型，代入关节运动量和所述数据获取部获得的关节转矩计算出当前机器人机械参数，与出厂机械参数进行比较即可判断机器人当前健康状态；

状态处理部，根据当前机器人运行状态和健康状态做出对应处理，如果状态正常，则更新机器人机械参数；如果状态异常，进行异常处理，异常处理方式包括减速运动和运动停止；

状态记录部，记录机器人状态参数；

其中S1～S3由数据获取部完成，S4～S6由运行状态检测部完成，S7～S9由健康状态检测部完成，S10由状态记录部完成，S11～S13由状态处理部完成。

2.根据权利要求1所述的控制方法，其特征在于：所述机器人各关节实际转矩T_r由以下公式计算得到

其中τ_mi为第i轴电机输出转矩，G_i为第i关节的减速比，

为第i轴电机角速度，

为第i轴的角加速度，J_mi为第i轴电机转子的转动惯量，c_mi为第i轴电机粘性阻尼系数，f_mi为第i轴电机转子的摩擦转矩；电机输出转矩等于电机电流乘以转矩常数，机器人实际转矩T_r是一个N×1维的列向量，N代表机器人自由度数，第i个元素为第i轴关节转矩τ_i，即有T_r＝[τ₁，τ₂，…，τ_N]^T。

3.根据权利要求1所述的控制方法，其特征在于：所述步骤S6包括：实际关节转矩减去预测关节转矩得到转矩偏差，根据转矩偏差判断机器人当前运行状态。

4.根据权利要求3所述的控制方法，其特征在于：将转矩偏差信号进行高通滤波，如果滤波后信号的绝对值大于第二阈值T₂，则认定机器人本体受到强烈的外部高频扰动；如果滤波后信号的绝对值大于第一阈值T₁且小于第二阈值T₂，则认定机器人本体收到一般外部高频扰动；第二阈值T₂大于第一阈值T₁。

5.根据权利要求3所述的控制方法，其特征在于：将转矩偏差信号进行低通滤波，如果滤波后信号的绝对值大于第二阈值T₂，则认定机器人本体受到强烈的外部低频扰动；如果滤波后信号的绝对值大于第一阈值T₁且小于第二阈值T₂，则认定机器人本体收到一般外部低频扰动；第二阈值T₂大于第一阈值T₁。

6.根据权利要求1所述的控制方法，其特征在于：所述步骤S12中的异常处理方式包括减速运动和运动停止。

Images