CN108227492B - 一种六自由度串联机器人末端负载动力学参数的辨识方法 - Google Patents

Abstract

本发明属于六自由度串联机器人领域，并公开了一种六自由度串联机器人末端负载动力学参数的辨识方法，包括以下步骤：1)建立六自由度串联机器人的动力学模型；2)记录六自由度串联机器人运动过程中的角度、角速度、角加速度，采集各关节电机电流信息；3)获得空载时各关节的力矩和运行时各关节实际负载力矩；4)获得带负载动力学参数的各关节理论负载力矩；5)比较实际负载力矩和理论负载力矩，求解负载动力学参数。本发明大大降低了多自由度几何法分析的复杂性，能够有效的简化算法，提高运算速度，采用具有交叉变异功能的动态粒子群算法，能有效避免粒子落入局部最优解，从而提高了算法的准确性。

Description

一种六自由度串联机器人末端负载动力学参数的辨识方法

技术领域

本发明属于六自由度串联机器人领域，更具体地，涉及一种六自由度串联机器人末端负载动力学参数的辨识方法。

背景技术

建立精确的六自由度串联机器人动力学模型是对多关节六自由度串联机器人及机械臂进行高精度控制、提高系统性能、控制器高可靠性应用的重要条件。六自由度串联机器人模型的准确性取决于六自由度串联机器人各连杆惯性参数及负载惯性参数的精确性。实际应用中，六自由度串联机器人各连杆惯性参数测量后，不随负载的变化而发生改变，因此，只需在负载发生变化时，对负载进行辨识，得到负载的质量、质心及惯性张量即可建立准确的六自由度串联机器人动力学模型。

六自由度串联机器人负载识别过程包括六自由度串联机器人建模，轨迹生成，数据采集，信号处理，参数估计和模型验证。通过六自由度串联机器人关节力矩获取方式的不同，六自由度串联机器人负载辨识的方法有两大类：一种是基于附加的力或者力矩传感器的测量方法，另一种不需要外接传感器，只需利用内部测量装置进行测量。得到负载辨识值后，将负载模型带入无负载时六自由度串联机器人动力学模型中，则可得到整个带负载六自由度串联机器人模型。

目前，六自由度串联机器人负载参数辨识方法主要采用最小二乘法或改进的最小二乘法获得高阶回归方程，该过程需要进行高阶矩阵求逆运算，算法复杂度高，可能出现连杆质量小于零或动力学参数解不满足约束条件的问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了六自由度串联机器人末端负载动力学参数的辨识方法，其利用力矩雅克比矩阵和六自由度串联机器人动力学模型将理论负载力矩中的负载动力学参数分离出来，再利用改进的具有交叉变异功能的动态粒子群算法辨识负载动力学参数，无需外接传感器，简化辨识过程，提高辨识效率。

为实现上述目的，按照本发明，提供了一种六自由度串联机器人末端负载动力学参数的辨识方法，其特征在于，包括以下步骤：

1)根据牛顿-欧拉法建立六自由度串联机器人空载时的动力学模型；

2)控制六自由度串联机器人按照设定的激励轨迹运动，记录六自由度串联机器人运动过程中的角度、角速度和角加速度，同时采集各关节电机的电流信息；

3)利用步骤1)建立的六自由度串联机器人空载动力学模型及步骤2)记录的角度、角速度和角加速度，获得空载时六自由度串联机器人各关节的力矩，并且基于各关节电机的电流信息，获得六自由度串联机器人运行时各关节的实际负载力矩；

4)根据六自由度串联机器人力矩雅克比矩阵，以及带负载动力学参数的末端负载力和力矩矩阵，得到带负载动力学参数的六自由度串联机器人各关节的理论负载力矩；

5)比较各关节的实际负载力矩和理论负载力矩，利用具有交叉变异功能的动态粒子群算法求解负载动力学参数；其中，待辨识的负载动力学参数表示如下：

P_load＝[m_load,s_x,s_y,s_z,I_xx,I_yy,I_zz,I_xy,I_xz,I_yz]^T

式中，m_load表示负载质量，x,y,z表示六自由度串联机器人末端负载坐标系的x,y,z轴，s_x,s_y,s_z分别表示表示负载在x,y,z方向的质心位置，

I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示六自由度串联机器人负载绕末端坐标轴x,y,z的质量惯性矩，绕xy,yz,zx方向的惯性积分别表示为I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ,其中ρ表示负载密度，υ表示负载体积。

优选地，步骤1)中，牛顿-欧拉法建立六自由度串联机器人动力学模型的过程包括：首先利用运动递推得到六自由度串联机器人关节角加速度、关节加速度及连杆加速度，从而求得连杆受力和力矩，再利用力的递推获得各关节的驱动力和驱动力矩，最终得到六自由度串联机器人关节力矩τ和角度q、角速度

角加速度

的关系式：

式中，D(q)表示机械臂惯性矩阵，

表示离心力与哥氏力矩阵，G(q)表示重力力矩，六自由度串联机器人关节力矩τ是上述力矩的合成。

优选地，步骤2)中激励轨迹采用有限项周期性傅里叶级数，关节i的激励轨迹表达式为：

式中，q_i,0为关节i的位置补偿，i＝1,2…,6，ω_f为基频，M为傅里叶级数的谐波项数目，a_i,e与b_i,e分别为正弦函数和余弦函数的幅值，t表示采样时间。

优选地，在六自由度串联机器人运动过程中采集N组信号，所述步骤2)包括记录带负载六自由度串联机器人各关节电流，从而转换为第n组信号中各关节的带负载力矩τ_tot,n；步骤3)包括将记录的六自由度串联机器人关节角度、角速度、加速度信息带入六自由度串联机器人动力学模型，获得六自由度串联机器人运动过程中第n组信号对应的关节空载力矩τ_link,n，从而得到六自由度串联机器人第n组信号的关节所受负载力τ_load,n：

τ_load,n＝τ_tot,n-τ_link,n；

式中，n＝1,2,3,…,N，τ_load,n,τ_tot,n,τ_link,n都为6×1矩阵，分别对应第n组信号六个关节的负载力矩，空载力矩和带负载力矩。

优选地，步骤4)中第n组信号对应负载产生的六自由度串联机器人末端力F_load,n和力矩M_load,n的表达式为：

式中，ω_n为3×1矩阵，表示第n组信号中六自由度串联机器人末端角速度，

为3×1矩阵，表示第n组信号中六自由度串联机器人末端角加速度，

为3×1矩阵，表示第n组信号中六自由度串联机器人末端加速度，

表示负载质心参数，

表示负载惯性张量。

优选地，将所述负载产生的六自由度串联机器人末端力F_load,n和力矩M_load,n分解为带负载惯性参数的表达式：

式中：F_q,n表示第n组信号中负载对六自由度串联机器人末端力与负载惯性参数无关的分量，N_U,n，N_M,n，N_T,n表示第n组信号中负载对六自由度串联机器人末端力矩与负载惯性参数无关的分量，U,M,T表示与负载惯性参数有关的分量；

所述步骤4)六自由度串联机器人带负载运动时，由负载产生的第n组信号中理论驱动力矩矩阵τ'_load,n表达式为：

式中，

表示运动轨迹中第n组信号对应六自由度串联机器人力矩雅克比矩阵，W_n＝[F_load,n；M_load,n]表示机器人末端受负载力及力矩矩阵，

优选地，步骤5)利用动态粒子群算法获得负载动力学参数步骤包括：

5.1)在六自由度串联机器人沿激励轨迹运动时，记录N个数据点，获得关节i对应实际负载力矩τ_load,n,i和理论负载力矩τ'_load,n,i，针对六自由度串联机器人实际负载力矩和理论负载力矩相等的目标建立评价函数f(P)：

5.2)初始化粒子规模L和迭代次数G，每个粒子维度为10，分别表示P_load中各负载惯性参数，确定粒子各参数的限制条件，每个粒子在空间中位置表示为

粒子速度表示为

其中j＝1,2,3,…,L，k＝1,2,3,…,G表示当前的迭代次数，随机初始化初代N个粒子在空间中的速度

和位置

5.3)获得第j个粒子在k+1次迭代后的速度和位置

根据第j个粒子在经过k次迭代后的速度和位置

及第k次迭代后该粒子自己搜索的历史最优值

和全部粒子搜索的历史最优值

得到，公式如下所示：

式中，

是惯性系数，表示粒子保持原有速度的系数，c₁是粒子跟踪自己历史最优值的权重系数，c₂是粒子跟踪群体最优值的权重系数，ξ,η∈[0,1]，代表随机数，r代表约束因子；

5.4)以γ₁的比例交叉位置

和位置

中负载质心参数s_x,s_y,s_z，以γ₂的比例随机初始化粒子在空间中的位置

5.5)所述第k+1次迭代后得到

带入评价函数，获得k+1次迭代后粒子j的最优位置

和群体最优位置

5.6)当误差函数f(P)达到收敛要求或者迭代次数k≥G算法结束，得到全部粒子搜索的历史最优值

即为负载动力学参数P_load，否则重复所述步骤5.3)。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1)本发明在将六自由度串联机器人理论负载力矩分解为带负载惯性参数的矩阵表示后，利用动态粒子群算法求解，有效的避免了最小二乘法计算无法满足约束条件的问题，同时无需使用代数法进行高达16次的矩阵求逆运算，大大降低了多自由度几何法分析的复杂性，能够有效的简化算法，提高运算速度。

2)本发明采用改进的具有交叉变异功能的动态粒子群算法，能够有效的避免粒子落入局部最优解，提高算法的准确性。

附图说明

图1是华中数控6008型六自由度串联机器人的结构图；

图2是本发明实施调用动态粒子群算法的求解步骤流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明以华中数控6008六自由度串联机器人为例，六自由度串联机器人结构如图1所示，六自由度串联机器人连杆参数表如表1所示。表1中i表示机器人第i关节，a_i-1表示关节i-1的轴线z_i-1到关节i的轴线z_i沿关节i-1的坐标轴x_i-1的测量距离，即连杆i-1的长度，α_i-1表示从关节i-1的轴线z_i-1到关节i的轴线z_i绕关节i-1的坐标轴x_i-1的旋转角度，d_i表示从关节i-1的坐标轴x_i-1到关节i的坐标轴x_i绕关节i的坐标轴z_i的测量距离，θ_i表示从关节i-1的坐标轴x_i-1到关节i的坐标轴x_i绕关节i的坐标轴z_i旋转的角度。

表1华中数控6008型六自由度串联机器人的连杆参数

参照图2，一种六自由度串联机器人末端负载动力学参数的辨识方法，具体实施步骤如下所示：

S1：将所述待辨识负载动力学参数表示为：

P_load＝[m_load,s_x,s_y,s_z,I_xx,I_yy,I_zz,I_xy,I_xz,I_yz]^T

式中，m_load表示负载质量，x,y,z表示六自由度串联机器人末端负载坐标系的x,y,z轴，s_x,s_y,s_z分别表示表示负载在x,y,z方向的质心位置，I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示六自由度串联机器人负载绕末端坐标轴x,y,z的质量惯性矩，绕xy,yz,zx方向的惯性积分别表示为I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ，ρ表示负载密度，υ表示负载体积。

S2：根据牛顿-欧拉法建立华数6008六自由度串联机器人空载时的动力学模型，其建模过程包括，首先利用运动递推得到6008六自由度串联机器人关节角加速度、关节加速度及连杆加速度，从而求得各连杆受力和力矩，再利用力的递推获得各关节的驱动力和驱动力矩，最终得到六自由度串联机器人关节力矩τ和角度q、角速度

角加速度

的关系式：

式中，D(q)表示机械臂惯性矩阵，

表示离心力与哥氏力矩阵，G(q)表示重力力矩，六自由度串联机器人关节力矩τ是上述力矩的合成。

S3：设计激励轨迹为有限项周期性傅里叶级数，激励6008六自由度串联机器人动作并记录六自由度串联机器人运动过程中2000组信号的角度q、角速度

角加速度

同时采集六自由度串联机器人运动过程中各关节电机电流信息，激励轨迹表达式为：

式中，q_i,0为关节i的位置补偿，a_i,e与b_i,e分别为正弦函数和余弦函数的幅值，t表示采样时间。

S4：利用记录的六自由度串联机器人关节角度q、角速度

加速度

信息和所述六自由度串联机器人动力学模型获得空载时六自由度串联机器人第n组信号对应的关节力矩τ_link,n，基于各关节电机电流信息，获得第n组信号对应的带负载关节力矩τ_tot,n，从而得到第n组信号对应的六自由度串联机器人各关节受负载力τ_load,n：

τ_load,n＝τ_tot,n-τ_link,n

式中，n＝1,2,3,…,2000，τ_load,n,τ_tot,n,τ_link,n都为6×1矩阵，分别对应第n组信号六个关节的负载力矩，空载力矩和带负载力矩。

S5：获得六自由度串联机器人力矩雅克比矩阵，以及带负载动力学参数的末端负载力和力矩矩阵，第n组信号对应负载产生的六自由度串联机器人末端力矩阵F_load,n和力矩矩阵M_load,n的表达式为：

式中，x,y,z表示六自由度串联机器人末端负载坐标系的x,y,z轴,[ω_x,n ω_y,nω_z,n]^T表示第n组信号中六自由度串联机器人末端角速度在坐标轴x,y,z中的分量，

表示第n组信号中六自由度串联机器人末端角加速度在坐标系轴x,y,z中的分量，

表示第n组信号中六自由度串联机器人末端加速度在坐标系中的分量，[s_x s_y s_z]^T表示负载质心参数；

S6：将所述负载产生的第n组信号对应的六自由度串联机器人末端力F_load,n和力矩M_load,n分解为带负载惯性参数的表达式：

对所述实例中华数6008六自由度串联机器人有：

式中：F_q,n表示第n组信号中负载对六自由度串联机器人末端力与负载惯性参数无关的分量，N_U,n，N_M,n，N_T,n表示第n组信号中负载对六自由度串联机器人末端力矩与负载惯性参数无关的分量，U,M,T表示与负载惯性参数有关的分量，ω_x,n,ω_y,n,ω_z,n分别表示第n组信号中机器人末端角速度在末端坐标系x,y,z轴的分量，

分别表示第n组信号中机器人末端角加速度在末端坐标系x,y,z轴的分量，

分别表示第n组信号中机器人末端加速度在末端坐标系x,y,z轴的分量。

S7：根据第n组信号六自由度串联机器人力矩雅克比矩阵，以及带负载动力学参数的末端负载力和力矩矩阵，获得带负载动力学参数的六自由度串联机器人各关节理论负载力矩τ'_load,n的表达式为：

式中，

表示运动轨迹中第n组信号对应六自由度串联机器人力矩雅克比矩阵，

S8：建立本发明动态粒子群算法的评价函数，根据所述2000组信号中关节i对应实际负载力矩τ_load,n,i和理论负载力矩τ'_load,n,i相等的目标建立评价函数f(P)：

S9：初始化粒子为80和迭代次数为200，每个粒子维度为10，分别表示P_load中各负载惯性参数，确定粒子各参数的限制条件，每个粒子在空间中位置表示为

粒子速度表示为

其中j＝1,2,3,…,80，表示第j个粒子，k＝1,2,3,…,200表示当前的迭代次数，随机初始化初代80个粒子在空间中的速度

和位置

S10：获得第j个粒子在k+1次迭代后的速度和位置

根据第j个粒子在经过k次迭代后的速度和位置

及第k次迭代后该粒子自己搜索的历史最优值

和全部粒子搜索的历史最优值

得到，获得公式如下所示：

S11：对第k+1次迭代后的80个粒子，以0.2的概率交叉两个粒子质心参数s_x,s_y,s_z，以0.1的概率随机变异单个粒子的各参数。

S12：比较实际负载力矩和第k+1次迭代后的理论负载力矩，获得k+1次迭代后粒子j的最优位置

和群体最优位置

当误差函数f(P)达到收敛要求或者迭代次数k≥200算法结束，得到全部粒子搜索的历史最优值

即为负载动力学参数P_load。

S13：本发明实例中六自由度串联机器人末端负载实际动力学参数表示为P′_load＝[4.881,0.0687,-0.0037,0.0249,0.0041,0.0853,0.0869,0,-0.0032,0]^T，辨识得到负载为P_load＝[4.733,0.072,-0.011,0.0305,0.0073,0.0674,0.0792,0.002,-0.008,-0.003]^T。

综上所述，本发明提供了一种六自由度串联机器人末端负载动力学参数的辨识方法，不需要外接传感器，避免使用最小二乘法求解不满足约束条件及高维矩阵求逆运算的问题，使用改进的动态粒子群算法，有效的减小了算法的复杂性，提高运算速度，此外，利用本发明中六自由度串联机器人负载辨识的方法可建立六自由度串联机器人的高精度模型，提高六自由度串联机器人的加工精度和运行速度。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种六自由度串联机器人末端负载动力学参数的辨识方法，在六自由度串联机器人运动过程中采集N组信号，其特征在于，包括以下步骤：

1)根据牛顿-欧拉法建立六自由度串联机器人空载时的动力学模型；

2)控制六自由度串联机器人按照设定的激励轨迹运动，记录六自由度串联机器人运动过程中各关节的角度、角速度和角加速度；同时采集各关节电机的电流信息；

所述步骤2)包括记录带负载六自由度串联机器人各关节电机的电流，从而转换为第n组信号中各关节的带负载力矩τ_tot,n；

3)利用步骤1)建立的六自由度串联机器人空载时的动力学模型及步骤2)记录的角度、角速度和角加速度，获得空载时六自由度串联机器人各关节的力矩，并且基于各关节电机的电流信息，获得六自由度串联机器人运行时各关节的实际负载力矩；

步骤3)包括将记录的六自由度串联机器人运动过程中各关节的角度、角速度、角加速度信息带入六自由度串联机器人空载时的动力学模型，获得六自由度串联机器人运动过程中第n组信号对应的关节空载力矩τ_link,n，从而得到六自由度串联机器人第n组信号的关节所受负载力矩τ_load,n：

τ_load,n＝τ_tot,n-τ_link,n；

式中，n＝1,2,3,…,N，τ_load,n,τ_tot,n,τ_link,n都为6×1矩阵；

4)根据六自由度串联机器人力矩雅克比矩阵，以及带负载动力学参数的末端负载力和力矩矩阵，得到带负载动力学参数的六自由度串联机器人各关节的理论负载力矩；

第n组信号对应负载产生的六自由度串联机器人末端负载力F_load,n和力矩矩阵M_load,n的表达式为：

式中，ω_n为3×1矩阵，表示第n组信号中六自由度串联机器人末端角速度，

为3×1矩阵，表示第n组信号中六自由度串联机器人末端角加速度，

为3×1矩阵，表示第n组信号中六自由度串联机器人末端加速度，

表示负载质心参数，

表示负载惯性张量；

将所述负载产生的六自由度串联机器人末端负载力F_load,n和力矩矩阵M_load,n分解为带负载惯性参数的表达式：

式中：F_q,n表示第n组信号中负载对六自由度串联机器人末端负载力与负载惯性参数无关的分量，N_U,n，N_M,n，N_T,n表示第n组信号中负载对六自由度串联机器人末端力矩与负载惯性参数无关的分量，U,M,T表示与负载惯性参数有关的分量；

所述步骤4)六自由度串联机器人带负载运动时，由负载产生的第n组信号中理论驱动力矩矩阵τ'_load,n表达式为：

式中，

表示运动轨迹中第n组信号对应六自由度串联机器人力矩雅克比矩阵，W_n＝[F_load,n；M_load,n]表示机器人末端受负载力及力矩矩阵，

5)比较各关节的实际负载力矩和理论负载力矩，利用具有交叉变异功能的动态粒子群算法求解负载动力学参数；其中，待辨识的负载动力学参数表示如下：

P_load＝[m_load,s_x,s_y,s_z,I_xx,I_yy,I_zz,I_xy,I_xz,I_yz]^T

式中，m_load表示负载质量，x,y,z表示六自由度串联机器人末端负载坐标系的x,y,z轴，sx,sy,sz分别表示负载在x,y,z方向的质心位置，

I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示六自由度串联机器人负载绕末端坐标轴x,y,z的质量惯性矩，绕xy,yz,zx方向的惯性积分别表示为I_xy＝∫∫∫_Vxyρdυ、Iyz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ,其中ρ表示负载密度，υ表示负载体积；

步骤5)利用动态粒子群算法获得负载动力学参数步骤包括：

5.1)在六自由度串联机器人沿激励轨迹运动时，记录N个数据点，获得关节i对应实际负载力矩τ_load,n,i和理论负载力矩τ'_load,n,i，针对六自由度串联机器人实际负载力矩和理论负载力矩相等的目标建立评价函数f(P)：

5.2)初始化粒子规模L和迭代次数G，每个粒子维度为10，分别表示P_load中各负载惯性参数，确定粒子各参数的限制条件，每个粒子在空间中位置表示为

粒子速度表示为

其中j＝1,2,3,…,L，k＝1,2,3,…,G表示当前的迭代次数，随机初始化初代N个粒子在空间中

的速度和位置

5.3)获得第j个粒子在k+1次迭代后的速度和位置

根据第j个粒子在经过k次迭代后的速度和位置

及第k次迭代后该粒子自己搜索的历史最优值

和全部粒子搜索的历史最优值

得到，公式如下所示：

式中，

是惯性系数，表示粒子保持原有速度的系数，c₁是粒子跟踪自己历史最优值的权重系数，c₂是粒子跟踪群体最优值的权重系数，ξ,η∈[0,1]，代表随机数，r代表约束因子；

5.4)以γ₁的比例交叉位置

和位置

中负载质心参数s_x,s_y,s_z，以γ₂的比例随机初始化粒子在空间中的位置

5.5)所述第k+1次迭代后得到

带入评价函数，获得k+1次迭代后粒子j的最优位置

和群体最优位置

5.6)当误差函数f(P)达到收敛要求或者迭代次数k≥G算法结束，得到全部粒子搜索的历史最优值

即为负载动力学参数P_load，否则重复所述步骤5.3)。

2.根据权利要求1所述的一种六自由度串联机器人末端负载动力学参数的辨识方法，其特征在于，步骤1)中，牛顿-欧拉法建立六自由度串联机器人空载时的动力学模型的过程包括：首先利用运动递推得到六自由度串联机器人关节角加速度、关节加速度及连杆加速度，从而求得连杆受力和力矩，再利用力的递推获得各关节的驱动力和驱动力矩，最终得到六自由度串联机器人关节力矩τ和角度q、角速度

角加速度

的关系式：

式中，D(q)表示机械臂惯性矩阵，表示离心力与哥氏力矩阵，G(q)表示重力力矩，六自由度串联机器人关节力矩τ是上述力矩的合成。

3.根据权利要求1所述的一种六自由度串联机器人末端负载动力学参数的辨识方法，其特征在于，步骤2)中激励轨迹采用有限项周期性傅里叶级数，关节i的激励轨迹表达式为：

式中，q_i,0为关节i的位置补偿，i＝1,2…,6，ω_f为基频，M₀为傅里叶级数的谐波项数目，a_i,e与b_i,e分别为正弦函数和余弦函数的幅值，t表示采样时间。

Images