CN106647529A - 一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统 - Google Patents

Abstract

一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统，属于机器人技术领域。当操作六轴机器人示教盒进行示教时能直接牵引机器人末端，使机器人跟随牵引力到达预定的位置；在牵引的过程中能自动的计算并预测机器人末端所施加的牵引力的方向，六轴机器人末端自动做出位姿调整，不仅使牵引轨迹更加平滑而且控制精度更高，示教过程花费时间更短；六维力传感器感知操作人员对其输入的指令，使六轴工业机器人控制器自动的记录当前位置，并随着指令的改变，能自动切换到抓取功能，操作简单。通信基于工业以太网Ethernet进行数据交换，数据引入了嵌入式系统进行处理，使得本智能示教系统还具有响应时间少、实用性强、可移植性应用价值高等特点。

Description

一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统

技术领域

本发明主要涉及一种工业机器人示教系统，属于机器人技术领域，特别是一种用于六轴工业机器人示教过程中的轨迹精确跟踪示教系统。

背景技术

六轴工业机器人是工业自动化领域的典型制造装备，为了使工业机器人执行搬运等工作任务，需要对其进行示教或者离线编程。目前，传统的六轴工业机器人示教系统大部分采用间接示教方式，通过操作示教盒移动机器人到达指定位置，然后手动操作示教盒将经过的位置记录下来，这种示教方式对操作人员的相关知识储备要求比较高，效率低，过程复杂。而在实际应用中又产生了降低操作人员的使用门槛，简化示教步骤，提高示教效率的市场需求。因此基于现有的六轴工业机器人开发智能示教系统具有很重要的现实意义。

论文“基于力/力矩传感器的直接示教系统研究”，刘昆等，《自动化与仪表》，2016,31(5)，第10-14页，公开了一种基于力/力矩传感器的直接示教系统，该论文介绍了通过一个力/力矩传感器感知操作者的示教力，并通过数据采集卡收集力/力矩的电压模拟信号，根据6个方向模拟电压的变化转化成其各个方向的位移，并于示教过程中通过示教盒手动记录特征轨迹点。然而，该系统并未涉对牵引力方向的计算与预测算法，仅仅把6个方向模拟电压的变化转化成其各个方向的位移，当牵引力方向与三个坐标轴成一定角度时，会出现较大的位置偏差。同时该系统仍然使用传统的数据采集卡与上位机(PC)的模式，系统占用空间大，可移植性弱。因此，适时适当设计一种基于嵌入式系统、具备牵引力方向预测与人机互动功能的智能示教系统非常有必要。

论文“基于顺应性跟踪控制的工业机器人直接示教系统”，徐建明等，《高技术通讯》，2015,25(5)，第500-507页，公开了一种基于顺应性跟踪控制的工业机器人直接示教系统，该系统基于力偏差信号调节机械臂末端执行器的运动速度，实现机械臂对牵引力的顺应性跟踪。该系统未涉及对受力方向的计算与预测，仅仅通过力控制器调节机械臂末端执行器的参考速度，因此在当牵引力方向与三个坐标轴成一定角度时，仍会出现较大的位置偏差，不能实现真正的顺应性。同时该系统不能通过人机互动对示教位置快速记录。

专利CN103213136A公开了一种用于工业机器人牵引示教的方法和系统，该专利提出了一种基于三维压力传感器和三维位姿传感器的手持式示教装置。该装置仅涉及如何改进力传感器获取压力信号的方式，在对压力信号的处理算法上仍然使用传统的根据压力差的大小调整运动速度，并没有对所施加的力方向进行计算与分析，因此会出现工业机器人控制器仍然不知道下一个目标点的位姿坐标，不能实现真正的顺应性牵引示教。也没有通过人机互动对示教的位置进行实时记录的功能。

专利CN105058396A公开了一种机器人示教系统及其控制方法，该专利所述机器人示教系统包括：传感模块、机器人控制器及上位机；其中传感模块包括力传感器、力传感器调制器以及体感捕捉设备。该系统实际上是通过体感捕捉设备获取操作者的指令，再将指令传送给机器人控制器，由机器人控制器执行相应的位姿调整指令。力传感器仅仅作为一个检测装置，并不是通过力传感器获取牵引指令，进而使机器人做出顺应性调整。

发明内容

本发明的目的在于针对上述传统的六轴工业机器人不能直接而柔顺地牵引机器人末端执行器到达指定空间位置，实时与机器人交互，将经过位置数据存储的问题。当操作六轴机器人示教盒进行示教时能直接牵引机器人末端，使机器人跟随牵引力到达预定的位置；在牵引的过程中能自动的计算并预测机器人末端所施加的牵引力的方向，六轴机器人末端自动做出位姿调整，不仅使牵引轨迹更加平滑而且控制精度更高，示教过程花费时间更短；其次，六维力传感器可以感知操作人员对其输入的指令，使六轴工业机器人控制器自动的记录当前位置，并随着指令的改变，能自动切换到抓取功能，操作简单。最后，通信基于工业以太网Ethernet进行数据交换，数据引入了嵌入式系统进行处理，使得本智能示教系统还具有响应时间少、实用性强、可移植性应用价值高等特点。

为实现上述目的，本发明采用的技术方案为一种基于顺应性跟踪的六轴工业机器人智能示教系统，该示教系统包括六维力信号感知系统、力方向解算模块模块、六轴工业机器人系统、动作执行模块；六维力信号感知系统包括圆形载荷单元(1)、法兰连接结构(2)、力信号数据输入接口(3)数据存储盒(4)、力信号数据输出接口(5)、Ethernet通信网线(6)；

力方向解算模块包括树莓派Ethernet输入接口(7)、基于linux的嵌入式控制系统树莓派(8)、六维力信号解耦单元(9)、力坐标变换处理单元(10)、树莓派IO数据输出接口(11)、继电器(12)、路由器(13)；

六轴工业机器人系统包括六轴工业机器人控制器(14)、六轴工业机器人控制器IO数据输入单元(15)、机器人控制器Ethernet输入接口(16)、运动学逆解计算单元(17)、运动驱动单元(18)；

动作执行模块包括六轴工业机器人机械本体(19)，气动抓手(20)，底座(21)。

六维力传感器的圆形载荷单元(1)作为整个系统的直接受力单元感获取外界受力情况信息，圆形载荷单元(1)与法兰连接结构(2)连接，从而与六轴机器人本体(19)末端进行连接；圆形载荷单元(1)通过电缆线与力信号数据输入接口(3)连接，通过力信号数据输入接口(3)将数据存储到数据存储盒(4)，数据存储盒(4)将数据发送到力信号数据输出接口(5)；通过Ethernet通信网线(6)与路由器(13)连接；

在力方向解算模块中，基于linux嵌入式控制系统的树莓派(8)设置有六维力信号解耦单元(9)、力坐标变换处理单元(10)两个功能单元，树莓派(8)通过以太网线将其Ethernet输入接口(7)接入路由器(13)；树莓派IO数据输出接口(11)与继电器(12)连接；

六轴工业机器人控制器(14)设置有运动学逆解计算单元(17)、运动驱动单元(18)，继电器(12)另一端与六轴工业机器人控制器(14)的IO数据输入单元(15)连接；机器人控制器Ethernet输入接口(16)通过以太网线接入路由器(13)。

六轴工业机器人机械本体(19)通过电缆线与六轴工业机器人控制器(14)连接，在六轴工业机器人机械本体(19)的末端通过法兰连接圆形载荷单元(1)再连接气动抓手(20)；六轴工业机器人机械本体(19)的底部固定在底座(21)上。

本发明的有益效果是：由于本发明设计了一种力方向感知与预测的算法，使工业机器人末端沿着该方向做出相应的调整，从而使牵引轨迹更加平滑，控制精度更高，实现了人与工业机器人的柔性互动；其次，本发明采用了工业以太网Ethernet作为通信总线，因此系统内的各个组件都被赋予了IP地址，有利于实现机器人物联网。引入嵌入式系统树莓派作为力信号处理模块，减小了整个智能示教系统的体积，可靠稳定，可移植性强，适用范围广。最后，采用了六维力传感器检测六轴工业机器人末端力信号的输入，赋予了示教系统感知人的命令功能，六轴工业机器人根据不同的力信号输入命令分别可以进行位移变换与角度变换的转变、快速记录当前位置，以及控制末端气动抓手等动作，赋予了六轴工业机器人更加智能化的功能。

附图说明

图1为智能示教系统的主体结构。

图2为六轴工业机器人系统结构。

图3为本智能示教系统控制方法软件执行流程图。

图4为本发明与传统示教系统的牵引效果对比图。

图中：1、圆形载荷单元，2、法兰连接结构，3、力信号数据输入接口，4、数据存储盒，5、力信号数据输出接口，6、Ethernet通信网线，7、树莓派Ethernet输入接口，8、基于linux的嵌入式控制系统树莓派，9、六维力信号解耦单元，10、力坐标变换处理单元，11、树莓派IO数据输出接口，12、继电器，13、路由器，14、六轴工业机器人控制器、15、六轴工业机器人控制器IO数据输入单元，16、机器人控制器Ethernet输入接口，17、运动学逆解计算单元，18、运动驱动单元，19、六轴工业机器人机械本体，20、气动抓手，21、底座。

具体实施方式

如图1-4所示，首先，六轴工业机器人控制器(14)与基于linux的嵌入式控制系统树莓派(8)完成参数初始化，圆形载荷单元(1)检测机器人末端受力情况，受力情况再通过力信号数据输入接口(3)发送到数据存储盒(4)进行动态存储，然后通过力信号数据输出接口(5)与Ethernet通信网线(6)接入路由器(13)，与此同时，数据存储盒(4)将获得一个IP地址。

其次，力信号通过路由器(13)端口的传递后从树莓派Ethernet输入接口(7)进入基于linux的嵌入式控制系统树莓派(8)，在系统树莓派中，力信号要经过六维力信号解耦单元(9)、力坐标变换处理单元(10)的信号处理过程，进而获得六轴工业机器人末端的六个维度的受力信息。获得六个维度的力信息后，一方面系统树莓派将处理信息通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，从而计算出受力的方向在以工具中心位置(TCP)为原点的空间坐标系八个象限中的位置。另一方面，通过方向预测算法，将六个维度的力大小通过数学运算，获得受力方向单位向量与X、Y、Z轴正方向的夹角α、β、γ，再通过刚度系数矩阵，将受力的大小转换为对应机器人末端调整位移d，从而通过这四个参数及方向预测算法，运算出机器人末端下一个目标点的空间位置坐标。最后将下一个目标点的空间位置坐标通过树莓派Ethernet输入接口(7)传输到路由器(13)，此时路由器(13)已经分别为树莓派(8)与六轴工业机器人控制器(14)分配了IP地址，因此下一个目标点的空间位置坐标顺利传递到六轴工业机器人控制器(14)。

最后，六轴工业机器人控制器(14)通过运动学逆解计算单元(17)将一个目标点的空间位置坐标转换为六个轴的相应的转角，再经过运动驱动单元(18)使得六个关节的电机获得扭矩，从而使六轴工业机器人末端做出了相应的位置与姿态的调整，从而实现六轴工业机器人末端在牵引力作用下的顺应性跟踪功能。

当牵引机器人末端到达操作空间的某一位置时，如果想调整当前的姿态，通过在气动抓手(20)的末端轻轻敲击一下，此时六维力传感器将获得一次短暂的敲击信号，此时敲击信号将转换为绕Y轴的转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次位置与姿态转换的命令。

当六轴工业机器人的位置和姿态在牵引力的作用下调整好后，如果想记录当前位置，只需轻轻敲击气动抓手(20)的末端两次，此时敲击信号将转换为绕Y轴的两次转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次记录当前位置的命令。机器人控制器通过将当前位置数据输出到一个文本文件中，并实时保存，到达下一个记录位置时执行相同的过程来记录机器人的位置与姿态。

当到达某个位置要执行抓取动作时，只需轻轻敲击气动抓手(20)的末端三次，此时敲击信号将转换为绕Y轴的两次转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次气动抓手开或者关的命令。从而控制气动抓手执行相应的抓取动作。

树莓派(8)基于linux操作系统，其供电为5V直流电源。树莓派作为本系统的控制核心，用于实现力信息采集、信号变换、力传感器六个通道的数据解耦、坐标变换以及方向预测算法。

与圆形载荷单元(1)连接的数据存储盒(4)采用Sunrise M8128作为模数转换的核心元件，将采集到的六个通道的模拟信号转换成数字量，通过Ethernet接口送树莓派处理。

系统通电后，首先对系统进行初始化，扫描力输入信号，等待启动键输入。当有力信号输入时，系统将进入力方向解算模块。

力方向解算模块中，使用的六维传感器采用矩阵解耦算法实现解耦，计算步骤如下：

1)获取通道1至通道6的原始数据

假设获得的原始数据向量为：

RAW＝[rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6] (1)

上式中rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6分别代表从圆形载荷单元上采集到的沿X轴、Y轴、Z轴和绕X轴、Y轴、Z轴六个维度上的模拟电压的信号经过模数转换后的数字电压信号，单位为伏特V。

2)将原始数转化为以mV/V为单位的数据

激励电压大小为EXC＝5.002240，六维力传感器六个通道上的增益常量为Gain＝[gain1,gain2,gain3,gain4,gain5,gain6]，其中gain1,gain2,gain3,gain4,gain5,gain6分别代表每个通道上的增益常量，六维力传感器6个通道上的放大器零点偏移为AmpZero＝[AZ1,AZ2,AZ3,AZ4,AZ5,AZ6]，其中AZ1,AZ2,AZ3,AZ4,AZ5,AZ6分别代表每个通道上的放大器零点偏移，由以上数据得到：

[DAT]＝1000*(RAW-AmpZero)/65535*5/Gain/EXC (2)

3)计算解耦载荷

由传感器的类型确定解耦矩阵[Decoupled]，它是一个常量，需要通过对传感器进行标定后得到，本发明所采用的解耦矩阵为

最后，得到解耦后的载荷为：

[Result]＝[Decoupled]*[DAT]＝[FX,FY,FZ,MX,MY,MZ] (3)

上式中FX,FY,FZ分别代表原始数据经解耦后沿X轴、Y轴、Z轴方向的力，MX,MY,MZ分别代表原始数据经解耦后绕X轴、Y轴、Z轴方向的力矩。

力坐标变换与力方向预测算法如下：

通过公式(4)将传感器上的受力信息转换为机器人末端受力信息。F_d代表机器人末端工具中心位置(TCP)受力大小，F_i代表六维力传感器载荷单元受力大小，J代表雅克比矩阵，表示从关节速度到操作空间速度的广义传动比，(J^T)^-1表示雅克比矩阵的转置矩阵再求逆矩阵

F_d＝(J^T)^-1F_i (4)

为了获得六轴工业机器人控制器所要执行的下一位置的信息，设机器人末端TCP位置为P₁(x₁,y₁,z₁),牵引力所指向的下一位置为P₂(x₂,y₂,z₂)，则有P₁(x₁,y₁,z₁)与P₂(x₂,y₂,z₂)之间的距离d为：

设每次输入机器人控制器的位置增量Δ_x＝d，以机器人末端TCP位置P₁(x₁,y₁,z₁)为坐标原点，若要计算出该方程需要知道P₂(x₂,y₂,z₂)的各个坐标，根据牵引力的解耦数据[FX,FY,FZ,MX,MY,MZ]，获得各个分力的大小，进而根据公式(6)计算出牵引力及其投影与X,Y,Z轴正方向的夹角α,β,γ

并由此得到计算出下一位置的公式如下：

将获得的下一位置的坐标P₂(x₂,y₂,z₂)，通过Ethernet总线的方式传输到六轴工业机器人控制器，机器人控制器通过程序驱动各个关节电机到达P₂(x₂,y₂,z₂)，从而实现位置调整与六轴工业机器人的顺应性跟踪控制。

Claims (4)

1.一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统，其特征在于：该示教系统包括六维力信号感知系统、力方向解算模块、六轴工业机器人系统、动作执行模块；六维力信号感知系统包括圆形载荷单元(1)、法兰连接结构(2)、力信号数据输入接口(3)数据存储盒(4)、力信号数据输出接口(5)、Ethernet通信网线(6)；

力方向解算模块包括树莓派Ethernet输入接口(7)、基于linux的嵌入式控制系统树莓派(8)、六维力信号解耦单元(9)、力坐标变换处理单元(10)、树莓派IO数据输出接口(11)、继电器(12)、路由器(13)；

六轴工业机器人系统包括六轴工业机器人控制器(14)、六轴工业机器人控制器IO数据输入单元(15)、机器人控制器Ethernet输入接口(16)、运动学逆解计算单元(17)、运动驱动单元(18)；

动作执行模块包括六轴工业机器人机械本体(19)，气动抓手(20)，底座(21)；

六维力传感器的圆形载荷单元(1)作为整个系统的直接受力单元感获取外界受力情况信息，圆形载荷单元(1)与法兰连接结构(2)连接，从而与六轴机器人本体(19)末端进行连接；圆形载荷单元(1)通过电缆线与力信号数据输入接口(3)连接，通过力信号数据输入接口(3)将数据存储到数据存储盒(4)，数据存储盒(4)将数据发送到力信号数据输出接口(5)；通过Ethernet通信网线(6)与路由器(13)连接；

在力方向解算模块中，基于linux嵌入式控制系统的树莓派(8)设置有六维力信号解耦单元(9)、力坐标变换处理单元(10)两个功能单元，树莓派(8)通过以太网线将其Ethernet输入接口(7)接入路由器(13)；树莓派IO数据输出接口(11)与继电器(12)连接；

六轴工业机器人控制器(14)设置有运动学逆解计算单元(17)、运动驱动单元(18)，继电器(12)另一端与六轴工业机器人控制器(14)的IO数据输入单元(15)连接；机器人控制器Ethernet输入接口(16)通过以太网线接入路由器(13)；

六轴工业机器人机械本体(19)通过电缆线与六轴工业机器人控制器(14)连接，在六轴工业机器人机械本体(19)的末端通过法兰连接圆形载荷单元(1)再连接气动抓手(20)；六轴工业机器人机械本体(19)的底部固定在底座(21)上。

2.根据权利要求1所述的一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统，其特征在于：首先，六轴工业机器人控制器(14)与基于linux的嵌入式控制系统树莓派(8)完成参数初始化，圆形载荷单元(1)检测机器人末端受力情况，受力情况再通过力信号数据输入接口(3)发送到数据存储盒(4)进行动态存储，然后通过力信号数据输出接口(5)与Ethernet通信网线(6)接入路由器(13)，与此同时，数据存储盒(4)将获得一个IP地址；

其次，力信号通过路由器(13)端口的传递后从树莓派Ethernet输入接口(7)进入基于linux的嵌入式控制系统树莓派(8)，在系统树莓派中，力信号要经过六维力信号解耦单元(9)、力坐标变换处理单元(10)的信号处理过程，进而获得六轴工业机器人末端的六个维度的受力信息；获得六个维度的力信息后，一方面系统树莓派将处理信息通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，从而计算出受力的方向在以工具中心位置(TCP)为原点的空间坐标系八个象限中的位置；另一方面，通过方向预测算法，将六个维度的力大小通过数学运算，获得受力方向单位向量与X、Y、Z轴正方向的夹角α、β、γ，再通过刚度系数矩阵，将受力的大小转换为对应机器人末端调整位移d，从而通过这四个参数及方向预测算法，运算出机器人末端下一个目标点的空间位置坐标；最后将下一个目标点的空间位置坐标通过树莓派Ethernet输入接口(7)传输到路由器(13)，此时路由器(13)已经分别为树莓派(8)与六轴工业机器人控制器(14)分配了IP地址，因此下一个目标点的空间位置坐标顺利传递到六轴工业机器人控制器(14)；

最后，六轴工业机器人控制器(14)通过运动学逆解计算单元(17)将一个目标点的空间位置坐标转换为六个轴的相应的转角，再经过运动驱动单元(18)使得六个关节的电机获得扭矩，从而使六轴工业机器人末端做出了相应的位置与姿态的调整，从而实现六轴工业机器人末端在牵引力作用下的顺应性跟踪功能；

当牵引机器人末端到达操作空间的某一位置时，如果想调整当前的姿态，通过在气动抓手(20)的末端轻轻敲击一下，此时六维力传感器将获得一次短暂的敲击信号，此时敲击信号将转换为绕Y轴的转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次位置与姿态转换的命令；

当六轴工业机器人的位置和姿态在牵引力的作用下调整好后，如果想记录当前位置，只需轻轻敲击气动抓手(20)的末端两次，此时敲击信号将转换为绕Y轴的两次转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次记录当前位置的命令；机器人控制器通过将当前位置数据输出到一个文本文件中，并实时保存，到达下一个记录位置时执行相同的过程来记录机器人的位置与姿态；

当到达某个位置要执行抓取动作时，只需轻轻敲击气动抓手(20)的末端三次，此时敲击信号将转换为绕Y轴的两次转矩输入，通过树莓派(8)的处理后，通过树莓派IO数据输出接口(11)发送至继电器(12)，再通过六轴工业机器人控制器IO数据输入单元输入六轴工业机器人控制器(14)，机器人控制器(14)将获得一次气动抓手开或者关的命令；从而控制气动抓手执行相应的抓取动作；

树莓派(8)基于linux操作系统，其供电为5V直流电源；树莓派作为本系统的控制核心，用于实现力信息采集、信号变换、力传感器六个通道的数据解耦、坐标变换以及方向预测算法；

与圆形载荷单元(1)连接的数据存储盒(4)采用Sunrise M8128作为模数转换的核心元件，将采集到的六个通道的模拟信号转换成数字量，通过Ethernet接口送树莓派处理。

3.根据权利要求2所述的一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统，其特征在于：系统通电后，首先对系统进行初始化，扫描力输入信号，等待启动键输入；当有力信号输入时，系统将进入力方向解算模块；

力方向解算模块中，使用的六维传感器采用矩阵解耦算法实现解耦，计算步骤如下：

1)获取通道1至通道6的原始数据

假设获得的原始数据向量为：

RAW＝[rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6] (1)

上式中rawchn1,rawchn2,rawchn3,rawchn4,rawchn5,rawchn6分别代表从圆形载荷单元上采集到的沿X轴、Y轴、Z轴和绕X轴、Y轴、Z轴六个维度上的模拟电压的信号经过模数转换后的数字电压信号，单位为伏特V；

2)将原始数转化为以mV/V为单位的数据

激励电压大小为EXC＝5.002240，六维力传感器六个通道上的增益常量为Gain＝[gain1,gain2,gain3,gain4,gain5,gain6]，其中gain1,gain2,gain3,gain4,gain5,gain6分别代表每个通道上的增益常量，六维力传感器6个通道上的放大器零点偏移为AmpZero＝[AZ1,AZ2,AZ3,AZ4,AZ5,AZ6]，其中AZ1,AZ2,AZ3,AZ4,AZ5,AZ6分别代表每个通道上的放大器零点偏移，由以上数据得到

[DAT]＝1000*(RAW-AmpZero)/65535*5/Gain/EXC (2)

3)计算解耦载荷

由传感器的类型确定解耦矩阵[Decoupled]，它是一个常量，需要通过对传感器进行标定后得到，本发明所采用的解耦矩阵为

$\[Decoupled\]=\left[\begin{array}{cccccc}0.17295& 0.47904& 1.55800& 48.69171& -0.41251& -49.1168\\ -0.10446& -53.17226& 0.81224& 28.22852& 1.59298& 28.19603\\ 157.97079& 0.06428& 170.75100& -1.73402& 169.15631& -1.71752\\ -0.17370& -0.13172& -4.84564& 0.09752& 4.30491& 0.04443\\ 4.99251& -0.07994& -2.65909& -0.11702& -2.56574& 0.03157\\ 0.00302& 1.60690& 0.04707& 1.69792& 0.04619& 1.63002\end{array}\right]$

最后，得到解耦后的载荷为：

[Result]＝[Decoupled]*[DAT]＝[FX,FY,FZ,MX,MY,MZ] (3)

上式中FX,FY,FZ分别代表原始数据经解耦后沿X轴、Y轴、Z轴方向的力，MX,MY,MZ分别代表原始数据经解耦后绕X轴、Y轴、Z轴方向的力矩。

4.根据权利要求2所述的一种面向六轴工业机器人轨迹精确跟踪控制的智能示教系统，其特征在于：力坐标变换与力方向预测算法如下：

通过公式(4)将传感器上的受力信息转换为机器人末端受力信息；F_d代表机器人末端工具中心位置(TCP)受力大小，F_i代表六维力传感器载荷单元受力大小，J代表雅克比矩阵，表示从关节速度到操作空间速度的广义传动比，(J^T)^-1表示雅克比矩阵的转置矩阵再求逆矩阵

F_d＝(J^T)^-1F_i (4)

为了获得六轴工业机器人控制器所要执行的下一位置的信息，设机器人末端TCP位置为P₁(x₁,y₁,z₁),牵引力所指向的下一位置为P₂(x₂,y₂,z₂)，则有P₁(x₁,y₁,z₁)与P₂(x₂,y₂,z₂)之间的距离d为：

$d=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2}---\left(5\right)$

设每次输入机器人控制器的位置增量Δ_x＝d，以机器人末端TCP位置P₁(x₁,y₁,z₁)为坐标原点，若要计算出该方程需要知道P₂(x₂,y₂,z₂)的各个坐标，根据牵引力的解耦数据[FX,FY,FZ,MX,MY,MZ]，获得各个分力的大小，进而根据公式(6)计算出牵引力及其投影与X,Y,Z轴正方向的夹角α,β,γ

$\left\{\begin{array}{c}\frac{FY}{FX}=tan\mathrm{\α}\\ \frac{\sqrt{{\mathrm{FX}}^2+{\mathrm{FY}}^2}}{FZ}=tan\mathrm{\γ}\\ \mathrm{\α}+\mathrm{\β}=\frac{\mathrm{\π}}{2}\end{array}\right.---\left(6\right)$

并由此得到计算出下一位置的公式如下：

$\left\{\begin{array}{c}{x}_2=d*sin\mathrm{\γ}*cos\mathrm{\α}\\ y_2=d*sin\mathrm{\γ}*cos\mathrm{\β}\\ z_2=d*cos\mathrm{\γ}\end{array}\right.---\left(7\right)$

将获得的下一位置的坐标P₂(x₂,y₂,z₂)，通过Ethernet总线的方式传输到六轴工业机器人控制器，机器人控制器通过程序驱动各个关节电机到达P₂(x₂,y₂,z₂)，从而实现位置调整与六轴工业机器人的顺应性跟踪控制。