一种基于电机电流测量的3P3R机械臂末端力估计方法

摘要

本发明提出一种基于电机电流测量的3P3R机械臂末端力估计方法，属于机器人自动化装配技术领域。该方法首先在3P3R机械臂每个关节的驱动电机上安装一个电流采集模块，每个电流采集模块实时采集对应关节驱动电机的电流值。在任一时刻，根据采集得到的每个驱动电机电流值计算得到该时刻对应关节的电机转矩，进而得到该时刻机械臂每个移动关节所受推力和每个转动关节输出转矩，从而获取该时刻机械臂原始末端力。利用时间延时估计算法和同时输入状态估计算法去除原始末端力的过程干扰项，最终得到该时刻3P3R机械臂末端力的估计值。本发明简便易操作，可以得到精度较高的估计结果，具有很高的实用价值。

说明书

技术领域

本发明涉及一种基于电机电流测量的3P3R机械臂末端力估计方法，属于机器人自动化装配技术领域。

背景技术

在当前制造领域的发展背景下，产品生产的效率和精度要求逐渐提高，但面对当前制造领域的生产技术相对落后、自动化程度不高的情况下，机器人作业是解决该发展问题的一项行之有效的措施。相比于人工作业，机器人显然具有高效率，高适用性，能在危险区域作业的优点。机器人技术的发展是解决当前生产力落后的重要措施，而制约着机器人技术发展的其中一项关键因素就是机器人用于真实作业时，其精度有时往往达不到要求。机器人作业过程是机器人带动安装在末端的工具完成各种指令动作，因此对机器人末端力的估计是保证机器人高精度完成任务的一项重要指标。这就需要更加精确的机器人末端力的估计方法。

在近年来的力估计控制算法的研究中，Choi等人在其论文“A novel method forestimating external force:Simulation study with a 4-DOFrobot manipulator.”中提出一种通过在机器人末端安装力矩传感器测量关节力矩，利用期望的机器人模型并结合时间延时估计(TDE)和同时输入状态估计(SISE)估计机器人末端力的方法，在论文的仿真实验中作者引入高斯噪声，库仑摩擦等充分模拟现实情况，结果验证该算法可行，但限于力矩传感器的精度，测量到的机械臂末端力的精度在实际机器人作业过程中往往达不到要求。因此对于探究更有效的机器人末端力估计方法，保证其在真实作业过程中的精度要求是亟需解决的问题。

3P3R机械臂，是由3个移动关节和3个转动关节组成的机械臂，是在大工件产品作业过程中常用到的一种机器人产品。其模型简图如附图图1所示，图中矩形关节代表移动关节，由空间z轴从下向上，关节1，2，3为移动关节，其中移动关节1沿x轴移动，移动关节2沿y轴移动，移动关节3沿z轴移动，z_i代表第i个关节的z轴，d_i代表第i个移动关节的可以移动的距离。柱状关节代表转动关节，从空间x轴方向，分别为关节4，5，6,其中关节4绕x轴转动，关节5绕y轴转动，关节6绕z轴转动。，l_ix,l_iy,l_iz代表零点位置时第i个关节到第i+1个关节的x,y,z方向的连杆长度。机械臂的移动关节，结构采用丝杠螺母组合，关节驱动电机与丝杠固结，其余关节部分与螺母固结，变丝杠转动为螺母的移动。转动关节结构为关节上安装的驱动电机带动关节转动。3P3R机械臂在工作时，每个关节上安装的驱动电机输出的转矩经过各对应的传动机构输出对应关节动作，从而由各个关节连接而成的3P3R机械臂可以完成各种作业动作。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种基于电机电流测量的3P3R机械臂末端力估计方法。本发明方法通过在机械臂各关节的驱动电机上放置电流采集模块采集驱动电机的电流值，不需要力矩传感器即可计算出机械臂各关节力矩值，从而计算得到机械臂的末端力，方法简便易操作，并且可以得到精度较高的估计结果，具有很高的实用价值。

本发明提出一种基于电机电流测量的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)。

本发明的特点及有益效果在于：

1、本发明方法通过测量机械臂各关节上的驱动电机的电机电流计算对应电机转矩，从而计算得到机械臂的末端力，方法简便易操作，并且可以得到精度较高的估计结果。

2、本发明方法利用时间延时估计算法使得机械臂在初始测量时将与关节旋转角有关的测量误差减小，提高机械臂稳定性，增强鲁棒性。

3、本发明方法通过电机电流测量值估计机械臂末端力，采用同时输入状态估计(SISE)算法过滤过程噪声，在一定程度上保证了求解精度。

附图说明

图1是3P3R机械臂结构示意图。

图2是为本发明方法的整体流程框图。

具体实施方式

本发明提出一种基于电机电流测量的3P3R机械臂末端力估计方法，下面结合附图进一步详细说明如下。

本发明提出的一种基于电机电流测量的3P3R机械臂末端力估计方法，整体流程如图2所示，包括以下步骤：

1)在3P3R机械臂每个关节的驱动电机(一般为闭环控制交流伺服电机)上安装一个电流采集模块(所述电流采集模块可采用任意型号，量程1A)，共放置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分别为驱动电机的电阻和电抗，为相位。

因驱动电机的电阻和电抗不变，电压与电流、电阻成正比，则电机转矩与电流的关系可记为T＝K_TI，其中K_T为电机转矩-电流比例参数，通过实验得到。

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

3-1)由能量守恒定律和带动丝杠转动的电机转矩可得到t时刻移动关节1在x方向所受推力F_1xt：

其中，T_1t为关节1在t时刻的电机转矩，η为能量利用效率(取0.9)，F_1xt为t时刻移动关节1在x方向所受推力，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)机械臂的转动关节(关节4、5、6)，每个电机带动对应转动关节完成转动动作，忽略传动机构的力矩，则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。

对于t时刻3P3R机械臂末端所受的力和力矩的计算步骤分别如下：

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

4-2)对于3P3R机械臂，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)；

由步骤1)-步骤4)得到机械臂末端的受力和力矩，其中包含有重力和摩擦力引起的力矩，在步骤5)中将会进行对这些干扰项的过滤。

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

过程干扰项主要包括运动过程中机械臂关节重力造成的力矩，结构摩擦力造成的力矩。此外，在采集电流时也会存在测量噪声。因此，为提高估计末端力的精度，应将这些干扰项和噪声误差过滤掉。具体步骤如下：

5-1)利用时间延时估计算法(TIDE)将机械臂在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_mea(t)为步骤4)得到的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时刻机械臂末端力的估计值和步骤4)得到的t时刻机械臂的原始力矩值T_mea(t)关系为：

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

5-2)将T_est(t)作为输出量，T_mea(t)作为状态量，ω_k(t)作为输入量，利用同时输入状态估计算法(SISE)对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)。