CN107866823B - 一种基于位置矢量法的工业机器人几何参数标定方法 - Google Patents

Abstract

一种基于位置矢量法的工业机器人几何参数标定方法：建立基于基坐标系oxyz下的机器人位置矢量模型，从机器人手册寻找并获取机器人零位状态下方向矢量和连接矢量的名义值，在机器人手册给定的各关节运动范围，在示教器上任意设定机器人各组关节角，控制机器人各个关节运动到设定的各组关节角值q_ij，用激光跟踪仪对安装在机器人末端G的靶标进行检测，获取被测量机器人末端姿态和位置数据，建立机器人几何参数标定的目标函数E_j，使用遗传算法求解目标函数E_j，获取被测量机器人方向矢量和连接矢量的最优解。本发明解决了相邻关节平行及垂直时所引起的奇异性问题，同时基于该模型建立了工业机器人几何参数标定误差优化目标函数。

Description

一种基于位置矢量法的工业机器人几何参数标定方法

技术领域

本发明涉及一种几何参数的标定方法，尤其涉及一种工业机器人几何参数的标定方法，属于精密计量与计算机应用领域。

背景技术

高端制造业的持续发展提高了对工业机器人的精度要求，尤其是在激光焊接、激光切割以及航空航天等应用领域。工业机器人定位性能的衡量指标主要为重复定位精度和绝对定位精度。目前工业机器人的重复定位精度可达到0.02mm～0.1mm，而绝对定位精度仅为毫米级，机器人的重复定位精度在示教编程中较为重要。随着第四次工业革命的开始，示教编程已经逐渐无法满足工业机器人在高端制造业的发展和应用需求，因此对工业机器人绝对定位精度要求较高的离线编程得到了快速地发展。机器人标定能够较好地提高机器人的绝对定位精度。机器人标定分为关节级标定、运动学参数(即几何参数)标定与非运动学标定。研究发现，机器人的几何参数误差是影响机器人作业精度的主要误差源，约占总误差的90％，通过对机器人几何参数标定，可以有效提高机器人定位精度。

机器人几何参数标定通常分为建模，测量、辨识和补偿四步。其中，工业机器人常用的几何参数模型是DH模型，该模型通过齐次变换矩阵来描述相邻连杆之间的空间关系。但是，当机器人相邻关节旋转轴线平行或接近平行及垂直或接近垂直时出现奇异点，无法满足模型连续性的要求，直接影响标定结果的准确性。为了解决该问题，在传统的DH模型基础上有学者提出了改进的DH模型(MDH模型)，通过增加一个旋转参数，弥补了DH模型的缺陷，解决了相邻关节旋转轴线平行或接近平行时出现奇异点；但当相邻两轴线垂直或接近垂直时，该模型仍具有奇异性。机器人的位姿数据常用三坐标测量机、视觉系统和激光跟踪仪等设备来进行测量，其中，激光跟踪仪因其具有快速、测量精度高、无接触测量等优点，得到了较为广泛的应用。辨识是从测量数据中获取机器人实际模型参数信息的过程，其辨识的结果对机器人绝对定位精度的提高有直接影响。一些学者相应提出最小二乘法、LM方法、卡尔曼滤波法等辨识方法。考虑到运动学参数标定属于复杂的非线性优化问题，而遗传算法在解决复杂优化问题时有独到之处。因此，近年来已有学者针对遗传算法在机器人标定中的应用开展了研究，提出了基于遗传算法的六自由度串联机器人误差标定方法，通过建立机器人D-H误差模型，并引入平行误差角来建立机器人的实际模型，采用浮点型二进制方法对机器人工具中心实际位置进行编码，将整数部分和小数部分每个数字按照二进制编码得到编码后的数据，通过计算种群的适应值按照转轮选择法设定选择和变异操作。该模型通过增加一个旋转参数，弥补了DH模型存在的当机器人相邻关节旋转轴线平行或者接近平行时出现奇异点的缺陷，但当相邻两轴垂直或接近垂直时，该模型仍具有奇异性，导致机器人误差标定准确度低。另外该发明使用的遗传算法采用浮点型二进制方法，需要频繁编码、解码，交叉及变异操作时需要先计算群体中每个个体适应值，再根据适应值按照赌轮选择法选择交叉及变异个体，导致完成一代进化需要时间长，优化效率低，而且需依据经验确定交叉和变异的概率，算法鲁棒性差。

综合上述分析，当前对相关领域研究工作存在的不足主要是：(1)当机器人相邻两轴垂直或接近垂直时，现有模型具有奇异性，并且基于现有模型所建立的优化目标函数较为复杂，难以满足机器人几何参数标定对模型的连续性和简约性要求；(2)机器人几何参数标定属于具有复杂约束的非线性优化问题，需要优化的变量数目多且各个变量间数值差异大，传统遗传算法不仅需要依据经验确定交叉、变异概率等控制参数，而且在选择、交叉和变异执行过程中需要频繁地编码、解码及计算个体的适应值，算法优化效率低、鲁棒性差。

发明内容

本发明提供一种能够提高误差识别精度和效率的基于位置矢量法的工业机器人几何参数标定方法。

本发明的技术方案为：

一种基于位置矢量法的工业机器人几何参数标定方法，其特征在于，包括以下步骤：

步骤1建立基于基坐标系oxyz下的机器人位置矢量模型，所述位置矢量模型包括方向矢量和连接矢量，

步骤2从机器人手册寻找并获取机器人零位状态下方向矢量和连接矢量的名义值η_mi和ρ_m(i+1)，i＝1,2,…,n，n为机器人关节数目且n为正整数，η_mi在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为η_mix、η_miy和η_miz，ρ_m(i+1)在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为ρ_m(i+1)x、ρ_m(i+1)y和ρ_m(i+1)z，

步骤3令j＝1，j为测量序数，

步骤4在机器人手册给定的各关节运动范围，在示教器上任意设定机器人各组关节角q_ij，i＝1,2,…,n，n为机器人关节数目，j依次取值为1,2,…,40，控制机器人各个关节运动到设定的各组关节角值q_ij，

步骤5用激光跟踪仪对安装在机器人末端G的靶标进行检测，获取第j次测量机器人末端姿态和位置数据，其中第j次测量机器人末端在基坐标系oxyz下姿态坐标和位置坐标的测量值分别为r_aj和p_aj，r_aj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_ajx、r_ajy和r_ajz，p_aj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_ajx、p_ajy和p_ajz，

步骤6建立机器人几何参数标定的目标函数E_j，计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标的名义值r_mj和位置坐标的名义值p_mj，r_mj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_mjx、r_mjy和r_mjz，p_mj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_mjx、p_mjy和p_mjz，

步骤7使用遗传算法求解步骤6所建立的目标函数E_j，获取第j次测量机器人方向矢量和连接矢量的最优解及E_j，i＝1,2,…,n,n为机器人关节数目且n为正整数，j＝1,2,…,40,

步骤8如果j＞40，则转入步骤9，否则，令j＝j+1，返回步骤4，

步骤9计算由步骤7所述遗传算法求得的40次测量机器人方向矢量和连接矢量最优解的平均值及其目标函数值E^*，i＝1,2,…,n,n为机器人关节数目且n为正整数，

步骤10如果E^*<0.01，则转入步骤12，否则，进入步骤11，

步骤11用由步骤9获得机器人方向矢量和连接矢量最优解的平均值代替方向矢量和连接矢量的名义值η_mix、η_miy、η_miz、ρ_m(i+1)x、ρ_m(i+1)y、ρ_m(i+1)z转入步骤3，

步骤12取机器人方向矢量和连接矢量最优解的平均值作为几何参数标定值，i＝1,2,…,n。

本发明的有益效果在于：

本发明基于方向矢量和连接矢量构建了机器人位置矢量模型，该模型解决了相邻关节平行或接近平行及垂直或接近垂直时所引起的奇异性问题，同时基于该模型建立了工业机器人几何参数标定误差优化目标函数，该目标函数具有简约的表达形式，有利于机器人几何参数标定过程中参数优化算法的实现；机器人几何参数标定属于具有复杂约束的非线性优化问题，不仅需要优化的变量数目多，而且各个变量间数值差异大，本发明的遗传算法，采用实数编码直接表示每一待优化的方向矢量和连接矢量的坐标分量，不需要编码解码，控制参数仅有种群规模和子代种群规模，不需要依据经验确定交叉和变异概率等参数；采用交叉变异混合策略，不仅每代待进化的两个体从父代中随机选择，不需要事先计算个体适应值，而且能够同时实现交叉和变异功能，同传统遗传算法相比不仅完成一代进化所需要时间少，优化效率高，而且算法简单，鲁棒性强，适宜于求解基于位置矢量法的方向矢量和连接矢量最优解，能够实现对工业机器人几何参数进行快速准确标定。

附图说明

图1是串联工业机器人结构简图。

图2是位置矢量模型示意图。

图3是本发明的流程图。

图4是机器人几何参数标定目标函数计算的流程图。

图5是用遗传算法搜索机器人方向矢量和连接矢量的优化过程图。

具体实施方式

一种基于位置矢量法的工业机器人几何参数的标定方法，所述工业机器人包括机器人底座、依次串接的第一关节、第二关节、……、第n关节和机器人末端，并且，机器人底座与第一关节串接，第n关节与机器人末端串接，具体包括如下步骤：

步骤1建立基于基坐标系oxyz下的机器人位置矢量模型，所述位置矢量模型包括方向矢量和连接矢量，

步骤1.1将机器人所有关节角设为零，使所述工业机器人进入零位状态，控制第一关节单独转动，用激光跟踪仪测得当前状态的机器人末端运动轨迹，再取其拟合圆的圆心O'及该圆所在平面的法向量f'，拟合圆可采用最小二乘法，

步骤1.2将第一关节回到零位，控制第二关节单独转动，用激光跟踪仪测得当前状态的机器人末端运动轨迹，再取其拟合圆的圆心O”及该圆所在平面的法向量f”，拟合圆可采用最小二乘法，

步骤1.3将法向量f'作为z轴，将法向量f”作为y轴，过圆心O'且平行于法向量f'做直线l'，过圆心O”且平行于法向量f”做直线l”，将l'和l”两者投影的交点作为原点o，根据右手定则确定x轴，建立机器人基坐标系oxyz，在基坐标系oxyz上建立η_i、ρ_i+1及L，η_i为沿第i个关节轴线的方向矢量，i＝1,2,…,n,n为机器人关节数目且n为正整数，方向矢量η_i在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为η_ix、η_iy和η_iz，ρ_i+1为连接关节i到关节i+1的连接矢量，连接矢量ρ_i+1在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为ρ_(i+1)x、ρ_(i+1)y和ρ_(i+1)z，L为基坐标系oxyz的原点o距离机器人底座之间的距离，

步骤2从机器人手册寻找并获取机器人零位状态下方向矢量和连接矢量的名义值η_mi和ρ_m(i+1)，i＝1,2,…,n，n为机器人关节数目且n为正整数，η_mi在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为η_mix、η_miy和η_miz，ρ_m(i+1)在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为ρ_m(i+1)x、ρ_m(i+1)y和ρ_m(i+1)z，

步骤3令j＝1，j为测量序数，

步骤4在机器人手册给定的各关节运动范围，在示教器上任意设定机器人各组关节角q_ij，i＝1,2,…,n，n为机器人关节数目，j依次取值为1,2,…,40，控制机器人各个关节运动到设定的各组关节角值q_ij，

步骤5用激光跟踪仪对安装在机器人末端G的靶标进行检测，获取第j次测量机器人末端姿态和位置数据，其中第j次测量机器人末端在基坐标系oxyz下姿态坐标和位置坐标的测量值分别为r_aj和p_aj，r_aj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_ajx、r_ajy和r_ajz，p_aj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_ajx、p_ajy和p_ajz，

步骤6建立机器人几何参数标定的目标函数E_j，计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标的名义值r_mj和位置坐标的名义值p_mj，r_mj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_mjx、r_mjy和r_mjz，p_mj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_mjx、p_mjy和p_mjz，

步骤6.1将机器人几何参数标定的雅克比矩阵J_e表示为：

并计算雅克比矩阵J_e，J_e由J_e1、J_e2、J_e3、J_e4、J_e5、J_e6子矩阵组成，其中J_e1、J_e2、J_e3、J_e4均为3行3n列的子矩阵，J_e5和J_e6为n行3n列的子矩阵，n为机器人关节数目，

步骤6.1.1计算第j次测量时机器人关节i相对于基坐标系的旋转矩阵

其中，

式中V_ij＝1-cos(q_ij)，S_ij＝sin(q_ij)，R(q_ij,η_mi)简写为R_ij，表示第j次测量时机器人底座相对于基坐标系不做旋转，取E_3×3为3行3列的单位矩阵，

步骤6.1.2分别建立矩阵A、矩阵B及矩阵C，

其中，

式中，X_ij＝η_mixV_ij，Y_ij＝η_miyV_ij，Z_ij＝η_mizV_ij，表示对矩阵求转置，

步骤6.1.3计算雅克比矩阵J_e的子矩阵J_e1：

计算子矩阵J_e1的各元素J_e1(s,s’)，J_e1(s,s’)表示子矩阵J_e1的第s行第s’列的元素，s依次取值为1、2、3，s’依次取值为3(i-1)+1、3(i-1)+2、3(i-1)+3，i依次取值为1、2、3、…、n，并令：

J_e1(1,3(i-1)+1)＝A(3,2),J_e1(1,3(i-1)+2)＝B(3,2),J_e1(1,3(i-1)+3)＝C(3,2),

J_e1(2,3(i-1)+1)＝A(1,3),J_e1(2,3(i-1)+2)＝B(1,3),J_e1(2,3(i-1)+3)＝C(1,3),

J_e1(3,3(i-1)+1)＝A(2,1),J_e1(3,3(i-1)+2)＝B(2,1),J_e1(3,3(i-1)+3)＝C(2,1),

其中，A(3,2)、B(3,2)、C(3,2)分别表示取矩阵A、矩阵B及矩阵C的第3行第2列元素，A(1,3)、B(1,3)、C(1,3)分别表示取矩阵A、矩阵B及矩阵C的第1行第3列元素，A(2,1)、B(2,1)、C(2,1)分别表示取矩阵A、矩阵B及矩阵C的第2行第1列元素，

步骤6.1.4计算雅克比矩阵J_e的子矩阵J_e2，

J_e2为3行3n列矩阵，其中的所有元素均为零，

步骤6.1.5计算雅克比矩阵J_e的子矩阵J_e3，

步骤6.1.5.1计算距离d_i,

当i为n时，d_i＝ρ_m(n+1)，当i为n-1,n-2,…,1时，d_i＝ρ_m(i+1)+R_ijd_i+1，式中R_ij的计算公式见步骤6.1.1所列，

步骤6.1.5.2计算向量v1、v2及v3：

v1、v2及v3均为3行1列向量，

步骤6.1.5.3计算子矩阵J_e3：

计算子矩阵J_e3的各元素J_e3(s,s’)，J_e3(s,s’)表示子矩阵J_e3的第s行第s’列的元素，s依次取值为1、2、3，s’依次取值为3(i-1)+1、3(i-1)+2、3(i-1)+3，i依次取值为1、2、3、…、n，并令：

J_e3(s,3(i-1)+1)＝v1(s,1),J_e3(s,3(i-1)+2)＝v2(s,1),J_e3(s,3(i-1)+3)＝v3(s,1),

v1(s,1)、v2(s,1)及v3(s,1)分别表示向量v1、v2及v3的第s行第1列元素，s依次取值为1、2、3，

步骤6.1.6计算雅克比矩阵J_e的子矩阵J_e4，

J_e4＝[R_1j,R_2j,…,R_ij,…,R_n-1j,R_nj]，

式中R_ij为3行3列矩阵，计算公式见步骤6.1.1，

步骤6.1.7计算雅克比矩阵J_e的子矩阵J_e5，

子矩阵J_e5为n行3n列矩阵，其中第1行中第1列至第3列的元素值分别为η_m1x、η_m1y、η_m1z，其余元素值均为零，J_e5第2行中第4列至第6列的元素值分别为η_m2x、η_m2y、η_m2z，其余元素值均为零，依次类推，J_e5第n行中第3n-2列至第3n列的元素值分别为η_mnx、η_mny、η_mnz，其余元素值均为零，

步骤6.1.8计算雅克比矩阵J_e的子矩阵J_e6，

J_e6为n行3n列矩阵，其中的所有元素均为零，

步骤6.2计算第j次测量机器人末端G在基坐标系oxyz下姿态的名义值r_mj和位置的名义值p_mj,

姿态的名义值r_mj等于R_1j、R_2j、…、R_nj的连乘，表示为

位置的名义值p_mj等于连加，表示为

步骤6.3计算机器人末端G在基坐标系oxyz下的姿态误差和位置误差姿态误差在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为和位置误差在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为和

步骤6.4获取第j次测量机器人几何参数标定的目标函数E_j，

目标函数E_j是的函数，并用f(·)表示，其中

式中，分别表示取矩阵的第3行第2列、第1行第3列、第2行第1列元素，其中，

其中T为对矩阵求转置，为[J_e]的伪逆矩阵，为求矩阵的谱范数，

步骤7使用遗传算法求解步骤6所建立的目标函数E_j，获取第j次测量机器人方向矢量和连接矢量的最优解及E_j，i＝1,2,…,n,n为机器人关节数目且n为正整数，j＝1,2,…,40,

步骤7.1随机产生初始种群，

设置初始进化代数t＝1，选择种群规模为10的种群，以6n×10维的实数向量作为初始种群，其中6n×1维的实数向量作为种群中的第k个体在第t代的值k＝1,2,…,10，表示为：其中分别对应第j次测量机器人方向矢量沿x轴、y轴和z轴的坐标分量η_ixj、η_iyj、η_izj(i＝1,2,...,n)最优解的可能取值，分别对应第j次测量机器人连接矢量沿x轴、y轴和z轴的坐标分量ρ_(i+1)xj、ρ_(i+1)yj、ρ_(i+1)zj最优解的可能取值，分别在[-0.01+η_mix,0.01+η_mix]、[-0.01+η_miy,0.01+η_miy]、[-0.01+η_miz,0.01+η_miz]和[-0.5+ρ_m(i+1)x,0.5+ρ_m(i+1)x]、[-0.5+ρ_m(i+1)y,0.5+ρ_m(i+1)y]、[-0.5+ρ_m(i+1)z,0.5+ρ_m(i+1)z]数值区域内随机产生10组和作为初始种群的个体

步骤7.2从初始种群中随机选择两个个体作为父代，采用交叉变异混合策略产生子代，子代和父代共同构成新家族

从初始种群k＝1,2,…,10中随机选择两个个体作为父代，采用交叉变异混合策略产生10个子代c为子代个体数，c＝1,2,…,10，其中子代中的第σ个变量的值根据均匀分布在区间中随机产生，为第t代的两个父代和的第σ个变量之间的欧式距离，即变量σ＝1,2,…,6n，两个父代和交叉变异混合策略产生的10个子代共同构成新家族w为新家族的个体数，w＝1,2,…,12，

步骤7.3计算新家族中每一个体的目标函数值，将其由小到大排序，用目标函数值最小的两个个体代替父代构成新的初始种群

根据步骤6.4所述的目标函数计算在第t代新家族中每一个体的目标函数值w＝1,2,…,12，对由小到大排序，取排序后新家族的目标函数值最小的个体mpos1^t和次小的个体mpos2^t代替初始种群中两父代个体构成新的初始种群

步骤7.4如果进化代数t<100，则t＝t+1，重复步骤7.2～7.4，否则转入步骤7.5，

步骤7.5取出目标函数值最小的个体mpos1¹⁰⁰，该个体对应第j次测量机器人方向矢量和连接矢量的最优解 及其目标函数值E_j(mpos1¹⁰⁰)，

步骤8如果j＞40，则转入步骤9，否则，令j＝j+1，返回步骤4，

步骤9计算由步骤7所述遗传算法求得的40次测量机器人方向矢量和连接矢量最优解的平均值及由步骤7使用遗传算法求解平均值的目标函数E^*，i＝1,2,…,n,n为机器人关节数目且n为正整数，

步骤10如果E^*<0.01，则转入步骤12，否则，进入步骤11，

步骤11用由步骤9获得机器人方向矢量和连接矢量最优解的平均值代替方向矢量和连接矢量的名义值η_mix、η_miy、η_miz、ρ_m(i+1)x、ρ_m(i+1)y、ρ_m(i+1)z转入步骤3，

步骤12取机器人方向矢量和连接矢量最优解的平均值作为几何参数标定值，i＝1,2,…,n。

以下结合附图对本发明做进一步的说明：

1、对埃夫特ER10L-C10六自由度串联工业机器人建立机器人位置矢量模型，从机器人手册寻找并获取机器人零位状态下方向矢量和连接矢量的名义值η_mi、ρ_mi如表1所示，i＝1,2,…,n，这里n＝6。

表1 ER10L-C10机器人方向矢量和连接矢量的名义值

2、为了证实本方法的正确性，从示教器设定ER10L-C10机器人各组关节角q_ij，该组关节角使机器人末端在其作业空间内尽可能均匀分布，如表2所示，i＝1,2,…,6，j＝1,2,…,40。

表2 ER10L-C10机器人各组关节角

3、根据所列公式计算雅克比矩阵J_e，J_e为12行36列矩阵。

4、将表2设定的不同测量序数j下各组关节角q_ij值代入步骤6.2，计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标名义值r_mj和位置坐标名义值p_mj。

5、在表2设定的各组关节角q_ij上分别加入位于区间[-0.01，0.01]上服从均匀分布的随机数重新生成40组关节角将生成的关节角代入步骤6.2，计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标和位置坐标值，将其作为机器人末端G在基坐标系oxyz下姿态坐标测量值r_aj和位置坐标测量值p_aj，随机数产生采用Matlab中的rands函数引入。

6、初始化算法参数

种群规模和子代种群规模均取10。

7、随机产生初始种群

使用遗传算法标定机器人方向矢量和连接矢量时，种群规模取值为10，目标函数值E_j由方向矢量和连接矢量的坐标分量 36个参数决定，种群中第k个个体在第t代的值中对应第j次测量机器人方向矢量的坐标分量η_ixj、η_iyj、η_izj(i＝1,2,…,6)的最优解的可能取值，初始值分别在[-0.01+η_mix,0.01+η_mix],[-0.01+η_miy,0.01+η_miy],[-0.01+η_miz,0.01+η_miz]区间上随机产生；A_19k,A_20k,...,A_36k分别对应第j次测量机器人连接矢量的坐标分量ρ_(i+1)xj、ρ_(i+1)yj、ρ_(i+1)zj的最优解的可能取值，的初始值分别在[-0.5+ρ_m(i+1)x,0.5+ρ_m(i+1)x],[-0.5+ρ_m(i+1)y,0.5+ρ_m(i+1)y],[-0.5+ρ_m(i+1)z,0.5+ρ_m(i+1)z]区间上随机产生。

8、从初始种群中随机选择两个个体作为父代，采用交叉变异混合策略产生子代，子代和父代共同构成新家族。

9、根据机器人末端G在基坐标系oxyz下的姿态误差和位置误差及已建立机器人几何参数标定的目标函数，计算新家族的目标函数值，将目标函数值由小到大排序，同时新家族个体也随目标函数值排序，如果几个目标函数值相等，则相等的目标函数值及其个体任意排序，用排在最前面的两个个体即目标函数值最小的两个个体代替父代构成新的初始种群

10、进化代数t大于100时，算法终止

用上述算法搜索第j次测量机器人方向矢量和连接矢量的坐标分量的最优解附图5为第40次测量机器人几何参数目标函数优化过程，由图可见在约70代时就已搜索到方向矢量和连接矢量的最优解，未优化时目标函数值为0.86，优化后目标函数值为0.0057。

11、当测量序数j等于40时，计算由改进遗传算法求得40次测量机器人方向矢量和连接矢量最优解的平均值及其目标函数值E*见表3，由表3可见，E*<0.01，则取作为机器人几何参数标定值。

表3 ER10L-C10机器人方向矢量和连接矢量的标定值

12、为了进一步验证所提出方法的正确性，可以采用现有技术所述的基于距离误差的迭代补偿方法对机器人几何参数标定结果进行实验验证，补偿前机器人绝对定位误差的最大值、平均值和标准差分别为3.480、2.025和1.170mm，采用所提出方法对机器人几何参数标定后绝对定位误差的最大值、平均值和标准差分别为2.681、0.879和0.935mm，证实了所提出标定方法能有效提高机器人绝对定位精度。

Claims (1)

1.一种基于位置矢量法的工业机器人几何参数标定方法，所述工业机器人包括机器人底座、依次串接的第一～n关节和机器人末端，并且，机器人底座与第一关节串接，第n关节与机器人末端串接，其特征在于，包括以下步骤：

步骤1建立基于基坐标系oxyz下的机器人位置矢量模型，所述位置矢量模型包括方向矢量和连接矢量，

步骤2从机器人手册寻找并获取机器人零位状态下方向矢量和连接矢量的名义值η_mi和ρ_m(i+1)，i＝1,2,…,n，n为机器人关节数目且n为正整数，η_mi在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为η_mix、η_miy和η_miz，ρ_m(i+1)在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为ρ_m(i+1)x、ρ_m(i+1)y和ρ_m(i+1)z，

步骤3令j＝1，j为测量序数，

步骤4在机器人手册给定的各关节运动范围内，在示教器上任意设定机器人各组关节角q_ij，i＝1,2,…,n，n为机器人关节数目，j依次取值为1,2,…,40，控制机器人各个关节运动到设定的各组关节角值q_ij，

步骤5用激光跟踪仪对安装在机器人末端G的靶标进行检测，获取第j次测量机器人末端姿态和位置数据，其中第j次测量机器人末端在基坐标系oxyz下姿态坐标和位置坐标的测量值分别为r_aj和p_aj，r_aj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_ajx、r_ajy和r_ajz，p_aj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_ajx、p_ajy和p_ajz，

步骤6建立机器人几何参数标定的目标函数E_j，计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标的名义值r_mj和位置坐标的名义值p_mj，r_mj在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为r_mjx、r_mjy和r_mjz，p_mj在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为p_mjx、p_mjy和p_mjz，

步骤7使用遗传算法求解步骤6所建立的目标函数E_j，获取第j次测量机器人方向矢量和连接矢量的最优解及E_j，i＝1,2,…,n,n为机器人关节数目且n为正整数，j＝1,2,…,40,

步骤8如果j＞40，则转入步骤9，否则，令j＝j+1，返回步骤4，

步骤9计算由步骤7所述遗传算法求得的40次测量机器人方向矢量和连接矢量最优解的平均值及平均目标函数值E^*，i＝1,2,…,n,n为机器人关节数目且n为正整数，

步骤10如果E^*<0.01，则转入步骤12，否则，进入步骤11，

步骤11用由步骤9获得机器人方向矢量和连接矢量最优解的平均值代替方向矢量和连接矢量的名义值η_mix、η_miy、η_miz、ρ_m(i+1)x、ρ_m(i+1)y、ρ_m(i+1)z转入步骤3，

步骤12取机器人方向矢量和连接矢量最优解的平均值作为几何参数标定值，i＝1,2,…,n；

步骤6所述的机器人几何参数标定的目标函数E_j的建立方法具体为：

步骤6.1将机器人几何参数标定的雅克比矩阵J_e表示为：

并计算雅克比矩阵J_e，J_e由J_e1、J_e2、J_e3、J_e4、J_e5、J_e6子矩阵组成，其中J_e1、J_e2、J_e3、J_e4均为3行3n列的子矩阵，J_e5和J_e6为n行3n列的子矩阵，n为机器人关节数目，

步骤6.1.1计算第j次测量时机器人关节i相对于基坐标系的旋转矩阵

其中，

式中V_ij＝1-cos(q_ij)，S_ij＝sin(q_ij)，R(q_ij,η_mi)简写为R_ij，表示第j次测量时机器人底座相对于基坐标系不做旋转，取E_3×3为3行3列的单位矩阵，

步骤6.1.2分别建立矩阵A、矩阵B及矩阵C，

其中，

式中，X_ij＝η_mixV_ij，Y_ij＝η_miyV_ij，Z_ij＝η_mizV_ij，表示对矩阵求转置，

步骤6.1.3计算雅克比矩阵J_e的子矩阵J_e1：

计算子矩阵J_e1的各元素J_e1(s,s’)，J_e1(s,s’)表示子矩阵J_e1的第s行第s’列的元素，s依次取值为1、2、3，s’依次取值为3(i-1)+1、3(i-1)+2、3(i-1)+3，i依次取值为1、2、3、…、n，并令：

J_e1(1,3(i-1)+1)＝A(3,2),J_e1(1,3(i-1)+2)＝B(3,2),J_e1(1,3(i-1)+3)＝C(3,2),

J_e1(2,3(i-1)+1)＝A(1,3),J_e1(2,3(i-1)+2)＝B(1,3),J_e1(2,3(i-1)+3)＝C(1,3),

J_e1(3,3(i-1)+1)＝A(2,1),J_e1(3,3(i-1)+2)＝B(2,1),J_e1(3,3(i-1)+3)＝C(2,1),

其中，A(3,2)、B(3,2)、C(3,2)分别表示取矩阵A、矩阵B及矩阵C的第3行第2列元素，A(1,3)、B(1,3)、C(1,3)分别表示取矩阵A、矩阵B及矩阵C的第1行第3列元素，A(2,1)、B(2,1)、C(2,1)分别表示取矩阵A、矩阵B及矩阵C的第2行第1列元素，

步骤6.1.4计算雅克比矩阵J_e的子矩阵J_e2，

J_e2为3行3n列矩阵，其中的所有元素均为零，

步骤6.1.5计算雅克比矩阵J_e的子矩阵J_e3，

步骤6.1.5.1计算距离d_i,

当i为n时，d_i＝ρ_m(n+1)，当i为n-1,n-2,…,1时，d_i＝ρ_m(i+1)+R_ijd_i+1，

步骤6.1.5.2计算向量v1、v2及v3：

v1、v2及v3均为3行1列向量，

步骤6.1.5.3计算子矩阵J_e3：

计算子矩阵J_e3的各元素J_e3(s,s’)，J_e3(s,s’)表示子矩阵J_e3的第s行第s’列的元素，s依次取值为1、2、3，s’依次取值为3(i-1)+1、3(i-1)+2、3(i-1)+3，i依次取值为1、2、3、…、n，并令：

J_e3(s,3(i-1)+1)＝v1(s,1),J_e3(s,3(i-1)+2)＝v2(s,1),J_e3(s,3(i-1)+3)＝v3(s,1),

v1(s,1)、v2(s,1)及v3(s,1)分别表示向量v1、v2及v3的第s行第1列元素，s依次取值为1、2、3，

步骤6.1.6计算雅克比矩阵J_e的子矩阵J_e4，

J_e4＝[R_1j,R_2j,…,R_ij,…,R_n-1j,R_nj]，

步骤6.1.7计算雅克比矩阵J_e的子矩阵J_e5，

子矩阵J_e5为n行3n列矩阵，其中第1行中第1列至第3列的元素值分别为η_m1x、η_m1y、η_m1z，其余元素值均为零，J_e5第2行中第4列至第6列的元素值分别为η_m2x、η_m2y、η_m2z，其余元素值均为零，依次类推，J_e5第n行中第3n-2列至第3n列的元素值分别为η_mnx、η_mny、η_mnz，其余元素值均为零，

步骤6.1.8计算雅克比矩阵J_e的子矩阵J_e6，

J_e6为n行3n列矩阵，其中的所有元素均为零，

步骤6.2计算第j次测量机器人末端G在基坐标系oxyz下姿态坐标的名义值r_mj和位置坐标的名义值p_mj,

姿态坐标的名义值r_mj等于R_1j、R_2j、…、R_nj的连乘，表示为

位置坐标的名义值p_mj等于连加，表示为

步骤6.3计算机器人末端G在基坐标系oxyz下的姿态误差和位置误差姿态误差在基坐标系oxyz下绕x轴、y轴和z轴的角度分量分别为和位置误差在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为和

步骤6.4获取第j次测量机器人几何参数标定的目标函数E_j，

目标函数E_j是的函数，并用f(·)表示，其中

式中，分别表示取矩阵的第3行第2列、第1行第3列、第2行第1列元素，其中，

其中T为对矩阵求转置，为[J_e]的伪逆矩阵，为求矩阵的谱范数，

步骤1所述的基于基坐标系oxyz下的机器人位置矢量模型的建立方法为：

步骤1.1将机器人所有关节角设为零，使所述机器人进入零位状态，控制第一关节单独转动，用激光跟踪仪测得当前状态的机器人末端运动轨迹，再取其拟合圆的圆心O'及该圆所在平面的法向量f'，

步骤1.2将第一关节回到零位，控制第二关节单独转动，用激光跟踪仪测得当前状态的机器人末端运动轨迹，再取其拟合圆的圆心O”及该圆所在平面的法向量f”，

步骤1.3将法向量f'作为z轴，将法向量f”作为y轴，过圆心O'且平行于法向量f'做直线l'，过圆心O”且平行于法向量f”做直线l”，将l'和l”两者投影的交点作为原点o，根据右手定则确定x轴，建立机器人基坐标系oxyz，在基坐标系oxyz上建立η_i、ρ_i+1及L，η_i为沿第i个关节轴线的方向矢量，i＝1,2,…,n,n为机器人关节数目且n为正整数，方向矢量η_i在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为η_ix、η_iy和η_iz，ρ_i+1为连接关节i到关节i+1的连接矢量，连接矢量ρ_i+1在基坐标系oxyz下沿x轴、y轴和z轴的坐标分量分别为ρ_(i+1)x、ρ_(i+1)y和ρ_(i+1)z，L为基坐标系oxyz的原点o距离机器人底座之间的距离，步骤7中使用遗传算法求解步骤6所建立的目标函数E_j的具体方法如下：

步骤7.1随机产生初始种群，

设置初始进化代数t＝1，选择种群规模为10的种群，以6n×10维的实数向量作为初始种群，其中6n×1维的实数向量作为种群中的第k个体在第t代的值表示为：其中分别对应第j次测量机器人方向矢量沿x轴、y轴和z轴的坐标分量η_ixj、η_iyj、η_izj(i＝1,2,...,n)最优解的可能取值，分别对应第j次测量机器人连接矢量沿x轴、y轴和z轴的坐标分量ρ_(i+1)xj、ρ_(i+1)yj、ρ_(i+1)zj最优解的可能取值，分别在[-0.01+η_mix,0.01+η_mix]、[-0.01+η_miy,0.01+η_miy]、[-0.01+η_miz,0.01+η_miz]和[-0.5+ρ_m(i+1)x,0.5+ρ_m(i+1)x]、[-0.5+ρ_m(i+1)y,0.5+ρ_m(i+1)y]、[-0.5+ρ_m(i+1)z,0.5+ρ_m(i+1)z]数值区域内随机产生10组和作为初始种群的个体

步骤7.2从初始种群中随机选择两个个体作为父代，采用交叉变异混合策略产生子代，子代和父代共同构成新家族

从初始种群中随机选择两个个体作为父代，采用交叉变异混合策略产生10个子代c为子代个体数，c＝1,2,…,10，其中子代中的第σ个变量的值根据均匀分布在区间中随机产生， 为第t代的两个父代和的第σ个变量之间的欧式距离，即变量σ＝1,2,…,6n，两个父代和交叉变异混合策略产生的10个子代共同构成新家族w为新家族的个体数，w＝1,2,…,12，

步骤7.3计算新家族中每一个体的目标函数值，将其由小到大排序，用目标函数值最小的两个个体代替父代构成新的初始种群

根据步骤6.4所述的目标函数计算在第t代新家族中每一个体的目标函数值对由小到大排序，取排序后新家族的目标函数值最小的个体mpos1^t和次小的个体mpos2^t代替初始种群中两父代个体构成新的初始种群

步骤7.4如果进化代数t<100，则t＝t+1，重复步骤7.2～7.4，否则转入步骤7.5，

步骤7.5取出目标函数值最小的个体mpos1¹⁰⁰，该个体对应第j次测量机器人方向矢量和连接矢量的最优解及E*，i＝1,2,…,n,n为机器人关节数目且n为正整数，j＝1,2,…,40。