CN109514602A - 一种基于负载自适应辨识的工业机器人力矩补偿控制方法 - Google Patents

Abstract

本发明公开了一种基于负载自适应辨识的工业机器人力矩补偿控制方法，包括根据机器人整体的受力平衡分析，在考虑摩擦的基础上采用牛顿‑欧拉法推导建立带负载工况下的机器人动力学模型；求解机器人负载辨识最优激励轨迹；计算负载惯性参数；在基于负载惯量匹配的基础上，补偿机器人带载工况下运动时负载所消耗的关节驱动力矩，并通过实验进一步验证和优化。本发明方法考虑了负载对机器人作业性能的影响，在无需任何外部传感器的前提下，通过简单易实现的操作完成了对任意负载的自适应辨识和力矩补偿控制，有效提高机器人带载工况下的控制精度，对于高速、大负载运动下的工业机器人搬运、搅拌摩擦焊等工艺具有十分重要的意义。

Description

一种基于负载自适应辨识的工业机器人力矩补偿控制方法

技术领域

本发明涉及工业机器人的动力学模型辨识和前馈控制，特别是涉及一种基于负载自适应辨识的工业机器人力矩补偿控制方法。

背景技术

在降低机器人的本体质量、提高负载能力的同时保证机器人的控制精度是工业机器人研究和应用领域中的难点。现有市场上绝大部分工业机器人只是在考虑机器人空载模型的情况下对控制器进行设定和参数调定，而当机器人末端实际添加负载时会导致控制性能不同程度的下降，尤其对于重载机器人应用而言问题更是突出。为了满足重载机器人高速、高精度、高负载等典型能力需求，必须要充分考虑负载对于机器人控制性能的影响。如何快速准确辨识负载的动力学参数，从而建立起带载动力学模型，并由此设计相应的负载自适应控制律以改进控制器的轨迹跟踪性能是解决带负载机器人高精度控制的关键。

关于机器人负载辨识方面的研究目前业界有一定的成果。根据获取关节力矩测量数据方法的不同可将机器人负载辨识方法分为基于机器人反作用力传感器的外部模型法与基于内部测量装置的内部模型法。例如通过力/力矩传感器来测量负载施加到机器人末端执行器的力矩，或者将已知负载模型添加到预先获得的空载机器人本体动力学模型中，从而得到整个带负载机器人模型，但并不是所有负载模型可直接获得。此外国内外很多学者把负载当作机器人本体的一部分进行参数辨识。

上述负载辨识方法中，对于基于机器人反作用力传感器的外部模型法与基于内部测量装置的内部模型法都需要价格昂贵的外部传感器，限制了其适用面。而将负载当做机器人本体的一部分进行辨识的方法需要视负载类型不同而不断重复进行辨识实验，方法费时耗力。其次在控制方法方面，现有先进控制方法大部分都需要修改常规三闭环PID控制系统结构，算法的可行性很大程度依赖于实际控制系统的开放程度，普遍存在计算复杂度较大的问题，对控制系统的实时计算能力要求较高。如何根据所辨识的带载动力学模型设计更为合理、更易实现的控制律对负载效应进行在线补偿应该是一种更好的解决问题的思路。

发明内容

发明目的：为了克服上述已有的技术不足，对负载辨识提供了一种基于负载自适应辨识的工业机器人力矩补偿控制方法。该方法通过建立负载可辨识模型，同时借助于机器人本体动力学模型中已知的惯性参数，对负载的动力学参数进行单独辨识，无需借助特殊传感器，并且该方法在负载变化时，无需再次辨识机器人本体的动力学模型，具有较高的效率和工程应用价值。在控制方法方面，该方法采用在传统PID控制系统中加入基于负载惯量匹配的力矩补偿控制模块，无需修改传统PID控制系统结构，并解决了由于机器人末端添加负载而导致的控制精度降低问题，从而提高机器人带载工况下的控制精度。

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

一种基于负载自适应辨识的工业机器人力矩补偿控制方法，包括以下步骤：

(1)带负载工况下的机器人动力学建模

根据机器人整体的受力平衡分析，在考虑摩擦的基础上采用牛顿-欧拉法推导建立带负载工况下的机器人动力学模型；

(2)优化激励轨迹

选取五阶傅里叶级数作为关节激励轨迹形式，以最小条件数为优化指标，通过遗传算法求解机器人负载辨识最优激励轨迹；

(3)负载惯性参数辨识

驱动机器人按照最优激励轨迹运动，采样机器人关节角度和电机电流值，采用递推最小二乘法，计算负载惯性参数；

(4)基于负载惯量匹配的机器人力矩补偿控制

在基于负载惯量匹配的基础上，补偿机器人带载工况下运动时负载所消耗的关节驱动力矩，并通过实验进一步验证和优化。

进一步的，步骤(1)包括以下步骤：

(11)根据机器人关节受力分析，确定相对于各关节首端坐标系、作用于各关节上的力和力矩公式，对于关节i，坐标系i建立在关节i的首端，坐标系i+1建立在关节i与关节i+1交汇处，牛顿-欧拉等式给出相对于坐标系i，作用在关节i上的力和力矩公式为：

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (1)；

其中，为关节i单独运动时产生的相对于坐标系i的力，为关节i单独运动时产生的相对于坐标系i的力矩，ⁱp_i+1为坐标系i+1相对于坐标系i的位移矢量，ⁱf_ei为环境作用在关节i上的力，ⁱτ_ei为环境作用在关节i上的力矩，ⁱf_i为相对于坐标系i，作用在关节i上的力，ⁱτ_i为相对于坐标系i，作用在关节i上的力矩，ⁱT_i+1为坐标系i+1相对于坐标系i的转换矩阵，r_i为ⁱf_ei的力臂，转轴为坐标系i的z轴，ⁱ⁺¹f_i+1为相对于坐标系i+1，作用在关节i+1上的力，ⁱ⁺¹τ_i+1为相对于坐标系i+1，作用在关节i+1上的力矩；

(12)考虑关节摩擦力的影响，建立关节简化线性摩擦力模型，摩擦力模型为：

其中，τ_fi表示关节i的摩擦力，f_ci表示关节i的库伦摩擦系数，f_vi表示关节i的粘滞摩擦系数，表示关节i的角速度；

(13)通过牛顿-欧拉法迭代过程，进行六次正向迭代和六次逆向迭代，得到各关节期望输出力矩模型；

(14)通过对机器人的本体动力学模型公式进行模型变换和动力学参数重组，得到关于基础动力学参数的机器人线性化动力学模型为：

其中，为观测矩阵，K_st＝[K₁ ^T,K₂ ^T,…,K_n ^T]^T，为13×n的标准惯性参数集，K_i＝[xx_i,xy_i,xz_i,yy_i,yz_i,zz_i,mx_i,my_i,mz_i,m_i,I_ai,f_vi,f_ci]，xx_i,xy_i,xz_i,yy_i,yz_i,zz_i为6个惯性张量，mx_i,my_i,mz_i为三维坐标轴的一阶惯性矩，m_i为关节i的质量，I_ai为电机i的惯性矩，f_vi表示关节i的粘滞摩擦系数，f_ci表示关节i的库伦摩擦系数；

对标准惯性参数集K_st中部分参数进行消去和重组，获取可标定的最小惯性参数集为：

其中，K为b×1(b＜13n)的最小惯性参数集，为对应的回归矩阵；

(15)机器人末端负载在各关节处产生的作用力矩如下：

τ_l＝J^TF_l (6)；

其中，τ_l为负载在各关节的作用力矩；J^T为机器人雅克比矩阵的转置；F_l为负载作用在机器人末端的广义力；

结合式机器人的本体动力学模型公式和式(4)，得到带负载工况下机器人完整动力学模型为：

τ＝τ_b+τ_l (7)；

其中，τ,τ_b,τ_l分别为带载工况下，机器人各关节期望输出力矩，机器人本体运动消耗的关节驱动力矩，负载运动消耗的关节驱动力矩；

(16)对模型进行参数线性化，得到机器人末端负载在机器人各个关节的作用力矩关于惯性参数的线性表达式为：

其中，τ_l为负载在各个关节的作用力矩；w_l为关于负载惯性参数的系数矩阵；K_l＝[m_l mx_l my_l mz_l xx_l xy_l xz_l yy_l yz_l zz_l]^T为负载的相关惯性参数；m_l为负载质量；mx_l,my_l,mz_l分别为负载相对于末端坐标系的侧倾力矩、俯仰力矩和横摆力矩；

为相对于末端坐标系，负载的惯性矩阵。

更进一步的，步骤(13)中迭代过程如下：

牛顿-欧拉法单步正向迭代过程为：给定关节0的初值⁰w₀、a₀以及输入机器人关节1的角度q₁，计算角速度角加速度根据公式(9)-(11)求出关节1的¹w₁、a₁，进而根据公式(12)和(13)求出关节1的¹f_o1、¹τ_o1；然后输入关节2的角度q₂，计算角速度角加速度求出关节2的²w₂、a₂、²f_o2、²τ_o2；直到迭代求出末端杆件n的ⁿw_n、a_n、ⁿf_on、ⁿτ_on；

单步逆向迭代过程为：给定作用在末端法兰盘上的力和力矩ⁿ⁺¹f_n+1,ⁿ⁺¹τ_n+1，根据公式(14)和(15)计算出连杆n-1作用在连杆n上的力ⁿf_n和力矩ⁿτ_n；然后，将其变换到坐标系n-1中得^n-1f_n、^n-1τ_n，结合在正向迭代过程中计算出的^n-1f_o(n-1)、^n-1τ_o(n-1)和已知的环境作用力^{n- 1}f_e(n-1)、^n-1τ_e(n-1)，求出关节n-2作用在关节n-1上的力^n-1f_n-1和力矩^n-1τ_n-1；以此类推，直至迭代计算出关节0作用在关节1上的力¹f₁、¹τ₁；

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (14)；

ⁱτ_i＝ⁱτ_Oi+ⁱT_i+1 ⁱ⁺¹τ_i+1+ⁱp_i+1×ⁱ⁺¹f_i+1-ⁱτ_ei-ⁱf_ei×r (15)；

Γ_i＝ⁱτ_i·[0 0 1]^T (16)；

其中，ⁱw_i表示相对于坐标系i，关节i的角速度，ⁱw_i-1表示相对于坐标系i-1，关节i的角速度；q_i表示关节i的角度；ⁱT_i-1表示坐标系i相对于坐标系i-1的方位；ⁱz_i表示相对于坐标系i，沿轴z_i的单位向量的坐标；m_i表示关节i的质量；ⁱS_i表示相对于坐标系i，关节i的质心坐标，其表示为[X_i Y_i Z_i]，其中X_i、Y_i、Z_i分别为关节i的质心坐标在坐标系i的x、y、z方向上的三个分量；m_i ⁱS_i表示相对于坐标系i，关节i的一阶转矩，其计算方式为m_i ⁱS_i＝[mX_imY_i mZ_i]，其中mX_i、mY_i、mZ_i分别为关节i的一阶转矩在坐标系i的x、y、z方向上的三个分量；ⁱJ_i表示相对于坐标系i，关节i的惯性矩阵；Γ_i表示电机i期望输出力矩；a_i表示杆件i的加速度，g表示重力向量；

最终得到的机器人的本体动力学模型公式为：

其中，M(q)为惯量矩阵，为Coriolis力和离心力矩阵，为关节摩擦力项，G(q)为重力项，τ_b为各个关节的期望输出力矩矩阵。

进一步的，步骤(2)包括以下步骤：

(21)选取五阶周期性傅里叶级数作为激励轨迹，其表达式为：

其中，q_i,0为关节位置初始值，ω_f为基频，N为谐波个数，每个傅里叶级数有2N+1个参数，对应于q_i,0,a_i,k,b_i,k(k＝1,2,...,N)，k＝1,2,…,N表示傅里级数的阶数；

(22)确定激励轨迹选取时的约束条件

①关节空间约束机器人在关节空间的约束为：

其中，q_li、q_Fi分别为关节i的位置下限和上限，为关节i速度的绝对值的上限值，为关节i加速度的绝对值的上限值；

②笛卡尔空间约束

机器人在笛卡尔空间满足的约束为：

其中，S代表机器人获得的工作空间，{s(q)}表示机器人末端的运动轨迹；

激励轨迹选取对应的目标函数为：

F(X)＝f+P (21)；

其中，X为决策变量，F(X)为目标函数，f＝Cond(W)为回归矩阵W的条件数；P为惩罚系数；

机器人激励轨迹选取问题归结为多变量、非线性优化问题，对应的优化问题的数学模型为：

(23)通过遗传算法求解式(19)对应的激励轨迹选取优化问题，得到最优激励轨迹。

更进一步的，所述步骤(23)包括以下步骤：

(a)在系统变量中选取合适的决策变量X作为算法的个体，确定种群规模N_p、变异概率pe、交叉概率CR和最大进化代数i_max；

(b)生成初始种群P₀；

(c)对第i代种群进行交叉、变异操作；

(d)计算父代种群和交叉变异后的种群适应度值，选择适应度值最高的N_p个体生成下一代种群P_i+1；

(e)如果达到最大进化代数则终止，选取当前种群中的最优个体X_best,i+1作为轨迹规划的解；否则i＝i+1，转到(c)。

进一步的，步骤(3)包括以下步骤：

(31)设置递推参数初始值设置递推参数初值和P₀；

(32)驱动机器人跟随参考轨迹运动，在采样周期内，对关节角度、角速度、角加速度、负载作用力矩进行采集，获取新的观测矩阵w_lk和负载作用力矩矩阵τ_lk；

具体步骤如下：

①通过快速傅里叶变换求取各关节角度采样值的频谱并滤波；

②求取关节角速度频谱：在第①步求解的关节位置频谱上乘以下式得到关节角速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5}；

③求取关节角加速度频谱：在第②步求解的角速度频谱上乘以下式得到关节角加速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5}；

其中，f_s表示采样频率，k表示频谱阶数；

④将关节角度采样值的频谱、位置频谱、关节角速度频谱和关节角加速度频谱分别作快速傅里叶逆变换，从而计算出各关节角度、角速度和角加速度；

(33)根据式(23)～式(25)所示的RLS法计算当前负载惯性参数递推估计值

RLS估算法表示为：

其中，其中，和分别为根据前k-1次和前k次采样数据估算得到的负载惯性参数；w_lk为负载惯性参数观测矩阵在第k次采样得到的超定矩阵；τ_lk为负载作用力矩在第k次采样得到的超定矩阵，E_k称为增益矩阵；W_k＝[w_l1 w_l2 … w_lk]^T为负载惯性参数观测矩阵在前k次采样得到的超定矩阵；

(34)如果步骤(33)估算出的惯性参数值满足精度要求，则直接输出，否则在采样周期内转回到步骤(33)，继续循环计算。

进一步的，步骤(4)包括以下步骤：

结合步骤(3)辨识出的负载惯性参数，考虑负载影响的机器人力矩补偿控制原理，通过在传统伺服三闭环PID控制系统中添加基于动力学模型的力矩前馈控制模块，以改善机器人的运动性能，整体系统的控制律如下式所示：

其中，u为各电机的驱动力矩；u_p为伺服三闭环PID控制输入力矩；τ_l为基于负载惯量匹配的力矩补偿控制输入；w_l为关于负载惯性参数的系数矩阵；K_l为负载的相关惯性参数。

有益效果：与现有技术相比，本发明通过建立负载可辨识模型，同时借助于机器人本体动力学模型中已知的惯性参数，对负载的动力学参数进行单独辨识，无需借助特殊传感器，并且该方法在负载变化时，无需再次辨识机器人本体的动力学模型，具有较高的效率和工程应用价值。在控制方法方面，该方法采用在传统PID控制系统中加入基于负载惯量匹配的力矩补偿控制模块，无需修改传统PID控制系统结构，并解决了由于机器人末端添加负载而导致的控制精度降低问题，从而提高机器人带载工况下的控制精度。

附图说明

图1是本发明的流程图；

图2是牛顿-欧拉法迭代过程图；

图3是遗传算法流程图；

图4是递推最小二乘法流程图；

图5是基于负载惯量匹配的机器人力矩补偿控制。

具体实施方式

下面结合附图和具体实施方式对本发明的技术方案做进一步说明。

应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

如图1所示，一种基于负载自适应辨识的工业机器人力矩补偿控制方法，包括以下步骤：

(1)带负载工况下的机器人动力学建模

根据机器人整体的受力平衡分析，在考虑摩擦的基础上采用牛顿-欧拉法推导建立带负载工况下的机器人动力学模型。具体步骤包括：

(11)根据机器人关节受力分析，确定相对于各关节首端坐标系，作用于各关节上的力和力矩公式，对于关节i，坐标系i建立在关节i的首端，坐标系i+1建立在关节i与关节i+1交汇处，牛顿-欧拉等式给出相对于坐标系i，作用在关节i上的力和力矩公式为：

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (1)；

其中，为关节i单独运动时产生的相对于坐标系i的力，为关节i单独运动时产生的相对于坐标系i的力矩，ⁱp_i+1为坐标系i+1相对于坐标系i的位移矢量，ⁱf_ei为环境作用在关节i上的力，ⁱτ_ei为环境作用在关节i上的力矩，ⁱf_i为相对于坐标系i，作用在关节i上的力，ⁱτ_i为相对于坐标系i，作用在关节i上的力矩，ⁱT_i+1为坐标系i+1相对于坐标系i的转换矩阵，r_i为ⁱf_ei的力臂，转轴为坐标系i的z轴，ⁱ⁺¹f_i+1为相对于坐标系i+1，作用在关节i+1上的力，ⁱ⁺¹τ_i+1为相对于坐标系i+1，作用在关节i+1上的力矩；

(12)考虑关节摩擦力的影响，建立关节简化线性摩擦模型，摩擦力模型为：

其中，τ_fi表示关节i的摩擦力，f_ci表示关节i的库伦摩擦系数，f_vi表示关节i的粘滞摩擦系数，表示关节i的角速度；

(13)通过牛顿-欧拉法迭代过程，进行六次正向迭代和六次逆向迭代，得到各关节期望输出力矩模型，如图2所示，迭代过程如下：

牛顿-欧拉法单步正向迭代过程为：给定关节0的初值⁰w₀、a₀以及输入机器人关节1的角度q₁，计算角速度角加速度根据公式(4)-(6)求出关节1的¹w₁、a₁，进而根据公式(7)和(8)求出关节1的¹f_o1、¹τ_o1；然后输入关节2的角度q₂，计算角速度角加速度求出关节2的²w₂、a₂、²f_o2、²τ_o2；直到迭代求出末端杆件n的ⁿw_n、a_n、ⁿf_on、ⁿτ_on；

单步逆向迭代过程为：给定作用在末端法兰盘上的力和力矩ⁿ⁺¹f_n+1,ⁿ⁺¹τ_n+1，根据公式(9)和(10)计算出连杆n-1作用在连杆n上的力ⁿf_n和力矩ⁿτ_n；然后，将其变换到坐标系n-1中得^n-1f_n、^n-1τ_n，结合在正向迭代过程中计算出的^n-1f_o(n-1)、^n-1τ_o(n-1)和已知的环境作用力^{n- 1}f_e(n-1)、^n-1τ_e(n-1)，求出关节n-2作用在关节n-1上的力^n-1f_n-1和力矩^n-1τ_n-1；以此类推，直至迭代计算出关节0作用在关节1上的力¹f₁、¹τ₁；

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (9)；

ⁱτ_i＝ⁱτ_Oi+ⁱT_i+1 ⁱ⁺¹τ_i+1+ⁱp_i+1×ⁱ⁺¹f_i+1-ⁱτ_ei-ⁱf_ei×r (10)；

Γ_i＝ⁱτ_i·[0 0 1]^T (11)；

其中，ⁱw_i表示相对于坐标系i，关节i的角速度，ⁱw_i-1表示相对于坐标系i-1，关节i的角速度；q_i表示关节i的角度；ⁱT_i-1表示坐标系i相对于坐标系i-1的方位；ⁱz_i表示相对于坐标系i，沿轴z_i的单位向量的坐标；m_i表示关节i的质量；ⁱS_i表示相对于坐标系i，关节i的质心坐标，其可以表示为[X_i Y_i Z_i]，其中X_i、Y_i、Z_i分别为关节i的质心坐标在坐标系i的x、y、z方向上的三个分量；m_i ⁱS_i表示相对于坐标系i，关节i的一阶转矩，其计算方式为m_i ⁱS_i＝[mX_i mY_imZ_i]，其中mX_i、mY_i、mZ_i分别为关节i的一阶转矩在坐标系i的x、y、z方向上的三个分量；ⁱJ_i表示相对于坐标系i，关节i的惯性矩阵；Γ_i表示电机i期望输出力矩；a_i表示杆件i的加速度，g表示重力向量；

最终得到的机器人的本体动力学模型公式为：

其中，M(q)为惯量矩阵，为Coriolis力和离心力矩阵，为关节摩擦力项，G(q)为重力项，τ_b为各个关节的期望输出力矩矩阵；

(14)通过对式(12)进行模型变换和动力学参数重组，得到关于基础动力学参数的机器人线性化动力学模型为：

其中，为观测矩阵，K_st＝[K₁ ^T,K₂ ^T,…,K_n ^T]^T，为13×n的标准惯性参数集，K_i＝[xx_i,xy_i,xz_i,yy_i,yz_i,zz_i,mx_i,my_i,mz_i,m_i,I_ai,f_vi,f_ci]，xx_i,xy_i,xz_i,yy_i,yz_i,zz_i为6个惯性张量，mx_i,my_i,mz_i为三维坐标轴的一阶惯性矩，m_i为关节i的质量，I_ai为电机i的惯性矩，f_vi表示关节i的粘滞摩擦系数，f_ci表示关节i的库伦摩擦系数；

对标准惯性参数集K_st中部分参数进行消去和重组，获取可标定的最小惯性参数集为：

其中，K为b×1(b＜13n)的最小惯性参数集，为对应的回归矩阵。

(15)机器人末端负载在各关节处产生的作用力矩如下：

τ_l＝J^TF_l (15)；

其中，τ_l为负载在各关节的作用力矩；J^T为机器人雅克比矩阵的转置；F_l为负载作用在机器人末端的广义力。

结合式(14)和(15)，得到带负载工况下机器人完整动力学模型为：

τ＝τ_b+τ_l (16)；

其中，τ,τ_b,τ_l分别为带载工况下，机器人各关节期望输出力矩，机器人本体运动消耗的关节驱动力矩，负载运动消耗的关节驱动力矩。

(16)对模型进行参数线性化，得到机器人末端负载在机器人各个关节的作用力矩关于惯性参数的线性表达式为：

其中，τ_l为负载在各个关节的作用力矩；w_l为关于负载惯性参数的系数矩阵；K_l＝[m_l mx_l my_l mz_l xx_l xy_l xz_l yy_l yz_l zz_l]^T为负载的相关惯性参数；m_l为负载质量；mx_l,my_l,mz_l分别为负载相对于末端坐标系的侧倾力矩，俯仰力矩，横摆力矩；

为相对于末端坐标系，负载的惯性矩阵。

(2)优化激励轨迹生成

选取五阶傅里叶级数作为关节激励轨迹形式，以最小条件数为优化指标，通过遗传算法求解机器人负载辨识最优激励轨迹。具体步骤包括：

(21)选取五阶周期性傅里叶级数作为激励轨迹，其表达式为：

其中，q_i,0为关节位置初始值，ω_f为基频，N为谐波个数，每个傅里叶级数有2N+1个参数，对应于q_i,0,a_i,k,b_i,k(k＝1,2,...,N)，k＝1,2,…,N表示傅里级数阶数；

(22)确定激励轨迹选取时的约束条件

①关节空间约束

机器人在关节空间的约束为：

其中，q_li、q_Fi分别为关节i的位置下限和上限，为关节i速度的绝对值的上限值，为关节i加速度的绝对值的上限值；

②笛卡尔空间约束

机器人在笛卡尔空间满足的约束为：

其中，S代表机器人可以获得的工作空间，{s(q)}表示机器人末端的运动轨迹；

激励轨迹选取对应的目标函数为：

F(X)＝f+P (21)；

其中，X为决策变量，F(X)为目标函数，f＝Cond(W)为回归矩阵W的条件数；P为惩罚系数，由于f的值通常在几千以内，当激励轨迹不满足约束条件时，直接给其加上一个相对非常大的惩罚值10^8，将该激励轨迹直接从优化算法中剔除，如果该激励轨迹满足约束条件，则不添加惩罚值；

机器人激励轨迹选取问题归结为多变量、非线性优化问题，对应的优化问题的数学模型为：

(23)通过遗传算法求解式(19)对应的激励轨迹选取优化问题，得到最优激励轨迹。

如图3所示，遗传算法求解包括以下步骤：

(a)在系统变量中选取合适的决策变量X作为算法的个体，确定种群规模N_p、变异概率pe、交叉概率CR、最大进化代数i_max等算法控制参数；

(b)生成初始种群P₀；

(c)对第i代种群进行交叉、变异操作；

(d)计算父代种群和交叉变异后的种群适应度值，选择适应度值最高的N_p个体生成下一代种群P_i+1；

(e)如果达到最大进化代数则终止，选取当前种群中的最优个体X_best,i+1作为轨迹规划的解；否则i＝i+1，转到(c)。

(3)负载惯性参数辨识

驱动机器人按照最优激励轨迹运动，采样机器人关节角度和电机电流值，采用递推最小二乘法，计算负载惯性参数。如图4所示，具体步骤包括：

(31)设置递推参数初始值设置递推参数初值P₀(一般选取各元素为0，P₀＝αI，其中α为大的实数)；

(32)驱动机器人跟随参考轨迹运动，在采样周期内，对关节角度、角速度、角加速度、负载作用力矩进行采集，获取新的观测矩阵w_lk和负载作用力矩矩阵τ_lk；

更进一步的，对步骤(32)中采集的数据进行滤波处理，得到关节角速度和角加速度的具体步骤如下：

①通过快速傅里叶变换求取各关节角度采样值的频谱并滤波；

②求取关节角速度频谱：在第①步求解的关节位置频谱上乘以下式得到关节角速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5} (23)；

③求取关节角加速度频谱：在第②步求解的角速度频谱上乘以下式得到关节角加速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5} (24)；

其中，f_s表示采样频率，k表示频谱阶数；

④将关节角度采样值的频谱、位置频谱、关节角速度频谱和关节角加速度频谱分别作快速傅里叶逆变换，从而计算出各关节角度、角速度和角加速度。

(33)根据式(23)～式(25)所示的最小递推二乘RLS法计算当前负载惯性参数递推估计值

RLS估算法表示为：

其中，其中，和分别为根据前k-1次和前k次采样数据估算得到的负载惯性参数；w_lk为负载惯性参数观测矩阵在第k次采样得到的超定矩阵；τ_lk为负载作用力矩在第k次采样得到的超定矩阵，E_k称为增益矩阵。W_k＝[w_l1 w_l2 … w_lk]^T为负载惯性参数观测矩阵在前k次采样得到的超定矩阵。

(34)如果第(33)步估算出的惯性参数值满足精度要求，则直接输出，否则在采样周期内转回到第(33)步，继续循环计算。

(4)基于负载惯量匹配的机器人力矩补偿控制

在基于负载惯量匹配的基础上，补偿机器人带载工况下运动时负载所消耗的关节驱动力矩。如图5所示，具体步骤包括：

(1)根据生成的机器人运动轨迹，结合第(3)步辨识出的负载惯性参数，得到机器人带载工况下运动时负载所消耗的关节驱动力矩τ_l。

(2)考虑负载影响的机器人力矩补偿控制原理,将第(1)步得到的负载消耗的关节驱动力矩τ_l作为力矩前馈控制模块，添加到传统伺服三闭环PID控制系统中，以改善机器人的运动性能，整体系统的控制律如式(26)所示

其中，u为各电机的驱动力矩；u_p为伺服三闭环PID控制输入力矩；τ_l为基于负载惯量匹配的力矩补偿控制输入。

总之，本发明的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，包括首先根据机器人整体的受力平衡分析，在考虑摩擦的基础上采用牛顿-欧拉法推导建立带负载工况下的机器人动力学模型；其次选取五阶傅里叶级数作为关节激励轨迹形式，以最小条件数为优化指标，通过遗传算法求解机器人负载辨识最优激励轨迹；然后通过机器人跟随最优激励轨迹运动的实验，实时采集关节角度与电机驱动电流，并采用递推最小二乘法在线估计负载惯性参数；最后基于负载惯量匹配原理设计力矩补偿控制律，以补偿机器人带载工况下运动时负载所额外消耗的关节驱动力矩。本发明方法考虑了负载对机器人作业性能的影响，在无需任何外部传感器的前提下，通过简单易实现的操作完成了对任意负载的自适应辨识和力矩补偿控制，从而可解决末端负载对机器人所带来的控制精度降低问题，有效提高机器人带载工况下的控制精度，对于高速、大负载运动下的工业机器人搬运、搅拌摩擦焊等工艺具有十分重要的意义。

Claims (7)

1.一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，包括以下步骤：

(1)带负载工况下的机器人动力学建模

根据机器人整体的受力平衡分析，在考虑摩擦的基础上采用牛顿-欧拉法推导建立带负载工况下的机器人动力学模型；

(2)优化激励轨迹

选取五阶傅里叶级数作为关节激励轨迹形式，以最小条件数为优化指标，通过遗传算法求解机器人负载辨识最优激励轨迹；

(3)负载惯性参数辨识

驱动机器人按照最优激励轨迹运动，采样机器人关节角度和电机电流值，采用递推最小二乘法，计算负载惯性参数；

(4)基于负载惯量匹配的机器人力矩补偿控制

在基于负载惯量匹配的基础上，补偿机器人在带载工况下运动时负载所消耗的关节驱动力矩，并通过实验进一步验证和优化。

2.根据权利要求1所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，步骤(1)包括以下步骤：

(11)根据机器人关节受力分析，确定相对于各关节首端坐标系、作用于各关节上的力和力矩公式，对于关节i，坐标系i建立在关节i的首端，坐标系i+1建立在关节i与关节i+1交汇处，牛顿-欧拉等式给出相对于坐标系i，作用在关节i上的力和力矩公式为：

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (1)；

其中，为关节i单独运动时产生的相对于坐标系i的力，为关节i单独运动时产生的相对于坐标系i的力矩，ⁱp_i+1为坐标系i+1相对于坐标系i的位移矢量，ⁱf_ei为环境作用在关节i上的力，ⁱτ_ei为环境作用在关节i上的力矩，ⁱf_i为相对于坐标系i，作用在关节i上的力，ⁱτ_i为相对于坐标系i，作用在关节i上的力矩，ⁱT_i+1为坐标系i+1相对于坐标系i的转换矩阵，r_i为ⁱf_ei的力臂，转轴为坐标系i的z轴，ⁱ⁺¹f_i+1为相对于坐标系i+1，作用在关节i+1上的力，ⁱ⁺¹τ_i+1为相对于坐标系i+1，作用在关节i+1上的力矩；

(12)考虑关节摩擦力的影响，建立关节简化线性摩擦力模型，摩擦力模型为：

其中，τ_fi表示关节i的摩擦力，f_ci表示关节i的库伦摩擦系数，f_vi表示关节i的粘滞摩擦系数，表示关节i的角速度；

(13)通过牛顿-欧拉法迭代过程，进行六次正向迭代和六次逆向迭代，得到各关节期望输出力矩模型；

(14)通过对机器人的本体动力学模型公式进行模型变换和动力学参数重组，得到关于基础动力学参数的机器人线性化动力学模型为：

其中，为观测矩阵，K_st＝[K₁ ^T,K₂ ^T,…,K_n ^T]^T，为13×n的标准惯性参数集，K_i＝[xx_i,xyi,xz_i,yy_i,yz_i,zz_i,mx_i,my_i,mz_i,m_i,I_ai,f_vi,f_ci]，xx_i,xy_i,xz_i,yy_i,yz_i,zz_i为6个惯性张量，mx_i,my_i,mz_i为三维坐标轴的一阶惯性矩，m_i为关节i的质量，I_ai为电机i的惯性矩，f_vi表示关节i的粘滞摩擦系数，f_ci表示关节i的库伦摩擦系数；

对标准惯性参数集K_st中部分参数进行消去和重组，获取可标定的最小惯性参数集为：

其中，K为b×1(b＜13n)的最小惯性参数集，为对应的回归矩阵；

(15)机器人末端负载在各关节处产生的作用力矩如下：

τ_l＝J^TF_l (6)；

其中，τ_l为负载在各关节的作用力矩；J^T为机器人雅克比矩阵的转置；F_l为负载作用在机器人末端的广义力；

结合式机器人的本体动力学模型公式和式(4)，得到带负载工况下机器人完整动力学模型为：

τ＝τ_b+τ_l (7)；

其中，τ,τ_b,τ_l分别为带载工况下，机器人各关节期望输出力矩，机器人本体运动消耗的关节驱动力矩，负载运动消耗的关节驱动力矩；

(16)对模型进行参数线性化，得到机器人末端负载在机器人各个关节的作用力矩关于惯性参数的线性表达式为：

其中，τ_l为负载在各个关节的作用力矩；w_l为关于负载惯性参数的系数矩阵；K_l＝[m_lmx_l my_l mz_l xx_l xy_l xz_l yy_l yz_l zz_l]^T为负载的相关惯性参数；m_l为负载质量；mx_l,my_l,mz_l分别为负载相对于末端坐标系的侧倾力矩、俯仰力矩和横摆力矩；

为相对于末端坐标系，负载的惯性矩阵。

3.根据权利要求2所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，步骤(13)中迭代过程如下：

牛顿-欧拉法单步正向迭代过程为：给定关节0的初值⁰w₀、a₀以及输入机器人关节1的角度q₁，计算角速度角加速度根据公式(9)-(11)求出关节1的¹w₁、a₁，进而根据公式(12)和(13)求出关节1的¹f_o1、¹τ_o1；然后输入关节2的角度q₂，计算角速度角加速度求出关节2的²w₂、a₂、²f_o2、²τ_o2；直到迭代求出末端杆件n的ⁿw_n、a_n、ⁿf_on、ⁿτ_on；

单步逆向迭代过程为：给定作用在末端法兰盘上的力和力矩ⁿ⁺¹f_n+1,ⁿ⁺¹τ_n+1，根据公式(14)和(15)计算出连杆n-1作用在连杆n上的力ⁿf_n和力矩ⁿτ_n；然后，将其变换到坐标系n-1中得^n-1f_n、^n-1τ_n，结合在正向迭代过程中计算出的^n-1f_o(n-1)、^n-1τ_o(n-1)和已知的环境作用力^{n- 1}f_e(n-1)、^n-1τ_e(n-1)，求出关节n-2作用在关节n-1上的力^n-1f_n-1和力矩^n-1τ_n-1；以此类推，直至迭代计算出关节0作用在关节1上的力¹f₁、¹τ₁；

ⁱf_i＝ⁱf_Oi+ⁱT_i+1 ⁱ⁺¹f_i+1-ⁱf_ei (14)；

ⁱτ_i＝ⁱτ_Oi+ⁱT_i+1 ⁱ⁺¹τ_i+1+ⁱp_i+1×ⁱ⁺¹f_i+1-ⁱτ_ei-ⁱf_ei×r_i (15)；

Γ_i＝ⁱτ_i·[0 0 1]^T (16)；

其中，ⁱw_i表示相对于坐标系i，关节i的角速度，ⁱw_i-1表示相对于坐标系i-1，关节i的角速度；q_i表示关节i的角度；ⁱT_i-1表示坐标系i相对于坐标系i-1的方位；ⁱz_i表示相对于坐标系i，沿轴z_i的单位向量的坐标；m_i表示关节i的质量；ⁱS_i表示相对于坐标系i，关节i的质心坐标，其表示为[X_i Y_i Z_i]，其中X_i、Y_i、Z_i分别为关节i的质心坐标在坐标系i的x、y、z方向上的三个分量；m_i ⁱS_i表示相对于坐标系i，关节i的一阶转矩，其计算方式为m_i ⁱS_i＝[mX_i mY_imZ_i]，其中mX_i、mY_i、mZ_i分别为关节i的一阶转矩在坐标系i的x、y、z方向上的三个分量；ⁱJ_i表示相对于坐标系i，关节i的惯性矩阵；Γ_i表示电机i期望输出力矩；a_i表示杆件i的加速度，g表示重力向量；

最终得到的机器人的本体动力学模型公式为：

其中，M(q)为惯量矩阵，为Coriolis力和离心力矩阵，为关节摩擦力项，G(q)为重力项，τ_b为各个关节的期望输出力矩矩阵。

4.根据权利要求1所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，步骤(2)包括以下步骤：

(21)选取五阶周期性傅里叶级数作为激励轨迹，其表达式为：

其中，q_i,0为关节位置初始值，ω_f为基频，N为谐波个数，每个傅里叶级数有2N+1个参数，对应于q_i,0,a_i,k,b_i,k(k＝1,2,...,N)，k＝1,2,…,N表示傅里级数的阶数；

(22)确定激励轨迹选取时的约束条件

①关节空间约束机器人在关节空间的约束为：

其中，q_li、q_Fi分别为关节i的位置下限和上限，为关节i速度的绝对值的上限值，为关节i加速度的绝对值的上限值；

②笛卡尔空间约束

机器人在笛卡尔空间满足的约束为：

其中，S代表机器人获得的工作空间，{s(q)}表示机器人末端的运动轨迹；

激励轨迹选取对应的目标函数为：

F(X)＝f+P (21)；

其中，X为决策变量，F(X)为目标函数，f＝Cond(W)为回归矩阵W的条件数；P为惩罚系数；

机器人激励轨迹选取问题归结为多变量、非线性优化问题，对应的优化问题的数学模型为：

(23)通过遗传算法求解式(19)对应的激励轨迹选取优化问题，得到最优激励轨迹。

5.根据权利要求4所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，所述步骤(23)包括以下步骤：

(a)在系统变量中选取合适的决策变量X作为算法的个体，确定种群规模N_p、变异概率pe、交叉概率CR和最大进化代数i_max；

(b)生成初始种群P₀；

(c)对第i代种群进行交叉、变异操作；

(d)计算父代种群和交叉变异后的种群适应度值，选择适应度值最高的N_p个体生成下一代种群P_i+1；

(e)如果达到最大进化代数则终止，选取当前种群中的最优个体X_best,i+1作为轨迹规划的解；否则i＝i+1，转到(c)。

6.根据权利要求1所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，步骤(3)包括以下步骤：

(31)设置递推参数初始值设置递推参数初值和P₀；

(32)驱动机器人跟随参考轨迹运动，在采样周期内，对关节角度、角速度、角加速度、负载作用力矩进行采集，获取新的观测矩阵w_lk和负载作用力矩矩阵τ_lk；

具体步骤如下：

①通过快速傅里叶变换求取各关节角度采样值的频谱并滤波；

②求取关节角速度频谱：在第①步求解的关节位置频谱上乘以下式得到关节角速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5}；

③求取关节角加速度频谱：在第②步求解的角速度频谱上乘以下式得到关节角加速度频谱；

jω(k)＝j2πk*f_s k＝{0,1,2,…,5}；

其中，f_s表示采样频率，k表示频谱阶数；

④将关节角度采样值的频谱、位置频谱、关节角速度频谱和关节角加速度频谱分别作快速傅里叶逆变换，从而计算出各关节角度、角速度和角加速度；

(33)根据式(23)～式(25)所示的RLS法计算当前负载惯性参数递推估计值

RLS估算法表示为：

其中，其中，和分别为根据前k-1次和前k次采样数据估算得到的负载惯性参数；w_lk为负载惯性参数观测矩阵在第k次采样得到的超定矩阵；τ_lk为负载作用力矩在第k次采样得到的超定矩阵，E_k称为增益矩阵；W_k＝[w_l1 w_l2 … w_lk]^T为负载惯性参数观测矩阵在前k次采样得到的超定矩阵；

(34)如果步骤(33)估算出的惯性参数值满足精度要求，则直接输出，否则在采样周期内转回到步骤(33)，继续循环计算。

7.根据权利要求1所述的一种基于负载自适应辨识的工业机器人力矩补偿控制方法，其特征在于，步骤(4)包括以下步骤：

结合步骤(3)辨识出的负载惯性参数，考虑负载影响的机器人力矩补偿控制原理，通过在传统伺服三闭环PID控制系统中添加基于动力学模型的力矩前馈控制模块，以改善机器人的运动性能，整体系统的控制律如下式所示：

其中，u为各电机的驱动力矩；u_p为伺服三闭环PID控制输入力矩；τ_l为基于负载惯量匹配的力矩补偿控制输入；w_l为关于负载惯性参数的系数矩阵；K_l为负载的相关惯性参数。