[发明专利]一种工业机械臂末端负载动力学参数估计方法

权利要求书

1.一种工业机械臂末端负载动力学参数估计方法，其特征在于：包括如下步骤：

步骤1)：通过牛顿-欧拉迭代方法构建工业机械臂本体动力学模型；步骤2)：通过多静态点负载模型构建工业机械臂负载参数辨识方程；步骤3)：求解得到不同负载的力学参数；所述步骤3)具体包括：利用多静态点采集关节力矩数据，采样数据通过低通滤波处理后带入负载辨识方程，通过最小二乘法拟合形成线性方程组以求解。

2.根据权利要求1所述的一种工业机械臂末端负载动力学参数估计方法，其特征在于，所述步骤1)具体包括如下：

根据机械臂动力学可得：其中，τ_m为机器人关节扭矩输出、τ_ext为人为拖动力、τ_f为摩擦总力，q、分别为机械臂的关节位置、速度和加速度，为机械臂惯性力，为耦合力，G(q)为重力，当机械臂运动速度处于较低状态时，即和非常小，计算说明此时项非常小，故机械臂各关节输出扭矩可表示为：

此处和分别为机械臂重力和关节摩擦力的估计值。

3.根据权利要求1所述的一种工业机械臂末端负载动力学参数估计方法，其特征在于，所述步骤2)具体包括如下：

利用多静态点估计工业机械臂末端负载参数辨识方法，其中，建立工业机械臂末端负载动力学参数模型，如下：

根据机械臂动力学可得：其中τ_m为机器人关节扭矩输出、τ_f为摩擦总力，q、分别为机械臂的关节位置、速度和加速度，为机械臂惯性力，为耦合力，G(q)为重力，当机器人处于静止状态时，和均为零，计算说明此时项为零，摩擦力与关节运动速度有关，τ_f项也为零，故机械臂各关节输出扭矩可表示为：

此处为机械臂重力的估计值，以KD协作机械臂的运动模型为基础，直接得到此时的显式方程，得到如下6个方程：

t1＝0；

t2＝(M5*g*x5*cos(q2+q3+q4+q5))/2+(M6*g*x6*cos(q2+q3+q4+q6))/2+M4*a4*g*cos(q2+q3)+M5*a4*g*cos(q2+q3)+M6*a4*g*cos(q2+q3)-(M5*g*y5*sin(q2+q3+q4+q5))/2-(M6*g*y6*sin(q2+q3+q4+q6))/2+(M6*g*z6*sin(q2+q3+q4+q5))/2+M3*g*x3*cos(q2+q3)-M3*g*y3*sin(q2+q3)+(M6*g*x6*cos(q2+q3+q4-q5-q6))/4+M3*a3*g*cos(q2)+M4*a3*g*cos(q2)+M5*a3*g*cos(q2)+M6*a3*g*cos(q2)+(M6*g*y6*sin(q2+q3+q4-q5-q6))/4+M2*g*x2*cos(q2)-M2*g*y2*sin(q2)+(M5*g*x5*cos(q2+q3+q4-q5))/2-(M6*g*x6*cos(q2+q3+q4-q6))/2+(M5*g*y5*sin(q2+q3+q4-q5))/2-(M6*g*y6*sin(q2+q3+q4-q6))/2+(M6*g*x6*cos(q2+q3+q4+q5+q6))/4-(M6*g*z6*sin(q2+q3+q4-q5))/2-(M6*g*y6*sin(q2+q3+q4+q5+q6))/4-M5*d5*g*sin(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)+M4*g*x4*cos(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+(M6*g*x6*cos(q2+q3+q4+q5-q6))/4+(M6*g*x6*cos(q2+q3+q4-q5+q6))/4+(M6*g*y6*sin(q2+q3+q4+q5-q6))/4-(M6*g*y6*sin(q2+q3+q4-q5+q6))/4；

t3＝M3*g*x3*cos(q2+q3)-M3*g*y3*sin(q2+q3)-M5*d5*g*sin(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)+M4*g*x4*cos(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+M4*a4*g*cos(q2+q3+q4)*cos(q4)+M5*a4*g*cos(q2+q3+q4)*cos(q4)+M6*a4*g*cos(q2+q3+q4)*cos(q4)+M5*g*x5*cos(q2+q3+q4)*cos(q5)+M4*a4*g*sin(q2+q3+q4)*sin(q4)+M5*a4*g*sin(q2+q3+q4)*sin(q4)+M6*a4*g*sin(q2+q3+q4)*sin(q4)-M5*g*y5*cos(q2+q3+q4)*sin(q5)-M6*g*y6*sin(q2+q3+q4)*cos(q6)+M6*g*z6*cos(q2+q3+q4)*sin(q5)-M6*g*x6*sin(q2+q3+q4)*sin(q6)+M6*d5*g*sin(q2+q3+q4)*cos(q5)^2-(M6*g*y6*sin(q2+q3+q4)*cos(q5+q6)*cos(q5))/2-(M6*g*x6*sin(q2+q3+q4)*sin(q5+q6)*cos(q5))/2+M6*g*x6*cos(q2+q3+q4)*cos(q5)*cos(q6)-M6*g*y6*cos(q2+q3+q4)*cos(q5)*sin(q6)-(M6*g*y6*cos(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+(M6*g*x6*sin(q5-q6)*sin(q2+q3+q4)*cos(q5))/2-M6*a4*g*sin(q2+q3+q4)*cos(q5)^2*sin(q4)+M6*g*y6*sin(q2+q3+q4)*cos(q5)^2*cos(q6)+M6*g*x6*sin(q2+q3+q4)*cos(q5)^2*sin(q6)-(M6*d5*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q6))/2+(M6*g*z6*sin(q2+q3+q4)*cos(q5+q6)*sin(q5)*sin(q6))/2-(M6*g*z6*sin(q2+q3+q4)*sin(q5+q6)*cos(q6)*sin(q5))/2-(M6*d5*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6))/2+(M6*a4*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q4)*sin(q6))/2+(M6*a4*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q4)*cos(q6))/2+(M6*d5*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q6))/2+(M6*g*z6*cos(q5-q6)*sin(q2+q3+q4)*sin(q5)*sin(q6))/2+(M6*g*z6*sin(q5-q6)*sin(q2+q3+q4)*cos(q6)*sin(q5))/2-(M6*d5*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6))/2+(M6*a4*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q4)*sin(q6))/2-(M6*a4*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q4)*cos(q6))/2+(M6*a4*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6)*sin(q4))/2-(M6*a4*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q4)*sin(q6))/2+(M6*a4*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6)*sin(q4))/2+(M6*a4*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q4)*sin(q6))/2；

t4＝M4*g*x4*cos(q2+q3+q4)-M6*d5*g*sin(q2+q3+q4)-M5*d5*g*sin(q2+q3+q4)-M4*g*y4*sin(q2+q3+q4)-M5*g*z5*sin(q2+q3+q4)+M5*g*x5*cos(q2+q3+q4)*cos(q5)-M5*g*y5*cos(q2+q3+q4)*sin(q5)-M6*g*y6*sin(q2+q3+q4)*cos(q6)+M6*g*z6*cos(q2+q3+q4)*sin(q5)-M6*g*x6*sin(q2+q3+q4)*sin(q6)+M6*d5*g*sin(q2+q3+q4)*cos(q5)^2-(M6*g*y6*sin(q2+q3+q4)*cos(q5+q6)*cos(q5))/2-(M6*g*x6*sin(q2+q3+q4)*sin(q5+q6)*cos(q5))/2+M6*g*x6*cos(q2+q3+q4)*cos(q5)*cos(q6)-M6*g*y6*cos(q2+q3+q4)*cos(q5)*sin(q6)-(M6*g*y6*cos(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+(M6*g*x6*sin(q5-q6)*sin(q2+q3+q4)*cos(q5))/2+M6*g*y6*sin(q2+q3+q4)*cos(q5)^2*cos(q6)+M6*g*x6*sin(q2+q3+q4)*cos(q5)^2*sin(q6)-(M6*d5*g*sin(q2+q3+q4)*sin(q5+q6)*cos(q5)*sin(q6))/2+(M6*g*z6*sin(q2+q3+q4)*cos(q5+q6)*sin(q5)*sin(q6))/2-(M6*g*z6*sin(q2+q3+q4)*sin(q5+q6)*cos(q6)*sin(q5))/2-(M6*d5*g*cos(q5-q6)*sin(q2+q3+q4)*cos(q5)*cos(q6))/2+(M6*d5*g*sin(q5-q6)*sin(q2+q3+q4)*cos(q5)*sin(q6))/2+(M6*g*z6*cos(q5-q6)*sin(q2+q3+q4)*sin(q5)*sin(q6))/2+(M6*g*z6*sin(q5-q6)*sin(q2+q3+q4)*cos(q6)*sin(q5))/2-(M6*d5*g*sin(q2+q3+q4)*cos(q5+q6)*cos(q5)*cos(q6))/2；

t5＝-(g*sin(q2+q3+q4)*(2*M5*x5*sin(q5)-M6*y6*cos(q5-q6)+M6*x6*sin(q5-q6)+M6*y6*cos(q5+q6)+M6*x6*sin(q5+q6)+2*M5*y5*cos(q5)-2*M6*z6*cos(q5)))/2；

t6＝M6*x6*((g*sin(q5-q6)*sin(q2+q3+q4))/2-(g*sin(q2+q3+q4)*sin(q5+q6))/2+g*cos(q2+q3+q4)*cos(q6))-M6*y6*((g*sin(q2+q3+q4)*cos(q5+q6))/2+g*cos(q2+q3+q4)*sin(q6)+(g*cos(q5-q6)*sin(q2+q3+q4))/2)。