改进PROSAC算法的并联机器人末端位姿检测方法

高国琴 韩 滢

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

0 引 言

随着现代农业的发展，机器人自动分拣水果成为发展趋势。并联机构具有结构稳定、精度高、动态性能好等优点，满足机器人自动分拣水果对稳定性的要求。基于此，研制一种水果分拣并联机器人。当并联机器人自动分拣水果时，实现对末端执行器位置和姿态的实时检测是成功抓取并分拣水果的前提。同时，水果分拣并联机器人末端执行器的位姿反映了机器人的运动状态，是分析机器人末端执行器动态特性以及进一步实现并联机器人实时闭环控制的重要参数[1]。

目前，并联机器人末端位姿检测方法可分为接触式检测和非接触式检测。非接触式检测方法可避免测量力对机器人的受力干扰，适用于末端位姿静态和动态检测。非接触式检测包括超声检测、激光检测、机器视觉检测等。基于机器视觉的末端位姿检测方法相对于其他方法具有非接触、适用性强、高性价比等优点，尤其适用于具有多运动自由度、运动轨迹复杂和难以直接检测的并联机器人末端位姿检测[2]。机器视觉检测方法可分为单目、双目和多目。与单目相比，双目视觉无须添加几何约束，可获取视野范围内任意一点的三维信息，为机器人的运动控制提供更准确的末端位姿参数[3]；与多目相比，双目视觉所要求匹配的图像信息少，减小了立体匹配难度，在保证一定检测准确率的条件下具有更快的检测速度[4-5]。因此，本文研究水果分拣并联机器人的双目视觉末端位姿检测方法。

在双目视觉末端位姿检测过程中，需要对相机采集到的一对图像进行立体匹配，并将立体匹配结果代入双目视觉模型求出末端位姿，故而立体匹配的准确率和实时性是直接影响双目视觉末端位姿检测精度和实时性的主要因素[6]。采用双目视觉对水果分拣并联机器人进行末端位姿检测时，光照、噪声干扰等不确定因素造成立体匹配中包含较多的错误匹配，因此，为提高末端位姿检测的精度，需要对立体匹配结果进行提纯。

现有提纯算法包括RANSAC算法和PROSAC算法[7-8]。PROSAC算法的鲁棒性优于RANSAC算法，主要原因是RANSAC算法将所有采样点列为相同等级，而PROSAC算法首先对采样点进行排序，然后从较高等级的采样点进行迭代，再通过假设和验证获得最优解[9]。该算法提出后得到了广泛应用，有研究结合ORB和PROSAC算法进行实时全景图像拼接[10]；还有研究提出一种结合极线几何约束的PROSAC算法以提高立体匹配的准确率[11]。但上述方法应用于水果分拣并联机器人双目视觉末端位姿检测时，不能保证选取采样点的分散性导致模型参数估计易陷入局部极值，从而模型参数估计精度不高；同时该方法在得到候选模型后将其余所有采样点检验一遍，会耗费时间验证错误候选模型，从而末端位姿检测的实时性降低。考虑到水果分拣并联机器人末端位姿检测对精度和实时性要求较高，本文通过穿插取点和预检验候选模型的方法对PROSAC算法进行改进，以提高模型参数估计的精度和速度，并进一步将改进的PROSAC算法应用于末端位姿检测中的立体匹配提纯，兼顾立体匹配的实时性和准确率，进而提高水果分拣并联机器人双目视觉末端位姿的检测精度。

1 改进的PROSAC算法设计

根据PROSAC基本思想，首先按Hamming距离相似度对匹配点对进行排序，然后抽样计算两幅图像中匹配点对的目标模型H，最后利用目标模型验证剩余匹配点对并去除误匹配点对。

假设匹配点对的数量为N，由于Hamming距离相似度越高则匹配点对的质量越高，故而将Hamming距离相似度作为匹配质量的定量表示[12]。将匹配点对按Hamming距离相似度降序排列后的集合定义为UN，从UN中抽取n组较高质量的匹配点对组成集合序列M，通过采样计算得到的候选模型为F，设置内点误差阈值W、最大内点数N和最大迭代次数K。算法具体步骤如下[13]：

(1) 按照Hamming距离相似度对匹配点对做降序排列，得到匹配对集UN。

(2) 从UN中选取前n组较高质量的匹配点对，组成匹配对子集M。

(3) 从M中随机选取m组匹配点对组成样本集，求解候选模型F：设(x,y,1)和(x′,y′,1)分别为立体图像对中一对正确匹配点对a(x,y)和a′(x′,y′)的齐次坐标，将m组匹配点对代入式(1)求解F矩阵参数[14]。

(1) (4) 通过候选模型F和误差阈值W检测剩余匹配点对，得到满足候选模型F的匹配点对和满足候选模型F的匹配点对数(内点数d)。

(5) 根据终止条件判断是否结束迭代过程，否则重复步骤(2)-步骤(5)。终止条件：当内点数超过所设定的最大内点数或者k次采样后的内点数与k-1次采样后的内点数相比没有增加。

针对未改进的PROSAC算法应用于水果分拣并联机器人双目视觉末端位姿检测时存在的模型参数估计精度不高以及验证错误候选模型耗时问题，通过穿插取点和预检验候选模型的方法对PROSAC算法进行改进，以提高模型参数估计的精度和速度。首先为解决随机采样生成的样本集点间距很近导致模型参数估计精度不高的问题，将排序后得到的高质量匹配对集M等分成t(t>m)份，从中抽取m+1份并从每份中抽取一组匹配点对构成样本集S，然后从样本集S中抽取m组匹配点对求出候选模型F。为解决验证错误候选模型耗时的问题，在得到候选模型F后验证第m+1组匹配点对是否满足该模型，若是，则继续寻找满足候选模型F的匹配点对，否则重新选取样本集M并抽样计算候选模型F。

改进PROSAC算法的流程如图1所示，其中：最大迭代次数K=lg(1-Pc)/lg(1-em)；Pc为置信率，一般取值为0.95；e为抽样后计算得到的内点概率且每次迭代后进行更新，设置初始值为0.5，即假设所有匹配点对中约有一半为正确匹配点对；m为抽样个数，取值为8；内点误差阈值取值为5。

图1 改进的PROSAC算法流程

当内点数超过所设定的最大内点数或者k次采样后的内点数与k-1次采样后的内点数相比没有增加时，返回包含内点数最多的候选模型参数矩阵F以及满足该模型的匹配对集。最后利用匹配对集内所有匹配点对通过最小二乘法进一步优化候选模型参数矩阵F得到目标模型参数矩阵H。

2 水果分拣并联机器人末端位姿检测

将所提出的改进PROSAC算法应用于本课题组研制的新型4-DOF水果分拣并联机器人末端位姿检测，以验证其正确性和有效性。

图2 水果分拣并联机器人

水果分拣并联机器人机构原型如图2所示，该机构通过四组支链连接动、静平台，支链的主动臂和静平台上的减速器由涨紧套固定连接，通过伺服电机实现主动臂的转动，进而实现末端执行器的三维空间平动和绕竖直轴的转动[15]。

本文的研究对象是水果分拣并联机器人末端执行器，在实际工业测量中，复杂的图像背景不利于直接定位末端执行器的中心点。为减少工业环境对末端位姿检测的影响，在该机构末端执行器的正上方安装点阵圆形标定板，利用点阵圆形标定板的漫反射特性提高双目视觉成像中特征点与背景图像的灰度对比度，从而便于识别和提取特征点，适用于对实时性要求高的并联机器人末端位姿检测。因此，本文将并联机器人末端位姿检测问题转换为点阵圆形标定板的中心点位姿检测问题，在得到点阵圆形标定板中心点位姿后经过仿射变换求得末端执行器位姿。

对于该水果分拣并联机器人，首先通过双目相机采集机构末端图片，并基于ORB算法进行特征点检测和立体预匹配。然后采用改进的PROSAC算法对立体预匹配结果进行提纯，以提高立体匹配的准确率。最后将提纯后的匹配点对代入双目视觉模型，求出并联机器人末端位姿参数。

2.1 图像采集和特征点检测

2.1.1图像采集和处理

通过Dalsa CamExpert图像采集软件进行图像采集，两个G3-M2420工业相机固定安装在末端执行器的上方，可实现直径D=400 mm、高度H=100 mm的圆柱体空间定位。镜头SV-0614H通过C-mount接口安装到相机上，相机通过GigE Vision连接到图像采集卡，从而连接到计算机上。图3为同一时刻左右两个相机采集到的点阵圆形标定板原始图像。

图3 相机采集的原始图像

由于所采集立体图像对中的特征点对不在同一扫描线上，因此采用极线校正法[16]对立体图像对进行投影变换使其满足扫描线特性，可大大减少立体匹配的搜索空间，提高立体匹配速度。校正结果如图4所示。

图4 极线校正后的图像

2.1.2特征点检测

为保证所研究并联机器人末端位姿检测的实时性，采用简单快速的ORB算法进行特征点提取和描述。ORB算法由O-FAST特征提取算子和rBRIEF特征描述算子组成，实时性比传统的SIFT、SURF算法有数量级的提高[17-18]，能够满足水果分拣并联机器人双目视觉末端位姿检测的实时性要求。

2.2 立体匹配及三维坐标求解

2.2.1基于ORB算法的立体预匹配

ORB算法中的rBRIEF特征描述子以二进制形式存在，因此采用Hamming距离对特征点匹配与否进行判断，即通过对两个特征点的二进制串进行异或处理，筛选出正确的匹配点对[19]。Hamming距离可表示如下：

(2)

2.2.2基于改进PROSAC算法的立体预匹配结果提纯

本文采用穿插取点和预检验候选模型的方法对PROSAC算法进行改进，并将改进的PROSAC算法应用到立体匹配中，通过对预匹配结果进行提纯以提高立体匹配的准确率，进而末端位姿检测的精度得以提高。

2.2.3相机模型参数求解

采用针孔模型对相机成像模型进行分析和建立。如图5所示，定义四个坐标系：(1) 世界坐标系{W}={OW-XWYWZW}，描述物体和相机的空间位置；(2) 相机坐标系{C}={OC-XCYCZC}，原点设置在相机光心上，ZC轴与光轴重合并垂直于成像平面；(3)
图像像素坐标系{O}={o-uv}，图像右上方为原点；(4)
图像物理坐标系{I}={OI-XIYI}，光轴与图像平面的交点设为原点[20]。

图5 针孔成像模型

根据针孔成像模型，空间中任一点M(XW,YW,ZW)在图像上的像素投影点为m(u,v)，两者的关系为:

(3)

式中：ZC为焦点到像平面的距离；M1、M2为相机内外参数矩阵。通过将标定板中各标志点的像素坐标和世界坐标代入式(3)，求解出相机的内外参数。

2.2.4特征点三维坐标求解

假设点P的三维空间坐标为PW(XW,YW,ZW)，在左相机拍摄图像中的像素坐标为pl(ul,vl)，在右相机拍摄图像中的像素坐标为pr(ur,vr)。则根据投影定理有：

(4)

式中：sl、sr表示比例因子；Ml=Al[RlTl]和Mr=Ar[RrTr]分别为左右投影矩阵，Al、Ar、Rl、Rr、Tl、Tr分别为左右相机内参矩阵、左右相机坐标系相对于世界坐标系的旋转矩阵和平移矩阵。将式(4)展开，则有：

(5)

(6)

联立式(5)和式(6)，可得：

A×PW=B

(7)

因此：

PW=(ATA)-1ATB

(8)

将特征点的左右像素坐标代入式(8)，即可求得特征点的三维坐标。

2.3 水果分拣并联机器人位姿参数求解

2.3.1并联机器人坐标系定义

水果分拣并联机器人坐标系如图6所示，建立世界坐标系{W}={OW-XWYWZW}、左相机坐标系{C}={OC-XCYCZC}和目标坐标系{P}={OP-XPYPZP}，其中：世界坐标系原点OW位于并联机器人静平台的中心点，ZW轴垂直于静平台平面；左相机坐标系原点OC为左相机平面中心点，XC轴和YC轴平行于左相机平面；目标坐标系原点OP位于P2点，则OP的位姿可表示并联机器人的末端位姿。因此，水果分拣并联机器人的位姿信息包括点OP在世界坐标系下的三维空间坐标和并联机器人末端执行器的三个姿态角(俯仰角α、翻滚角β，航向角γ)。根据水果分拣并联机器人的运动特点，其运动过程中α和β相对于定坐标系不变，只有γ在变化，故该并联机器人的末端位姿参数可表示为(x,y,z,γ)。

图6 水果分拣并联机器人坐标系

2.3.2位置参数计算

将标定板的中心圆形标志点设为目标标志点P，将点P的左右像素坐标代入式(8)求出其世界坐标P(xW,yW,zW)。已知标定板与末端执行器的相对位置保持不变，通过仿射变换即可求得末端执行器的位置参数。假设仿射变换中的旋转矩阵为：

(9)

平移矩阵为：

N=[NxNyNz]T

(10)

则末端执行机构的位置可表示为：

(11)

2.3.3姿态参数计算

一般并联机器人包括三个姿态角α、β、γ，其中：α为绕X轴旋转的俯仰角，β为绕Y轴旋转的翻滚角，γ为绕Z轴旋转的航向角。假设目标坐标系{P}绕坐标轴旋转变换之后与世界坐标系重合，则旋转矩阵表示为：

(12)

则俯仰角、翻滚角和航向角分别表示为：

(13)

已知末端执行器上两点M1和M2在目标坐标系下的坐标M1=(x1,y1,z1)T、M2=(x2,y2,z2)T，在世界坐标系下的坐标MW1=(xW1,yW1,zW1)T、MW2=(xW2,yW2,zW2)T，则有：

(14)

(15)

由式(14)、式(15)得：

MW1-MW2=R(M1-M2)

(16)

求出的R即为旋转矩阵，将R代入式(13)可得到并联机器人的末端姿态参数γ。

3 实验与结果分析

实验平台包括硬件平台和软件平台。硬件平台：操作系统Windows 7，处理器 Intel( R) Core(TM) i7-3770，主频3.40 GHz，内存8 GB，Teledyne Dalsa面阵相机(Nano-M2420)，VST镜头(SV-0614H)。软件平台：VC++，OpenCV库，MATLAB。

实验方法：为验证本文所提出改进PROSAC算法的水果分拣并联机器人双目视觉末端位姿检测方法相比于未改进PROSAC算法的检测方法的有效性，首先在立体匹配阶段进行三组对比实验。实验一为原始立体图像对，实验二为光照变化后的立体图像对，实验三为加入方差0.01的高斯噪声后的立体图像对，分别对每组立体图像对采用未改进的PROSAC算法和改进的PROSAC算法提纯ORB立体预匹配结果，并依据立体匹配的准确率和时耗对两种立体匹配方法进行比较分析。

定时采集36幅图像，分别采用未改进的PROSAC算法和改进的PROSAC算法对ORB立体预匹配结果进行提纯，从提纯后的立体匹配结果中提取目标标志点对，将其代入双目视觉模型获取水果分拣并联机器人末端位姿参数，并与激光跟踪仪Leica AT901和电子罗盘Honeywell-HMR3100测得的末端执行器位姿对比，从而验证所提出水果分拣并联机器人末端位姿检测方法的有效性。

实验结果及分析：图7-图9分别为三组立体匹配实验结果图，表1-表3分别为三组立体匹配的准确率和时耗统计。由表1-表3可知，与采用未改进的PROSAC算法进行立体匹配提纯相比，采用改进的PROSAC算法将原始立体图像对的立体匹配准确率从92.5%提高至96.1%，耗时从0.085 s降低至0.062 s；将光照变化后立体图像对的立体匹配准确率从90.4%提高至94.2%，耗时从0.112 s降低至0.087 s；将增加噪声后立体图像对的立体匹配准确率从89.0%提高至93.9%，耗时从0.139 s降低至0.101 s。由此可看出，改进的PROSAC算法降低了运算复杂度，提高了立体匹配的准确率和实时性，且对光照变化和噪声干扰有良好的鲁棒性。

图7 原始立体图像对的立体匹配结果

图8 光照变化后立体图像对的立体匹配结果

图9 增加噪声后立体图像对的立体匹配结果

表1 原始立体图像对的立体匹配结果统计

表2 光照变化后立体图像对的立体匹配结果统计

表3 增加噪声后立体图像对的立体匹配结果统计

图10为水果分拣并联机器人的末端位姿检测误差图，其中：(a)为未改进PROSAC算法的末端位姿检测误差图，(b)为改进PROSAC算法的末端位姿检测误差图。表4为两种检测方法的误差结果统计。

图10 水果分拣并联机器人的末端位姿检测误差

表4 末端位姿检测误差结果统计

由表4分析可得，与未改进PROSAC算法的末端位姿检测方法相比，改进PROSAC算法的水果分拣并联机器人末端位姿检测方法，其位姿分量x的位置误差平均绝对值降低了53.9%，标准差降低了53.2%；y的位置误差平均绝对值降低了65.5%，标准差降低了67%；z的位置误差平均绝对值降低了66.9%，标准差降低了66.6%；γ的姿态误差平均绝对值降低了47%，标准差降低了56.6%。

4 结 语

针对一种水果分拣并联机器人，本文提出一种改进PROSAC算法的水果分拣并联机器人双目视觉末端位姿检测方法。主要结论如下：

(1) 通过穿插取点和预检验候选模型的方法对PROSAC算法进行改进，克服了PROSAC算法应用于水果分拣并联机器人双目视觉末端位姿检测时存在的模型参数估计精度不高和验证错误候选模型耗时问题。基于此，进一步提出了一种改进PROSAC算法的水果分拣并联机器人末端位姿检测方法，解决了双目视觉末端位姿检测中光照、噪声干扰等外部因素造成的末端位姿检测精度降低问题。

(2) 将改进PROSAC算法的水果分拣并联机器人末端位姿检测方法与未改进PROSAC算法的检测方法进行实验对比，实验结果表明，所提出改进PROSAC算法的并联机器人末端位姿检测方法，其位姿分量x、y、z、γ的误差平均绝对值分别降低了53.9%、65.5%、66.9%、47%，误差标准差分别降低了53.2%、67%、66.6%、56.6%。因此，所提出的改进PROSAC算法的水果分拣并联机器人末端位姿检测方法提高了末端位姿检测精度。