一种改进的SCARA机器人动力学参数辨识方法与流程

本发明属于scara机器人的动力学参数辨识领域，具体涉及一种改进的scara机器人动力学参数辨识方法。

背景技术：

随着工业机器人应用于生命医疗、激光焊接、汽车电子等高精密领域，机器人技术向高速高精度方向发展。工业机器人的控制策略可以分为两种：(1)按照机器人实际轨迹与期望轨迹间的偏差进行负反馈控制。这种控制方法控制律简单，易于实现，称为“运动控制”，例如pid控制、模糊控制、鲁棒控制等。(2)充分考虑机器人的动力学特性，设计出更加精细的非线性控制律，即基于模型的控制策略，这种控制方法称为“动态控制”，例如重力补偿控制、计算力矩法、内模控制等。

由于机器人时变、强耦合和非线性的动力学特性，仅仅基于反馈的“运动控制”策略，难以满足工业机器人高速高精度的控制要求。目前，国产工业机器人大部分仍采用传统pid控制策略。所以，设计基于机器人动力学模型的控制策略是实现高速高精度运动控制的有效方法，该类控制策略要以机器人的精准动力学参数为基础。机器人动力学参数的获取方法可分为：解体测量法、cad法以及整体辨识法。机器人结构复杂，很多参数难以直接测量得到，采用计算机建模的cad法忽略了机器人的装配误差和材料的分布特性，整体辨识法能够考虑机器人实际工作中的各种因素影响，因而受到了广泛关注。

机器人动力学参数的辨识过程中，激励轨迹一般采用傅里叶级数的形式，既可以保证激励轨迹的连续周期性，以便于多次重复采样，又能充分激励机器人的动力学特性。但传统傅里叶级数无法保证机器人关节的角速度和角加速度在轨迹的起始和结束时刻为零；并且未经优化的傅里叶级数会导致观测矩阵的条件数过大，导致力矩和关节位置等的采样误差会过于放大动力学参数的辨识误差。由于观测矩阵条件数是多峰函数，当采用基本遗传算法(sga)对傅里叶级数进行优化时，容易导致局部最优解，优化结果容易早熟。

另外，在采样数据处理时，除了多次采样求均值，以提高信噪比之外，如何获取有效角加速度信号是一个比较棘手的问题。由于采样得到的速度信号存在噪声，直接对速度信号进行差分，得到的加速度信号有很大误差。所以，一般通过对采样得到的关节角度进行反向拟合，再两次微分求角加速度，但两次微分容易造成传递误差。

技术实现要素：

针对具体scara机器人，采用5阶傅里叶级数做为激励轨迹的基本形式，并用5阶多项式取代传统傅里叶级数中的常数项，使得机器人各关节角速度和角加速度在激励轨迹的起始和结束时刻为零。为增强全局寻优能力，采用基于排挤机制的小生境遗传算法对傅里叶级数的系数进行优化，尽可能减小观测矩阵的条件数。为避免直接对关节角度两次微分带来的传递误差，对采样得到的关节角速度数据拟合成傅里叶级数形式，再微分得到关节加速度信号。

为实现以上的技术目的，本发明提供的一种改进的scara机器人动力学参数辨识方法，包括以下步骤：

(1)根据实际情况，采用lagrange法建立包含摩擦项的scara机器人完整动力学模型；

(2)对所述步骤(1)中完整动力学模型进行线性化，将其表示为回归矩阵和机器人动力学参数向量乘积的形式；

(3)采用5阶傅里叶级数做为激励轨迹的基本形式，并用5阶多项式取代传统傅里叶级数中的常数项，使得机器人各关节角速度和角加速度在激励轨迹的起始和结束时刻为零；

(4)为增强全局寻优能力，采用基于排挤机制的小生境遗传算法对步骤(3)中得到的傅里叶级数的系数进行优化，尽可能减小观测矩阵的条件数；

(5)通过高精度编码器采集关节角度和角速度，为避免速度测量的噪声给加速度计算带来的影响，对采样得到的关节角速度数据拟合成傅里叶级数形式，再对关节速度的傅里叶级数进行微分，得到关节加速度信号，避免直接对关节角度两次微分带来的传递误差；

(6)根据采样得到的机器人各个关节输入力矩，以及各个关节输出的角度、角速度和角加速度，构造观测矩阵，采用加权最小二乘法对动力学参数进行辨识。

进一步地，所述步骤(3)中得到改进傅里叶级数的激励轨迹如下：

解得5次多项式的系数为：

式中：i表示第i个关节，ωf为傅里叶级数的基频，每个关节的基频相同，ai,k、bi,k为傅里叶级数的系数，θ0i为偏移量，m为傅里叶级数的阶数，决定轨迹的带宽，ai,k、bi,k、θ0i为自由系数；t0＝0，ttf＝2π/(2πωf)，分别表示周期运动的起始和结束时刻。

进一步地，所述步骤(4)中基于排挤机制的小生境遗传算法与传统遗传算法相比具有以下区别：

记第i个个体为pi，第j个个体为pj，则个体pi和pj之间的欧式距离如下式所示；

||pi-pj||

取一较小整数ξ，f(·)表示个体的适应度，在对每一代进行选择、交叉、变异操作之前，比较种群中任意两个个体的相似性，若||pi-pj||＜ξ，则表明pi和pj的相似程度较大；通过对适应度较小者施加一较强的惩罚函数，是其适应度变得极小，从而排挤掉其中适应度较小者；即若：f(pi)＞f(pj)，则令f(pi)＝δ·f(pj)(δ为一很小的正数)，则在以后的进化中f(pj)会以极大的概率被淘汰掉。

进一步地，所述步骤(5)中关节速度的傅里叶级数表达为：

采用线性最小二乘法得到参数ai,k、bi,k，再对关节速度的傅里叶级数进行微分，得到关节加速度信号。

如附图1所示，本发明的工作原理：将上述步骤(3)和步骤(4)优化改进后的傅里叶级数作为机器人各关节激励轨迹，运行时对机器人各关节输入力矩、输出角度、角速度进行采样；对采样得到的关节速度信号反向拟合成优化后傅里叶级数的速度表达形式，再通过微分获取准确的关节加速度信号，这样避免了直接对关节位置信号反向拟合，再两次微分求角加速度带来的传递误差；最后考虑测量噪声的影响，采用加权最小二乘对动力学参数进行辨识。

根据以上方案，可实现以下的有益效果：

(1)针对传统傅里叶级数无法保证关节角速度和角加速度在轨迹起始和结束时刻为零，导致机器人运行时抖振的问题，提出用5阶多项式取代传统傅里叶级数中的常数项，使得机器人各关节角速度和角加速度在激励轨迹的起始和结束时刻为零。

(2)针对未经优化的傅里叶级数会导致观测矩阵条件数过大的问题，采用全局寻优能力更强的基于排挤机制的小生境遗传算法对傅里叶级数系数进行优化，尽可能减小观测矩阵的条件数，提高动力学参数的辨识精度。

(3)针对传统方法直接对关节角度进行反向拟合，再两次微分求角加速度，带来的传递误差问题，提出直接对关节角速度进行反向拟合，再求微分获取角角速度，避免了两次微分带来的传递误差。

附图说明

图1为本发明所述的一种改进的scara机器人动力学参数辨识方法的辨识流程图；

图2为本发明所述scara机器人的本体结构图；

图3为本发明所述scara机器人连杆坐标系；

图4基于排挤机制的小生境遗传算法进行傅里叶级数优化的原理图。

具体实施方式

为使本发明的目的、技术方案和带来的有益效果更加的清楚明白，下面参照附图，结合具体scara机器人，对本发明作进一步详细说明。

步骤(1)：采用lagrange法建立scara机器人前两关节完整动力学模型。

由附图2和附图3可见scara机器人第3、4两关节公用一个连杆，第2、3、4关节的伺服电机、同步带、滚珠丝杆、升降台等都安装在关节2的连杆上，因此关节1、关节2决定scara机器人的平面定位精度和轨迹跟踪精度，关节3决定了竖直方向上的运动。由于关节3与前两关节垂直，所以前两关节的运动不会对关节3产生影响。所以，仅对关节1和关节2进行动力学建模及动力学参数辨识。

考虑到关节摩擦，scara机器人动力学模型可以用二阶非线性微分方程来表示，具体如式(1)所示。

式中，θ∈rⁿ为关节角度矢量，为关节角速度矢量，为关节角加速度矢量，d(θ)∈r^n×n为机器人惯性矩阵，为离心力和哥氏力矩阵，g(θ)∈rⁿ为重力力矩向量，为摩擦力矩向量，τ∈rⁿ为控制力矩向量。

将式(1)表示为矩阵的标量形式，如式(2)所示：

根据scara机器人结构，利用拉格朗日法对scara机器人进行动力学建模，得到式(2)中的各项参数如下：

d21＝d12

c0＝m2l1(x2sin(θ2)+l2sin(θ2)+y2cos(θ2))

g1＝0,g2＝0

式中，m1、m2分别为连杆1和连杆2的质量，l1、l2分别为连杆1和连杆2的长度，(x1,y1,z1)、(x2,y2,z2)分别为前两连杆的质心在坐标系1和坐标系2中的位置坐标，izz1、izz2分别为连杆1和连杆2沿z轴的转动惯量，fci和fvi分别表示第i关节的库伦摩擦系数和粘性摩擦系数。

步骤(2)：对完整动力学模型进行线性化，将其表示为回归矩阵和机器人动力学参数向量乘积的形式。

对于工业机器人，存在一依赖于机器人参数的参数向量，使得机器人动力学方程可以线性化，即：

式中，是由机器人广义坐标及其各阶导数构成的关节变量函数；φ∈r^p为包含摩擦系数的未知定常矩阵，称为基本参数集(bps，baseparameterset)，即机器人待辨识的动力学参数，n为关节数目，p为待辨识的动力学参数数目。

根据式(2)的scara机器人动力学方程具体形式，得到线性化方程的显示表达如式(4)所示。

式中各项参数为：

φ2＝m2(x2+l2)

φ3＝m2y2

φ5＝fc1,φ6＝fv1,φ7＝fc2,φ8＝fv2

y21＝0

步骤(3)：传统傅里叶级数的改进。

传统的m阶傅里叶级数定义为：

式中：i表示第i个关节，ωf为傅里叶级数的基频，每个关节的基频相同，ai,k、bi,k为傅里叶级数的系数，θ0i为偏移量，m为傅里叶级数的阶数，决定轨迹的带宽，ai,k、bi,k、θ0i为自由系数。

相应的关节运行速度和加速度如下：

在机器人动力学参数辨识的过程中，为了保证运行的连续周期性，以及起始和结束时刻的平稳性，需要激励轨迹满足以下条件：

式中，t0＝0，ttf＝2π/(2πωf)，分别表示周期运动的起始和结束时刻，传统的傅里叶级数难以保证上述条件。

为满足上述条件，对传统傅里叶级数的激励轨迹进行改进，将常数项θ0i用5次多项式来代替，得到改进傅里叶级数的激励轨迹如下：

解得5次多项式的系数为：

步骤(4)：傅里叶级数系数的优化。

机器人动力学参数辨识包括：机器人各关节跟踪特定的激励轨迹、通过采样或间接计算得到关节的输入力矩、关节角度、角速度、角加速度，估计动力学参数φ。机器人各关节跟踪激励轨迹时，在采样时刻t1,t2,…,tn对关节输入力矩等进行n次间接采样，则可以得到式(10)所示方程组。

yn·φ＝τn(10)

即：

式中，yn为由θ、构成的观测矩阵，右侧为不同采样时刻的两关节输出力矩向量。

定义观测矩阵yn的条件数为cond(yn)＝||yn^-1||·||yn||，则为了减小测量误差对动力学参数辨识的影响，需要对傅里叶级数进行优化，以得到最小的条件数。

确定激励轨迹的形式之后，以一定频率对式(9)所示的改进傅里叶级数进行采样，得到对应的关节角度、角速度和角加速度，共采集n组数据，从而构造得观测矩阵yn。以观测矩阵yn的条件数作为目标函数，即为适应度函数，为增强全局寻优能力，采用基于排挤机制的小生境遗传算法对傅里叶级数的系数进行优化，获得最小条件数时对应的系数ai,k、bi,k。

基于排挤机制的小生境遗传算法寻优步骤如下：

1)选择编码策略：取m＝5，改进的傅里叶级数共有20个待优化参数，因此将20个参数进行实数编码，构成可行解的集合；

2)确定适应度函数：根据式(11)计算得到观测矩阵yn，并以其条件数作为目标函数，即为适应度函数；

3)建立排挤机制：记第i个个体为pi，第j个个体为pj，则个体pi和pj之间的欧式距离如式(12)所示。

||pi-pj||(12)

取一较小整数ξ，f(·)表示个体的适应度值。在对每一代进行选择、交叉、变异操作之前，比较种群中任意两个个体的相似性，若||pi-pj||＜ξ，则表明pi和pj的相似程度较大。通过对适应度较小者施加一较强的惩罚函数，是其适应度变得极小，从而排挤掉其中适应度较小者。即若：f(pi)＞f(pj)，则令f(pi)＝δ·f(pj)(δ为一很小的正数)，则在以后的进化中f(pj)会以极大的概率被淘汰掉。

3)确定遗传算法参数：共进化400代，设置种群规模为50，交叉概率为0.4，变异概率为0.1；

4)选择操作：种群中个体的选择操作采用轮盘赌法，并且将种群中个体适应度值取倒数，使得适应度值越小的个体在轮盘中被选中的概率越大。选择操作控制进化方向，使得下一代种群优于上一代种群；

5)交叉操作：以一定概率对种群中的个体进行染色体交叉操作，被选中染色体的交叉位置相同；

6)变异操作：对种群中染色体以一定概率进行变异操作，得到新的种群个体；

7)判断是否已完成预定的进化代数，达到则停止进化。基于排挤机制的小生境遗传算法进行傅里叶级数优化的原理图如附图4所示。

步骤(5)：关节角度、角速度和角加速度的采样与处理。

采用优化改进后的傅里叶级数作为机器人激励轨迹，即作为scara机器人两关节的位置给定，在伺服驱动器作用下进行运行，由高精度编码器采集关节输出角度和角速度。

然而，采样得到的数据易受测量噪声的影响。所以多次重复执行激励轨迹，然后对采样得到的关节角度、角速度取均值，以提高信噪比，即：

式中，l为激励轨迹执行的周期数，j为第j个运行周期，θj(g)、分别表示第j个运行周期中第g个采样点处的关节角度、角速度。

为了避免速度测量的噪声给加速度计算带来的影响，传统方法是根据采样得到的各关节角度数据，反向拟合成傅里叶级数形式，再根据微分得到各关节角速度和角加速度。但目前伺服电机一般采用高精度编码器，根据关节角度求角速度也经过滤波处理，得到的角速度信号比较准确，所以本发明仅对采样得到的关节角速度数据拟合成傅里叶级数形式，关节速度的傅里叶级数表达为：

采用线性最小二乘法得到参数ai,k、bi,k，再对关节速度的傅里叶级数进行微分，得到关节加速度信号，避免了两次微分带来的传递误差。

步骤(6)：采用加权最小二乘法对惯性参数进行辨识。

根据采样得到的关节角度、角速度和角加速度，求得观测矩阵yn，并对关节输入力矩进行采样，得到力矩向量τn。

式(11)的求解可采用加权最小二乘法对其进行求解。在数据的采样中，关节角度和角速度是通过高分辨率的光电编码器经过滤波处理后测得，相比于关节力矩的测量噪声可以忽略不计。此时，惯性φ参数的加权最小二乘估计为：

式中，是动力学参数向量的估计值，σ为力矩测量噪声方差的对角矩阵。力矩测量噪声方差通过下式进行估算：

式中，l为激励轨迹执行的周期数，ν为一个周期内采样的点数，τij(g)表示第j周期内第i关节在第g次采样得到的力矩输出，表示第i关节在第g次采样得到的力矩输出均值。

如附图1所示，本发明的工作原理：将上述步骤(3)和步骤(4)优化改进后的傅里叶级数作为机器人各关节激励轨迹，运行时对机器人各关节输入力矩、输出角度、角速度进行采样；对采样得到的关节速度信号反向拟合成优化后傅里叶级数的速度表达形式，再通过微分获取准确的关节加速度信号，这样避免了直接对关节位置信号反向拟合，再两次微分求角加速度带来的传递误差；最后考虑测量噪声的影响，采用加权最小二乘对动力学参数进行辨识。

以上所述，仅为发明较佳的具体实施方式，但发明的保护范围并不局限于此，任何熟悉该技术的人在发明所揭露的技术范围内，均可想到的变化或替换都涵盖在发明的保护范围之内。因此，发明的保护范围应该以权利要求保护的范围为准。