基于预标定基坐标系及MIEKF算法的工业机器人标定方法

林耿聪，肖曙红，杨林，卢浩文，张建华

(1.广东工业大学机电工程学院，广东广州 510006；2.佛山智能装备技术研究院，广东佛山 528000；3.佛山华数机器人有限公司，广东佛山 528000)

0 前言

目前，工业机器人因其高灵活性被广泛应用于汽车行业、物流搬运、航空航天和医疗手术等行业[1-2]，这些行业对机器人绝对定位精度的要求高。然而因核心部件制造精度和装配精度等细微偏差的叠加影响，造成绝对定位精度基本都在毫米级上，难以满足上述行业的高精度工作要求[3]。提高机器人绝对定位精度的手段有：提高零部件加工精度和装配精度，以及通过运动学标定技术提高绝对定位精度。前者会使制造成本增加，且效果有限；后者操作简便易实现，且能大幅度提升绝对定位精度。运动学标定方法的步骤为：建立误差模型、点位采集、参数辨识及参数补偿[4-5]。

目前大部分模型是基于DH(Denavit-Hartenberg)模型而改进的MDH模型[6]。点位采集指采集机器人运动空间中的点位数据，主要采用激光跟踪仪、拉线测量装置和球杆仪等采集仪器获取数据[7-9]；参数误差补偿是指将算法辨识的几何参数误差补偿到机器人控制器上，从而提高精度；采用的辨识算法有最小二乘法、L-M算法、粒子群算法、扩展卡尔曼滤波算法等。LI等[10]基于MDH模型对SCARA机器人建立距离误差模型和位置误差模型，用LS进行参数辨识并补偿；GONG等[11]先用最小二乘法辨识机器人的几何误差参数，再补偿由自重产生的柔度误差及热误差；GAO等[12]对辨识矩阵进行奇异值分解后，去掉冗余参数和相关参数，再用改进最小二乘法辨识机器人的几何参数误差；何晓煦等[13]先用L-M算法识别几何参数误差并补偿，然后基于空间相似性对补偿后的残差进行补偿；JIANG等[14]利用EKF算法初步辨识几何参数误差，其结果作为PF算法的初值再进行辨识，从而完成机器人的参数误差补偿，实现机器人定位精度提升；寇斌等人[15]基于粒子群算法收敛速度慢的缺点，提出一种动态粒子群算法，有效辨识出机器人几何误差参数。

综上研究，本文作者将以HSR-JR630型工业机器人为研究对象，推导机器人的正运动学模型，并提出位置数据在测量坐标系和基坐标系的快速转换方法；考虑位置误差模型完整最小连续性的建模方法，在模型建模时引入减速比及相关性分析去除冗余参数；最后用MIEKF(Modified Iterated Extended Kalman Filter)算法对去除冗余参数后的模型进行参数辨识，并补偿验证机器人的绝对定位精度。

1 建立运动学模型

1.1 MDH模型

R(Xi,αi)·R(Yi,βi)=

(1)

其中：cθi表示cosθi；sθi表示sinθi；cαi表示cosαi；sαi表示sinαi。

1.2 机器人正运动学

根据HSR-JR630型工业机器人的结构参数确定其MDH参数，如表1所示。

表1 HSR-JR630型工业机器人的理论MDH参数

(2)

2 运动学位置误差模型

2.1 基坐标系和工具坐标系的预标定

首先根据测量坐标系、机器人基坐标系和机器人工具坐标系的位姿关系得到下式：

(3)

(4)

通过克罗内克积⊗运算和拉直算子vec()[16]，可以将式(4)变换得到：

(5)

其中：I3为三阶单位矩阵。

文中的标定测量了m组机器人末端位置，可将式(5)转化为超定方程组：

Ewm=pinv(Hwm)·Pwm

(6)

其中：Hwm为3m×15的矩阵；Ewm为1×15的待求解列向量；Pwm为3m×1的列向量。

(7)

(8)

2.2 建立位置误差模型

2.2.1 关节间的微分

(9)

(10)

则相邻关节间相对于当前坐标系{i}的微分误差矩阵为

(11)

2.2.2 位置误差模型

(12)

由式(10)(12)得：

(13)

(14)

其中：ΔX=[ΔθΔdΔaΔαΔβΔrΔbΔt]T为40×1的各个参数误差列向量；Δb=[dbxdbydbzδbxδbyδbz]T；Δt=[dtxdtydtz]T代表了构造机器人基坐标系误差和工具坐标系偏差；J为3×40的辨识雅克比矩阵。

2.3 基于相关系数及复共线性分析冗余参数

首先要确定冗余参数个数，对于测量了m组机器人运动位姿，则有：

ΔPm=JmΔX

(15)

对式(15)的辨识雅克比矩阵Jm进行奇异值分解，得：

(16)

其中：Σ=diag(σ1，σ2，…，σr)，r(r≤40)为Jm的秩，设置σr>10-5才是Jm的有效秩。则根据Σ得到r=33，即存在7个冗余参数。

其次，进行相关性和复共线性分析确定冗余参数，用辨识雅克比矩阵Jm求取相关系数矩阵W来确定参数误差的相关性，采用Z-Score变换对Jm进行标准化处理，则标准化矩阵Z为

(17)

(18)

(19)

(20)

根据标准化矩阵Z求取相关系数矩阵，辨识雅克比矩阵的相关系数矩阵为

W=1/(3m-1)ZTZ

(21)

根据式(21)可知，dbz和Δd1、δbz和Δθ1、dtz和Δd6、Δd2和Δd3、dtx和Δa6之间的相关系数都为1，所以它们每两者都存在相关性，因此选择去除相关参数Δd1、Δθ1、Δd6、Δd2和Δa6。

另外，Δθ6、Δα6对应的辨识雅克比矩阵列分别和工具坐标系偏差dtx、dty、dtz对应的辨识雅克比矩阵列存在复共线性，即：

Jm(Δθ6)=-tyJm(dtx)+txJm(dty)

(22)

Jm(Δα6)=-tzJm(dty)+txJm(dtz)

(23)

其中：tx、ty和tz分别为工具坐标系位置的x、y、z分量，故Δθ6和Δα6也为冗余参数。

综上，Δθ1、Δθ6、Δd1、Δd2、Δd6、Δa6、Δα6为冗余参数。剔除干扰模型的冗余参数后，待辨识的几何误差参数变为33个，故Jm变为3m×33，ΔX为33×1的列向量，ΔPm为3m×1的列向量。

3 改进迭代扩展卡尔曼滤波算法

改进迭代扩展卡尔曼滤波算法是EKF算法[17]的改进形式。算法原理是针对位置误差模型的辨识雅克比矩阵参数量纲不同，对辨识雅克比矩阵进行参数量纲一化，即通过对辨识雅克比矩阵进行列缩放从而进行参数归一化，确保辨识雅克比矩阵列对各个参数误差的影响度一致，提高辨识状态参数的鲁棒性。再提出了遗忘因子优化协方差阵防止其病化，从而保证状态参数能够快速收敛。并且利用观测值估计迭代，每次迭代更新都保持测量位置不变，只通过残差矢量更新计算状态参数，达成观测方程的更新，找出最优的状态参数估计。

由m个标定点组成的辨识雅克比矩阵Jm求解缩放系数的对角方差阵D[18]步骤如下：

(24)

D=diag(l1,l2,…,lm)

(25)

根据式(14)和式(25)可得每个标定点的辨识雅克比矩阵和待辨识参数转化为

Hk=JkD-1

(26)

ΔXk=DΔX

(27)

其中：ΔXk和Hk分别为系统状态参数和系统观测矩阵。则系统的状态方程和观测方程为

ΔXk|k-1=Ak-1ΔXk-1|k-1+wk-1

(28)

Zk=HkΔXk|k-1+Vk

(29)

其中：wk-1为系统随机过程噪声向量；Ak-1为状态转移矩阵，因几何参数误差不随机器人空间位置变化而变化，则Ak-1=I；Zk为机器人末端位置误差(ΔP)；Vk为测量噪声。

改进迭代扩展卡尔曼滤波步骤如下：

几何参数误差的预测：

ΔXk|k-1=ΔXk-1|k-1

(30)

预测误差协方差矩阵：

Pk|k-1=Pk-1|k-1+Qk-1

(31)

卡尔曼增益矩阵：

(32)

更新残差矢量：

ΔZk=Zk-HkΔXk|k-1

(33)

更新几何误差状态方程：

ΔXk|k=ΔXk|k-1+Kk|kΔZk

(34)

用遗忘因子γk计算更新修正的协方差矩阵：

Pk|k=γk(I-Kk|kHk)Pk|k-1(I-Kk|kHk)T+

(35)

式中的遗忘因子，由下式决定：

(36)

其中：tr()为矩阵的迹；I33×33为单位矩阵，Rk为测量噪声的协方差矩阵，Rk=10-3I3×3，Q=10-5I33×33，P0|0=I33×33，初始化误差ΔX0|0=0；k|k-1表示第k次迭代的先验值，k|k表示第k次迭代后估计值；当k为m倍时，表示一轮迭代完毕，需要重新从第一个标定点位开始迭代，迭代更新4轮后停止；然后将迭代辨识的结果ΔXk|k经D转换为实际的几何参数误差ΔX，并补偿到机器人控制器上。

ΔX=D-1ΔXk|k

(37)

4 实验结果

4.1 实验现场布局

将用HSR-JR630型工业机器人对上述所提出的标定方法进行标定实验，实验现场布置如图1所示。位置数据采集利用Leica AT960MR激光跟踪仪及配套的激光反射靶球和点位采集软件Spatial Analyer。

图1 实验现场

4.2 实验结果分析

基于建立的位置误差模型和预标定得到的基坐标系及工具坐标系，用MIEKF算法辨识后的几何参数误差结果如图2所示。可以看出：MIEKF算法经过观测矩阵的归一化、遗忘因子优化协方差矩阵及利用观测位置迭代，几何参数误差在两轮迭代内就可以快速收敛到稳定值。

图2 MIEKF算法辨识得到的参数误差

根据辨识的结果对预标定的基坐标系和工具坐标系进行修正后的实际坐标为

表2为MIEKF算法辨识得到HSR-JR630型机器人的各个关节实际几何参数误差。

使用MIEKF算法标定后的精度再与EKF算法标定的进行比较，可以得到HSR-JR630型机器人的位置精度如表3和图3所示。

表3 位置误差的对比 单位：mm

从表3和图3可知：未校正时，机器人的最大误差和平均误差分别为3.328、2.011 mm；用MIEKF标定后的最大误差和平均误差分别为0.397、0.122 mm，精度提升了88.07%。并且实验结果表明，MIEKF算法比EKF辨识出机器人的几何参数误差精度更准，提升的机器人的绝对定位精度效果更好。

5 结论

本文作者提出了基于预标定基坐标系及MIEKF算法的运动学标定方法，该方法考虑了激光跟踪仪采集数据的效率，建立完整最小连续性的误差模型以及高效率的辨识算法。利用该方法在HSR-JR630型工业机器人上进行实验，结果表明：基坐标系和工具坐标系的预标定有效减少了两者拟合时间，提高标定效率。应用MIEKF算法在辨识机器人几何参数误差时克服了对初始参数条件敏感的问题，具有辨识精度高、收敛速度快和高鲁棒性的特点。最大误差和平均误差分别从3.328、2.011 mm减小到0.397、0.122 mm，有效提高了HSR-JR630型机器人的绝对定位精度，为工业机器人运动学标定技术提供了新的方法和思路，具有很重要的实际应用价值。