一种基于力传感器的机器人末端接触力补偿方法及系统与流程

本发明涉及机器人控制技术领域，具体涉及一种基于力传感器的机器人末端接触力补偿方法及系统。

背景技术：

随着中国经济以及科学技术的快速发展，工业机器人用来代替人类完成简单，重复性高，危险的任务是大势所趋。机器人按所完成的任务类型可以将其分为与外部约束环境接触和不与外部环境接触两种。当涉及到工业生产中的产品抛光，打磨，装配等工作时，需要机器人与外部约束环境相接触，利用六维力传感器来感知接触力是当前常用的方法。

六维力传感器通常安装在机器人末端，能够检测大小和方向不断变化的三维力和三维力矩信息。但是，六维力传感器在随机器人运动的过程中，由于其位姿不断发生变化，同时受安装在传感器上的操作工具重力的影响，传感器的零点也会随之不断变化，这就导致传感器测量上产生一定偏差，降低了其测量精度，进而影响机器人的操作。

技术实现要素：

为解决上述问题，本发明提供一种基于力传感器的机器人末端接触力补偿方法及系统，能够提高保障机器人操作的准确度。

为了实现上述目的，本发明提供以下技术方案：

根据本发明第一方面实施例提供的一种基于力传感器的机器人末端接触力补偿方法，包括：

步骤s100、计算六维力传感器的精度，判断六维力传感器的精度是否满足工艺需求；计算六维力传感器各轴力耦合度，判断六维力传感器各轴力耦合度是否满足工艺需求；

步骤s200、当六维力传感器的精度和各轴力耦合度满足工艺需求时，获取不少于三组机器人工具末端在不同姿态下的六维力传感器读数，利用最小二乘法计算出工具重力和传感器零点；

步骤s300、当六维力传感器与外界环境接触时，计算工具末端接触力；

步骤s400、将传感器零点、工具重力以及工具末端接触力作为六维力传感器的补偿量。

进一步，所述步骤s100中，计算六维力传感器的精度，判断六维力传感器的精度是否满足工艺需求，包括：

读取六维力传感器当前三维力和三维力矩读数；

将六维力传感器调整到设定位置上，并记录每个设定位置上的三维力fx、fy、fz的稳定波形；其中，六维力传感器的设定位置分别为：第一设定位置：x+轴朝上，z轴保持水平姿态；第二设定位置：x+轴朝下，z轴保持水平姿态；第三设定位置：y+轴朝上，z轴保持水平姿态；第四设定位置：y+轴朝上，z轴保持水平姿态；第五设定位置：z轴竖直朝上，x、y轴保持水平姿态；第六设定位置：z轴竖直朝下，x、y轴保持水平姿态；

在六维力传感器处于第一设定位置、第三设定位置、第五设定位置时，不加任何外部力的情况下，通过每个姿态下的x、y、z三个轴的读数计算三轴之间的耦合度，最后将三个设定位置下的耦合度取平均值，得到平均耦合度；

根据平均耦合度判断六维力传感器的各轴力耦合度是否满足工艺生产要求。

进一步，所述步骤s100中，计算六维力传感器各轴力耦合度，判断六维力传感器各轴力耦合度是否满足工艺需求，包括：

通过以下公式计算6个设定位置在稳定姿态下三个方向力的平均值fxa，fya，fza：

fxa＝(fx1+fx2+fx3+fx4+fx5+fx6)/6

fya＝(fy1+fy2+fy3+fy4+fy5+fy6)/6

fza＝(fz1+fz2+fz3+fz4+fz5+fz6)/6

取每个轴上最大值fxmax、fymax、fzmax和最小值fxmin、fymin、fzmin，通过以下公式计算得出fx、fy、fz、fx、fy、fz：

根据至少三组计算结果得到平均值

通过公式s＝|g1-g2|计算精度偏差，其中，

若精度偏差s在设定阈值以内，则判定六维力传感器精度满足工艺需求。

进一步，所述步骤s200包括：

通过以下公式计算在六维力传感器中的力/力矩与工具重心：

mx'＝fz'*y-fy'*z

my'＝fx'*z-fz'*x

mz'＝fy'*x-fx'*y1)

其中，fx’、fy’、fz’是工具重力在六维力传感器中的三维力分量，mx’、my’、mz’是三维力所引起的三维力矩；

当机器人工具末端不受外力时，通过以下公式计算工具重力以及传感器零点：

fx＝fx'+fx

fy＝fy'+fy

fz＝fz'+fz

mx＝mx'+mx

my＝my'+my

mz＝mz'+mz2)

其中，fx，fy，fz为工具重力，fx，fy，fz为传感器零点；

将公式2)带入公式1)中得公式3)；

mx＝fz*y-fy*z+kx

my＝fx*z-fz*x+ky

mz＝fy*x-fx*y+kz3)

其中：

kx＝mx+fy*z-fz*y

ky＝my+fz*x-fx*z

kz＝mz+fx*y-fy*x4)

将公式3)转换成矩阵形式：

记录不少于三个机器人末端不同姿态下的六维力传感器读数，利用最小二乘法计算出工具重心在六维力传感器中的坐标(x，y，z)以及常数kx，ky，kz；

选取不少于三组机器人工具末端的不同姿态，将公式4)转换成公式6)的矩阵形式，同样利用最小二乘法计算出六维力传感器自身零点(mx，my，mz)，(fx，fy，fz)；

根据公式2)和公式5)求得工具重力：

进一步，所述步骤s300包括：

当六维力传感器与工具水平安装时，工具坐标系到六维力传感器坐标系的线性变换矩阵为；其中为旋转矩阵，为工具坐标系在传感器坐标系中的位置表达，l为工具重心到传感器坐标系原点的距离；

末端接触力在传感器坐标系[fs，ms]和工具坐标系中[ft，mt]通过公式7)表示：

机器人的工具末端接触力通过公式求得。

根据本发明第二方面实施例提供的一种基于力传感器的机器人末端接触力补偿系统，所述系统包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述任一项所述的基于力传感器的机器人末端接触力补偿方法的步骤。

本发明的有益效果是：本发明公开一种基于力传感器的机器人末端接触力补偿方法及系统，所述方法为：首先计算六维力传感器的精度，判断六维力传感器的精度是否满足工艺需求；计算六维力传感器各轴力耦合度，判断六维力传感器各轴力耦合度是否满足工艺需求；当六维力传感器的精度和各轴力耦合度满足工艺需求时，获取不少于三组机器人工具末端在不同姿态下的六维力传感器读数，利用最小二乘法计算出工具重力和传感器零点；当六维力传感器与外界环境接触时，计算工具末端接触力；最后，将传感器零点、工具重力以及工具末端接触力作为六维力传感器的补偿量。本发明能够提高保障机器人操作的准确度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一种基于力传感器的机器人末端接触力补偿方法的流程示意图；

图2是本发明实施例中六维力传感器与机器人末端法兰，工具与六维力传感器连接所用转接盘示意图；

图3是本发明实施例中工具重力在六维力传感器坐标系等效示意图；

图4是本发明实施例中六维力传感器与工具相对位姿示意图，其中，100，六维力传感器；200，转换盘；300，工具。

具体实施方式

以下将结合实施例和附图对本公开的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本公开的目的、方案和效果。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

参考图1，如图1所示为一种基于力传感器的机器人末端接触力补偿方法，包括以下步骤：

步骤s100、计算六维力传感器的精度，判断六维力传感器的精度是否满足工艺需求；计算六维力传感器各轴力耦合度，判断六维力传感器各轴力耦合度是否满足工艺需求；

步骤s200、当六维力传感器的精度和各轴力耦合度满足工艺需求时，获取不少于三组机器人工具末端在不同姿态下的六维力传感器读数，利用最小二乘法计算出工具重力和传感器零点；

步骤s300、当六维力传感器与外界环境接触时，计算工具末端接触力；

步骤s400、将传感器零点、工具重力以及工具末端接触力作为六维力传感器的补偿量。

在六维力传感器使用过程中，由于工艺的不同，对六维力传感器的检测精度也会有不同要求，但无论何种工艺，只要是使用到六维力传感器，都需要对进行工具重力补偿和传感器自身零点补偿。然而，在使用过程中，即使六维力传感器在进行工具重力补偿和传感器自身零点补偿后，得到的工具与外部约束环境接触力依然有误差存在的情况。

为解决上述技术问题，发明人在分析解决上述问题的过程中发现，这是由于不同使用场合对六维力传感器的精度要求不尽相同，本实施例中，首先验证六维力传感器各轴耦合情况，判断机械轴的耦合度是否满足工艺需求，其次对六维力传感器的精度进行检查，判断六维力传感器的检测精度是否满足工艺需求。

其中，六维力传感器各轴力耦合度及精度检测步骤如下：

一、通过转接盘将六维力传感器与机器人末端法兰盘连接固定，并将机器人控制器与六维力传感器模数转换模块连接好。六维力传感器上电初始化后，通过codesys软件读取六维力传感器当前三维力和三维力矩读数。

二、通过机器人示教器，将六维力传感器调整到设定位置上，并记录每个设定位置上的三维力fx、fy、fz的稳定波形。其中，六维力传感器的设定位置分别为：第一设定位置：x+轴朝上，z轴保持水平姿态；第二设定位置：x+轴朝下，z轴保持水平姿态；第三设定位置：y+轴朝上，z轴保持水平姿态；第四设定位置：y+轴朝上，z轴保持水平姿态；第五设定位置：z轴竖直朝上，x、y轴保持水平姿态；第六设定位置：z轴竖直朝下，x、y轴保持水平姿态；

三、在六维力传感器处于第一设定位置、第三设定位置、第五设定位置时，不加任何外部力的情况下，通过每个姿态下的x、y、z三个轴的读数计算三轴之间的耦合度，最后将三个设定位置下的耦合度取平均值，得到平均耦合度；

四、根据步骤3)计算的耦合度，结合工艺生产要求，判断六维力传感器的各轴力耦合度是否满足工艺生产要求。

五、通过以下公式计算6个设定位置在稳定姿态下三个方向力的平均值fxa，fya，fza：

fxa＝(fx1+fx2+fx3+fx4+fx5+fx6)/6

fya＝(fy1+fy2+fy3+fy4+fy5+fy6)/6

fza＝(fz1+fz2+fz3+fz4+fz5+fz6)/6

取每个轴上最大值fxmax、fymax、fzmax和最小值fxmin、fymin、fzmin，通过以下公式计算得出fx、fy、fz、fx、fy、fz：

根据至少三组计算结果得到平均值从而减小单次计算的误差。

通过公式s＝|g1-g2|计算精度偏差，其中，

若精度偏差s在设定阈值以内，则判定六维力传感器精度满足工艺需求。

其中，设定阈值根据传感器的工艺需求进行设定，例如，在某系列的传感器中，其工艺需求为精度偏差不超过36n，则，当每次进行精度校准时，s的取值都应该在36n之内。

在一个优选的实施例中，所述步骤s200包括：

工具重力重心求解：

经过对六维力传感器各轴力耦合度以及精度检测后，若达到要求便可认为六维力传感器满足工艺需求，下一步便可对六维力传感器进行补偿。在机器人与外部约束环境接触时，六维力传感器器数值由传感器本身零点误差、工具重力以及工具与外界接触力三部分构成。工具通过转接盘与李传感器连接固定，如图2所示为六维力传感器与机器人末端法兰，工具与六维力传感器连接所用转接盘示意图，如图3所示为工具重力在六维力传感器坐标系等效示意图。

在如图3中，工具重力在六维力传感器中的力/力矩与工具重心可用公式1)表达：

mx'＝fz'*y-fy'*z

my'＝fx'*z-fz'*x

mz'＝fy'*x-fx'*y1)

其中，fx’、fy’、fz’是工具重力在六维力传感器中的三维力分量，mx’、my’、mz’是三维力所引起的三维力矩。

在机器人工具末端没有与外部环境接触时，六维力传感器读数由工具重力以及传感器零点两部分组成。可以用公式2)进行表示：

fx＝fx'+fx

fy＝fy'+fy

fz＝fz'+fz

mx＝mx'+mx

my＝my'+my

mz＝mz'+mz2)

其中fx，fy，fz为六维力传感器读数，fx，fy，fz为传感器零点。

将公式2)带入公式1)中可得公式3)

mx＝fz*y-fy*z+kx

my＝fx*z-fz*x+ky

mz＝fy*x-fx*y+kz3)

其中：

kx＝mx+fy*z-fz*y

ky＝my+fz*x-fx*z

kz＝mz+fx*y-fy*x4)

由公式3)可写成矩阵形式：

记录不少于三个机器人末端不同姿态下的六维力传感器读数，利用最小二乘法便可计算出工具重心在六维力传感器中的坐标(x，y，z)以及常数kx，ky，kz。

接着，求解传感器零点及工具重力：

同理，选取不少于三组机器人工具末端的不同姿态，由公式4)可写成公式6)的矩阵形式，同样利用最小二乘法便可计算出六维力传感器自身零点(mx，my，mz)，(fx，fy，fz)。

由公式2)和公式5)便可求得工具重力：

接着，求解机器人工具末端接触力：

由上述步骤便可求得在六维力传感器未受外力作用下的传感器零点，工具重力，当六维力传感器与外界环境接触时，六维力传感器读数由工具末端接触力、传感器零点、工具重力三部分组成；于是，末端接触力在六维力传感器坐标系表示下的读数便可求得；由于六维力传感器通过转接盘与工具连接，因此六维力传感器与工具的姿态相同，位置不同。

如图4所示为六维力传感器100与工具300相对位姿示意图，当六维力传感器100与工具300水平安装无夹角时，则工具坐标系到六维力传感器坐标系的线性变换矩阵为：其中为旋转矩阵，为工具坐标系在传感器坐标系中的位置表达，l为工具300的重心到传感器坐标系原点的距离。

由于传感器与机器人末端法兰姿态相同，则旋转矩阵为3x3的单位阵，则末端接触力在传感器坐标系[fs，ms]和工具坐标系中[ft，mt]通过公式7)表示：

机器人的工具300末端接触力通过公式求得。

本发明的实施例还提供一种基于力传感器的机器人末端接触力补偿系统，所述系统包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述实施例提供的基于力传感器的机器人末端接触力补偿方法的步骤。

可见，上述方法实施例中的内容均适用于本系统实施例中，本系统实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件的实现方式，以嵌入式软件加载到处理器中，以有效利用各种传感器采集的数据，进行基于力传感器的机器人末端接触力补偿。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来。

所述处理器可以是中央处理单元(central-processing-unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital-signal-processor，dsp)、专用集成电路(application-specific-integrated-circuit，asic)、现场可编程门阵列(field-programmable-gate-array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种基于力传感器的机器人末端接触力补偿系统的控制中心，利用各种接口和线路连接整个基于力传感器的机器人末端接触力补偿系统的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述基于力传感器的机器人末端接触力补偿系统的各种功能。所述存储器可主要包括存储程序区和存储数据区，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart-media-card，smc)，安全数字(secure-digital，sd)卡，闪存卡(flash-card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

尽管本公开的描述已经相当详尽且特别对几个所述实施例进行了描述，但其并非旨在局限于任何这些细节或实施例或任何特殊实施例，而是应当将其视作是通过参考所附权利要求，考虑到现有技术为这些权利要求提供广义的可能性解释，从而有效地涵盖本公开的预定范围。此外，上文以发明人可预见的实施例对本公开进行描述，其目的是为了提供有用的描述，而那些目前尚未预见的对本公开的非实质性改动仍可代表本公开的等效改动。