一种基于姿态均匀分布的工业机器人TCP标定方法与流程

本发明属于工业机器人末端工具标定技术领域，具体涉及一种基于姿态均匀分布的工业机器人tcp标定方法。

背景技术：

随着我国自动化制造业的快速发展，人工成本不断上升，工业机器人越来越广泛地应用于工业自动化制造领域。在实际应用中，机器人通过安装在末端法兰上的工具来完成各种作业任务，而工具中心点(toolcentralpoint，简称tcp)相对于末端位置的偏移量大多是未知的，或者不准确的，机器人工具tcp标定就是计算工具端相对于机器人末端坐标系的位置辨识过程，通常在不借助外部测量工具的条件下,通过一定方法精确地计算偏移量的大小。工具标定的精度直接影响到机器人的轨迹精度，因此，准确的tcp标定方法对机器人的应用具有重要作用。

目前工业机器人工具标定方法主要有辅助设备法和固定参考点法。

中国发明专利申请“用于标定工业机器人系统的工具中心点的方法”(申请公布号cn106462140a)和中国发明专利申请“一种应用于工业机器人的tcp在线快速标定方法及装置”(申请公布号cn105945948a)分别各自公布了一种应用于工业机器人的tcp在线快速标定方法。但其共同特征都是需要为标定专门设计一种具有光电传感器的辅助设备，这类标定方法增加了额外的硬件成本，不利于现场标定。

固定参考点法是在工业机器人末端的工作范围内设置一个固定点，然后通过调整工业机器人的关节角度，使机器人末端工具tcp多次对齐到该固定点，从而实现tcp标定。由于其无需提供外部设备，实施简易，因此成为当前主流的标定方法。目前国内外工业机器人生产商均为其生产的机器人提供了基于固定参考点的标定方法，其中以“四点法”或“六点法”居多。固定参考点法在实施过程中，影响标定精度的关键因素是多次tcp点与固定参考点重合时的机器人姿态之间的差异度，然而当前“四点法”或“六点法”在实际操作时，工作人员对机器人姿态的选择分布具有随机性，尤其是当四点都分布在参考点一侧时，因分布位置的集中限制了机器人能做姿态的幅度，导致减少了各测量点之间姿态的差异度，因此导致标定误差增大。

为解决“四点法”的标定误差问题，中国发明专利申请“一种工业机器人tcp标定方法”(申请公布号cn106502208a)公开了一种多点tcp标定方法，方法首先通过多点测量数据构建超定方程组，再采用最小二乘法实现工具标定，算法实现简便，一定程度上减小了标定误差，但该方法仍未解决由于机器人姿态选择分布的随机性导致姿态之间差异度无法优化的问题，由于超定方程组系数矩阵由不同测量点时机器人末端姿态矩阵之差构成，当机器人姿态分布不均匀时，会增大矩阵条件数而形成病态使标定结果产生误差。

技术实现要素：

为解决固定参考点法在实施过程中存在的现有技术不足，本发明提供了一种基于姿态均匀分布的工业机器人tcp标定方法，本方法的目的在于解决机器人姿态选择分布的随机性，使机器人姿态在各测量点绕固定参考点均匀分布，最大限度增大各测量点之间机器人姿态的差异度，实现提高标定精度的目标。

一种基于姿态均匀分布的工业机器人tcp标定方法，包括以下步骤：

步骤一：采用工业机器人自带的“六点法”进行初始标定；

步骤二：采用近似垂直姿态创建初始测量点位形；

步骤三：构建以固定参考点为球心的球面均匀分布的虚拟点；

步骤四：创建并筛选出各理想测量点位形；

步骤五：调节各机器人实际测量点位形，实施基于球面均匀分布的精确标定。

比较好的是，本发明的步骤一为：选取固定参考点，采用一般工业机器人自带的“六点法”进行初步标定；标定结果作为tcp的初始标定值。

比较好的是，本发明的步骤二为：首先用末端工具近似垂直的姿态使tcp点与固定参考点重合，由示教器读取此时的机器人第六轴末端法兰盘端面中心位置和姿态数据；再结合步骤一的tcp初始标定值计算得到机器人重定位到固定参考点的近似位姿，上述数据作为初始测量点位形的关键数据，为后续计算提供基准。

比较好的是，本发明的步骤三为：以固定参考点为球心，以参考点到机器人重定位前的原始tcp的长度为半径构建虚拟球面，在该球面上创建一定数量沿球面均匀分布的虚拟点。

比较好的是，本发明的球面均布虚拟点的产生方法，包括如下步骤：

步骤3.1、首先在以原点为球心的单位球面上产生随机分布的虚拟点作为初始状态，其坐标由球面坐标公式计算；

步骤3.2、采用力学斥力迭代法逐次更新虚拟点的坐标，最终使各点沿单位球面均匀分布；

步骤3.3、将单位球面均布的虚拟点映射为以参考点为球心的球面均布虚拟点；

步骤3.4、创建矢量旋转矩阵，转换虚拟点的位置，使其中首个虚拟点与创建初始测量点时的法兰盘中心位置重合，同时保持各虚拟点的球面均匀分布。

比较好的是，本发明的步骤四包括如下步骤：

步骤4.1、计算各理想测量点位形对应的机器人关节角：首先根据步骤三的虚拟点数据确定各理想测量点位形对应的机器人第六轴末端位姿，再根据运动学逆解得到理想测量点位形对应的机器人关节角；

步骤4.2、筛选理想测量点位形：按步骤4.1得到的机器关节角，在离线编程仿真软件中逐组调节虚拟机器人到对应理想测量点位形，剔除其中关节角超限、连杆之间发生碰撞的位形组。

比较好的是，本发明的步骤五包括如下步骤：

步骤5.1、得到实际测量点位形数据：通过机器人的示教调试步骤四筛选后可供调节的机器人理想测量点位形，得到实际测量点位形对应的机器人关节角及其对应的机器人第六轴末端位姿；

步骤5.2、标定计算，得出最终标定结果：首先通过最小二乘球面拟合得到最终标定的固定参考点坐标，再根据运动学约束关系得到每个测量点位形计算的tcp位置标定值，取平均值作为最终的tcp位置标定结果。

本发明的有益之处在于：提供了一种基于姿态均匀分布的工业机器人tcp固定参考点标定方法。一、解决了当前固定参考点法标定时机器人姿态选择的随机性和分布的不均匀性，使机器人姿态在各测量点绕固定参考点均匀分布，最大限度增大了各测量点之间机器人姿态的差异度，有效提高了标定精度。二、通过离线编程仿真有效地筛选出可用于实际操作的测量点位形，避免了不必要的操作时间，提高了工作效率。

附图说明

图1为标定方法的整体步骤流程框图。

图2a为单位球面上创建随机虚拟点的示意图。

图2b为单位球面上完成虚拟点均匀分布的示意图。

图2c为虚拟点完成从单位球面到参考点球面映射的示意图。

图2d为虚拟点在参考点球面上通过矢量旋转变换后形成最终分布的示意图。

图3为固定参考点球面均匀分布虚拟点的创建过程示意图。

图4为创建各理想测量点位形示意图。

具体实施方式

以下将结合实施例和附图对本发明的构思、结构、实施过程及产生的效果进行进一步清晰、完整的描述，以充分理解本发明的目标、特征和效果。当然，所描述的实施例只是本发明的一部分实施例，而不是全部。基于本发明的实施例，本领域的技术人员在不付出创造性劳动的前提下所获得的其他实施例，均属于本发明保护的范围。

如图1所示，一种基于姿态均匀分布的工业机器人tcp重定位方法，包括以下步骤：

步骤一：采用工业机器人自带的“六点法”进行初始标定；

步骤二：采用近似垂直姿态创建初始测量点位形；

步骤三：构建以固定参考点为球心的球面均匀分布的虚拟点；

步骤四：创建并筛选出各理想测量点位形；

步骤五：调节各机器人实际测量点位形，实施基于球面均匀分布的精确标定。

结合图1、图2a、图2b、图2c、图2d、图3、图4所示，本发明基于姿态均匀分布的工业机器人tcp标定方法，其具体过程包括如下步骤：

步骤一：初始标定。方法是：选取固定参考点pr，采用一般工业机器人自带的“六点法”进行初步标定。标定结果作为tcp的初始值，其中tcp位置数据记为姿态数据用zyx欧拉角表示，记为可转换为其对应的3×3姿态矩阵，记为

步骤二：创建初始测量点位形。方法是：用末端工具近似垂直的姿态使tcp点与固定参考点重合，以此时的机器人第六轴末端法兰盘端面中心(即机器人默认的原始tcp，重定位前)的位置记为p0(p0x,p0y,p0z)，机器人的姿态记为e0(e0x,e0y,e0z)，可由示教器读出p0和e0。e0可转换为对应的姿态矩阵，记为r0。根据步骤一的tcp初始标定值可计算得到机器人重定位到固定参考点的近似位姿，固定参考点位置记为pr(prx,pry,prz)，姿态记为er(erx,ery,erz)，er对应的姿态矩阵记为rr，上述pr,rr(或er)，p0,r0(或e0)四个变量数据即为初始测量点位形的关键数据，为后续计算提供基准。

步骤三：构建以固定参考点为球心的球面均匀分布的虚拟点。方法是：以参考点pr为球心，以prp0的长度r为半径构建虚拟球面sr(pr,r)，在该球面上创建n个沿球面均匀分布的虚拟点(即各相邻点之间均等距离，点数n>4)，球面均布虚拟点可在matlab软件下模拟产生，产生方法的具体步骤如下：

需要特别说明：下述步骤3.1和3.2中的出现的虚拟点pi应完整表述为^s0pi，左上标s0表示其在单位球面上，由于步骤3-1和3-2中涉及的相关变量均在单位球面内计算，对应的变量符号众多，为简化书写，这里统一省略了左上标s0。

步骤3.1、首先在以原点o(0,0,0)为球心的单位球面上s0(o,1)，产生随机分布的n个点作为初始状态，虚拟点pi(i＝1～n)的坐标采用如下球面坐标公式(1)进行计算。

其中，ai＝2π·ξi,bi＝asin(2·ξi-1)(i＝1～n)，ξi(i＝1～n)表示(0，1)之间的随机数，可由matlab函数rand(n,1)产生。为表示当前值为初始状态，在点坐标符号上加上标0，记为

步骤3.2、根据力学斥力原理，采用迭代法逐次更新虚拟点pi(i＝1～n)的坐标，最终使各点沿单位球面均匀分布。具体计算方法为：

下列各变量的右上标k表示该变量的第k次迭代值，k从0开始，表示初始状态，

s3.2.1)计算任意两点之间的矢量差

s3.2.2)计算任意两点之间的距离

s3.2.3)计算相对于任意一点pi^k，该点与其他各点之间的斥力之和fi^k：

s3.2.4)计算合力的径向分量和切向分量其中ri^k＝pi^k-o(i＝1～n)，表示各虚拟点相对于球心的径向矢量；

s3.2.5)计算虚拟点pi^k的更新移动速度vi^k+1：其中，g为斥力常数，更新速度的初值可设为vi⁰＝0；

s3.2.6)计算更新后的虚拟点坐标pi^k+1：pi^k+1＝pi^k+vi^k+1，pi^k+1＝pi^k+1/||pi^k+1||(i＝1～n)；pi^k+1经过上述更新并单位化后可满足仍在单位球面上；

s3.2.7)结束迭代条件判定：当各点更新速度的最大值满足时，结束迭代，否则返回到s3.2.1)继续下一次迭代，其中e为更新速度阈值常数。

经过上述步骤得到的pi(i＝1～n)可认为已近似沿单位球面均匀分布，其近似度取决于s3.2.7)中阈值e的取值大小，为进行下一步的映射变换，后续记为^s0pi(i＝1～n)，前述已做说明。

步骤3.3、将上述单位球面s0(o,1)均布的虚拟点^s0pi(i＝1～n)映射到以参考点pr为球心的球面sr(pr,r)上的点^srpi(i＝1～n)。即：^srpi＝pr+r·^s0pi(i＝1～n)

步骤3.4、创建矢量旋转矩阵rot，转换虚拟点^srpi(i＝1～n)的位置，使^srp1与步骤二创建初始测量点时的法兰盘中心位置p0重合，同时保持^srpi(i＝1～n)的球面均匀分布。经过步骤3.3后，^srpi(i＝1～n)已经形成了虚拟点的均匀分布，但由于步骤3.1中虚拟点^s0pi(i＝1～n)产生之初的分布是随机的，因此步骤3.3映射后得到的^srpi(i＝1～n)并不包含p0。由于初始测量点位形同时具备姿态数据r0(或e0)，为后续计算提供了姿态计算基准，为此，需要通过转换使^srp1与p0重合，其他各点^srpi(i＝2～n)随之转换以保持均匀分布。方法是：

s3.4.1)创建矢量旋转矩阵rot01。首先构建转换前矢量：vbef＝(^srp1-pr)/||^srp1-pr||，转换后矢量：vaft＝(p0-pr)/||p0-pr||。由此得到旋转轴矢量n＝cross(vbef,vaft)，记n＝(nx,ny,nz)，旋转角θ＝acos(vbef·vaft)，则旋转矩阵rot01可由goldman公式计算如下：

由此可见矢量旋转矩阵rot01是(^srp1,p0,pr)的函数，根据上述方法构建矢量旋转矩阵求解的自定义函数caltwovecrotmtx，使得rot01＝caltwovecrotmtx(^srp1,p0,pr)。

s3.4.2)转换更新虚拟点^srpi(i＝1～n)的位置：pi＝rot·^srpi(i＝1～n)。此时等式左边得到的pi(i＝1～n)是最终得到的满足sr(pr,r)球面均匀分布，同时p1与初始测量点p0重合条件的虚拟点序列，至此虚拟点创建完成，由于此时及后续计算中的pi(i＝1～n)已明确在sr(pr,r)球面上，故至此可省略上标sr。

步骤四、创建并筛选出理想测量点位形。通过调节机器人关节角，满足以下两个要求：

要求1：机器人第六轴末端法兰盘中心处于各虚拟点pi(i＝1～n)处；

要求2：同时机器人末端工具tcp点pm(i＝1～n)与固定参考点pr重合(pipm(i＝1～n)连线通过pr)。

上述两个要求都满足时对应的机器人位形即为机器人理想的测量点位形。步骤四的目的就是通过计算得到满足上述位形要求同时不发生碰撞和关节超限的机器人关节角。

步骤4.1、计算各理想测量点位形对应的机器人关节角，具体方法如下：

s4.1.1)确定各理想测量点位形对应的机器人第六轴末端位姿矩阵⁰t6i(i＝1～n)，包括位置坐标⁰p6i(i＝1～n)和姿态矩阵⁰r6i(i＝1～n)：

s4.1.1.1)第六轴末端位置位于虚拟点处，因此，⁰p6i＝pi(i＝1～n)，满足要求1。

s4.1.1.2)第六轴末端姿态矩阵可由步骤二初始测量点位形姿态矩阵r0通过旋转变换得到。旋转矩阵rot1i的计算方法和步骤3.4.1)中rot01的相同，即

rot1i＝caltwovecrotmtx(pi,p1,pr)，(i＝1～n)，因此，⁰r6i＝rot1i·r0(i＝1～n)。

由于旋转前后的pipr(i＝1～n)和p1pr两矢量以pr为旋转原点，同时步骤二初始测量点tcp与pr重合。因此满足要求2.

s4.1.1.3)由上述两步构造位姿矩阵得：

s4.1.2)利用运动学逆解方法解出位姿矩阵⁰t6i(i＝1～n)对应的机器关节角qi(i＝1～n)，运动学逆解方法采用笔者在机械设计与制造，2016，4：190-194上发表的文章“abb1410工业机器人的旋量运动学逆解方法”，需要说明的是：该论文以abb1410机器人为例，采用旋量法解决了abb1410为代表的一类6自由度工业机器人逆解问题，因此不局限于abb1410，具有广泛的通用性。

步骤4.2、筛选理想测量点位形。按步骤4.1得到的n组机器人关节角qi(i＝1～n)，在离线编程仿真软件中逐组调节虚拟机器人到对应理想测量点位形，剔除其中关节角超限、连杆之间发生碰撞的位形组，剩下的n组为可实施的理想测量点位形，要求n≥4。重新调整编组后，其对应的机器人关节角记为qi(i＝1～n)。

步骤五、调节各机器人实际测量点位形，实施基于球面均匀分布的精确标定。

步骤5.1、得到实际测量点位形数据。按步骤5.1的n组机器人关节角qi(i＝1～n)，逐组调节实际机器人到各理想测量点位形，由于步骤四的计算是依据步骤一初始标定结果得出的，而初始标定“六点法”存在固有误差，因此，各理想测量点位形的机器人末端工具tcp并不与固定参考点pr重合，但已十分接近，因此通过机器人线性运动功能，在不改变机器人tcp姿态的情况下，微调机器人tcp使其与pr重合，每次理想测量点修正后的位形称为实际测量点位形，由示教器读出并记录每次实际测量点位形对应的机器人关节角和第六轴末端位姿数据。其中，修正后关节角仍记为qi(i＝1～n)，修正后的第六轴末端位置和姿态矩阵仍记为⁰p6i(i＝1～n)和⁰r6i(i＝1～n)，位姿数据也可通过运动学正解计算得到。

步骤5.2、标定计算，得出最终标定结果。方法如下：

s5.2.1)对步骤5.1得到的m组第六轴末端位置⁰p6i(xi,yi,zi)(i＝1～n)进行最小二乘球面拟合，拟合得到的球心坐标o0(x0,y0,z0)即为最终标定的固定参考点坐标，仍记为pr(prx,pry,prz)。拟合球半径记为r0。最小二乘球面拟合方法如下：

构造函数：(x0,y0,z0,r0)是使h取得最小值的那组参数，要求同时满足：由此得到：

其中，由式(3)三元一次方程组可解得(u0,v0,w0)，然后得到即得pr。

s5.2.2)根据运动学约束关系有pr＝r6ipmi+p6i，由此得：取均值作为最终的tcp位置标定结果，即为：

s5.2.3)至此，实现了对tcp的位置标定。

tcp标定的重点是针对位置标定，对于工具的姿态标定，由于同一工具的姿态可有多种选择模式，没有唯一性，仍可沿用一般“六点法”中姿态的标定方法，不再赘述。

实施例

为了验证本发明的具体执行情况，采用末端固定焊枪的新时达stepsa1800型六自由度工业机器人进行tcp标定实验，根据厂商提供的焊枪仿真模型，理论tcp是(6.00,-12.00,400.00)。根据图1所述的标定步骤流程图，执行步骤如下：

步骤一：在机器人末端工作空间内设置一个固定参考点，采用机器人示教器自带的“六点法”标定得到初始的tcp位姿坐标为：

步骤二：以焊枪末端近似垂直的姿态接触参考点，定为首个测量点位形，记录机器人第六轴末端法兰中心位姿坐标：

p0＝(877.991,-95.674,876.982)，e0＝(-0.0086,-34.95,-179.99)；

结合初始tcp位姿坐标计算得初步标定的固定参考点坐标为：

pr＝(1111.867,-83.200,555.746)，er＝(179.99,0.1889,-0.0035)。

步骤三：构建以固定参考点pr为球心，半径为prp0的球面均匀分布的虚拟点。设虚拟点数n＝20。采用力学斥力迭代法得到虚拟点pi(i＝1～n)的坐标如表1所示：

表1

步骤四：创建并筛选出理想测量点位形。

步骤4.1：计算各理想测量点位形对应的机器人关节角。首先根据表1虚拟点数据确定各理想测量点位形对应的机器人第六轴末端位姿如表2所示：

表2

由表2再根据运动学逆解得到理想测量点位形对应的机器人关节角如表3所示：

表3

步骤4.2、筛选理想测量点位形。按步骤4.1得到的20组机器人关节角，在离线编程仿真软件中逐组调节虚拟机器人到对应理想测量点位形，剔除其中关节角超限、连杆之间发生碰撞的位形组。经过筛选，保留其中序号为1、2、5、9、11、12、15、16、17和19的位形，其他序号对应的位形予以提出剔除。经过筛选后的位形共有10组。

步骤五、调节各机器人实际测量点位形，实施基于球面均匀分布的精确标定。

步骤5.1、得到实际测量点位形数据。经筛选后共得到10组可供调节的机器人实际测量点位形。经过机器人示教调试，10组实际测量点位形对应的机器人关节角如表4所示：

表4

根据表4数据进行运动学正解得到10组实际测量点位形对应的机器人第六轴末端位姿如表5所示：

表5

步骤5.2、标定计算，得出最终标定结果。

根据上述表5实际机器人测量位形数据和公式(3)拟合得到固定参考点坐标：

pr＝(1112.359,-82.974,555.376)

进一步根据公式(4)得到最终的工具tcp位置标定结果为：

pm＝(6.17,-12.05,399.01)

与初始标定的结果相比，最终标定结果已十分接近理论的tcp值，进一步考虑到现场装配等因素造成的实际值与理论值略有偏差，由此可见，经过本方法标定的结果已十分接近实际值，大大提高了测量精度，验证了本方法的正确性和可行性。