一种多关节机械臂动力学参数辨识激励轨迹的优化方法与流程

本发明涉及一种多关节机械臂动力学参数辨识激励轨迹优化方法。

背景技术：

在近些年来，基于动力学模型的控制对于提升多关节机械臂动态性能变得越来越重要。但因多关节机械臂的动力学的复杂性，致使难以获得精确的动力学参数，而不精确的动力学参数导致无法建立精确的动力学模型，使得基于模型的控制难以实现。故通过动力学参数辨识方法掌握精确的动力学模型和动力学参数的研究引起重视。

精确的动力学参数辨识取决于多关节机械臂的动力学模型推导、激励轨迹的设计与优化、辨识数据的采集与清洗、辨识算法四个部分。设计和优化辨识激励轨迹能充分激励机械臂的动力学性能，较大程度地避免建模的不精确、实际测量数据噪声等带来的扰动因素，进而减小动力学参数辨识的偏差，提高机械臂动力学参数的辨识精度。

常用的寻优求解方法主要有fmincon、粒子群算法及改进的遗传算法等，这些方法都是在激励轨迹约束边界内找到一条非局部的最小优化目标值。激励轨迹优化的fmincon方法，很容易陷入局部极值，需要进行大量筛选工作。遗传算法通过模拟生物界的遗传特点，从概率角度采用随机方式搜索优化问题在其约束范围内的最优解，但是生物界进化的不可知性导致遗传算法收敛速度慢和早熟，迭代次数多，易陷入局部极值，从而影响了动力学参数辨识。

技术实现要素：

本发明是为了解决上述现有技术存在的不足之处，提供一种多关节机械臂动力学参数辨识激励轨迹的优化方法，以期能为多关节机械臂动力学参数辨识优化出较好的激励轨迹，且优化的辨识激励轨迹满足边界要求并具有较好的激励性，从而为进一步提升多关节机械臂动力学性能奠定基础。

本发明解决技术问题采用如下技术方案：

本发明一种多关节机械臂动力学参数辨识激励轨迹的优化方法的特点是按如下步骤进行：

步骤1、运用牛顿-欧拉法建立多关节机械臂的动力学模型，线性化处理所述动力学模型中的非线性项，从而获得多关节机械臂线性化的动力学模型；

步骤2、基于随机数模拟参数法求解所述多关节机械臂线性化的动力学模型，得到多关节机械臂的最小动力学参数集，并获得多关节机械臂动力学模型的简化模型；

步骤3、建立多关节机械臂动力学参数辨识激励轨迹的数学模型，并根据所述多关节机械臂动力学模型的简化模型，获得观测矩阵函数，再根据所述观测矩阵函数建立由辨识激励轨迹的优化目标函数与约束条件构成的辨识激励轨迹模型；

步骤4、根据量子遗传算法对所述辨识激励轨迹模型进行优化，获得满足关节约束条件和在空间范围内变动的激励轨迹，所述激励轨迹用于多关节机械臂动力学的参数辨识。

本发明所述的多关节机械臂动力学参数辨识激励轨迹的优化方法的特点也在于，所述步骤4是按如下过程进行：

步骤4.1、设定种群数量n，设定最大进化代数tmax与量子旋转门旋转角度，当代进化代数为t；初始化t＝1；初始化辨识激励轨迹模型的第t代进化的优化参数全部为设定优化目标为优化目标函数的全局最小值；

步骤4.2、在[0,1]区间内生成n组第t代进化的随机数并与第t代进化的优化参数进行比较，从而获得第t代进化的所有优化参数对应的二进制解；

步骤4.3、通过所述第t代进化的优化参数的二进制解获得由所述观测矩阵函数组成的广义观测矩阵函数值，进而对第t代进化的广义观测矩阵函数值的条件数进行求解，得到第t代进化的优化目标函数值；

步骤4.4、选取第t代进化的优化目标函数值与第t-1代进化的优化目标函数值中的较小值作为判别标准；当t＝1时，第t-1代进化的优化参数的二进制解全部为“1”，优化目标函数值为无穷大；

步骤4.5、利用量子旋转门对将所述判别标准所对应的优化参数进行调整，得到第t+1代进化的优化参数；

步骤4.6、将t+1赋值给t后，判断t>tmax是否成立，若成立，则保存第tmax代进化的优化参数及对应的优化目标函数值作为局部最优参数和目标函数值；否则，返回步骤4.2顺序执行；

步骤4.7、选取种群中所有局部最优目标函数值的最小值作为全局最优目标函数值，并输出全局最优目标函数值对应的优化参数值，从而将所述优化参数值带入到步骤3所建立的辨识激励轨迹的数学模型中，进而获得满足关节约束条件和在空间范围内变动的激励轨迹。

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

1、本发明通过对多关节机械臂动力学性能分析，建立所述多关节机械臂线性化动力学模型，并通过随机数法确定所述多关节机械臂的最小动力学参数集，引入有限项傅里叶级数法建立辨识激励轨迹，将最小动力学参数集对应广义观测矩阵的条件数设定为辨识激励轨迹优化目标函数，采用量子遗传算法优化出适用于所述多关节机械臂动力学参数辨识的激励轨迹，解决了激励轨迹优化算法迭代次数多，易陷入局部极值的问题。

2、本发明基于量子遗传算法优化出满足关节约束条件和在空间范围内变动的激励轨迹，该优化算法优化激励轨迹能够快速收敛并具有较好的鲁棒性，为多关节机械臂动力学参数辨识所需的激励轨迹优化提供了新的方法。

附图说明

图1为本发明中量子遗传算法优化激励轨迹流程图；

图2为本发明中目标函数优化过程图；

图3a为本发明中优化后激励轨迹的前三个关节角度与时间关系图；

图3b为本发明中优化后激励轨迹的前三个关节角速度与时间关系图；

图3c为本发明中优化后激励轨迹的前三个关节角加速度与时间关系图。

具体实施方式

本实施例中，一种基于量子遗传算法的多关节机械臂动力学参数辨识激励轨迹优化方法是按如下方式进行：

步骤1、运用牛顿-欧拉法建立多关节机械臂的动力学模型如式(1)所示：

式(1)中，τ表示机械臂的各关节力矩，q、表示机械臂的关节角位置、关节角速度和关节角加速度，m表示惯性矩阵，c表示科式力和离心力，g表示重力。

线性化处理动力学模型中的非线性项，获得多关节机械臂线性化动力学模型如式(2)所示：

式(2)中，w为由机械臂关节角度q、关节角速度和关节角加速度组成的观测矩阵，φ为机械臂的标准动力学参数。

步骤2、求解多关节机械臂动力学最小参数集，获得多关节机械臂动力学模型的简化模型；

步骤2.1根据多关节机械臂线性化的动力学模型，设定n个采集点的广义观测矩阵函数如式(3)所示：

式(3)中，ψ为n个采集点的广义观测矩阵，z为运动关节的数量，m为标准动力学参数的数量。

ψ的最小线性无关列组对应着标准动力学参数集φ的基参数空间。最小线性无关列数也是ψ的空间维度k，而m-k列是需要删除和线性整合的。ψ的空间维度k对应着标准动力学参数集φ的基参数的个数，此k列贡献度最大。筛选出来的m-k列对应标准参数集φ中贡献度几乎为零的参数和贡献度是相关性的参数，其中，贡献度为零的参数需要删除，贡献度是相关性的参数则需要基于基参数进行线性组合。

步骤2.2、生成一组随机数用于模拟机械臂关节的运动参数，从而建立随机广义观测矩阵ψrandom，且随机广义观测矩阵为非列满秩矩阵；

步骤2.3、对随机广义观测矩阵ψrandom进行qr分解，得到第一q矩阵和第一r矩阵如式(4)所示：

通过对ψrandom进行分解求解动力学参数贡献度。但是，由于多关节机械臂动力学参数的回归矩阵ψrandom是非列满秩的矩阵，对其标准qr分解不具有唯一性。但是通过householder方式的qr分解能够解决ψrandom的标准qr分解不唯一的问题。

利用式(5)构造列变换原始向量θ：

式(5)中，为第一r矩阵对角线值为零的列位置，为第一r矩阵对角线值为非零的列位置；

步骤2.4、定义列变换单位向量集合[e1,e2,…,ei,…,em]；ei为第i个元素为“1”，其他元素均为“0”的一个列变换单位向量；

步骤2.5、利用式(6)求解随机广义矩阵ψrandom的列变换旋转矩阵p：

p＝[eθ1eθ2...eθi...eθm](6)

式(6)中，θi为列变换原始向量θ的第i个元素，eθi为第θi个元素为“1”，其他元素均为“0”的一个列变换单位向量；

步骤2.6、利用式(7)对随机广义观测矩阵ψrandom进行列变换：

ψrandomp＝[ψ1,ψ2](7)

式(7)中，ψ1为k个独立的列向量，ψ2为m-k个需删除、组合的列向量；

步骤2.7、利用式(8)求解多关节机械臂最小动力学参数集的基参数φ1与基于基参数的可线性组合的参数φ2：

步骤2.8，利用式(9)对ψrandomp进行qr分解，得到第二q矩阵[q1,q2]和第二r矩阵[rkrm-k]；

式(9)中，rk为第二r矩阵中k个独立的列向量，rm-k为第二r矩阵中m-k个列向量。

步骤2.9、利用式(10)求解多关节机械臂动力学最小参数集φmin：

φmin＝φ1+βφ2(10)

式(10)中，β为多关节机械臂动力学的基参数φ1与可线性组合的参数φ2的组合系数，并有：

β＝rk^-1rm-k(11)

简化后的多关节机械臂的动力学模型如式(12)所示：

式(12)中，wmin为机械臂动力学最小参数集对应的广义观测矩阵ψ1的基观测矩阵。

步骤3、建立多关节机械臂动力学参数辨识激励轨迹的数学模型如式(13)所示：

式(13)中，ωf表示基频，αl,i、βl,i是n阶傅里叶级数激励轨迹辨识优化参数，其中，l＝1、2、……、n，qi0是第i关节位置的常数项。

根据步骤2多关节机械臂动力学模型的简化模型，获得广义观测矩阵函数如式(14)所示：

激励轨迹的优化是一个多变量非线性约束优化问题，本发明选取广义观测矩阵ψ的条件数cond(ψ)作为激励轨迹的优化目标函数，一方面本文选取最小二乘法进行参数辨识计算，当cond(ψ)越小，ψ矩阵病态越小，辨识计算的结果就精确；另一方面，从实际物理意义来看，cond(ψ)越小，优化计算的激励轨迹能在较大约束范围内变化，充分激励机械臂的动态性能。根据广义观测矩阵函数建立辨识激励轨迹的优化目标函数与约束函数如式(15)所示：

式(15)中，qimin，qimax分别为第i关节关节角度的最小值与最大值，为第i关节关节角速度的最大值，为第i关节关节角加速度的最大值。

步骤4、如图1所示，根据量子遗传算法对步骤3激励轨迹进行优化；

步骤4.1、设定种群数量n，设定最大进化代数tmax，当代进化代数为t；初始化t＝1；设定优化目标为优化目标函数的全局最小值；初始化辨识激励轨迹模型的第t代进化的优化参数全部为如式(16)所示：

步骤4.2、在[0,1]区间内生成n组第t代进化的随机数并与第t代进化的优化参数进行比较，从而获得第t代进化的所有优化参数对应的二进制解，如式(17)所示：

式(17)中，j为计算机生成的随机数，al,i、bl,i分别为激励轨迹的优化参数αl,i、βl,i对应的二进制解。

步骤4.3、通过第t代进化的优化参数的二进制解获得由观测矩阵函数组成的广义观测矩阵函数值，进而对第t代进化的广义观测矩阵函数值的条件数进行求解，得到第t代进化的优化目标函数值；

步骤4.4、选取第t代进化的优化目标函数值与第t-1代进化的优化目标函数值中的较小值作为判别标准；当t＝1时，第t-1代进化的优化参数的二进制解全部为1，优化目标函数值为无穷大；

步骤4.5、利用量子旋转门对将判别标准所对应的优化参数进行调整，得到第t+1代进化的优化参数，如式(18)所示：

式(18)中，α'l,i，β'l,i为调整后的激励轨迹优化参数，u(θ)为量子旋转门函数，且θ为量子旋转门旋转角度，θ取值如表1所示：

表1：量子旋转门旋转角度调整策略

表1中xi＝[al,ibl,i]为第t代进化的优化参数值的二进制解，besti是当前最优优化参数值的二进制解，f(xi)是第t代进化的优化参数值的二进制解对应的优化目标函数值，f(besti)是当前最优优化参数值的二进制解对应的优化目标函数值，s(αl,i,βl,i)是旋转角方向，δi是旋转角θ的大小，即θ＝s(αl,i,βl,i)·δi。

步骤4.6、将t+1赋值给t后，判断t>tmax是否成立，若成立，则保存第tmax代进化的优化参数及对应的优化目标函数值作为局部最优参数和目标函数值；否则，返回步骤4.2顺序执行。

步骤4.7、选取种群中所有局部最优目标函数值的最小值作为全局最优目标函数值，并输出全局最优目标函数值对应的优化参数值，从而将优化参数值带入到步骤3所建立的辨识激励轨迹的数学模型中，进而获得满足关节约束条件和在空间范围内变动的激励轨迹。

实施例1：

多关节机械臂动力学参数辨识激励轨迹优化方法按如下步骤进行：

步骤1、建立多关节机械臂的动力学模型，并对动力学模型进行线性化处理。

步骤2、求解最小动力学参数集。

步骤3、建立激励轨迹的数学模型，基频为0.2π,总时间为10s，优化激励轨迹的约束值如表2所示：

表2：机械臂前三关节的关节角度、角速度及角加速度约束值

步骤4、初始化激励轨迹的优化参数，求解对应的二进制解与优化目标函数值。

步骤5、比较获得优化目标函数的最小值，记录最优激励轨迹优化参数值，将其作为下一次迭代进化的对象。当迭代次数达到最大值时，输出结果。图2为目标函数优化过程图，从图中可以看到在25代左右趋近于稳定，100代以后优化目标函数值变化很小，最终优化目标值在9.47左右，该算法具有快速的收敛性。图3a、图3b、图3c为优化后的激励轨迹图，从图中曲线可以看到优化的激励轨迹满足各关节的约束，且较大范围的占据机械臂的运动空间，说明了该算法满足了激励轨迹优化需求。