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

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

An end force estimation method for 3P3R manipulator based on motor current measurement

The invention provides a terminal force estimation method of 3P3R manipulator based on motor current measurement, which belongs to the technical field of robot automatic assembly. Firstly, a current acquisition module is installed on the drive motor of each joint of the 3P3R manipulator, and each current acquisition module collects the current value of the corresponding joint drive motor in real time. At any moment, the motor torque corresponding to the joint is calculated according to the current value of each driving motor collected, and then the thrust of each moving joint and the output torque of each rotating joint of the manipulator at that time are obtained, so as to obtain the original end force of the manipulator at that time. Time delay estimation algorithm and simultaneous input state estimation algorithm are used to remove the process disturbance term of the original end force. Finally, the estimated value of the end force of the 3P3R manipulator at this time is obtained. The invention is simple and easy to operate, and can obtain high accuracy estimation results, and has high practical value.

【技术实现步骤摘要】
一种基于电机电流测量的3P3R机械臂末端力估计方法
本专利技术涉及一种基于电机电流测量的3P3R机械臂末端力估计方法，属于机器人自动化装配

技术介绍
在当前制造领域的发展背景下，产品生产的效率和精度要求逐渐提高，但面对当前制造领域的生产技术相对落后、自动化程度不高的情况下，机器人作业是解决该发展问题的一项行之有效的措施。相比于人工作业，机器人显然具有高效率，高适用性，能在危险区域作业的优点。机器人技术的发展是解决当前生产力落后的重要措施，而制约着机器人技术发展的其中一项关键因素就是机器人用于真实作业时，其精度有时往往达不到要求。机器人作业过程是机器人带动安装在末端的工具完成各种指令动作，因此对机器人末端力的估计是保证机器人高精度完成任务的一项重要指标。这就需要更加精确的机器人末端力的估计方法。在近年来的力估计控制算法的研究中，Choi等人在其论文“Anovelmethodforestimatingexternalforce:Simulationstudywitha4-DOFrobotmanipulator.”中提出一种通过在机器人末端安装力矩传感器测量关节力矩，利用期望的机器人模型并结合时间延时估计(TDE)和同时输入状态估计(SISE)估计机器人末端力的方法，在论文的仿真实验中作者引入高斯噪声，库仑摩擦等充分模拟现实情况，结果验证该算法可行，但限于力矩传感器的精度，测量到的机械臂末端力的精度在实际机器人作业过程中往往达不到要求。因此对于探究更有效的机器人末端力估计方法，保证其在真实作业过程中的精度要求是亟需解决的问题。3P3R机械臂，是由3个移动关节和3个转动关节组成的机械臂，是在大工件产品作业过程中常用到的一种机器人产品。其模型简图如附图图1所示，图中矩形关节代表移动关节，由空间z轴从下向上，关节1，2，3为移动关节，其中移动关节1沿x轴移动，移动关节2沿y轴移动，移动关节3沿z轴移动，zi代表第i个关节的z轴，di代表第i个移动关节的可以移动的距离。柱状关节代表转动关节，从空间x轴方向，分别为关节4，5，6,其中关节4绕x轴转动，关节5绕y轴转动，关节6绕z轴转动。，lix,liy,liz代表零点位置时第i个关节到第i+1个关节的x,y,z方向的连杆长度。机械臂的移动关节，结构采用丝杠螺母组合，关节驱动电机与丝杠固结，其余关节部分与螺母固结，变丝杠转动为螺母的移动。转动关节结构为关节上安装的驱动电机带动关节转动。3P3R机械臂在工作时，每个关节上安装的驱动电机输出的转矩经过各对应的传动机构输出对应关节动作，从而由各个关节连接而成的3P3R机械臂可以完成各种作业动作。
技术实现思路
本专利技术的目的是为克服已有技术的不足之处，提出一种基于电机电流测量的3P3R机械臂末端力估计方法。本专利技术方法通过在机械臂各关节的驱动电机上放置电流采集模块采集驱动电机的电流值，不需要力矩传感器即可计算出机械臂各关节力矩值，从而计算得到机械臂的末端力，方法简便易操作，并且可以得到精度较高的估计结果，具有很高的实用价值。本专利技术提出一种基于电机电流测量的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)作为输出量，本文档来自技高网...

【技术保护点】
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的计算表达式如下：

【技术特征摘要】
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；具体计算步骤如...

【专利技术属性】
技术研发人员：徐静，张贶恩，侯志民，董海明，陈垦，吴丹，王国磊，张继文，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京,11