[发明专利]一种快速收敛的机器人动力学参数辨识轨迹优化方法

摘要

本发明公开了一种快速收敛的机器人动力学参数辨识轨迹优化方法，包括步骤：S1、定义激励轨迹非线性优化的表达式；S2、简化机器人动力学参数辨识激励轨迹非线性优化的目标函数计算量；S3、在不改变搜索空间大小的前提下，简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的约束条件数目；S4、计算傅里叶级数系数的取值范围减小非线性优化的搜索空间，加快搜索速度；S5、使用内部点算法加快非线性优化的收敛速度。本发明解决有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化计算量大，收敛时间长的问题。

主权项

1.一种快速收敛的机器人动力学参数辨识轨迹优化方法，其特征在于，包括如下步骤：S1、定义有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的表达式：式中，目标函数为矩阵H的条件数；优化变量为有限傅里叶级数系数其中n为机器人的关节数目，和是傅里叶级数的系数，其中l＝1,2,…,N，N为傅里叶级数项数，q_io是关节i的偏置；约束条件为机器人各关节角度、角速度和角加速度范围约束；S2、简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的目标函数计算量，包括如下步骤：S2.1、使用算法时间复杂度为线性阶O(n)的递归牛顿欧拉方法进行机器人逆动力学建模，可以得到如下的机器人逆动力学模型：式中，Γ为各关节力矩；q,分别为各关节的转角、角速度和角加速度；δ为各连杆的标准动力学参数，且δ＝[δ₁ δ₂ … δ_n]^T，δ_k为连杆k的标准动力学参数且δ_k＝[L_xxk L_xyk L_xzk L_yyk L_yzk L_zzk l_xk l_yk l_zk m_k f_vk f_ck I_ak]^T；S2.2、对机器人动力学方程进行回归矩阵解耦，具体包括：S2.2.1、依次令矢量δ的第i个动力学参数等于1，其余动力学参数等于0，然后代入所示的InvDYN(·)函数中，则除去零项后，剩余项均为第i个动力学参数的相关项；S2.2.2、将剩余项赋值给回归矩阵H的相关列，最终得到完整的回归矩阵H，从而得到解耦之后的机器人逆动力学模型：式中，被称作回归矩阵，是关于各关节的转角、角速度和角加速度q,的函数；S2.3、使用数值方法将回归矩阵简化为基础回归矩阵具体包括步骤：S2.3.1、将矩阵和机器人标准动力学参数δ分成两部分，则式(5)逆动力学方程可以写成如下式(6)的形式：式中，H_b为矩阵所有n_b个线性无关列组成的子矩阵；H_d为剩下的n_d个全零列向量和线性相关列组成的子矩阵；δ_b为基础动力学参数；δ_d为对动力学不起作用的动力学参数；S2.3.2、设置换矩阵P＝[Pb Pd]满足如下式(7)：HP＝[Hb Hd]，    (7)则有：Hb＝HPb，    (8)随机生成M组且满足M>>len(δ)，将M组代入回归矩阵函数可以得到回归矩阵H_M，如式(9)所示，对H_M使用QR分解可以得到正则上三角矩阵R，S2.3.3、遍历矩阵R的对角元素，记录其中非零对角元素在矩阵中的列号，依次写入数组db，定义行列数等于机器人标准动力学参数δ个数的单位矩阵P'，依照数组db记录的列号，按顺序从单位矩阵P'中取出对应的列组成新矩阵，该矩阵为P_b，将P_b代入式(8)则可以算得基础回归矩阵S2.4、使用中间变量替代重复出现项，减少基础回归矩阵的计算量，具体包括：遍历基础回归矩阵的符号表达式，提取重复出现的项，使用中间变量替换，计算的时候，先计算中间变量，再计算各项基础回归矩阵的元素；S2.5、在一个有限傅里叶级数周期T内，以一定的间隔Δt生成时间序列t，满足：t＝[t1 t2 … tT/Δt]T，    (3)将时间序列t代入式(4)所示的有限傅里叶级数公式，得到T/Δt组式中，q_i，和分别为关节i的角度、角速度和角加速度；ω_f是傅里叶级数的基础角频率；和是傅里叶级数的系数，其中l＝1,2,…,N_i；q_io是关节i的偏置；i＝1,2,…,n，n为机器人的关节数目；S2.6、将T/Δt组代入基础回归矩阵合并成一个矩阵H_T/Δt，计算矩阵H_T/Δt的条件数作为有限傅里叶级数激励轨迹非线性优化的目标函数；S3、在不改变搜索空间大小的前提下，简化有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化的约束条件数目，具体包括如下步骤：S3.1将所述步骤S2.5得到的T/Δt组代入机器人各关节角度、角速度和角加速度的约束条件中：可以得到6×T/Δt×n组不等式约束，式中，q_min,q_max,分别是各关节角度、角速度、角加速度的最小值和最大值，均为n×1的矢量，n为机器人的关节数目；S3.2将所述步骤S3.1的6×T/Δt×n组不等式约束进行合并简化成6×n不等式约束：式中，min(·)为求最小值函数；max(·)为求最大值函数；S4、计算有限傅里叶级数系数的取值范围减小非线性优化的搜索空间，加快搜索速度；具体包括如下步骤：S4.1在式(4)的约束范围内，随机生成(2×N+1)组代入式(4)所示的有限傅里叶级数公式，得到一个关于[(2×N+1)×n]个有限傅里叶级数系数的线性方程组，可以解得一组n个关节的有限傅里叶级数系数；S4.2重复所述步骤S4.1K次，K取10000，则可以得到K组有限傅里叶级数系数那么有限傅里叶级数系数的约束范围为：将式(12)添加入非线性优化的约束条件中；S5、使用内部点算法进行有限傅里叶级数机器人动力学参数辨识激励轨迹非线性优化，加快非线性优化的收敛速度。