一种基于近似度加权平均插值法的机器人精度补偿方法

摘要

一种基于近似度加权平均插值法的机器人精度补偿方法，针对任意一目标点，在样本数据库中选择其邻域内的样本点基于相似度的判别规则以及相似相近矢量插值方法的使用条件进行进一步筛选，当基于相似度的判别规则以及相似相近矢量插值算法的使用条件，判断出目标点与其邻域内的若干个样本点的位姿矢量之间足够相似时，再根据矢量间的相似度指标确定各样本点对目标点的影响权值，采用近似度加权平均插值算法对目标点的位姿误差进行插值预测。本发明可显著提高机器人的绝对定位精度，有效避免传统标定方法无法实现在整个工作空间都达到同样效果的高精度补偿缺陷，适用于机器人的非几何误差补偿，能有效应用于对机器人精度要求高的领域。

权利要求

1.一种基于近似度加权平均插值法的机器人精度补偿方法，其特征是按如下步骤：步骤S1、对机器人进行标定与校准；

步骤S2、利用激光跟踪仪在工作空间中对随机位姿点进行广泛采样，得到样本点的位姿矢量，并计算出相应的位姿误差矢量，建立样本数据库；

步骤S3、引入两矢量夹角的正弦和矢量之差的比值近似度子指标λ以及两矢量形成的三角形面积近似度子指标 构建矢量间近似度指标体系，通过两个近似度子指标共同判断矢量的近似度，建立机器人位姿矢量及位姿误差矢量相似性度量方法；

步骤S4、基于近似度加权平均插值的补偿算法的设计：(a)各样本点Pi,i＝1,2,…,m对P点的影响权值qi的赋值方法的建立；(b)目标点的位姿误差的插值计算算法设计；

步骤S5、确立近似度判别准则及相似相近矢量插值描述的使用条件，包括：(a)两矢量可相互替换时的近似度的充分条件的设定；(b)近似度判别准则及相似相近矢量插值描述的使用条件的确定；

步骤S6、基于近似度判别准则及相似相近矢量插值描述的使用条件，判断矢量相近程度以对样本数据库中的样本点进行筛选，选取与目标点的位姿矢量及位姿误差矢量具有足够相似度的邻域样本点；

步骤S7、根据相近程度不同，对选取的与目标点具有足够定位误差相似度的各样本点赋予不同的权重，采用基于近似度加权平均插值的补偿算法，对目标点误差进行插值计算与预测；

步骤S8、通过求解机器人的运动学逆解，实现机器人在位姿点的绝对定位精度补偿。

2.根据权利要求1所述的一种基于近似度加权平均插值法的机器人精度补偿方法，其特征是步骤S3中所述的矢量间近似度指标的构建，具体步骤如下：机器人处于某一位姿时，其在机器人基坐标系下的位姿及位姿误差均可视为三维矢量；设任意两个矢量为ps和pt，它们之间的距离、夹角以及两矢量在空间上形成的三角形面积分别为|ps-pt|、 和SΔ，定义矢量间近似度子指标λ及  的数学表达式如下：两个矢量之间越接近，λ及 值越大，当近似度子指标λ的值为1和近似度子指标 的值为无穷时，两个矢量最为接近。

3.根据权利要求1所述的一种基于近似度加权平均插值法的机器人精度补偿方法，其特征是步骤S4中所述的基于近似度加权平均插值的补偿算法的设计，包括以下步骤：(a)建立各样本点Pi,i＝1,2,…,m对P点的影响权值qi的赋值方法；

设任意一目标点P(xP,yP,zP)，在其邻域内选取m个与之足够相近的样本点Pi(xi,yi,zi),i＝1,2,…,m，它们的位姿矢量为pi,i＝1,2,…,m、理论位姿矢量与实际位姿矢量之差为i＝1,2,…,m，则目标点P的位姿误差预测值矢量 可基于样本点的数据进行插值求取；

各样本点Pi,i＝1,2,…,m对P点的影响权值qi的大小与目标点位姿矢量相对其邻域内各样本点位姿矢量的两个近似度子指标有关，是两个近似度子指标的r次方倒数的加权和，如下式：其中，λi为待插点P的预测位姿矢量与样本点Pi的实际位姿矢量夹角的正弦和矢量之差的比值近似度子指标； 为待插点P的预测位姿矢量与样本点Pi的实际位姿矢量形成的三角形面积近似度子指标；ω1、ω2为两近似度子指标在影响权值qi运算中的权重；r为加权幂指数，0≤r≤2；qi为样本点Pi对P点的影响权重，0≤qi≤1，且(b)目标点的位姿误差的插值计算算法设计；

近似度加权平均空间插值公式设计如下：

4.根据权利要求1所述的一种基于近似度加权平均插值法的机器人精度补偿方法，其特征是步骤S5中所述的确立近似度判别准则及相似相近矢量插值描述的使用条件，包括以下步骤：步骤(a)两矢量可相互替换时的近似度的充分条件的设定；

设[λ]为给定允许两矢量相互替换时的近似度子指标λ的最小允可值，则有：同理， 为给定允许两矢量相互替换时的近似度子指标 的最小允可值，则有：上面两式作为确定两矢量可相互替换的近似度的充分条件；

步骤(b)确定近似度判别准则及相似相近矢量插值描述的使用条件：设ps与pt两矢量之间给定允许的最大误差的大小为ε，则有|ps-pt|≤ε

取|ps-pt|达到最大值ε时进行计算，则有

即

设给定允许的最大定位误差的大小为ε，邻域内各样本点的位姿误差矢量为 i＝1,

2,…,m，目标点P的实际位姿误差矢量为 若要误差小于ε，即结合权重公式，则有

因此若

成立，则 即可成立；

矢量间的参数取值满足以上公式，则在选定的邻域内各样本点和目标点之间的位姿矢量及位姿误差矢量具有较高的近似度。

说明书

一种基于近似度加权平均插值法的机器人精度补偿方法

技术领域

[0001] 本发明属于机器人无运动学模型标定技术领域，特别是一种基于近似度加权平均插值法的机器人精度补偿方法。

背景技术

[0002] 机器人的定位精度是衡量机器人性能的一个重要指标。影响机器人位姿误差的因素有很多，主要包括装配过程中产生的连杆几何参数实际与理论偏差而造成的机器人系统误差，以及由工作环境温度变化、运动副的运动误差和构件的弹性变形误差等导致的随机误差。工业机器人的重复定位准确度较高(只与随机误差有关，一般在0.1mm以下，可达0.01mm)，而绝对定位精度较差(与系统误差有关，仅几个毫米)，远无法满足高端制造机器人柔性自动化生产线、飞机装配等高端装备制造业提出的超精密高可靠性要求。

[0003] 提高机器人设计、加工、装配与控制系统的精度或进行机器人标定，是提高机器人的绝对定位精度的有效方法。前者通常对材料、加工设备与方法要求较高，导致生产成本激增，加上实际使用中结构磨损无法避免，长时间运行后机器人的绝对定位精度仍然无法保证。有关机器人的标定，国内外学者作了大量研究工作。标定的方法大致分为两类：基于运动学模型的标定技术和无运动学模型标定技术。前者具有操作性强、效率高、效果好等优点，应用较为普遍，相关研究主要集中于测量仪器的应用与创新、标定测量方法的改进、参数辨识算法等方面。Wampler等在并联机器人各个关节上安装高精度传感器，以辨识机构参数误差。Zhuang等利用电子经纬仪沿着Stwewart机构的边缘检测了其位姿误差所有信息。Besnard等提出了一种利用双倾角仪标定Gough-Stwewart平台的方法。邵珠峰，唐晓强等利用线尺工具完成了对一种平面柔性并联机构的自标定研究，该方法为全闭环控制少自由的柔性并联机构提供了可行的测量方法。Traslosheros利用一台手持摄像机与一个固定参考球组成的视觉测量系统作为主传感器对Delta并联机器人进行了标定研究。朱延河等设计出一种可以依次轮换锁紧Delta机构的支链平行四边形架锁紧装置，通过支链的传感器输出与理论模型输出的差别来辨识机构参数误差。Notash等针对RSI6自由度并联机构，提出了位姿对误差参数敏感度定义权重的加权最小二乘辨识算法。然而基于运动学模型的标定技术需要建立误差参数与机构末端误差的泛函，而装配间隙等所带来的误差约占机器人总误差源0.5％～1.0％的比重，且通常难以辨识，误差建模时还往往会忽略一些对末端误差影响较小且无法补偿的误差，导致所建立的误差模型无法完全代表实际机构。且因没有考虑机器人的位姿误差分布特点及作用规律，对影响定位精度的机器人非几何误差进行补偿及精度的提升能力有限，无法实现在整个工作空间均达到同样效果的高精度误差补偿，易导致某些位姿误差补偿的不足或过补偿发生。同时该类方法建模和参数辨识过程繁杂，耗时较多，因此更具通用性的无模型标定技术应运而生。

[0004] 无模型标定技术不依赖机器人的运动学模型，只关注机器人关节角或理论位姿的输入和末端位姿或者位姿误差之间的关系，方法简单且通用性强。李洪超等针对平面关节型机器人(简称SCARA)，通过运动学正逆解解出实际位姿与理论位姿之间驱动输入的差值，补偿理论输入，获得补偿后的动平台位姿，极大地提高了末端执行器的位姿精度。无运动学模型标定技术属于位形空间补偿方法，无需辨识机构的误差参数，可直接将末端误差转换成输入端误差补偿，具有计算方法简单，标定精度高的优势。专利“南京航空航天大学，成都飞机工业(集团)有限责任公司，一种用于工业机器人的空间立体网格精度补偿方法：中国，CN201110113246.6[P].2011-11-2.”(其相关论文：周炜的博士论文《飞机自动化装配工业机器人精度补偿方法与实验研究》，简称文献1)对于包络空间内划分的某个立方体网格内的任一点，采用空间网格划分的方法，采用空间插值的方法来对机器人的理论坐标进行修正，完成机器人在该点的绝对定位精度补偿。该方法存在以下不足：

[0005] 1)将工作空间划分为一系列的立方体网格的操作有较为苛刻的要求，例如立方体网格边长的合理选择需要通过大量的实验确定，立方体网格必须有8个顶点且要求立方体网格包络目标点，对立方体网格的顶点进行采样；

[0006] 2)矢量相似度指标仅考虑了矢量之间的距离与矢量间相似度的关联，而未考虑两矢量之间的夹角以及它们在空间上形成的三角形面积对矢量间相似度的影响，故在各样本点对目标点的影响权值的确定上不够精确，因此所能提高的精度有限；

[0007] 3)该方法中不能实现不同姿态下的对机器人绝对定位精度的补偿，且在误差测量方面的工作较大。

发明内容

[0008] 发明目的是为了克服现有技术中存在的不足，提供一种基于近似度加权平均插值法的机器人精度补偿方法，以提高机器人的绝对定位精度。

[0009] 本发明是通过以下技术方案实现的，具体流程如图1所示。

[0010] 针对任意一目标点(定位点)，在样本数据库中选择其邻域内的样本点基于相似度的判别规则以及相似相近矢量插值方法的使用条件进行进一步筛选，当利用相似度的判别规则以及相似相近矢量插值算法的使用条件，判断出目标点与其邻域内的若干个样本点的位姿矢量之间足够相似时，再根据矢量间的相似度指标确定各样本点对目标点的影响权值，采用近似度加权平均插值算法对目标点的位姿误差进行插值预测。

[0011] 本发明是基于机器人工作空间范围内样本数据库中样本点的位姿及其误差数据，遵循矢量相似度原理及距离加权反比插值法，针对工业机器人精度补偿而构建的一种以近似度指标确定权重的加权平均插值补偿运算的新方法，能大大减少传统的机器人标定方法所必须涉及的复杂误差补偿模型的构建以及求逆解的计算，简化计算过程的复杂度，并有效地避免标定方法无法适用于整个工作空间实现高精度补偿的缺陷，能实现插值点位姿误差的精确补偿，有效应用于对机器人精度要求高的领域。

[0012] 本发明所述的一种基于近似度加权平均插值法的机器人精度补偿方法，按如下步骤：

[0013] 步骤S1、对机器人进行标定与校准。包括机器人工具坐标系标定、激光位移传感器空间位姿标定、激光位移传感器非线性因素校准、基于激光测量的手眼关系标定等内容；

[0014] 步骤S2、创建样本数据库。利用激光跟踪仪在工作空间中对随机位姿点进行广泛采样，得到样本点的位姿矢量，并计算出相应的位姿误差矢量，建立样本数据库；

[0015] 步骤S3、矢量间近似度指标的构建及机器人位姿矢量及位姿误差矢量相似性度量方法的建立。考虑到两个矢量在空间上的接近程度不仅与它们之间的距离有关，还与矢量之间的夹角以及所形成的三角形面积有关，故引入两矢量夹角的正弦和矢量之差的比值近似度子指标λ(简称为近似度子指标1)以及两矢量形成的三角形面积近似度子指标 (简称为近似度子指标2)的概念，构建矢量间近似度指标体系，通过两个近似度子指标共同判断矢量的近似度，建立机器人位姿矢量及位姿误差矢量相似性度量方法；

[0016] 步骤S4、基于近似度加权平均插值的补偿算法的设计。包括：(a)各样本点Pi(i＝1,2,…,m)对P点的影响权值qi的赋值方法的建立；(b)目标点的位姿误差的插值计算算法设计；

[0017] 步骤S5、确立近似度判别准则及相似相近矢量插值描述的使用条件。基于近似度判别准则及相似相近矢量插值描述的使用条件，可对各样本点与目标点间的位姿矢量及位姿误差矢量是否足够近似进行判断，从而为后续实现在样本数据库中对目标点邻域内的样本点进行合理筛选，保证算法的补偿精度打下基础，包括：(a)两矢量可相互替换时的近似度的充分条件的设定；(b)近似度判别准则及相似相近矢量插值描述的使用条件的确定；

[0018] 步骤S6、样本点的筛选。基于近似度判别准则及相似相近矢量插值描述的使用条件，判断矢量相近程度以对样本数据库中的样本点进行筛选，选取若干个与目标点的位姿矢量及位姿误差矢量具有足够相似度的邻域样本点；

[0019] 步骤S7、误差插值预测。根据相近程度不同，对选取的(与目标点具有足够定位误差相似度的)各样本点赋予不同的权重，采用基于近似度加权平均插值的补偿算法，对目标点误差进行插值计算与预测；

[0020] 步骤S8、误差补偿。通过求解机器人的运动学逆解，实现机器人在位姿点的绝对定位精度补偿。

[0021] 本发明所述的步骤S3中矢量间近似度指标的构建，具体步骤如下：

[0022] 机器人处于某一位姿时，其在机器人基坐标系下的位姿及位姿误差均可视为三维矢量；为了对不同位姿矢量(或误差矢量)进行比较，以任意两个矢量ps和pt为例，它们在空间上的接近程度关联原理如图2所示，若它们之间的距离、夹角以及两矢量在空间上形成的三角形面积分别为|ps-pt|、 和SΔ，定义矢量间近似度子指标λ及 的数学表达式如下：

[0023]

[0024]

[0025] 两个矢量之间越接近，λ及 值越大，当近似度子指标λ的值为1和近似度子指标的值为无穷时，两个矢量最为接近；

[0026] 本发明所述的步骤S4基于近似度加权平均插值的补偿算法的设计，包括以下步骤：

[0027] (a)建立各样本点Pi(i＝1,2,…,m)对P点的影响权值qi的赋值方法；

[0028] 对于机器人在任一定位点处的位姿矢量(或位姿误差矢量)，如果已知其他若干个与之相近的位姿矢量(或位姿误差矢量)，则理论上可以通过近似度加权平均空间插值方法确定未知定位点的位姿矢量(或位姿误差矢量)；插值补偿算法原理如图3所示，设任意一目标点P(xP,yP,zP)，在其邻域内选取m个与之足够相近的样本点Pi(xi,yi,zi)i＝1,2,…,m，它们的位姿矢量为pi(i＝1,2,…,m)、理论位姿矢量与实际位姿矢量之差为则目标点P的位姿误差预测值矢量 可基于样本点的数据进行插值求取；

[0029] 各样本点Pi(i＝1,2,…,m)对P点的影响权值qi的大小与目标点(待插点)位姿矢量相对其邻域内各样本点位姿矢量的两个近似度子指标有关，是两个近似度子指标的r次方倒数的加权和，具体公式定义如下：

[0030]

[0031] 其中，λi为待插点P的预测位姿矢量与样本点Pi的实际位姿矢量夹角的正弦和矢量之差的比值近似度子指标，即近似度子指标1；为待插点P的预测位姿矢量与样本点Pi的实际位姿矢量形成的三角形面积近似度子指标，即近似度子指标2；ω1、ω2为两近似度子指标在影响权值qi运算中的权重；r为加权幂指数，0≤r≤2，通常取值为1或2，为简化计算，可取r＝1；qi为样本点Pi对P点的影响权重，0≤qi≤1，且

[0032] (b)目标点的位姿误差的插值计算算法设计；

[0033] 近似度加权平均空间插值公式设计如下：

[0034]

[0035] 本发明所述的步骤S5确立近似度判别准则及相似相近矢量插值描述的使用条件，包括以下步骤：

[0036] 步骤(a)两矢量可相互替换时的近似度的充分条件的设定；

[0037] 设[λ]为给定允许两矢量相互替换时的近似度子指标λ的最小允可值，则必须使[0038]

[0039] 同理， 为给定允许两矢量相互替换时的近似度子指标 的最小允可值，则必须使

[0040]

[0041] 上面两式即可作为确定两矢量可相互替换的近似度的充分条件；

[0042] 步骤(b)近似度判别准则及相似相近矢量插值描述的使用条件的确定；

[0043] 假设ps与pt两矢量之间给定允许的最大误差的大小为ε，则有

[0044] |ps-pt|≤ε；

[0045] 取|ps-pt|达到最大值ε时进行计算，则有

[0046]

[0047]

[0048] 即

[0049]

[0050]

[0051] 若又假设给定允许的最大定位误差的大小也为ε，邻域内各样本点的位姿误差矢量为 目标点P的实际位姿误差矢量为 若要误差小于ε，即

[0052]

[0053] 结合权重公式，则有

[0054]

[0055] 因此若

[0056]

[0057] 成立，则 即可成立；

[0058] 矢量间的参数取值满足以上公式，则认为在选定的邻域内各样本点和目标点之间的位姿矢量及位姿误差矢量具有较高的近似度，且目标点的位姿误差可用本方法计算。

[0059] 本发明相比现有技术，具有以下有益的技术效果：

[0060] 1)所述基于近似度加权平均插值法的机器人精度补偿方法为无运动学模型标定技术，该方法参考距离加权反比插值法的原理，考虑了不同位置矢量间存在矢量夹角的正弦和矢量之差的比值近似度差异，以及两矢量形成的三角形面积近似度差异，且两近似度将以不同的影响系数共同作用于位姿矢量或位姿误差矢量，方法更近似于机器人的实际模型，从而为精确误差补偿提供了理论依据。因此能够解决对机器人重复定位精度高而绝对定位精度低的问题。

[0061] 2)本方法通过相似度判断准则选用数据库样本进行空间插值实现精度补偿，无需考虑机器人运动学模型和误差源，方法简单且通用性强；在以近似度判别准则及相似相近矢量插值方法的使用条件对样本点进行筛选的过程中，一旦判断邻域任意样本点与目标点的位姿矢量具有足够的相似度，根据相近程度不同，对各样本点赋予不同的权重，即可实现目标点位姿误差的精确补偿，对样本点具体的数量和位置没有过高的要求，容易进行实施。

[0062] 3)本方法可显著提高机器人的绝对定位精度，有效避免传统标定方法无法实现在整个工作空间都达到同样效果的高精度补偿缺陷，适用于机器人的非几何误差补偿，能有效应用于对机器人精度要求高的领域。

[0063] 本发明属于无模型标定技术，不依赖机器人的运动学模型，只关注机器人关节角或理论位姿的输入和末端位姿或者位姿误差之间的关系。并通过近似度判别准则及相似相近矢量插值方法的使用条件对样本点进行筛选，对于样本点的要求更为宽泛，本发明在采样及计算精度具有优势。

附图说明

[0064] 图1为本发明补偿方法流程图。

[0065] 图2为两矢量的近似度关联原理图。

[0066] 图3为本发明插值补偿算法原理图。

[0067] 图4为德国KUKA KR150-2机器人外形及其连杆坐标系示意图。

[0068] 图5为文献1提出的基于空间插值的补偿算法原理图。

[0069] 图6为本发明精度补偿后的试验结果图。

具体实施方式

[0070] 下面结合附图，以6自由度的KUKA KR150-2机器人为例，对本发明作更进一步的说明。

[0071] (1)KUKA KR150-2机器人连杆坐标系的建立。

[0072] 德国KUKA公司生产的KR150-2型工业机器人是一种典型的6自由度转动关节的开链式工业机器人。额定负载150kg，最大作用范围2700mm，6轴，重复精确度<±0.06mm，控制系统KRC2。6个转动关节中的前三个关节主要用于确定机器人末端的位置，后三个关节用于确定机器人末端的姿态。关节1的轴线垂直于机器人底座平面；关节2、3的轴线相互平行且它们平行于机器人的底座平面；关节3、4的轴线垂直交错；关节4、5和6的轴线相交于一点。机器人外形及其连杆坐标系如图4所示，连杆理论参数如表1所示。图4及表1中的ai是第i关节连杆长度；αi是第i关节连杆扭角；θi是第i关节的关节转角；di是第i关节的关节偏置值；
扭角βi是绕着第i关节的连杆坐标系中yi轴转动的旋转参数。

[0073] 表1连杆理论参数

[0074] 序号 连杆长度a/mm 偏置值a/mm 连杆扭角α/(°) 转角θ/(°) 扭角β/(°)1 350 750 -90 θ1 0
2 1250 0 0 θ2 β2
3 55 0 90 θ3 0
4 0 1100 -90 θ4 0
5 0 0 90 θ5 0
6 0 230 0 θ6 0

[0075] 尽管本发明提出的方法属于无运动学模型标定技术类型的位形空间补偿方法，无需考虑机器人运动学模型和误差源，无需辨识机构的误差参数，可直接将末端误差转换成输入端误差补偿，但考虑到机器人运动学模型及机器人位姿误差模型，是机器人精度补偿方法研究的理论基础，故在介绍本发明之前，对KUKA KR150-2机器人运动学模型以及机器人位姿误差模型做一个简单介绍。

[0076] (2)机器人运动学模型

[0077] 采用Veitschegger和Wu提出的MD-H模型(Modified D-H模型)对KUKA KR150-2机器人进行建模，它在D-H运动学建模的基础上，综合考虑当相邻两关节轴线平行时，微小的平行度偏差将使实际公法线极大偏离理论公法线位置，导致关节轴姿态的微小变化引发D-H参数巨大变化的奇异现象，引入绕着每一个连杆坐标系中y轴转动的旋转参数(扭角β)，解决该奇异问题。相邻连杆坐标系i-1与i之间的相对平移和旋转关系可由一个齐次变换矩阵来描述。

[0078]

[0079] 式(1)中，S表示sin(·)，C表示cos(·)。例如Sαi表示sinαi，Cαi表示cosαi。Ai表示i-1与关节i之间坐标变换矩阵。ai、αi、θi、di及βi的含义同前。当相邻关节平行时，di定义为零。当相邻关节轴线不平行时，扭角βi值定义为零。

[0080] 对于n自由度的机器人，从机器人的基座开始依次进行坐标系的转换，并将每个齐次变换矩阵命名为A1,A2,…,An，那么便可得出从机器人末端坐标系到机器人基坐标系的总变换：

[0081]

[0082] 式(2)中，Tn为机器人末端坐标系到机器人基坐标系的齐次坐标变换矩阵。

[0083] 式(1)也可以用四个3×1矢量来进行描述

[0084]

[0085] 式(3)中，ni,oi,ai为三个旋转矢量，确定姿态；Pi为平移矢量，确定空间位置(position)。其中oi为方位矢量(orientation)；ai为接近矢量(approach)；ni为法向矢量(normal)，ni＝oi×ai。

[0086] (3)机器人位姿误差模型

[0087] 根据式(1)中Ai的正确描述依赖于它的5个连杆参数ai，αi，di，θi，βi。由于每个连杆的几何参数的公称值与真实值之间都存在着微小差别，这些误差可用Δai、Δαi、Δdi、Δθi、Δβi表示，其中Δθi为电子零误差，其它为连杆几何参数误差。假设连杆参数连续可导，对式(1)的5参数求全微分，即可求得变换误差dAi

[0088]

[0089] 由式(1)可以得到

[0090]

[0091] 式(5)中

[0092]

[0093] 同理有

[0094]

[0095]

[0096]

[0097]

[0098] 式(7)～式(10)中

[0099]

[0100]

[0101]

[0102]

[0103] 由此可得

[0104] dAi＝Ai(QaΔai+QαΔαi+QdΔdi+QθΔθi+QβΔβi)＝Ai(δAi)   (15)[0105] δAi＝QaΔai+QαΔαi+QdΔdi+QθΔθi+QβΔβi   (16)

[0106] 式(15)、(16)中，δAi为Ai的误差矩阵。一般地，di、ai的精度比αi、βi、θi要高，而且Δθi、Δαi、Δβi对机器人末端执行器的误差影响较大。

[0107] 将式(6)、式(11)～(14)代入式(16)中可得

[0108]

[0109] 式(17)中 ——分别为相对关节i坐标系x、y、z轴三方向的位置偏差。 ——分别为相对关节i坐标系x、y、z三轴的旋转偏差。

[0110] 令

[0111]

[0112] 式(18)中， 为相对关节i坐标系x、y、z轴三方向的位置偏差矢量。

[0113] 相对关节i坐标系x、y、z三轴的旋转偏差矢量为

[0114]

[0115] 如果每一关节均有参数误差dAi存在，则相邻关节均有微分摄动齐次变换，由式(2)可得

[0116]

[0117] 式(20)中，dTn为机器人末端坐标系相对于基座标的微分变化。

[0118] 忽略高阶微分，式(20)可以变化为

[0119]

[0120] 定义Ui＝AiAi+1…An，则式(21)可以变化为

[0121]

[0122] 式(22)中，δTn为Tn的误差矩阵，可表示为

[0123]

[0124] 式(23)中，dxn，dyn，dzn——相对机器人末端坐标系x、y、z轴三方向的位置偏差。

[0125] δxn，δyn，δzn——相对机器人末端坐标系x、y、z三轴的旋转偏差。

[0126] 进一步整理，用相对相邻连杆坐标系i-1与i之间的齐次变换矩阵Ai描述的位置偏差矢量 和旋转偏差矢量 来获得相对于机器人末端坐标系描述的位置偏差矢量(dn)和旋转偏差矢量(δn)。可得

[0127]

[0128]

[0129] 式(24)、(25)中 为Ui+1的三个3×1姿态矢量。 为Ui+1的一个3×1位置矢量。

[0130] 经过进一步整理

[0131]

[0132] 式(26)中Dn——相对机器人末端坐标系的位置偏差及旋转偏差；

[0133] Mθ、Md、Ma、Mα、Mβ——各对应关节参数的平移系数矩阵；

[0134] Rθ、Rα、Rβ——各对应关节参数的旋转系数矩阵；

[0135] Δθ＝(Δθ1 Δθ2 … Δθn)T

[0136] Δd＝(Δd1 Δd2 … Δdn)T

[0137] Δa＝(Δa1 Δa2 … Δan)T

[0138] Δα＝(Δα1 Δα2 … Δαn)T。

[0139] 由上述推导可知，矩阵Mθ、Md、Ma、Mα、Mβ以及Rθ、Rα、Rβ的各元素是由机器人关节参数组成的函数表示。由式(22)可知，随着机器人所处位置的改变，dTn因为对应着不同的Mθ、Md、Ma、Mα、Mβ、Rθ、Rα、Rβ和Tn，矩阵将发生变化。

[0140] 由此可求得机械手末端执行器由于连杆的参数误差引起的位置误差dn和姿态误差δn，即建立了位姿误差模型，可为误差补偿研究提供基础。

[0141] (4)机器人位置矢量近似度

[0142] 机器人处于某一位姿时其在机器人基坐标系下的位置及位置误差dn均可视为三维矢量。为了对两个位置矢量进行比较，本发明考虑到两个位置矢量在空间上的接近程度不仅与它们之间的距离有关，还与矢量之间的夹角以及所形成的三角形面积有关，故引入两位置矢量夹角的正弦和矢量之差的比值近似度子指标λ以及两位置矢量形成的三角形面积近似度子指标 的概念，通过两个近似度子指标共同判断位置矢量间的近似度(图2)。即对于任意两个位置矢量ps和pt，若它们之间的距离、夹角以及两矢量在空间上形成的三角形面积分别为|ps-pt|、 和SΔ，定义λ及 的数学表达式如下

[0143]

[0144] 式(27)中，λ——两矢量夹角的正弦和矢量之差的比值近似度子指标。为简便起见，后面简称为近似度子指标1。

[0145]

[0146] 式(28)中，——两矢量形成的三角形面积近似度子指标。为简便起见，后面简称为近似度子指标2。

[0147] 两个矢量之间越接近，λ及 值越大，当近似度子指标λ的值为1和近似度子指标的值为无穷时，两个矢量最为接近。

[0148] (5)不同定位点位姿矢量间近似度概念的扩展

[0149] 由式(17)和(22)可知dTn中的位置误差矢量dn的各个分量都可由机器人各杆件运动学参数的三角函数进行描述，且在机器人各关节可达范围内1阶连续可微。因此当两个定位点所对应的机器人各关节转角相差很小，则认为这两点的位置高度接近，且关节角度偏差越小，近似度就越高。通过分析相邻关节角配置的两定位点间位置的近似度，可以得出以下结论：

[0150] 当两个位置矢量达到高度相近时，在许可范围内，理论上可以用其中一个位置矢量来近似代替另外一个位置矢量，同时理论上也可以用其中一个位置的误差矢量来近似代替另外一个位置的误差矢量。当图2中的ps和pt高度相近时，可以用ps近似代替pt，与此同时，也可以用ps的误差 近似代替pt的误差

[0151] 以上讨论的是相近的两个位置矢量之间以及它们位置误差之间的关系，将其扩展到相近的多个位姿矢量(或位姿误差)，则其中某一位姿矢量(或位姿误差)可由其他位姿矢量(或位姿误差)最小二乘线性拟合。然而，当需要用若干个已知的位姿矢量(或位姿误差)来表示与之近似的某一未知位姿矢量(或位姿误差)时，需要确定每个已知位姿矢量(或位姿误差)的影响系数即权重。

[0152] (6)近似度加权平均空间插值法的原理

[0153] 插值是建立定位点位姿补偿模型的关键步骤，采用不同的插值方法建立的数学模型会有差别，对定位点位姿误差补偿的效果也会有差异。常用的插值方法一般有距离加权反比法、多项式二乘法以及克里金插值法。本发明提出的近似度加权平均空间插值方法参考距离加权反比插值法的思路，基于相近相似的原理：即两个矢量之间的距离|ps-pt|，夹角以及ps、pt两矢量在空间上形成的三角形面积SΔ越小，它们的性质就越相似，反之则相似性越小。它以插值点与样本点间的两个近似度子指标(近似度子指标1和近似度子指标2)共同决定权重份额进行赋权，与插值点相近程度越高的样本点赋予的权重越大。

[0154] 对于机器人在任一定位点处的位姿矢量(或位姿误差矢量)，如果已知其他几个与之相近点的位姿矢量(或位姿误差矢量)，则理论上可以通过近似度加权平均空间插值方法确定未知定位点的位姿矢量(或位姿误差矢量)。为提高未知定位点的插值逼近精度，需要对机器人工作空间的样本数据库中未知定位点邻域内的点进行筛选，选取若干个与目标点具有足够相似度的邻域样本点。则未知定位点的位姿误差可以由邻域内选取的各样本点的位姿误差插值计算。如图3所示，设有m个样本点Pi(i＝1,2,…,m)，对应的位姿矢量为pi(i＝1,2,…,m)、理论位姿矢量与实际位姿矢量之差为 则对于目标定位点(目
标点)P的位置误差矢量 的预测值可以通过以下步骤来求取。

[0155] 1)计算各样本点Pi(i＝1,2,…,m)对P点的影响权值qi

[0156] 设空间待插点(目标点)为P(xP,yP,zP)，其位姿误差预测值 未知，P点邻域内样本点Pi(xi,yi,zi)，i＝1,2,…,m，其位姿误差为 已知。利用近似度加权平均空间插值法对P点的位姿误差 进行插值。待插点P的位姿误差 取邻域内各样本点已知位姿误差 的加权平均。权的大小与待插点位姿矢量相对邻域内各样本点位姿
矢量间的两个近似度子指标有关，是两个近似度子指标的r次方倒数的加权和。具体公式如下：

[0157]

[0158] 式(29)中，λi——待插点P的预测位姿矢量与样本点Pi的实际位姿矢量夹角的正弦和矢量之差的比值近似度子指标，即近似度子指标1；

[0159] ——待插点P的预测位姿矢量与样本点Pi的实际位姿矢量形成的三角形面积近似度子指标，即近似度子指标2；

[0160] ω1、ω2——两近似度子指标在影响权值qi运算中的权重。

[0161] r——加权幂指数，0≤r≤2，通常取值为1或2，为简化计算，可取r＝1。

[0162] qi——样本点Pi对P点的影响权重，0≤qi≤1，且

[0163] 由式(29)可见，本发明提出的近似度加权平均空间插值方法属于一种加权平均插值法，可以进行确切的或者圆滑的方式插值。加权幂指数r控制着权系数如何随着空间待插点与样本点的相近程度的下降而下降。对于一个较大的加权幂指数r，较近的数据点被给定一个较高的权重份额，对于一个较小的加权幂指数r，权重比较均匀地分配给各数据点。当计算一个待插点时，配给的权重qi是一个非负小数，所有权重的总和等于1.0。

[0164] 2)预测目标点的位姿误差

[0165]

[0166] 通过式(30)计算，将求得的目标点P的位姿误差矢量预测值 对其理论定位坐标进行修正，用修正后的坐标来驱动机器人可提高P点的绝对定位精度。

[0167] 3)基于近似度加权平均插值法适用性的条件

[0168] 为了尽可能保证插值的精度，由式(30)建立的误差预测模型是建立在样本点与插值点间有着较高的定位误差相似度的前提上的。本发明提出一种比较易于操作的判断方法，下面对它进行介绍。

[0169] 设ps与pt为两位姿矢量，它们之间的距离为|ps-pt|，两矢量间的夹角为 ( 很小)，两矢量在空间上形成的三角形面积SΔ。[λ]为给定允许两矢量相互替换时近似度子指标λ的最小允可值，根据式(27)，则必须使

[0170]

[0171] 同理， 为给定允许两矢量相互替换时近似度子指标 的最小允可值，根据式(29)，则必须使

[0172]

[0173] 式(31)、(32)即可作为确定两矢量可相互替换的近似度的充分条件。

[0174] 假设ps与pt两矢量之间给定允许的最大误差的大小为ε，则有

[0175] |ps-pt|≤ε   (33)

[0176] 取|ps-pt|达到最大值ε时进行计算，根据式(31)，则有

[0177]

[0178] 根据式(32)，则有

[0179]

[0180] 即

[0181]

[0182] 将式(34)的结果代入式(36)，则有

[0183]

[0184] 若又假设给定允许的最大定位误差的大小也为ε，邻域内各样本点的位姿误差矢量为 目标点P的实际位姿误差矢量为 若要误差小于ε，即

[0185]

[0186] 结合式(30)，则有

[0187]

[0188] 因此若

[0189]

[0190] 成立，则式(38)即可成立。

[0191] 当矢量间的两个近似度子指标满足式(31)、(32)，或者矢量间的参数取值满足式(34)、式(37)以及矢量误差之差满足式(40)时，则认为在选定的邻域内各样本点和目标点的位置之间具有较高的近似度，且目标点的位置误差可用本发明方法计算。

[0192] 需要着重强调的是，由于机器人到达同一位姿存在着多组可能解，本发明提出的相近点实际是指样本点和目标点所对应的机器人各关节转角都很接近，即机器人各关节较小改变角度就可以从某点移至其它点，这个前提对于提出的补偿模型至关重要。

[0193] (7)实例验证

[0194] 为了对提出的基于近似度加权平均插值法的机器人精度补偿方法的有效性进行验证，本发明借用该文献1的实验数据，在机器人工作空间随机选取一点作为目标点进行精度补偿，对提出的精度补偿方法效果进行评价，并将本发明的补偿结果与该文献1的结果进行对比。

[0195] 研究的对象是KUKA公司生产的6自由度KR150-2型串联机器人，额定负载为1500N；测量工具是FARO SI型激光跟踪仪，测量精度为10μm+0.8μm/m。在试验过程中机器人，和激光跟踪仪的位置保持固定不变，机器人以机械零点为起点。

[0196] 具体思路如下：在机器人工作空间中选取任一位姿的试验点(目标点)，根据选定的试验点，根据矢量可相互替换时的近似度的充分条件，对其邻域内样本数据库中的点进行筛选，选取若干个与目标点具有足够定位误差相似度的邻域样本点。获得各样本点的位置参数与姿态参数及理论定位误差，随后根据式(29)计算各样本点对试验点的影响权值，并根据式(30)基于影响权值度量的机器人精度补偿方法预测出选定试验点处的误差，最后再将预测出的试验点的定位和用定位误差模型计算出的误差进行比较得到残差，通过残差来判断精度补偿方法的有效性。

[0197] 任意选取(x2000,y700,z1000,a0,b90,c0)和(x1450,y200,z1500,a0,b90,c0)作为2个目标试验点，前三个参数表示目标位置，后三个参数表示目标点姿态，姿态采用RPY表示方式，分别在样本数据库中对两实验点邻域内的样本点(样本点)进行合理筛选，各筛选出了8个样本点，为方便叙述及列表，分别对它们标以序号1～8及1’～8’(具体坐标分别见表2及表3)。公式(29)的ω1、ω2取值为ω1＝0.87，ω2＝0.13。按照上述思路，对本发明方法进行验证以及将本发明结果与文献1的结果进行对比，文献1提出的基于空间插值的补偿算法原理图可参见图5。具体结果如下：表2、表3分别为用机器人定位误差模型求得两目标点的邻域各样本点实际定位误差、位置以及矢径。表4、表5分别为采用本发明提出的基于近似度加权平均插值法对两目标点(x2000,y700,z1000)与(x1450,y200,z1500)预测绝对定位误差的过程参数。表6为本发明提出的方法与文献1的提出的方法对两目标点预测绝对定位误差的精度对比。

[0198] 表2目标点(x2000,y700,z1000)的邻域各样本点实际定位误差、位置以及矢径[0199]

[0200] 表3目标点(x1450,y200,z1500)的邻域各样本点实际定位误差、位置以及矢径[0201]

[0202] 表4本发明对目标点(x2000,y700,z1000)的仿真验证结果

[0203]

[0204] 表5本发明对目标点(x1450,y200,z1500)的仿真验证结果

[0205]

[0206] 表6本发明与文献1结果对比

[0207]

[0208] 从表6可以看出，利用该模型对机器人绝对定位精度进行补偿，可使目标点(x2000,y700,z1000)的绝对定位误差的最大值由补偿前的1.0011mm缩小到-0.0054609mm，最小值由补偿前的-0.2303mm缩小到-1.0237E-05mm；也可使目标点(x1450,y200,z1500)的绝对定位误差的最大值由补偿前的0.6606mm缩小到-0.00509mm，最小值由补偿前的-0.0698mm缩小到-0.00189mm，效果显著。

[0209] 从表6还可以看出，目标点(x2000,y700,z1000)以及目标点(x1450,y200,z1500)在经过本发明提出的机器人精度补偿方法补偿后，其预测定位误差与理论值非常接近。相对文献1的方法，两目标点的预测定位精度有了进一步提高，其中目标点(x2000,y700,z1000)的x坐标的预测定位残差由文献1的0.0019下降到0.00158499，残差缩小了16.5794％，y坐标的预测定位残差由文献1的-0.0061下降到-0.0054609，残差缩小了
10.4775％，z坐标的预测定位残差由文献1的-1E-04下降到-1.0237E-05，残差缩小了
89.7629％；目标点(x1450,y200,z1500)的x坐标的预测定位残差由文献1的-0.0037下降到-0.00313，残差缩小了15.4054％，z坐标的预测定位残差由文献1的-0.0024下降到-
0.00189，残差缩小了21.25％，但y坐标的预测定位残差由文献1的-0.0014上升到-
0.00509，有所增大。综合看来，应用本发明提出的基于近似度加权平均插值法进行机器人精度补偿在x、y、z轴三方向的位置误差显著减小，x、z轴方向的残差也明显优于文献1的算法，证明了本发明方法的有效性。

[0210] 为进一步验证了本发明方法的有效性。本发明在机器人工作空间随机取70个实验点，依据本发明方法进行精度补偿，精度补偿试验结果如图6所示，结果表明70个随机选取的验证点在经过补偿后的绝对定位误差的最大值为0.3761mm，平均值为0.1209mm，比未补偿前的1～3mm有了进一个数量级的提高，证明了建立在本发明理论上的基于空间插值的机器人精度补偿方法在理论上是正确的、在应用中是可行的。

[0211] 综上，本发明针对机器人标定高度依赖于构型的问题，提出了一种基于接近度加权平均插值的机器人精度补偿方法，利用工作空间上的邻近点进行误差预测。工作空间中任意一点的位置误差都可以通过相邻的样本点来估计。具体结论如下：

[0212] 1)机器人工作空间中相邻区域任意点间的绝对定位误差矢量之间存在着或多或少的相似性。考虑到两个矢量之间的距离，夹角，以及它们在空间上形成的三角形面积对相似度的影响，提出了相似度计算方法及基于相似度计算插值的使用条件。

[0213] 2)提出了基于近似度加权平均插值法的机器人精度补偿方法。该方法可以有效地提高机器人的绝对定位精度。并大大减少了传统的机器人标定方法所必须涉及的复杂误差补偿模型的构建以及求逆解的计算，大大简化了计算过程的复杂度，并有效地避免了标定方法无法适用于整个工作空间实现高精度补偿的缺陷。

[0214] 3)本发明方法一旦判断邻域任意样本点与目标点的位姿矢量具有足够的相似度，根据相近程度不同，对各样本点赋予不同的权重，即可实现插值点位置误差的精确补偿，对样本点具体的数量和位置没有过高的要求，容易进行实施。