基于极小极大算法的双臂机器人精度提升方法

2022-02-16祖洪飞陈章位毛晨涛陈广初

振动与冲击 2022年2期

祖洪飞，陈章位，毛晨涛，陈广初，李杰

(1.浙江理工大学机械与自动控制学院,杭州 310023；2.浙江大学流体动力与机电系统国家重点实验室,杭州 310014；3.杭州亿恒科技有限公司,杭州 311112；4.佛山市南海区质量技术监督检测所, 广东佛山 528200)

在机器人生产过程中往往会产生几何参数误差，对这些几何参数进行运动学校准是提高机器人精度的重要手段[1]。已有大量研究表明，利用校准后的几何参数，机器人末端的定位精度可以得到极大提升。然而目前对于双臂机器人的校准问题研究较少。而且，在协同抓取、机械加工等操作中，除定位精度外，协作精度也是一项重要的指标。但之前关于双臂机器人运动学校准的研究一般主要关注定位精度，而忽视了协作精度。因此，亟需寻找一种兼顾双臂各自的绝对位姿精度和双臂间协作位姿精度的校准算法，从而提升双臂机器人的综合性能。

双臂机器人的校准可分解为两臂各自几何参数校准及其坐标转换的校准。对于两臂各自几何参数的校准，其主要原理是：利用激光跟踪仪等测量设备获取机器人在不同位型下的末端点位数据，利用机器人前向运动学模型，搜索最优的几何参数以得到基于测量数据的机器人末端位置的最优拟合[2-4]。其中最常见的方法是通过对机器人末端绝对定位二乘误差最小化来辨识几何参数误差，其本质是一个非线性优化问题，求解算法有高斯牛顿法、莱文贝格-马夸特(L-M)法、卡尔曼滤波法等。高斯牛顿法通常是最小化残差平方和，其收敛速度较快，在机器人校准中得到广泛使用[5]。L-M法通过改善高斯-牛顿法中矩阵的正定性，增大了算法对初值的敏感性[6]。卡尔曼滤波法被用来减小测量误差和机器人重复性误差对辨识过程的影响[7]。

大多数机器人校准算法更关注定位精度，对姿态精度的研究相对较少。采用直接法进行位置和姿态的同时校准存在非同质性和坐标系建系策略的问题，导致校准结果和算法收敛性较差[8]。文献[9]通过在末端安装三个靶球并同时测量其点位数据，最小化三靶球定位精度的平方和来实现姿态精度的提升。为了提高双臂机器人两臂各自校准的位姿精度并同时考虑校准算法鲁棒性，本文提出了最小化三个靶球对应的最大定位误差的方法。基于此优化目标，本文使用二次序列规划(sequential quadratic programming,SQP)方法对极小极大问题进行求解。

以前关于双臂机器人运动学校准的研究注重其绝对位姿误差[10-11]，而忽视了协作位姿精度。协作位姿精度与机器人双臂间坐标转换关系的校准结果相关。本文以两臂间点位的距离来衡量协作定位精度，通过鲁棒的极小极大算法来提升双臂间的协作位姿精度。

1 鲁棒的校准问题建模

1.1 先进机器人测量系统与校准流程

图1是机器人测量系统，由双臂机器人、机器人控制器、靶球组件和激光跟踪仪组成。在图1(b)中，在机械手末端法兰盘上安装了三个可以原路反射激光的靶球，使用激光跟踪仪测量三个球心对应的实到位置。对于左右两臂分别随机选择k个机器人位型，在每个位型下同时记录各个关节轴的角度值θ和三个靶球的点位数据。机器人的几何参数校准即是在一定的性能指标下，对所测量点位数据的最优拟合。

图1 亿恒双臂机器人的测量试验平台搭建Fig.1 Construction of a measurement experiment platform for a dual-arm robot

双臂机器人的校准流程图如图2所示。首先，根据绝对定位精度的指标优化单臂的几何参数；然后，通过以协作定位精度作为目标函数来优化左右臂之间坐标转换的参数。单臂的几何参数校准和左右臂坐标转换参数校准都是通过最小化三个靶球对应的最差定位精度来实现的。通过空间三个点可以约束机器人末端工具坐标系六个自由度方向的运动，尽可能减少这三个点的定位误差，既可以保证工具坐标系的位姿精度，又增加了校准过程的鲁棒性，此外避免了位姿向量定义的参考坐标系依赖与单位归一化等问题。

图2 双臂机器人的校准过程流程图Fig.2 Flow chart of the calibration process of a dual-arm robot

1.2 双臂机器人性能指标定义

针对双臂机器人特性，我们在标准GB 12642 (工业机器人性能规范及其试验方法)的基础上提出复合的性能指标，重新定义了定向精度和协作精度。在标准ISO 9283中，对绝对定位精度的定义是

Δp=|y-p|

(1)

式中：y为单臂的实到测量位置；p为单臂末端的名义位置。如果按照标准ISO 9283将姿态精度定义为欧拉角的偏差，会在奇异位型的附近产生很大的计算偏差。因此，将绝对定向精度定义为[12]

(2)

协作定位精度描述了双臂机器人左右臂末端之间的相对位置距离准确度，可以将其描述为

Δd=|dm-do|

(3)

式中，位型为i的左臂末端和位型为j的右臂末端之间的距离测量值为dm=||yi-yj||,对应的名义值记为do=||pi-pj||。类似地，为了避免由于欧拉角奇异产生的问题，将协作定向精度描述为

(4)

1.3 协作定位误差的线性近似

(5)

可以得到

Δpj-Δpi

(6)

图3 双臂机器人左右臂末端间协作定位误差的线性近似Fig.3 Linear approximation of cooperative positioning error between the left and right arm ends of a dual-arm robot

(7)

(8)

对式(8)两边求微分，可以得到双臂机器人左臂位型为i和右臂位型为j的协作定位误差-几何参数的雅可比矩阵为

式中，JL和JR分别为相应位型下的左右臂各自的绝对定位误差-几何参数的雅可比矩阵。

1.4 机械臂的几何参数校准问题描述

机械臂末端中心的位姿可用下面的非线性映射描述

(10)

式中，矩阵T包含末端位置p、旋转矩阵R，映射f()描述了其与关节角度θ、几何参数x的关系。将靶球安装位置的工具坐标记为ta,tb和tc，则靶球中心的名义坐标为

[pa,pb,pc]=[Rta+p,Rtb+p,Rtc+p]

(11)

对式(11)两边求导得到

(12)

式中：下标s=a,b，c为靶球序号;Js为机器人末端绝对定位误差-几何参数雅可比矩阵。

对于左右两臂而言，对于单个靶球对应的最小二乘误差可以表示为

(13)

式中，向量ys为第s个靶球点位的测量值。将所有待辨识的参数定义为x=[μ;ta;tb;tc]，设计目标函数为

(14)

通过最小化三个靶球中绝对定位精度最差的二乘误差，在保证绝对定位精度的同时，对于末端姿态精度也有很大的提升。

注意到三个靶球在各个位型下具有固定的相对位置，因此，靶球间距离的测量与其名义值的偏差应小于由跟踪仪测量误差引入的不确定度。同时，为使约束条件在微分空间连续，本优化问题采用以下等价的约束条件

(15)

式中：tl,l=1,2,3为两个靶球间的距离偏差值，距离测量值为k个位型下的平均值；ε为由于测量误差引入的不确定度。将该非线性不等式约束重写为

(16)

即优化问题的不等式约束。单臂的几何参数校准优化问题可总结为

(17)

1.5 双臂机械臂间的坐标转换关系校准

完成左右两臂各自的几何参数校准之后，第二步是对左臂和右臂之间的坐标转换关系进行辨识。从左臂的基坐标系到右臂基坐标系的坐标变换矩阵可表示为

(18)

(19)

(20)

式中：xb，yb和zb为三维坐标量；Ψ，θ和φ为旋转变换的角度值，分别对应ZYX欧拉角绕X轴，Y轴和Z轴方向的转动角，这六个参数组成了左右臂间坐标转换的待辨识参数tB=[xb,yb,zb,Ψ,θ,φ]T。机器人左臂和右臂的前向运动学矩阵分别表示为

(21)

(22)

式中，下标L和R分别为对应左右臂的参数，则左右臂末端的协作定位名义计算值为

(23)

测量而得的距离为

dm=||yR-yL||

(24)

式中，向量yR和yL分别为在测量坐标系{M}下右臂和左臂末端的测量坐标值。根据测量和名义的距离偏差，协作定位误差可表示为

us(xL,xR,tB)=|dm-do|

(25)

在坐标转换关系辨识过程中，定义指标函数为

(26)

(27)

此问题与几何参数校准问题类似，也是极小极大优化搜索问题。

2 校准问题的求解

2.1 极小极大搜索算法的求解

单臂的几何参数校准问题可以等价于下述带有不等式约束的单目标优化问题[15]

(28)

式中，x为在校准问题中待辨识的几何参数

(29)

式(28)中描述的优化问题近似为序列二次规划子问题。SQP方法的核心思想是将非线性的非凸优化问题线性化后生成二次规划子问题，并通过迭代逐步逼近原问题的目标解。因为Fs(x)是可微的，对于任何搜索方向Δx，其方向导数为

(30)

其中,

I(x)={s∶Fs(x)=ψ(x)}

(31)

那么，近似的序列二次规划子问题可以表示为

subject to∶C(x)+G(x)Δx≤0

(32)

式中:Δx=xκ+1-xκ为迭代的步长，即每次迭代过程二次规划问题的解，上标κ为迭代次数；海塞矩阵Hk为目标函数二阶微分矩阵的近似，在迭代过程中通过BFGS方法[16]不断更新，约束条件中G(x)是不等式约束C(x)的梯度。该二次规划子问题的解Δx必须满足Karush-Kuhn-Tucker(KKT)条件[17]，KKT条件中目标函数的拉格朗日形式可表示为

λT[C(x)+G(x)Δx]

(33)

式中，λ≧0为拉格朗日算子，则可推导出二次规划子问题式(32)的KKT条件为

(34)

式中，运算符()+定义为

(35)

为了搜索满足KKT条件的迭代步长及算子(Δx,λ)的最优解，采用下式所示的主二元子梯度算法迭代寻优

(36)

式中，p为二次规划子问题的迭代次数，定义KKT算子为

(37)

令z=(Δx,λ)T，则搜索最优解z*过程中的更新方程是

zp+1=zp-αpTp

(38)

式中:Tp∈T(zp);αp为比例因子。

对于左右臂坐标转换参数校准问题，将式(28)中的参数替换为tB，有

(39)

其中,

(40)

其余步骤与单臂的几何参数校准方法类似。

2.2 目标函数及约束的梯度

对于单臂的几何参数校准，目标函数和约束的梯度可以通过解析和数值计算两种方法求得，采用解析式的计算精度更高，从而使优化算法的收敛速度更快。近似的序列二次规划问题式(32)的目标函数梯度可表示为

P(x)=HkΔx+∇Fs(x)|s∈I(x)

(41)

式中：海塞矩阵Hk可以通过BFGS法迭代更新；Fs(x)的梯度为

(42)

式中，Js为描述对应第s组绝对定位误差-几何参数误差的雅可比矩阵。此外，不等式约束式(29)的梯度可表示为

(43)

约束c(x)的梯度为

式中，t=[ta,tb,tc]T为描述在工具坐标系下的三个靶球对应的名义坐标值。

对于左右臂间的坐标转换参数校准，Us(x)的梯度为

(45)

(46)

2.3 凸性及收敛性分析

SQP将非凸的原非线性优化问题线性近似为迭代的二次规划子问题，可以证明，二次规划子问题是凸优化问题。根据凸优化理论，若想证明目标函数及约束条件是凸的，需满足

f(αx+βy)≤αf(x)+βf(y)

(47)

式中，α+β=1,α≥0,β≥0。

二次规划子问题的目标函数

(48)

式中，海塞矩阵Hk是正定的，因此

Q(αx+βy)-αQ(x)-βQ(y)=
-αβ(x-y)THk(x-y)≤0

(49)

目标函数的凸性由此得证。类似的，容易证明约束条件的凸性。对于凸优化问题，全局收敛到最优解的条件是[18]

(50)

式中，γp为比例因子，用于调节收敛速度。

2.4 极小极大搜索算法伪代码

作为总结，下面列出了极小极大算法的伪代码。首先初始化目标函数、约束条件和它们的梯度，然后采用主二元子梯度算法求解二次规划子问题的全局收敛解。更新待辨识的参数和相应的目标函数、约束条件、梯度、海塞矩阵等信息。在更新后的待辨识参数处重新生成新的二次规划子问题，不断迭代直至原校准问题收敛。

算法：极小极大搜索算法

输入：参数名义值(0)x

输出:辨识的最优参数x*

whilex未收敛do

Δx0=0,λ0=0

whileΔx未收敛do

主二元子梯度算法

endwhile

更新状态、约束、梯度和海塞矩阵：

xk+1=xk+Δxk,sk=Δxk,

qk=∇Fs(xk+1)|s∈I+GT(xk+1)λk+1-[∇Fs(xk)|s∈I+GT(xk)λk]

endwhile

3 试验结果分析

为验证本文所提出算法的有效性，选取双臂机器人yumi作为研究对象进行了试验验证。yumi机器人左右臂由两个七轴机械臂IRB14000组成，其MD-H参数列在表1中。双臂机器人的左右臂由于拥有冗余自由度，可以灵巧地到达工作空间任意点，能够完成双臂协同类人操作，广泛应用于医疗、化工等领域。本节对双臂机器人左右臂各选定100个位型点，测量机械臂末端的位姿信息，通过本文提出的方法对双臂机器人的几何参数与坐标转换进行辨识，与传统方法对比校准后的绝对精度和协作精度指标。

表1 机器人IRB14000的MD-H参数Tab.1 MD-H parameters of robot IRB14000

3.1 单臂绝对精度的试验结果分析

为验证本文提出算法对双臂机器人左右臂绝对精度的提升效果，以右臂为例，对不同算法的精度改善效果进行了试验验证，试验对比结果如图4和图5所示。其中：方法1是本文提出的单臂几何参数校准方法；方法2是单臂使用单靶球考虑绝对定位精度的几何参数校准方法。注意到三个靶球一共对应三个绝对定位精度，在图4和图5中只呈现误差最大的一项。

图4 右机械臂校准前的绝对精度指标Fig.4 Absolute accuracy index before calibration of the right manipulator

图5 右机械臂使用不同校准方法后绝对精度提升效果对比Fig.5 Comparison of absolute accuracy improvement effect of the right manipulator after using different calibration methods

从试验结果中可以看出，两种方法都能明显提升机器人的绝对定位和定向精度。从绝对定位与定向误差的角度来分析，本文提出的方法(方法1)的提升效果明显优于传统方法(方法2)，尤其对于绝对定向精度指标而言。左右臂相关绝对精度的统计指标详细列在表2中。从表2可以看出，对于左臂而言，方法1和方法2的提升效果几乎一致，表明方法2的优化靶球测量点位正好是方法1中三个靶球误差最大的那一点。使用方法1可以使校准的参数辨识过程有较强的鲁棒性，经过参数补偿后，左右臂的绝对定位精度分别提升了83.08%和88.29%，绝对定向精度分别提升了88.52%和94.96%。

表2 使用不同校准方法前后的左右臂绝对位姿精度指标Tab.2 Absolute position and posture accuracy indexes of the left and right arms before and after using different calibration methods

我们将辨识后的结构参数写回机器人控制器，并测试了不同于校准测量点的验证点绝对位姿精度。发现即使不参与校准的机器人位型，也能够很好地实现精度提升，相应的位姿精度提升效果如图6所示。从图6中可以看出，验证位型的绝对定位精度从校准前的6.85 mm提升了90.51%到0.65 mm，绝对定向精度从校准前的0.139 rad提升了96.40%到0.005 rad。

图6 右机械臂校准前后验证点的绝对精度提升效果对比Fig.6 Comparison of absolute accuracy improvement effect of the verification points of the right manipulator

3.2 双臂协作精度的试验结果分析

为验证本文提出算法对双臂机器人左右臂间的协作精度的提升效果，将机器人几何参数与坐标转换参数进行辨识并补偿后，对不同算法的精度改善效果进行了试验验证，试验对比结果如图7和图8所示。其中：方法1是本文提出的双臂机器人校准方法；方法2是单臂使用单靶球考虑绝对定位精度的几何参数校准方法，结合本文提出的左右臂间坐标转换参数校准方法。

图7 双臂机器人校准前的协作精度指标Fig.7 Collaboration accuracy index before calibration of the dual-arm robot

图8 双臂机器人使用不同校准方法后协作精度提升效果对比Fig.8 Comparison on the effect of collaboration accuracy improvement after using two-arm robots with different calibration methods

从试验结果中可以看出，两种方法都能明显提升机器人的协作定位和定向精度。从协作定位与定向误差分布的角度来分析，本文提出的方法(方法1)的提升效果明显优于传统方法(方法2)。对于协作定位精度，方法1分别有13.15%和83.52%的点位位型对应的协作定位误差分布在区间[0,0.1]mm和[0,0.5]mm中。而对于方法2，只有9.36%和81.61%的点位位型对应的协作定位误差分布在区间[0,0.1]mm和[0,0.5]mm中。对于协作定向精度，方法1提升效果更为明显，有60.37%和99.83%的点位位型对应的协作定向误差分布在区间[0,0.005]rad和[0,0.01]rad中，而方法2只有16.47%和72.70%的点位位型对应的协作定向误差分布在区间[0,0.005]rad和[0,0.01]rad中。