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

摘要

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

主权项

1.一种基于电机电流测量的3P3R机械臂末端力估计方法，其特征在于，该方法包括以下步骤：1)在3P3R机械臂每个关节的驱动电机上安装一个电流采集模块，共放置6个电流采集模块，令每个电流采集模块实时采集对应关节驱动电机的电流值；2)记第i个关节驱动电机的电流采集模块在t时刻采集的驱动电机电流值为Iit，i＝1,2,3..6,代表对应的关节编号，其中关节1、2、3为移动关节，关节4、5、6为转动关节；利用式(1)分别计算t时刻6个关节对应的电机转矩T1t，T2t，T3t，T4t，T5t，T6t；电机转矩T和驱动电机电流I的计算表达式如下：其中，U为驱动电机的电压，I为驱动电机电流，K为常值系数；R与X分别为驱动电机的电阻和电抗，为相位；3)分别计算t时刻机械臂每个移动关节所受推力和每个转动关节输出转矩；具体步骤如下：3‑1)t时刻移动关节1在x方向所受推力F1xt：其中，T1t为关节1在t时刻的电机转矩，η为能量利用效率，s1为关节1的丝杠导程，k1为关节1的减速器减速比：k1＝nmotor1/nScrew1，nmotor1为关节1的电机转速，nScrew1为关节1的丝杠转速；3‑2)重复步骤3‑1)，分别计算t时刻移动关节2在y方向所受推力F2yt和t时刻移动关节3在z方向所受推力F3zt，表达式分别如下：其中，s2为关节2的丝杠导程，k2为关节2的减速器的减速比：k2＝nmotor2/nScrew2，nmotor2为关节2的电机转速，nScrew2为关节2的丝杠转速；其中，s3为关节3的丝杠导程，k3为关节3的减速器的减速比：k3＝nmotor3/nScrew3，nmotor3为关节3的电机转速，nScrew3为关节3的丝杠转速；3‑3)t时刻各转动关节电机的输出转矩等于该时刻转动关节的电机转矩，分别记为T4t，T5t，T6t；4)计算t时刻机械臂原始末端力；t时刻机械臂原始末端力包括：机械臂末端t时刻x方向上的受力Fextxt，机械臂末端t时刻y方向上的受力Fextyt，机械臂末端t时刻z方向上的受力Fextzt，机械臂末端t时刻绕x轴方向的力矩Textxt，机械臂末端t时刻绕y轴方向的力矩Textyt，机械臂末端t时刻绕z轴方向的力矩Textzt；具体计算步骤如下：4‑1)t时刻机械臂末端三个方向上受力Fextxt，Fextyt，Fextzt，分别等于步骤3‑1)得到的对应方向上移动关节的所受推力，表达式如下：4‑2)t时刻机械臂末端绕三个坐标轴方向的力矩Textxt，Textyt，Textzt，计算表达式分别如下：其中，l4yt为关节4到关节5在t时刻y方向的长度，l5zt为关节5到关节6在t时刻z方向的长度，l6zt为关节6到末端在t时刻z方向的长度；l5xt为关节5到关节6在t时刻x方向的长度，l6xt为关节6到末端在t时刻z方向的长度；将Textxt，Textyt，Textzt合记为t时刻机械臂的原始力矩值Tmea(t)；5)对步骤4)得到的原始末端力去除过程干扰项，得到t时刻3P3R机械臂末端力的估计值；具体步骤如下：5‑1)利用时间延时估计算法将机械臂在t时刻末端力矩与各关节力矩的关系表示为：G(q(t))+Tf(q(t))+Test(t)＝Tmea(t)  (7)其中，G(q(t))为t时刻机械臂各关节重力引起的力矩，τf(q(t))为t时刻机械臂各关节摩擦力引起的力矩，Test(t)为t时刻机械臂末端力矩的估计值；记ts为延时量，将t‑ts时刻机械臂末端受力与关节力矩关系表示为：G(q(t‑ts)))+Tf(q(t‑ts)))+Test(t‑ts))＝Tmea(t‑ts))  (8)结合式(7)和式(8)得到：将重力造成干扰力矩变化ΔG(q(t‑ts))和摩擦力造成的干扰力矩变化ΔTf(q(t‑ts))均作为高斯噪声，将上述两项噪声和t时刻的测量噪声vk(t)均记入t时刻过程干扰项ωk(t)里，则t时刻机械臂末端力的估计值Test(t)和步骤4)t时刻机械臂的原始力矩值Tmea(t)关系为：Test(t)＝Tmea(t)‑ωk(t)  (10)5‑2)将Test(t)作为输出量，Tmea(t)作为状态量，ωk(t)作为输入量，利用同时输入状态估计算法进行过滤，得到t时刻机械臂末端力矩的估计值Test(t)；则t时刻3P3R机械臂末端力的估计值包括：机械臂末端t时刻x方向上的受力Fextxt，机械臂末端t时刻y方向上的受力Fextyt，机械臂末端t时刻z方向上的受力Fextzt和t时刻机械臂末端力矩的估计值Test(t)。