并联机器人视觉盲区末端位姿检测方法

高国琴，韩 滢

(江苏大学 电气信息工程学院，江苏 镇江 212013)

0 引言

目前，机器人在水果分拣中的应用越来越广泛，而并联机构具有结构稳定、精度高、动态性能好等优点，因此研制了一种水果分拣并联机器人。水果分拣并联机器人分拣水果时，末端执行器的位姿反映了机器人的运动状态且对实现闭环控制有重要意义，因此对末端位姿的实时检测是成功抓取并分拣水果的前提。

现有位姿检测方法中，双目视觉末端位姿检测具有适用性强、性价比高等优点，适用于难以直接检测的多运动自由度并联机器人末端位姿检测[1]。然而并联机器人由多支路构成，机构复杂，在双目视觉成像中末端执行器易受到机构自身支路的遮挡，导致双目视觉盲区末端位姿检测错误。针对双目视觉检测盲区问题，有研究采用多目相机获取球形视野，消除视觉盲区，并应用于电动轮椅的安全避障和手势识别[2]，但该方法应用于并联机器人末端位姿检测时存在相机标定困难和多幅图像难以实现高准确率匹配问题；有研究采用一对广角鱼眼镜头扩大视野范围，但该方法应用于并联机器人末端位姿检测时图像边缘畸变较大，导致立体匹配产生较大误差[3]。因此，上述研究应用于水果分拣并联机器人末端位姿检测不仅对硬件要求高，同时会带来后续图像处理难度。

为解决双目视觉盲区末端位姿错误检测的问题，考虑采用运动学正解得到末端位姿，然而并联机器人各部件的加工制造误差、装配过程中产生的误差、因重力在不同位形下的变形误差等，均会造成运动学模型建立不精确，进而导致基于运动学模型求解出的运动学正解存在较大误差，且该误差难以用一个准确的数学模型来描述[4]。RBF神经网络(RBFNN)具有较强的预测和泛化能力，适用于水果分拣并联机器人末端位姿检测的实时误差补偿。但RBFNN在进行样本训练时，存在权值随机选取而引起的预测精度不高问题，因此需要对权值进行优化以提高其预测精度[5]。针对这一问题，国内有研究提出GWO算法优化RBFNN权值并应用于模拟电路故障诊断[6]；国外有研究提出一种GWO算法优化RBFNN的油水相对渗透率预测模型，并与PSO(粒子群)、GA(遗传)和DE(差分进化)算法进行对比，验证了GWO算法优化RBFNN权值的优越性[7]。然而上述方法应用于并联机器人末端位姿误差补偿时，GWO算法易出现早熟收敛，致使网络预测精度不高。为解决这一问题，提出一种GWO算法和LM算法混合优化RBFNN权值的方法，并将混合优化的RBFNN应用于视觉盲区，通过对运动学正解进行误差补偿以提高末端位姿检测精度。

1 水果分拣并联机器人运动学正解

水果分拣并联机器人机构原型如图1所示，该机构主要由静平台、主(辅)动平台和连接动、静平台的四组支链组成。其中，静平台上安装有伺服电机、减速器和双目相机，每条支链都由主动臂和从动臂组成，主动臂与减速器通过涨紧套固定连结。利用与减速器配接的伺服电机实现主动臂转动，从而实现末端执行器的三自由度平动和绕竖直轴转动[8]。

图1 水果分拣并联机器人

为便于运动学正解求解，将并联机器人的主体机构—Cross-IV机械手简化为图2所示结构。

图2 Cross-IV机械手结构简图

如图2所示，Cross-IV机械手的动平台分为主平台和辅平台，将主平台和辅平台分别简化为质点P1和P2，P2为标定板中心点，建立参考坐标系O-xyz，原点静平台中心，则点P2在该坐标系中的位置矢量r可表示为：

(1)

式中，εi=1,3=0，εi=2,4=1；ei表示O到Ai的向量，|ei|表示动平台与静平台的内切圆半径差，Xi=(Xi1,Xi2,…,Xis)；γi表示静平台结构角，γi=(i-1)π/2；l1，l2，ui，wi分别表示支链i的主动臂和从动臂的杆长与单位矢量，其中，ui=(cosγicosθisinγicosθi-sinθi)T，θi表示主动臂i(i=1,2,3,4)的转角。

将式(1)改写为：

(2)

同时两端点乘各自的转置，得：

(3)

展开公式(3)得：

x2+y2-2(e+l1cosθi)(xcosγi+ysinγi)+

(4)

公式(4)展开得：

x2-2xecosγi+e2cos2γi-2l1xcosγicosθi+

2yesinγi+e2sin2γi-2l1ysinγicosθi+

(5)

根据θi(i=1,2,3,4)即可求出点P2的位置(x,y,z)和s。将s代入式(6)求出绕Z轴的转角γ：

γ=(2π/p)s

(6)

2 混合优化的RBFNN设计

为解决RBFNN对视觉盲区的运动学正解进行误差补偿时存在的预测精度不高问题，需对其网络权值进行优化。GWO算法具有较强的全局搜索能力，实现简单且收敛速度快[9]，但其优化RBFNN权值时存在因早熟收敛而引起的网络预测精度不高问题。针对该问题，提出一种GWO算法和LM算法混合优化RBFNN权值方法，不仅能避免GWO算法单独优化RBFNN权值时出现的早熟收敛问题，同时能利用GWO算法的全局优化性能和LM算法的局部优化性能提高RBFNN的预测精度。GWO算法和LM算法混合优化RBFNN权值的过程如下：

1) 建立RBFNN，设置测试误差阈值ε，阻尼因子μ，阻尼系数β，最大迭代次数K，初始迭代次数k=0；

2) 最近邻聚类算法确定RBFNN基函数的中心、宽度和初始权值w；

3) 输入权值构建新的RBFNN；

4) 计算网络目标误差函数E(x)；

5) 计算雅克比矩阵J；

6) 计算权值向量调整值Δw并进行权值更新wk+1=wk+Δw；

7) 判断是否E(w)<ε，满足则结束训练，否则执行步骤8)；

8) 计算E(wk+1)，判断是否E(wk+1)

9) 初始化灰狼种群，最大迭代次数T，初始化参数a、A、C；

10) 计算种群内每个个体的适应度值，确定α、β、δ灰狼；

11) 在定义域内随机产生α灰狼、β灰狼、δ灰狼的位置，并计算其余灰狼ω位置，更新参数a、A、C；

12) 判断是否达到最大迭代次数或精度，若达到，执行步骤3)，否则执行步骤10)。

具体权值优化流程图如图3所示。

图3 GWO算法和LM算法混合优化RBFNN权值流程图

2.1 GWO算法

GWO算法通过模拟自然界中灰狼寻找、包围和攻击猎物等狩猎机制的过程来完成优化工作。如图4所示，将灰狼族群划分为4个等级，分别为最优灰狼α，次优灰狼β、第三优灰狼δ和普通灰狼ω[10]，在追击猎物过程中，由α、β、δ灰狼进行猎物追捕，而普通灰狼ω跟随灰狼α、β、δ进行猎物围捕，猎物的位置则是问题的最优解。

图4 灰狼族群等级制度结构图

定义在S维搜索空间内第i只狼的位置为Xi=(Xi1,Xi2,…,Xis)，其中(i=1,2,…,N)，第i只狼与猎物间的距离为Di，则灰狼追捕猎物的行为描述如下：

Xi(t+1)=XP(t)-Ai·Di

(7)

Di=|Ci·XP-Xi|

(8)

式中，t为迭代次数，X(t)表示第t代灰狼个体当前的位置，XP(t)为第t代猎物的位置；Ai为收敛因子，C表示向量系数，且：

Ai=2ar1-a

(9)

Ci=2r2

(10)

式中，a随着迭代次数t的增加从2线性递减至0；r1,r2为[0,1]内的随机数。

定义灰狼族群向猎物追捕过程中，灰狼个体的位置向量更新方式为：

Dα=|C1·Xα(t)-Xω(t)|

(11)

Dβ=|C2·Xβ(t)-Xω(t)|

(12)

Dδ=|C3·Xδ(t)-Xω(t)|

(13)

X1=Xα-A1·Dα

(14)

X2=Xβ-A2·Dβ

(15)

X3=Xδ-A3·Dδ

(16)

Xω(t+1)=(X1+X2+X3)/3

(17)

式中，Xω(t+1)表示灰狼个体更新后的位置向量。

图5为说明上述公式的灰狼位置更新机制原理图，最优灰狼α、次优灰狼β和第三优灰狼δ对猎物的位置进行追踪，普通灰狼ω以此为参考在猎物周围随机更新位置，直到猎物在灰狼α、β或δ的随机圆内停止更新[11]。

图5 灰狼位置更新原理图

2.2 LM算法

LM算法是梯度下降法和高斯-牛顿法相结合的快速数值优化算法，其局部搜索能力强，具有局部快速收敛特性[12]。根据LM算法的基本思想，设ωk∈Rn表示经过第k次迭代后的网络权值向量，则新的权值向量ωk+1可根据下面的公式求得[13]：

ωk+1=ωk+Δω

(18)

Δω=-[JT(ω)J(ω)+μI]-1JT(ω)e(ω)

(19)

式(19)中,J(ω)为Jacobi矩阵，e(ω)=(e1,e2,…eN)T表示误差，I为单位矩阵，n为网络权值数量；μ为阻尼因子。

设误差函数为：

(20)

其中:t，o分别为网络各输出层的实际输出和期望输出。

3 运动学正解结合混合优化RBFNN误差补偿的末端位姿检测

首先在非视觉盲区采样，然后进行RBFNN训练，训练中采用GWO算法和LM算法混合优化网络权值，将训练好的混合优化RBFNN用于视觉盲区末端位姿检测，通过对运动学正解进行误差补偿以提高末端位姿检测精度。

3.1 RBFNN的训练样本

水果分拣并联机器人的工作空间较小且RBFNN拥有较强的泛化能力，因此在非视觉盲区进行采样。样本包括运动学正解和视觉检测的末端位姿，其中运动学正解为RBFNN训练的输入样本，视觉检测的末端位姿与运动学正解的差值为RBFNN训练的输出样本。

为保证混合优化RBFNN的输入与输出在[0，1]之间，对训练样本[Xo,Yo]进行归一化处理，即：

(21)

(22)

式中，xoi,min，xoi,max，yoi,min与yoi,max分别为[Xo,Yo]中xoi与yoi的最小值与最大值。

则样本学习后还原得到的网络输出yi,out为：

yi,out=yoi,min+yi(yoi,max-yoi,min)

(23)

3.2 GWO算法和LM算法混合优化RBFNN权值

首先通过最近邻聚类算法[14]确定RBFNN的径向基函数中心及初始权值，然后采用GWO算法和LM算法混合优化RBFNN权值，算法切换可通过精度或最大迭代步数实现。通过互以对方训练结果作为自己的初始群体或初始值，反复交替训练，直到达到所设置的测试误差阈值或达到训练的最大次数时输出最优权值。

3.3 运动学正解误差补偿

混合优化的RBFNN训练完成后，将其用于视觉盲区的运动学正解误差补偿，以提高视觉盲区的末端位姿检测精度。

4 实验与结果分析

实验平台包括硬件平台和软件平台。硬件平台：操作系统Windows7，处理器 Intel(R) Core(TM) i7-3770，主频3.40 GHz，内存8 GB。软件平台：MATLAB、VC++。

实验方法：为验证本文提出的GWO算法和LM混合优化RBFNN权值方法相比于GWO算法优化RBFNN权值方法以及未优化RBFNN权值方法的有效性，进行三组实验对比。在非视觉盲区采集300组样本数据，将运动学正解求得的末端位姿作为RBFNN训练的输入样本，视觉检测的末端位姿与运动学正解的差值作为输出样本。取2/3作为训练样本，1/3作为测试样本，分别采用未优化的RBFNN、GWO算法优化的RBFNN、GWO算法和LM算法混合优化的RBFNN进行样本训练和测试，并依据末端位姿(x,y,z,γ)各分量的测试误差对3种方法进行比较分析。

在视觉盲区进行末端位姿检测实验，首先通过运动学正解得到末端位姿，然后采用混合优化的RBFNN对运动学正解进行末端位姿误差补偿，将补偿前的末端位姿和补偿后的末端位姿分别与激光跟踪仪Leica AT901和电子罗盘Honeywell-HMR3100测得的实际末端位姿进行对比，以验证所提出的视觉盲区末端位姿检测方法的有效性。

实验结果和分析：针对相同样本，采用未优化的RBFNN、GWO算法优化的RBFNN、GWO算法和LM算法混合优化的RBFNN进行训练，所得测试样本误差分别如图6～8所示。

图6 未优化RBFNN的测试样本误差

图7 GWO算法优化RBFNN的测试样本误差

图8 GWO算法和LM算法混合优化RBFNN的测试样本误差

由图6～8分析得，GWO算法和LM算法混合优化RBFNN的测试误差明显低于未优化的RBFNN和GWO算法优化的RBFNN，说明所提出的混合优化RBFNN的预测效果更优。

分别采用基于运动学正解的末端位姿检测方法、运动学正解结合混合优化RBFNN误差补偿的末端位姿检测方法完成水果分拣并联机器人视觉盲区末端位姿检测实验，检测误差如图9～10所示，图9为基于运动学正解的末端位姿检测误差，图10为运动学正解结合混合优化RBFNN误差补偿的末端位姿检测误差，表2为检测误差统计结果。

图9 基于运动学正解的末端位姿检测误差

图10 运动学正解结合混合优化RBFNN误差补偿的末端位姿检测误差

由表1分析得，与基于运动学正解的末端位姿检测方法相比，运动学正解结合混合优化RBFNN误差补偿的末端位姿检测方法将视觉盲区末端位姿(x,y,z,γ)分量x误差的平均绝对值和标准差降低了54.4%和52.9%；y误差的平均绝对值和标准差降低了67.7%和62.8%；z误差的平均绝对值和标准差降低了54.7%和51.9%；γ误差的平均绝对值和标准差降低了52.9%和58.8%。

表1 视觉盲区末端位姿检测误差结果统计

5 结束语

为解决水果分拣并联机器人双目视觉盲区的末端位姿错误检测问题，并提高视觉盲区的末端位姿检测精度，提出一种运动学正解结合混合优化RBFNN误差补偿的末端位姿检测方法。主要结论如下：

1) 针对GWO算法优化RBFNN权值时易陷入早熟收敛而引起的网络预测精度不高问题，提出了一种GWO算法和LM算法混合优化RBFNN权值方法，并将混合优化的RBFNN应用于水果分拣并联机器人双目视觉盲区末端位姿检测，通过对运动学正解进行误差补偿，不仅解决了并联机器人受机构支路遮挡造成的视觉盲区末端位姿错误检测问题，同时提高了视觉盲区的末端位姿检测精度。

2) 将基于运动学正解的末端位姿检测方法，与运动学正解结合混合优化RBFNN误差补偿的末端位姿检测方法进行实验对比，实验结果表明运动学正解结合混合优化RBFNN误差补偿的末端位姿检测方法将视觉盲区末端位姿(x,y,z,γ)分量x，y，z，γ的误差平均绝对值分别降低了54.4%、67.7%、54.7%和52.9%，误差标准差分别降低了52.9%、62.8%、51.9%和58.8%。因此，所提出的末端位姿检测方法提高了视觉盲区水果分拣并联机器人的末端位姿检测精度。