CN107703747B - 一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法 - Google Patents

Abstract

本发明公开了一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，包括建立机器人的动力学模型；求解机器人参数标定最优激励轨迹；采样机器人关节角度和电机电流值，并进行滤波预处理；进行参数自标定，并通过实验进一步验证和优化。本发明方法可为基于动力学模型的机器人控制方法设计提供精确的动力学模型，同时，在进行机器人动力学参数自标定时，无需任何附加的外在传感器，方法简单易实现，可集成在控制器中，实现机器人动力学参数的高精度在线自标定，对于提高机器人搅拌摩擦焊接生产过程的控制精度和性能具有重要的作用，且具有非常重要的经济价值和应用前景。

Description

一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定 方法

技术领域

本发明涉及工业机器人参数标定方法，特别是涉及一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法。

背景技术

随着高速重载工业机器人在航空制造，汽车制造等面向搅拌摩擦焊应用行业的广泛应用，对机器人的运动平稳性、控制精度提出越来越高的要求。重载机器人在进行搅拌摩擦焊作业时，由于需要提供较大的末端下压力，导致机器人本身存在抖动和弹性变形，采用传统的比例-积分-微分(PID)控制方法时，由于并未考虑机器人的动力学特性，难以实现较高的性能和控制精度。基于动力学模型的控制方法可以大大提高机器人的动态特性和轨迹跟踪精度，从而有效提高生产效率。其难点在于获取精确的动力学模型，而不精确的动力学模型可能导致系统不稳定。因此，获取精确的动力学模型对于改善面向搅拌摩擦焊应用工艺的机器人控制性能具有非常重要的意义。

为了获取机器人精确的动力学模型，首先需要建立机器人整体的动力学模型数学形式，传统的动力学建模方法主要有拉格朗日法、虚功原理法、凯恩方法等，建模过程复杂，运算量较大，物理意义不明确，不适合采用计算机编程计算，因此不利于集成到机器人控制器中，无法实现参数在线自标定。然后对模型中的动力学参数进行实验标定，传统的模型参数获取方法主要有基于CAD仿真机器人实体模型的理论参数直接提取方法和基于五次多项式激励轨迹、附加外部力矩传感器的机器人动力学参数整体标定方法，这两种方法主要存在以下问题：①实际机器人由于杆件铸造误差和安装误差，通过CAD模型提取的理论参数与实际机器人参数之间的偏差较大，获取的动力学模型不精确；②机器人标定过程中基于五次多项式的激励轨迹实现复杂，缺乏周期性，导致标定结果容易受测量噪声的影响，标定结果不精确；③传统的机器人参数标定方法需要借助于外部力矩传感器，成本较高，不利于控制器集成。

发明内容

发明目的：为了克服已有的技术缺陷，解决现有基于模型的控制方法中精确机器人动力学模型难以获取的问题，一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，为基于动力学模型的机器人控制方法设计提供精确的动力学模型，进而提高机器人的控制精度和运动平稳性。

技术方案：一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，包括以下步骤：

(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)通过牛顿-欧拉法迭代过程，进行六次正向迭代和六次逆向迭代，得到各关节期望输出力矩模型，迭代公式如下：

牛顿-欧拉法单步正向迭代过程为：给定关节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_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)为重力项，Γ为各个关节的期望输出力矩矩阵；

(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)的最小惯性参数集，

为对应的回归矩阵。

进一步的，所述步骤(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 (18)

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

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

Min F(X)＝Min(f+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)驱动机器人沿着最优激励轨迹运动，以预定的采样时间间隔，在机器人运动周期内，采样关节角度和电机电流；

(32)对多次采样数据进行时域平均：

其中，J表示采样周期数，q_ri和τ_ri分别表示第i个周期采样的关节位置和电机力矩，

和分别表示平均化的关节位置和电机力矩，M表示采样次数；

通过快速傅里叶变换(FFT)将经过时域平均的采样数据从时域变换到频域，在频域中对数据进行滤波处理，进而得到关节角速度和关节角加速度值。

更进一步的，所述步骤(32)中在频域中对数据进行滤波处理，得到关节角速度和关节角加速度值的具体步骤为：

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

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

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表示频谱阶数；

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

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

(41)机器人跟踪最优激励轨迹运动时，在采样周期T_s中，对关节位置和电机力矩进行L次采样，得到超定线性方程：

其中，Y_fm＝[τ_t1 τ_t2 … τ_tL]^T，

Y_fm是nL×1的关节力矩向量，τ_ti表示第i次采样的关节力矩，通过采样的电机输出力矩值乘以关节力矩常数获得，

是nL×b的观测矩阵，b表示最小惯性参数个数；

(42)使用最小二乘法求解式(22)的超定线性方程，得到最小惯性参数集K为：

K＝(W_fm ^TW_fm)^-1W_fm ^TY_fm (23)

(43)按照动力学参数标定模型验证过程，首先选择一条验证轨迹，生成

将由标定过程获得的动力学参数向量带入到线性动力学模型中，动力学模型给出关节预测力矩τ_pred，同时让机器人按照该组关节数据运动，测得机器人实际关节力矩τ′_meas，量化比较τ_pred和τ′_meas，最终验证模型的正确性；

采用预测误差的残差均方根来量化模型的准确度，预测误差的残差均方根定义为：

其中，L为采样点总数，每个力矩信号的预测误差残差均方根给出了力矩预测的不准确性，两个值越接近，预测误差残差均方根越小，模型预测结果越好。

有益效果：与现有技术相比，本发明采用牛顿-欧拉法迭代建立机器人的六关节动力学模型，有效解决了传统建模方法建模过程复杂、运算量较大、难以用于计算机编程计算的问题，采取周期性五阶傅里叶级数作为参数标定实验中的机器人激励轨迹，以关节空间和笛卡尔空间约束为约束条件，以矩阵条件数最小为优化性能指标，通过遗传算法求解机器人参数标定最优激励轨迹，有效解决了基于五次多项式形式的机器人标定过程激励轨迹实现复杂，缺乏周期性，导致标定结果不精确的问题。本发明方法可为基于动力学模型的机器人控制方法设计提供精确的动力学模型，同时，在进行机器人动力学参数自标定时，无需任何附加的外在传感器，方法简单易实现，可集成在控制器中，实现机器人动力学参数的高精度在线自标定，对于提高机器人搅拌摩擦焊接生产过程的控制精度和性能具有重要的作用，且具有非常重要的经济价值和应用前景。

附图说明

图1是本发明方法的流程图；

图2是机器人杆件受力图；

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

图4是摩擦力模型图；

图5是遗传算法流程图；

图6是动力学参数标定模型验证过程图。

具体实施方式

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

如图1所示，一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，包括以下步骤：

(1)动力学建模

首先根据单杆件受力分析，获取相对于杆件首端坐标系，作用于各杆件上的力和力矩公式；其次考虑机器人关节摩擦的影响，建立机器人关节摩擦模型；然后通过牛顿-欧拉法迭代计算机器人各关节期望输出力矩模型；最后通过对机器人动力学模型进行模型变换和动力学参数重组，获取关于基础动力学参数的机器人线性化动力学模型。具体步骤包括：

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

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

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

其中，

为杆件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)考虑关节摩擦力的影响，建立如图3所示关节简化线性摩擦模型，摩擦力模型为：

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

表示关节i的角速度。

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

牛顿-欧拉法单步正向迭代过程为：

单步逆向迭代过程为：

ⁱ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的方位；表示相对于坐标系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方向上的三个分量；

表示相对于坐标系i，杆件i的惯性矩阵；ⁱJ_i表示相对于坐标系i，关节i的惯性矩阵；Γ_i表示电机i期望输出力矩，a_i表示杆件i的加速度，g表示重力向量。

正向迭代具体步骤为：给定杆件0的初值⁰w₀、

a₀以及输入机器人关节1的角度q₁，计算角速度角加速度求出杆件1的¹w₁、

a₁，进而求出杆件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，计算出连杆n-1作用在连杆n上的力ⁿf_n和力矩ⁿτ_n；计算出ⁿf_n、ⁿτ_n之后，将其变换到坐标系n-1中得^{n- 1}f_n、^n-1τ_n，结合在正向迭代过程中计算出的^n-1f_o(n-1)、^n-1τ_o(n-1)和已知的环境作用力^n-1f_e(n-1)、^n-1τ_e(n-1)，可以求出连杆n-2作用在连杆n-1上的力^n-1f_n-1和力矩^n-1τ_n-1；以此类推，直至迭代计算出杆件0作用在杆件1上的力¹f₁、¹τ₁。

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

其中，M(q)_6×6为惯量矩阵，

为Coriolis力和离心力矩阵，G(q)_6×1为重力项，Γ_6×1为各个关节的期望输出力矩矩阵。

通过对式(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)的最小惯性参数集，

为对应的回归矩阵。

(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 (18)

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

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

Min F(X)＝Min(f+P)

通过如图5所示的遗传算法流程图，求解式(19)对应的激励轨迹选取优化问题，得到最优激励轨迹。其基本步骤如下：

(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)驱动机器人沿着最优激励轨迹运动，以2ms为采样时间间隔，在机器人运动周期内，采样关节角度和电机电流，电机电流乘以对应关节转矩系数即为电机力矩。

(32)对多次采样数据进行时域平均：

其中，J表示采样周期数，q_i和τ_i分别表示第i个周期采样的关节位置和电机力矩，

和分别表示平均化的关节位置和电机力矩。

通过快速傅里叶变换(FFT)将经过时域平均的采样数据从时域变换到频域，在频域中对数据进行滤波处理，进而得到关节角速度和关节角加速度值，具体步骤如下：

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

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

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

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

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

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

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

(4)参数自标定与模型验证

首先根据经过滤波处理的采样数据，获取关于线性动力学模型的超定线性方程；然后通过选取最小二乘法进行参数自标定。具体步骤包括：

(41)机器人跟踪最优激励轨迹运动时，在采样周期T_s中，对关节位置和电机力矩进行L次采样，得到超定线性方程：

其中，Y_fm＝[τ_t1 τ_t2 … τ_tL]^T，

Y_fm是nL×1的关节力矩向量，τ_ti表示第i次采样的关节力矩，通过采样的电机输出力矩值乘以关节力矩常数获得，

是nL×b的观测矩阵，b表示最小惯性参数个数；

使用最小二乘法求解式(22)的超定线性方程，得到最小惯性参数集K为：

K＝(W_fm ^TW_fm)^-1W_fm ^TY_fm (23)

按照图6所示动力学参数标定模型验证过程，首先选择一条验证轨迹，生成

将由标定过程获得的动力学参数向量带入到线性动力学模型中，动力学模型给出关节预测力矩τ_pred，同时让机器人按照该组关节数据运动，测得机器人实际关节力矩τ′_meas，量化比较τ_pred和τ′_meas，最终验证模型的正确性。

为了提高验证的客观性，采用预测误差的残差均方根来量化模型的准确度，预测误差的残差均方根定义如下：

其中，L为采样点总数。每个力矩信号的预测误差残差均方根给出了力矩预测的不准确性，两个值越接近，预测误差残差均方根越小，模型预测结果越好。当模型验证结果不令人满意时，必须对先前标定过程中的一些步骤(包括对动力学模型进行再次验证、重新选取一条激励轨迹、选取不同的参数标定算法)进行重新考虑，然后重新标定。

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

Claims (4)

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)通过牛顿-欧拉法迭代过程，进行六次正向迭代和六次逆向迭代，得到各关节期望输出力矩模型，迭代公式如下：

牛顿-欧拉法单步正向迭代过程为：给定关节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_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)为重力项，Γ为各个关节的期望输出力矩矩阵；

(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)的最小惯性参数集，

为对应的回归矩阵；

(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 (18)

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

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

Min F(X)＝Min(f+P)

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

(3)驱动机器人按照最优激励轨迹运动，采样机器人关节角度和电机电流值，并采用快速傅里叶变换进行滤波预处理；

(4)采用最小二乘法求解以进行参数自标定，并通过实验进一步验证和优化；包括以下步骤：

(41)机器人跟踪最优激励轨迹运动时，在采样周期T_s中，对关节位置和电机力矩进行L次采样，得到超定线性方程：

其中，Y_fm＝[τ_t1 τ_t2…τ_tL]^T，

Y_fm是nL×1的关节力矩向量，τ_ti表示第i次采样的关节力矩，通过采样的电机输出力矩值乘以关节力矩常数获得，

是nL×b的观测矩阵，b表示最小惯性参数个数；

(42)使用最小二乘法求解式(22)的超定线性方程，得到最小惯性参数集K为：

K＝(W_fm ^TW_fm)^-1W_fm ^TY_fm (23)

(43)按照动力学参数标定模型验证过程，首先选择一条验证轨迹，生成将由标定过程获得的动力学参数向量带入到线性动力学模型中，动力学模型给出关节预测力矩τ_pred，同时让机器人按照该组关节数据运动，测得机器人实际关节力矩τ′_meas，量化比较τ_pred和τ′_meas，最终验证模型的正确性；

采用预测误差的残差均方根来量化模型的准确度，预测误差的残差均方根定义为：

其中，L为采样点总数，每个力矩信号的预测误差残差均方根给出了力矩预测的不准确性，两个值越接近，预测误差残差均方根越小，模型预测结果越好。

2.根据权利要求1所述的一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，其特征在于，所述步骤(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.根据权利要求1所述的一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，其特征在于，所述步骤(3)包括以下步骤：

(31)驱动机器人沿着最优激励轨迹运动，以预定的采样时间间隔，在机器人运动周期内，采样关节角度和电机电流；

(32)对多次采样数据进行时域平均：

其中，J表示采样周期数，q_ri和τ_ri分别表示第i个周期采样的关节位置和电机力矩，

和

分别表示平均化的关节位置和电机力矩，M表示采样次数；

通过快速傅里叶变换(FFT)将经过时域平均的采样数据从时域变换到频域，在频域中对数据进行滤波处理，进而得到关节角速度和关节角加速度值。

4.根据权利要求3所述的一种面向搅拌摩擦焊应用的重载机器人动力学参数自标定方法，其特征在于，所述步骤(32)中在频域中对数据进行滤波处理，得到关节角速度和关节角加速度值的具体步骤为：

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

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

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表示频谱阶数；

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

Images