[发明专利]一种基于电机电流测量的3P3R机械臂末端力估计方法

权利要求书

1.一种基于电机电流测量的3P3R机械臂末端力估计方法，其特征在于，该方法包括以下步骤：

1)在3P3R机械臂每个关节的驱动电机上安装一个电流采集模块，共放置6个电流采集模块，令每个电流采集模块实时采集对应关节驱动电机的电流值；

2)记第i个关节驱动电机的电流采集模块在t时刻采集的驱动电机电流值为I_it，i＝1,2,3..6,代表对应的关节编号，其中关节1、2、3为移动关节，关节4、5、6为转动关节；利用式(1)分别计算t时刻6个关节对应的电机转矩T_1t，T_2t，T_3t，T_4t，T_5t，T_6t；

电机转矩T和驱动电机电流I的计算表达式如下：

其中，U为驱动电机的电压，I为驱动电机电流，K为常值系数；R与X分别为驱动电机的电阻和电抗，为相位；

3)分别计算t时刻机械臂每个移动关节所受推力和每个转动关节输出转矩；具体步骤如下：

3-1)t时刻移动关节1在x方向所受推力F_1xt：

其中，T_1t为关节1在t时刻的电机转矩，η为能量利用效率，s₁为关节1的丝杠导程，k₁为关节1的减速器减速比：k₁＝n_motor1/n_Screw1，n_motor1为关节1的电机转速，n_Screw1为关节1的丝杠转速；

3-2)重复步骤3-1)，分别计算t时刻移动关节2在y方向所受推力F_2yt和t时刻移动关节3在z方向所受推力F_3zt，表达式分别如下：

其中，s₂为关节2的丝杠导程，k₂为关节2的减速器的减速比：k₂＝n_motor2/n_Screw2，n_motor2为关节2的电机转速，n_Screw2为关节2的丝杠转速；

其中，s₃为关节3的丝杠导程，k₃为关节3的减速器的减速比：k₃＝n_motor3/n_Screw3，n_motor3为关节3的电机转速，n_Screw3为关节3的丝杠转速；

3-3)t时刻各转动关节电机的输出转矩等于该时刻转动关节的电机转矩，分别记为T_4t，T_5t，T_6t；

4)计算t时刻机械臂原始末端力；

t时刻机械臂原始末端力包括：机械臂末端t时刻x方向上的受力F_extxt，机械臂末端t时刻y方向上的受力F_extyt，机械臂末端t时刻z方向上的受力F_extzt，机械臂末端t时刻绕x轴方向的力矩T_extxt，机械臂末端t时刻绕y轴方向的力矩T_extyt，机械臂末端t时刻绕z轴方向的力矩T_extzt；具体计算步骤如下：

4-1)t时刻机械臂末端三个方向上受力F_extxt，F_extyt，F_extzt，分别等于步骤3-1)得到的对应方向上移动关节的所受推力，表达式如下：

4-2)t时刻机械臂末端绕三个坐标轴方向的力矩T_extxt，T_extyt，T_extzt，计算表达式分别如下：

其中，l_4yt为关节4到关节5在t时刻y方向的长度，l_5zt为关节5到关节6在t时刻z方向的长度，l_6zt为关节6到末端在t时刻z方向的长度；l_5xt为关节5到关节6在t时刻x方向的长度，l_6xt为关节6到末端在t时刻z方向的长度；

将T_extxt，T_extyt，T_extzt合记为t时刻机械臂的原始力矩值T_mea(t)；

5)对步骤4)得到的原始末端力去除过程干扰项，得到t时刻3P3R机械臂末端力的估计值；具体步骤如下：

5-1)利用时间延时估计算法将机械臂在t时刻末端力矩与各关节力矩的关系表示为：

G(q(t))+T_f(q(t))+T_est(t)＝T_mea(t) (7)

其中，G(q(t))为t时刻机械臂各关节重力引起的力矩，τ_f(q(t))为t时刻机械臂各关节摩擦力引起的力矩，T_est(t)为t时刻机械臂末端力矩的估计值；

记t_s为延时量，将t-t_s时刻机械臂末端受力与关节力矩关系表示为：

G(q(t-t_s)))+T_f(q(t-t_s)))+T_est(t-t_s))＝T_mea(t-t_s)) (8)

结合式(7)和式(8)得到：

将重力造成干扰力矩变化ΔG(q(t-t_s))和摩擦力造成的干扰力矩变化ΔT_f(q(t-t_s))均作为高斯噪声，将上述两项噪声和t时刻的测量噪声v_k(t)均记入t时刻过程干扰项ω_k(t)里，则t时刻机械臂末端力的估计值T_est(t)和步骤4)t时刻机械臂的原始力矩值T_mea(t)关系为：

T_est(t)＝T_mea(t)-ω_k(t) (10)

5-2)将T_est(t)作为输出量，T_mea(t)作为状态量，ω_k(t)作为输入量，利用同时输入状态估计算法进行过滤，得到t时刻机械臂末端力矩的估计值T_est(t)；则t时刻3P3R机械臂末端力的估计值包括：机械臂末端t时刻x方向上的受力F_extxt，机械臂末端t时刻y方向上的受力F_extyt，机械臂末端t时刻z方向上的受力F_extzt和t时刻机械臂末端力矩的估计值T_est(t)。