一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法与流程

本发明涉及基于机器视觉的位姿检测系统，尤其涉及一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法。

背景技术：

随着现代农业的发展，水果分拣由机器人作业成为发展趋势。并联机构具有结构稳定、精度高、动态性能好等优点，适用于对运动稳定要求较高的水果分拣操作。为此，一种水果分拣并联机器人被研制。当水果分拣并联机器人分拣水果时，对末端执行器的位置和姿态的实时检测是成功抓取并分拣水果的前提。同时，水果分拣并联机器人末端执行器的位姿是反映该机器人运动状态的重要参数，对分析末端执行器的动态特性和进一步实现并联机器人的闭环实时控制均不可或缺。

目前，并联机器人位姿检测方法可分为接触式检测和非接触式检测。非接触式检测方法可避免测量力对机器人的受力干扰，适用于末端位姿静态和动态检测。非接触式检测包括超声检测、激光检测、机器视觉检测等。基于机器视觉的位姿检测方法相对于其它检测方法具有非接触、适用性强、高性价比等优点，尤其适用于具有多运动自由度、运动轨迹复杂和难以直接检测的并联机器人末端位姿检测。基于机器视觉的位姿检测系统按照相机数目可分为单目、双目和多目。与单目相比，双目视觉无需添加几何约束，可获取视野范围内任意一点的三维信息，为机器人的运动控制提供更准确的末端位姿参数信息；与多目相比，双目视觉所要求匹配的图像信息少，减小了立体匹配难度，在保证一定准确性的条件下，具有更快的检测速度。由于立体匹配的准确率是直接影响双目视觉机器人末端位姿检测精度的主要因素，而水果分拣并联机器人因光照、噪声干扰等不确定因素造成立体匹配中包含较多的错误匹配，引起位姿检测精度的降低。

同时水果分拣并联机器人机构复杂，由多支路构成，在双目视觉成像中末端执行器易受到机构自身支路的遮挡产生视觉盲区，造成双目视觉错误检测问题。因此考虑通过运动学正解获取末端位姿参数，然而运动学正解存在较大误差该误差难以用一个准确的数学模型来描述。rbf(径向基函数)神经网络具有较强的预测和泛化能力，适用于水果分拣并联机器人末端位姿检测的实时误差补偿，但其进行样本训练时存在随机选择权值导致预测精度不高的问题。

文献《基于改进orb算法无重叠视野域的目标交接》(黄新，黄柏华，熊显名，液晶与显示，2016，31(8):810-817)利用orb算法提取目标特征点并通过渐进式抽样一致性算法剔除误匹配点对，但该方法应用于水果分拣并联机器人双目视觉末端位姿检测时不能保证选取样本点的分散性，导致模型参数估计易陷入局部极值，致使模型参数估计精度不高；同时该方法在得到候选模型后需要将其余所有匹配点检验一遍，存在耗费时间验证错误候选模型的问题。因此该方法应用于本发明涉及的水果分拣并联器人双目视觉末端位姿检测时难以实现高精度实时检测。

文献《基于狼群算法的rbf神经网络模拟电路故障诊断》(颜学龙，丁鹏，马峻，计算机工程与应用，2017，53(19):152-156.)提出一种灰狼(gwo)优化算法优化rbf神经网络权值方法，并将其并应用于模拟电路故障诊断，然而gwo优化的rbf神经网络优化复杂多峰问题时易出现早熟收敛，导致rbf神经网络的预测精度降低，因此该方法应用于本发明涉及的水果分拣并联机器人视觉盲区运动学正解补偿时难以获得较好的补偿效果。

技术实现要素：

本发明的目的是为了克服上述现有技术的不足，针对水果分拣并联机器人因光照、噪声干扰等引起的双目视觉末端位姿检测精度降低问题，以及末端执行器易受机构自身支路遮挡造成的视觉盲区末端位姿错误检测问题，提出一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法。该方法不但能够解决光照、噪声干扰因素造成的水果分拣并联机器人双目视觉末端位姿检测精度降低问题，而且能够克服末端执行器受机构自身支路遮挡造成的视觉盲区末端位姿错误检测问题。

本发明采用的技术方案是采用如下步骤：

一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法，包括如下步骤：

步骤1，基于双目视觉的图像获取：基于双目视觉实时采集水果分拣并联机器人末端执行器的原始立体图像对，根据能否识别立体图像对中的标定板来判断是否为视觉盲区；

步骤2，针对非视觉盲区末端位姿检测：2.1采用bouguet算法对原始图像对进行极线校正保证其特征点在同一扫描线上；2.2基于orb算法对立体图像对进行特征点检测并基于hamming距离进行立体预匹配；2.3针对立体预匹配中存在的错误匹配问题，采用所提出的渐进式抽样一致性算法对立体预匹配结果进行提纯，其中通过穿插取点和预检验候选模型提高渐进式抽样一致性算法的模型参数估计精度和速度；2.4构建机构双目视觉模型，将提纯后的特征点对代入双目视觉模型进行三维重建，并计算出末端位姿参数；

步骤3，针对视觉盲区末端位姿检测：3.1在非视觉盲区采集rbf神经网络训练样本，以运动学正解为网络的输入样本，视觉检测的末端位姿与运动学正解的偏差值为网络的输出样本；3.2进行rbf神经网络训练，训练过程中采用gwo和lm算法混合优化rbf神经网络权值以提高网络预测精度；3.3将训练好的混合优化rbf神经网络用于视觉盲区末端位姿检测，通过混合优化的rbf神经网络对视觉盲区的运动学正解进行误差补偿以获得高精度末端位姿参数。

本发明提出一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法，通过采用上述技术方案后，其具有以下有益效果：

1、针对双目视觉末端位姿检测中因光照、噪声干扰等因素造成的末端位姿检测精度降低问题，本发明通过穿插取点和预检验候选模型的方法提高渐进式抽样一致性算法的模型参数估计精度和速度，将高精度快速渐进式抽样一致性算法应用于水果分拣并联机器人双目视觉末端位姿检测中的立体匹配提纯，在保证立体匹配实时性的同时提高立体匹配的准确率，从而提高双目视觉末端位姿检测精度。

2、针对双目视觉末端位姿检测时末端执行器易受机构自身支路遮挡造成的视觉盲区错误检测问题，本发明采用混合优化的rbf神经网络对视觉盲区的运动学正解进行误差补偿，以提高水果分拣并联机器人视觉盲区的末端位姿检测精度。

附图说明

以下结合附图和具体实施方式对本发明作进一步详细说明。

图1水果分拣并联机器人机构图。

图2双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法流程图。

图3极线校正后的立体图像对。(a)校正后的左图；(b)校正后的右图；

图4光照变换后立体图像对的立体匹配图。其中：图4(a)为基于orb算法的立体匹配效果图，图4(b)为高精度快速渐进式抽样一致性算法提纯后的立体匹配效果图。

图5增加噪声后立体图像对的立体匹配图。其中：图5(a)为基于orb算法的立体匹配效果图，图5(b)为高精度快速渐进式抽样一致性算法提纯后的立体匹配效果图。

图6双目视觉成像原理图。

图7非视觉盲区末端位姿检测误差图。

图8水果分拣并联机器人机构坐标系图。

图9gwo和lm混合优化rbf神经网络权值流程图。

图10gwo算法优化rbf神经网络的测试样本误差图。其中：图10(a)为采用gwo优化rbf神经网络的位置测试误差图，图10(b)为采用gwo优化rbf神经网络的姿态测试误差图。

图11gwo和lm混合优化rbf神经网络的测试样本误差图。其中：图11(a)为采用gwo-lm混合优化rbf神经网络的位置测试误差图，图11(b)为采用gwo-lm混合优化rbf神经网络的姿态测试误差图。

图12视觉盲区末端位姿检测误差图。

具体实施方式

下面结合附图进一步说明书本发明具体实施方式。

首先通过双目相机实时获取水果分拣并联机器人末端执行器的立体图像对，然后根据能否识别立体图像对中的标定板来判断是否为视觉盲区。针对非视觉盲区末端位姿检测，首先采用bouguet算法对原始图像对进行极线校正使其满足扫描线特性；然后采用orb算法进行特征点检测并基于hamming距离进行立体预匹配；接着通过穿插取点和预检验的方法提出一种高精度快速渐进式抽样一致性算法对立体预匹配结果进行提纯，以提高立体匹配的准确率和实时性；最后将提纯后的匹配点对代入双目视觉模型进行三维重建，计算出水果分拣并联机器人末端位姿参数。针对视觉盲区末端位姿检测，首先在非视觉盲区采集rbf神经网络的训练样本，其中运动学正解为输入样本，运动学正解与视觉检测末端位姿的差值为输出样本；然后进行rbf神经网络训练，训练时采用gwo算法和lm算法混合优化rbf神经网络权值以提高网络预测精度；最后将训练好的rbf神经网络应用于视觉盲区末端位姿检测，通过对运动学正解进行误差补偿以获取视觉盲区的高精度末端位姿参数。

具体实施方式以本课题组研发的水果分拣并联机器人为检测对象进行描述，参见图1，水果分拣并联机器人机构由静平台、主(辅)动平台和连接动、静平台的四组支链组成，能够实现末端执行器的三自由度平动和绕竖直轴的转动，双目相机固定安装在并联机器人上方，实时获取末端执行器图像信息。

其具体步骤如下：

1、参见图2，双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法流程图。通过图像采集和处理判断末端执行器所行区域是否为视觉盲区，将末端位姿检测范围分为非视觉盲区和视觉盲区。

2、参见图3，在非视觉盲区，首先采用bouguet算法使立体图像对满足扫描线特性。

将左右相机之间的旋转矩阵r采用如式(28)(29)的方式分解为左相机旋转矩阵rl和右相机旋转矩阵rr：

rl＝r^1/2(28)

rr＝r^-1/2(29)

利用式(30)将左相机采集的末端执行器图像的极点e1移动到无穷远处使双目视觉的极线达到水平状态：

式中，t为右相机相对于左相机的偏移矩阵，||t||为矩阵t的矩阵范数。

建立如式(31)所示的向量e2方向与相机主光轴方向正交，与e1垂直：

式中，tx为右相机相对于左相机在x方向上的偏移量，ty为右相机相对于左相机在y方向上的偏移量。

将向量e1和向量e2进行叉积运算得向量e3，如式(32)所示：

e3＝e1×e2(32)

则水平对准矩阵rrect如式(33)所示：

将左相机旋转矩阵rl和右相机旋转矩阵rr分别乘以水平对准矩阵rrect可获得左右图像进行校正的投影矩阵rl'和rr'，如式(34)所示：

3、参见图4(a)和图5(a)，采用orb算法对校正后的立体图像对进行特征点提取，并基于hamming距离进行立体预匹配。

定义特征点p与周围像素点的关系sp→k如式(35)所示：

式中，ip表示p点的灰度值，ip→k表示与p点周围对应编号为k的像素点灰度值，t是阈值，周围像素点就可分为d、s、b三种类型；统计d和b出现的总次数n，s出现的次数n，当n＞n时，p点则被选为候选特征点。

定义特征点p的方向θ如式(36)所示：

式中，x、y为像素点的图像坐标，i(x,y)为(x,y)位置的像素灰度值。

定义特征点特征描述符gm如式(37)所示：

式中，p(xi)为点xi的像素灰度值，p(yi)为点yi的像素灰度值，为m个点对(xi,yi),i＝1,2,…,m组成的矩阵，为方向θ对应的旋转矩阵。

选取256个具有高方差和高不相关性的测试点作为描述子，具体步骤为：

(1)对ofast算子检测到的特征点邻域的每个子窗口进行τ统计。

(2)对每个子窗口检测到的二进制描述串求平均值，并根据均值与0.5的偏差大小进行排序，将这些子窗口的二进制描述串存入到一个容器t中。

(3)进行贪婪算法搜索：

(a)移出容器t中顶层的第一项，放入到一个结果容器r中。

(b)然后用容器t的下一项与容器r中所有的二进制描述串进行比较，如果它们之间的相关性大于所设置的阈值。则舍弃该描述串；否则，将其添加到结果容器r中。

(c)重复(a)(b)步骤，直到结果容器r中有256个二进制字符串。如果完成循环仍低于256个，则提高相关性阈值，重新进行贪婪搜索，直到搜索到256个二进制描述字符串。

基于hamming距离进行立体预匹配，定义立体图像对的orb特征点描述子d1和d2为：

d1＝x0x1…x255(38)

d2＝y0y1…y255(39)

式中，x0x1…x255和y0y1…y255为二进制串。

则hamming距离如式(40)所示：

s(d1,d2)越小表示hamming距离相似度越高。

4、参见图4(b)和图5(b)，采用高精度快速渐进式抽样一致性算法对立体预匹配结果进行提纯。针对双目视觉末端位姿检测过程中因光照、噪声干扰引起的错误匹配问题，本发明提出了一种高精度快速渐进式抽样一致性算法对立体预匹配结果进行提纯，在保证立体匹配实时性的同时提高立体匹配的准确率，从而提高非视觉盲区水果分拣并联机器人末端位姿的检测精度。

为解决随机采样生成的样本集点间距很近导致渐进式抽样一致性算法模型参数估计精度不高的问题，本发明首先将排序后得到的高质量匹配对集m等分成t(t＞m)份，从中抽取m+1份并从每份中抽取一组匹配点对构成样本集s，然后从样本集s中抽取m组匹配点对求出临时候选模型f。同时，本发明在得到临时候选模型f后验证第m+1组匹配点对是否为该模型的支撑集，以避免验证错误候选模型耗时的问题。获取目标模型的具体步骤如下：

(1)对匹配对集un按照hamming距离相似度做降序排列；

(2)选取前n组较高质量的匹配对子集m；

(3)将匹配对子集m等分成t(t＞m)份，从中抽取m+1份并从每份中抽取一组匹配点对构成样本集s；

(4)从样本集s中抽取m组匹配点对求出临时候选模型f：设(x,y,1)和(x',y',1)为一对正确匹配点对的齐次坐标，将m组匹配点对代入式中，求解f矩阵参数；

(5)验证第m+1组匹配点对是否为该模型的支撑集，若是，则继续寻找临时候选模型f的支持集，否则重新选取样本集m；

(6)通过候选模型f和误差阈值w检测剩余匹配点对，得到候选模型f的支撑集和满足候选模型f的匹配点对数即模型内点数d；

(7)根据终止条件判断是否结束迭代过程，否则重复步骤(2)～(6)。

终止条件：当内点数超过所设定的阈值或者k次采样后的内点数与k-1次采样后得到的内点数相比没有增加，返回包含内点数最多的候选模型参数矩阵f以及该模型的支持集。

计算候选模型f所对应匹配点对的sampson距离d，如式(41)：

式中，m＝(u,v,1)^t为点m的齐次坐标，m'＝(u',v',1)^t为点m'的齐次坐标。

设定阈值k，将符合d＜k的点作为该候选模型f的内点，将包含内点数最多的候选模型f进行最小二乘法得到目标模型h，如式(42)：

式中(u,v,1)和(u',v',1)分别为两幅图像中一对正确的匹配点对m(u,v)和m'(u',v')的齐次坐标。

5、参见图6，根据双目视觉原理实现三维重建，将前期图像处理提取到的末端匹配点对带入双目视觉模型，最终获得并联机器人末端位姿信息。三维重建时，首先需要通过双目标定获得左右相机的内外参数，以确定三维坐标中空间物点与图像平面上像素投影点之间的对应关系。本发明采用张正友标定法对双目视觉系统进行相机标定，根据张正友平面标定流程：采集点阵圆形标定板图像对，再将标定板图像输入计算机并获取标定板上每个特征点的像素坐标，然后把标定板特征点的已知三维坐标载入计算机，通过将标定板的像素坐标和三维坐标代入式(43)，求解出相机的内外参数:

式中，s为任意数，a为相机的内参数矩阵，r为相机坐标系相对世界坐标系的旋转矩阵，t为相机坐标系相对世界坐标系的平移矩阵，(u,v,1)表示特征点在图像上投影点的齐次坐标，(xw,yw,zw,1)为特征点在世界坐标系下的齐次坐标(单位：m)。

假定末端特征点p在左右相机的投影点分别为pl和pr，则根据投影定理得到式(44)：

式中：pw为特征点p的世界坐标，ml＝al[rltl]和mr＝ar[rrtr]分别为左右相机的投影矩阵：al和ar分别为左右相机的内参矩阵，rl和rr分别为左右相机坐标系相对于世界坐标系的旋转矩阵，tl和tr分别为左右相机坐标系相对于世界坐标系的平移矩阵；sl，sr表示比例因子，将上式展开来则如式(45)(46)所示：

式中，(ul,vl,1)，(ur,vr,1)分别表示特征点p在左右图像上投影点的齐次坐标；(x,y,z,1)为特征点p在世界坐标系下的齐次坐标(单位：m)；表示左右投影矩阵ml和mr的第i行，第j列。

联立式(45)和式(46)，可得式(47)：

a*pw＝b(47)

式中：

从而得式(48)：

pw＝(a^ta)^-1a^tb(48)

将特征点的左右像素坐标代入式(48)，求得特征点的三维坐标(x,y,z)(单位：m)。

再将特征点p的旋转矩阵r表示为如式(49)所示：

式中，α为绕x轴旋转的俯仰角(单位：rad)，β为绕y轴旋转的翻滚角(单位：rad)，γ为绕z轴旋转的航向角(单位：rad)。

则可求出末端姿态角α、β、γ，如式(50)所示：

根据水果分拣并联机器人的运动特点，其运动过程中α和β相对于定坐标系不变，只有γ在变化，故该并联机器人的末端位姿参数可表示(x,y,z,γ)，其中x、y和z的单位为m，γ单位为rad。

至此，水果分拣并联机器人的末端位姿参数求解已完成。

6、参见图8，视觉盲区内采用运动学正解求出末端位姿。将末端执行器的标定板中心点简化为点p2，以静平台中心点为原点建立参考坐标系o-xyz，则点p2在该坐标系中的位置矢量r表示为式(51)：

式中，r为点p2的位置矢量，s为动平台主动平台与辅动平台之间的距离(单位：m)；为z轴方向的单位向量；ei表示o到ai的向量，ei＝e(cosγisinγi0)^t(i＝1,2,3,4)；γi表示静平台结构角(单位：rad)，γi＝(i-1)π/2；l1，l2，ui，wi分别表示支链i的主动臂和从动臂的杆长(单位：m)与单位矢量，其中ui＝(cosγicosθisinγicosθi-sinθi)^t，θi表示主动臂i(i＝1,2,3,4)的转角(单位：rad)。

同时两端点乘各自的转置得式(52)：

展开公式(52)得：

将θi(i＝1,2,3,4)值代入公式(53)即可求出x,y,z以及s，将s代入公式(54)即可求出绕z轴的转角γ：

γ＝(2π/p)s(54)

式中p为丝杠螺距(单位：m)。

至此，水果分拣并联机器人的运动学正解求解已完成，求得末端位姿参数(x,y,z,γ)，其中x、y和z的单位为m，γ单位为rad。

7、参见图9、图10和图11，采用gwo和lm算法混合优化rbf神经网络权值，以提高rbf神经网络的预测精度。针对gwo优化rbf时易出现早熟收敛导致网络预测精度降低的问题，本发明提出了一种gwo和lm算法混合优化rbf网络权值的方法。

在非视觉盲区进行rbf神经网络采样，其中输入样本为运动学正解，输出样本为视觉检测的末端位姿与运动学正解的偏差值。为保证优化rbf神经网络的输入输出值在[0，1]区间内，对其训练样本的集合[xo,yo]进行归一化处理，如式(55)所示：

式中，xoi,min，xoi,max，yoi,min与yoi,max分别为样本集合[xo,yo]中xoi与yoi的最小值与最大值。

则样本学习后还原得到的网络输出yi,out如式(57)所示：

yi,out＝yoi,min+yi(yoi,max-yoi,min)(57)

获取rbf神经网络训练样本后，进行rbf神经网络训练，并采用gwo算法和lm算法混合优化权值，具体步骤如下：

(1)建立rbf神经网络，设置测试误差阈值ε，阻尼因子μ，阻尼系数β，最大迭代次数k，初始迭代次数k＝0；

(2)最近邻聚类算法确定rbf神经网络基函数的中心、宽度和初始权值w；

(3)输入权值构建新的rbf神经网络；

(4)计算网络目标误差函数e(w)：

式中，ti、oi分别为第i层网络输出层的实际输出和期望输出；

(5)计算雅克比矩阵j(w)：

式中，ei(w)＝ti-oi，表示第i层网络输出误差，wi为第i层网络权值；

(6)计算权值向量调整值δwi并进行权值更新其中δwi＝-[j^t(w)j(w)+μi]^-1j^t(w)ei(w)，j(w)为雅克比矩阵，μ为阻尼因子，i为单位矩阵，ei(w)为第i层网络输出误差；

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

(8)计算e(w^k+1)，判断是否e(w^k+1)＜e(w^k)，满足则则令μ＝μ/β，k＝k+1，并执行步骤(9)，否则令μ＝μ*β，并执行步骤(6)；

(9)初始化灰狼种群[w1,w2,…,wn]，设置最大迭代次数t；

(10)计算每个灰狼的适应度值mse(w)，并按照适应度升序排列，选择前三名灰狼为α灰狼、β灰狼、δ灰狼：

式中，为第t次迭代的第i层网络期望输出，为第t次迭代的第i层网络实际输出；

(11)在定义域内随机产生α灰狼、β灰狼、δ灰狼的位置，根据式(61)计算其余灰狼ω位置并更新参数a，a，c：

式中，dα、dβ、dδ分别为α灰狼、β灰狼、δ灰狼与猎物间的距离向量，xα、xβ、xδ分别为α灰狼、β灰狼、δ灰狼当前位置，xω(t)|为第t次迭代的ω灰狼位置，xω(t+1)表示ω灰狼更新后的位置；r1、r2为[0,1]内的随机数，t为当前的迭代次数，t为最大迭代次数；

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

8、将训练好的混合优化rbf神经网络应用于视觉盲区末端位姿检测，通过对运动学正解进行误差补偿，从而提高视觉盲区的末端位姿检测精度。

实施例

本发明着重提出一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法，解决水果分拣并联机器人因光照、噪声干扰引起的末端位姿检测精度降低问题，以及末端执行器易受机构自身支路遮挡引起的视觉盲区末端位姿错误检测问题。

具体实施例以本课题组研发的水果分拣并联机器人末端位姿为检测对象进行描述，其具体实施方式如下：

1、采集水果分拣并联机器人末端执行器原始图像。通过双目相机采集水果分拣并联机器人末端执行器原始图像，其中，相机为teledynedalsa面阵相机(nano-m2420)，镜头为vst镜头(sv-0614h)，相机固定安装机构上方。此外，本检测系统采用计算机操作系统为windows7，处理器型号为intel(r)core(tm)i7-3770，主频为3.40ghz，内存为8gb。

2、对原始立体图像对进行极线校正。为保证进行立体匹配时特征点对在同一扫描线上，采用极线校正法使立体图像对满足扫描线特性，结果如图3所示。

3、采用orb算法对极线校正后的立体图像对进行特征点提取和描述并基于hamming距离进行立体预匹配，立体预匹配结果如图4(a)、5(a)所示。

4、针对光照、噪声因素引起的错误匹配问题，采用本发明提出的高精度快速渐进式抽样一致性算法对立体匹配结果进行提纯，提高立体匹配的准确率。从匹配对集中抽样计算两幅图像匹配点对的目标变换模型h，然后利用目标模型验证剩余匹配点对并去除误匹配点对。具体步骤如下：

(1)对匹配对集un按照hamming距离相似度做降序排列；

(2)选取前n组较高质量的匹配对子集m；

(3)将匹配对子集m等分成t(t＞m)份，从中抽取m+1份并从每份中抽取一组匹配点对构成样本集s；

(3)从样本集s中抽取m组匹配点对求出临时候选模型f，并验证第m+1组匹配点对是否为该模型的支撑集，若是，则继续寻找临时候选模型f的支持集，否则重新选取样本集m；

(4)通过候选模型f和误差阈值w检测剩余匹配点对，得到候选模型f的支撑集和满足候选模型f的匹配点对数即模型内点数d；

(5)根据终止条件判断是否结束迭代过程，否则重复步骤(2)～(6)。

终止条件：当内点数超过所设定的阈值或者k次采样后的内点数与k-1次采样后得到的内点数相比没有增加，返回包含内点数最多的候选模型参数矩阵f以及该模型的支持集。

计算候选模型f所对应匹配点对的sampson距离d：

式中，m＝(u,v,1)^t为点m的齐次坐标，m'＝(u',v',1)^t为点m'的齐次坐标。

设定阈值k，将符合d＜k的点作为该候选模型f的内点，将包含内点数最多的候选模型f进行最小二乘法得到目标模型h：

式中(u,v,1)和(u',v',1)分别为两幅图像中一对正确的匹配点对m(u,v)和m'(u',v')的齐次坐标。

采用高精度快速渐进式抽样一致性算法进行立体预匹配提纯结果如图4(b)、图5(b)所示，结果图表明了所提出的高精度快速渐进式抽样一致性算法具有较好的提纯效果，剔除了立体预匹配中的错误匹配点对。

5、求解水果分拣并联机器人末端位姿。根据水果分拣并联机器人运动特点，建立机构平台的定坐标系与动坐标系如图8所示，其中，{w}＝{ow-xwywzw}和{c}＝{oc-xcyczc}、{p}＝{op-xpypzp}分别表示世界坐标系、定坐标系和动坐标系，则水果分拣并联机器人末端位姿的变化可表示为中心点op点的位姿变化。本实施例采用张正友标定法实现双目视觉系统的相机标定，并通过前期图像处理和后期坐标变换求出水果分拣并联机器人末端位置信息如式(64)所示：

pw＝(a^ta)^-1a^tb(64)

式中，pw为标志点的世界坐标(单位：m)，(ul,vl)，(ur,vr)分别表示pw在图像上的像素投影点pl和pr的坐标；表示左右投影矩阵的第i行，第j列。

圆形标定板与末端执行器的相对位置保持不变，则末端执行器位置o^t为：

o^t＝mp^t+n(65)

式中，p^t为标定板位置参数，m为旋转矩阵m为，n为平移矩阵。

已知特征点p的旋转矩阵根据式(66)求出末端姿态角：

式中，α为绕世界坐标系x轴旋转的俯仰角(单位：rad)，β为绕世界坐标系y轴旋转的翻滚角(单位：rad)，γ为绕世界坐标系z轴旋转的航向角(单位：rad)。

根据水果分拣并联机器人运动特点，其运动过程中α和β相对于定坐标系不变，只有γ在变化，故该并联机器人的末端位姿参数可表示(x,y,z,γ)，其中x、y和z的单位为m，β单位为rad。

6、通过运动学正解求解视觉盲区的水果分拣并联机器人末端位姿，并采用混合优化的rbf神经网络对运动学正解进行误差补偿。首先在双目视觉检测区域内采集300组样本数据，样本包括运动学正解求得的末端位姿和视觉检测的末端位姿，取其中2/3作为训练样本，剩余1/3作为测试样本。将运动学正解求得的末端位姿作为rbf神经网络训练的输入样本，视觉检测末端位姿与运动学正解的偏差值作为网络的输出样本，训练中采用gwo算法和lm算法混合优化rbf神经网络权值，混合优化rbf神经网络的测试误差如图11所示。将训练好的混合优化rbf神经网络应用于视觉盲区末端位姿检测，对视觉盲区的运动学正解进行误差补偿，以提高视觉盲区的末端位姿检测精度。

本实施例定时采集非视觉盲区的12张图像，采用高精度快速渐进式抽样一致性算法对orb立体预匹配结果进行提纯，从提纯后的立体匹配结果中提取目标标志点对带入相机模型获取水果分拣并联机器人末端位姿参数，并与由激光跟踪仪leicaat901和电子罗盘honeywell-hmr3100测得的末端实际位姿参数进行对比，以验证所提出的非视觉盲区末端位姿检测方法的有效性。检测误差结果如图7所示，结果表明位姿分量x，y，z，γ的误差平均绝对值分别为0.388mm，0.306mm，0.275mm，0.393°，说明所提出的末端位姿检测方法在非视觉盲区具有较高的检测精度。

定时采集视觉盲区的12张图像，首先通过运动学正解得到末端位姿，然后采用混合优化的rbf对运动学正解进行误差补偿，将补偿后的运动学正解与由激光跟踪仪leicaat901和电子罗盘honeywell-hmr3100测得的末端实际位姿参数进行对比，以验证所提出的视觉盲区末端位姿检测方法的有效性。检测误差结果如图12所示，结果表明混合优化的rbf神经网络对运动学正解进行误差补偿后，其末端位姿分量x，y，z，γ的误差平均绝对值分别为0.671mm，0.436mm，0.431mm，0.455°，说明所提出的末端位姿检测方法在视觉盲区具有较高的检测精度。