基于单目视觉的高速并联机器人动态目标跟踪算法

梅江平，王 浩，张 舵，闫 寒，李 策

基于单目视觉的高速并联机器人动态目标跟踪算法

梅江平，王 浩，张 舵，闫 寒，李 策

(天津大学机构理论与装备设计教育部重点实验室，天津 300350)

在高速并联机器人视觉分拣系统中，由于安装误差等原因，视觉系统与传送带以及机器人与传送带之间会存在一定的角度偏差，导致目标物体在分拣系统中的定位精度较低，使机器人末端不能精确抓取动态目标．而且传统的单目视觉定位及跟踪算法存在精度低、运算速度慢等问题，影响视觉分拣系统的计算速度，降低系统的实时性．针对以上问题，提出了一种基于圆特征的单目视觉定位与传送带标定结合的动态目标跟踪算法，首先不需考虑相机坐标系与传送带坐标系的角度偏差，拍摄一个圆形目标，并以光心为顶点构造直椭圆锥的几何成像模型．在此基础上利用其几何关系求取圆心在相机坐标系下的深度信息，再结合传送带标定与张正友标定方法，确定传送带编码器的比例因子以及机器人坐标系和传送带坐标系之间的角度偏差，并实时跟踪圆形目标运动过程中圆心在机器人下的位置．最后，对所提算法和传统算法分别进行实验对比，结果表明，该动态跟踪算法通过圆锥投影的几何关系推导圆心的空间坐标，避免了传统复杂的非线性方程求解，定位精度和计算速度大幅提高．该算法消除了传统算法的角度偏差，平均位置误差为0.82mm，具有较高的定位精度．运行中系统耗时较短，运行效率得到了有效提高，且误抓率和漏抓率分别为0.2%和0，具有很高的实用价值．

单目视觉定位；高速并联机器人；相机标定；传送带标定；动态跟踪

作为应用最为成功的并联机构之一，高速并联机器人因具有刚度大、精度高、速度快等优势而广泛应用于电子、食品、医药等行业[1-2]．传统的机器人分拣生产线，常采用离线示教的方法对物品进行抓取分拣，该方法易受到周围环境的干扰．近年来，随着机器人与其相关领域的结合日益紧密，机器视觉技术的引入增强了机器人对环境变化的适应能力，使其向更加智能化的方向发展[3]．高速并联机器人视觉分拣系统，通过相机实时获取传送带上的图像，经图像处理分析得到目标物体的形状位置等信息，并传输给运动控制系统，实现高速并联机器人对动态目标的抓取 任务[4]．

单目视觉定位仅用一台相机完成定位而无需解决立体视觉中两相机间的最优距离和特征点匹配问题，具有简单易用、适用范围广等优点[5]．但由于其损失了目标点的深度信息而无法得到完整的图像信息，如何解决目标和投影图像之间的匹配问题是其研究的重点[6]．常用的基于单帧图像的方法利用点或直线等特征进行定位[7]，但均存在实时性低、精度差等缺点[8]．此外，在实际工程应用中由于安装误差等原因，相机与传送带以及机器人与传送带之间均有一定角度偏差，也会影响目标物体的定位精度，最终导致机器人末端不能精准地抓取动态目标[9]，如果采用提高加工精度的方法，则势必会增加成本．

针对上述问题，本文提出了一种基于圆特征的单目视觉定位与传送带标定结合的动态目标跟踪算法．该算法不需考虑相机和传送带之间的角度关系拍摄一个圆形目标，根据成像模型构造出一个以光心为顶点的斜椭圆锥，并将其映射为直椭圆锥，通过直椭圆锥的几何关系，计算圆心在相机下的空间三维坐标，避免了传统复杂的非线性代数方程求解，提高了计算速度和定位精度．再结合传送带标定和张正友标定方法，实时跟踪圆形目标的位置．最后与传统方法进行对比实验，验证了该视觉定位及跟踪算法的精确性，结果表明本文算法提高了分拣系统的实时性和定位精度．

1 基于圆特征的单目视觉定位

高速并联机器人视觉分拣系统一般应用于对轻小物料分拣的自动化生产线上，如图1所示，该系统主要分为机器视觉系统与机器人控制系统. 机器视觉系统利用工业相机实时获取传送带上物料的图像，实现对物料的识别定位等功能，并将所得信息发送到机器人控制系统，引导高速并联机器人完成物料的分拣工作．

图1 高速并联机器人视觉分拣系统

1.1 直椭圆锥参数求取

传统的单目视觉定位方法多采用点特征或线特征，基于曲线特征的定位算法如经典的利用共面曲线和非共面曲线，由于需要对几个高次多项式进行求解，算法比较复杂而较少应用[10]．圆作为二次曲线的一种且用其定位可以摆脱匹配问题而被广泛关注，本算法基于圆特征并运用圆锥投影的几何关系推导出圆心的空间坐标，避免了复杂的非线性方程求解．

由于实际安装中相机相对传送带会有一定偏角，光轴不垂直于传送带平面，故拍摄一个圆形目标在成像平面上所得图像被映射成椭圆．图2为目标在相机坐标系下的成像模型，通过图像处理分析得到椭圆轮廓的图像像素坐标(u，v)，其成像点的图像物理坐标(x，y)为

设相机坐标系为C，并以向量元素左上方字母表示其所在坐标系，后面类似记法与此含义相同，则成像点在相机坐标系下的位置为[Cxi，Cyi，f]T．

将相机光心作为顶点，以圆目标为底面形成一个斜圆锥，以成像椭圆为底面形成一个斜椭圆锥，由于斜椭圆锥几何关系不标准，很难进行几何运算，故拟对该斜椭圆锥进行变换．当给定光心至任意平面的距离时，必然存在一个与斜椭圆锥共用所有母线的直椭圆锥，故当构造其母线为任意长度时，可形成一个以光心为顶点的直椭圆锥，此椭圆可以在任意平面上，并将其记为1，直椭圆锥记为1，有

式中：C为椭圆1轮廓点在相机下的坐标；焦距由相机内参数标定获得；为母线长度，可取同一数量级的任意值．

求取椭圆1轮廓坐标集的平均值得到其中心0在相机下的坐标为

式中为椭圆轮廓坐标点的个数．

得到成像点在新相机坐标系下的位置为

式中：C′为新成像点在新相机坐标系下的位置；X3为C′的第3行元素．

1.2 椭圆拟合

由上述得到了椭圆2的轮廓点坐标，下面用椭圆拟合算法求取椭圆方程[11]．式(10)为椭圆的一般方程，由最小二乘法，拟合目标函数如式(11)所示．

为使函数最小，由式(12)求取、、、、的值．

最后由式(13)～(15)拟合出椭圆标准方程，如式(16)所示．

式中：(0，0)为椭圆中心坐标；为椭圆倾斜角；、分别为椭圆的长轴和短轴．

1.3 圆目标空间位姿求解

图3为新相机坐标系下圆目标和直椭圆锥2的投影关系，几何关系为

式中：1＝∠；2＝∠；为圆目标半径；为点1和2的距离．

图3 圆形目标与直椭圆锥的投影关系

在△1与△1中，由正弦定理得

式中∠1．解此方程得到．

在△和△2中分别利用正弦定理和余弦定理得

最终得到目标圆心在相机下的坐标为

式中(Cx，Cy，)T为成像平面椭圆圆心在相机下的坐标，由成像点通过椭圆拟合算法求得．

2 传送带标定

传送带标定即确定传送带和机器人之间的转换关系和编码器的比例因子[12]．设机器人坐标系为R，传送带坐标系为B，若知道任意点在传送带坐标系下的坐标B，则通过式(23)即可得到其在机器人坐标系下的坐标[13]．

如图4所示，让圆形目标在机器人工作空间随传送带移动一段距离，以始末位置圆心连线为轴，方向指向传送带运动方向，并记录两位置编码器的读数1、2．

图4 传送带标定

用机器人末端触碰始末位置目标圆心，从控制器中读出两圆心在机器人下的位置分别为R1＝(R1，R1，R1)T和R2＝(R2，R2，R2)T，有如下关系：

式中：DR为始末两点的距离；DB为传送带始末示数变化；为编码器比例因子．

因此当目标随传送带移动，由始末位置的编码器读数差D即可求得目标移动的距离为

接着控制机器人末端沿其R轴移动一段距离，由于机器人相对传送带有偏角，故机器人末端的轨迹不垂直于传送带移动方向，记终点的位置为R3＝(R3，R3，R3)T．

建立传送带坐标系BBBB，有如下关系：

其中

传送带基坐标系为

则传送带相对于机器人的转换矩阵为

3 基于传送带的相机标定

图5 相机针孔成像模型

机器人视觉分拣系统中的相机标定分为基于机器人的相机标定与基于传送带的相机标定[19]．前者相机位于机器人工作空间内，依靠机器人坐标系标定相机，确定二者转换矩阵，本文基于传送带对相机标定，是指相机安装在机器人工作空间外的情况．

相机从不同角度拍摄20张如图6所示的棋盘格，采用张正友标定法[20]，确定相机的内参数．外参数求取即依靠传送带坐标系作为媒介，确定相机坐标系与机器人坐标系之间的关系．

根据式(22)得到拍照时刻目标圆心在相机下的坐标，记为C，由式(34)可得圆心在传送带坐标系下的位置，并记录此时编码器读值为．

图6 黑白棋盘格标定板

接着，当圆目标随传送带移动到机器人工作空间时停止运动并记录编码器读数，可得此时圆心在传送带坐标系下的位置为

其中

然后，用机器人末端触碰目标圆心，从控制器读出其在机器人坐标系下的位置为

联立式(34)～(36)，有

因此，该综合算法通过从图像中获任意圆形目标圆心位置2就能够实时跟踪其在传送带上移动任意距离D后在机器人坐标系下的位置为

4 实验与数据

传统的动态目标定位算法没有考虑相机相对传送带的偏角，并且假定传送带坐标系与机器人坐标系仅有位置偏移而没有姿态变化，而本文算法均考虑了二者的角度偏差．

如图7所示，本实验采用瑞思机器人有限公司开发的D3S-500型Delta高速并联机器人视觉分拣系统进行，该系统采用Windows操作系统，相机选用Congnex-CAM-CIC-300-120-G，标称帧率为120帧/s，配套使用软件In-Sight Explore 5.4.0，并采用Ether CAT总线传输数据，控制器选用KeControl CP263/X及扩展I/O模块DM272/A和编码器模块MM240/A，伺服驱动器为清能德创CoolDrive-R4，并搭载200W多摩川伺服电机，圆形目标采用直径为60mm的圆形薄板．

图7 Delta机器人视觉分拣系统实验平台

首先，根据第2节所述方法进行传送带标定，使圆形薄板在机器人工作空间中随传送带运动一段距离，传送带速度为300mm/s．通过始末位置编码器脉冲值及圆心在机器人下的坐标由式(24)～(26)计算编码器比例因子0.078．接着，使机器人末端从终止位置圆心开始沿垂直于薄板运动方向移动任意距离，并测量终止位置圆心坐标，通过式(28)～(31)求得转换矩阵为

由欧拉角的定义，旋转矩阵＝()()()，进而解方程求出传送带坐标系与机器人坐标系、、轴之间的角度偏差分别为0.78°，＝0.52°，1.27°．

然后使圆形薄板从传送带开始端进入系统，在经过相机视野下时拍照，并通过图像处理算法在计算机中获取100个圆周点坐标，进而利用第1节中所提算法并根据式(17)～(22)求得圆心在相机下的空间坐标C2＝[10.641，12.847，510.085]T．当圆形薄板移动到机器人工作空间时停止运动，利用此时和拍照时刻的编码器脉冲的差值计算薄板移动的距离D，并由式(35)～(37)计算出相机相对传送带的转换矩阵

同样由欧拉角定义，推导出相机坐标系与机器人坐标系、、轴之间的角度偏差分别为′90.67°、′＝90.48°、′1.13°．至此，影响分拣系统定位精度的角度偏差均已求出．

考虑上述角度偏差，分别测量圆形薄板在5组不同运动距离D′下，在终止位置处其圆心在机器人下的实际位置，如表1所示．然后通过式(38)求取圆心在机器人下的计算位置，并以两位置的平均误差是否在毫米级内来验证视觉定位及跟踪算法的准确性．最后让圆形薄板运动同样的距离，传送带速度不变，用传统的动态跟踪算法[21]进行相同的5组实验，并测量每组的圆心在机器人下的位置，与本文算法所得结果进行误差比较．

表1 本文算法和传统算法的数据及误差

Tab.1 Date and errors of the proposed algorithm and the traditional algorithm

通过两种算法对比，可以看到本文算法的平均位置误差为0.816mm，而传统方法平均位置误差为1.455mm，显然本文算法的定位精度较高，且该算法的误差均在毫米级以内，验证了视觉定位算法的精确性，使得机器人末端能够实时跟踪目标物体的位置，为后续机器人高精确抓取目标奠定了基础．

如图8所示，在此基础上，分别用本文算法和传统算法对连续的200个圆形薄板进行循环抓取实验，机器人抓放行程为30mm，传送带速度均为500mm/s，并且保证放置的每个圆形薄板间隔近似相等，传送带共循环5次，测量每一次从第1个目标进入相机视野到最后一个目标放置完成时刻所消耗的时间，实验结果如表2所示．

显然本文算法耗时较短，抓取200个圆形薄板系统平均用时约为98.80s，而传统方法系统平均用时约为101.70s．相比较下，本文算法提高了该机器人视觉分拣系统的运行效率．最后，对总共约500个圆形薄板进行2次相同的循环抓取实验，并记录总的误抓数和漏抓数，如表3所示．通过计算可得本文算法漏抓率为0，误抓率为0.2%，相比传统算法均有提高，能满足一般的工程需求．

表2 本文算法与传统算法循环抓取时间对比

Tab.2 Comparisons of cyclic catching time between the proposed and traditional algorithms

图8 Delta机器人连续抓取动态目标

表3 本文算法与传统算法进行5次循环实验的总误抓数与漏抓数

Tab.3 Error and miss catching number of the proposed and traditional algorithms in five cycling tests

5 结 论

(1) 本文针对高速并联机器人抓取动态物料的需求，提出了一种基于圆特征的单目视觉定位与传送带标定相结合的动态目标跟踪算法，该算法获取平面圆形目标物的轮廓图像坐标，通过构建的直椭圆锥的几何成像模型，求取了圆心深度信息，进而结合传送带标定方法，可以实时跟踪运动薄板圆心在机器人下的位置．

(2) 与传统方法相比，本文算法考虑了由于安装误差等原因导致的相机与传送带以及机器人与传送带之间的角度偏差，定位精度较高，符合实际工程中机器人视觉分拣系统的运用．

(3) 在视觉定位算法中将斜椭圆锥映射为直椭圆锥，运用直椭圆锥投影的几何关系推导出目标圆心的空间坐标，避免了复杂的高阶方程求解，加快了运算速度，提高了计算精度．

(4) 对本文算法和传统算法分别进行了圆形薄板抓取实验，对比了两种算法的位置误差和系统运行时间，并计算了漏抓率与误抓率．结果表明该算法的定位精度较高，耗时较短，系统的运行效率得到了提高，符合动态物料分拣的实际工程需求．在未来，希望该算法不仅限于具有圆形特征的目标物体，且能够应用到更多的领域中．

［1］ 冯李航，张为公，龚宗洋，等. Delta系列并联机器人研究进展与现状[J]. 机器人，2014，36(3)：375-384.

Feng Lihang，Zhang Weigong，Gong Zongyang，et al. Developments of Delta-like parallel manipulators：A review[J]. Robot，2014，36(3)：375-384(in Chinese).

［2］ 刘海涛，熊 坤，贾昕胤，等. 3自由度冗余驱动下肢康复并联机构的运动学优化设计[J]. 天津大学学报：自然科学与工程技术版，2018，51(4)：357-366.

Liu Haitao，Xiong Kun，Jia Xinyin，et al. Kinematic optimization of a redundantly actuated 3-DOF parallel mechanism for lower-limb rehabilitation[J]. Journal of Tianjin University：Science and Technology，2018，51(4)：357-366(in Chinese).

［3］ 倪鹤鹏，刘亚男，张承端，等. 基于机器视觉的Delta机器人分拣系统算法[J]. 机器人，2016，38(1)：49-55.

Ni Hepeng，Liu Yanan，Zhang Chengduan，et al. Sorting system algorithms based on machine vision for Delta robot[J]. Robot，2016，38(1)：49-55(in Chinese).

［4］ 查 英，刘铁根，杜 东，等. 基于机器视觉的零件自动装配系统[J]. 天津大学学报，2006，39(6)：722-726.

Zha Ying，Liu Tiegen，Du Dong，et al. Part automatic assembling system based on machine vision[J]. Journal of Tianjin University，2006，39(6)：722-726(in Chinese).

［5］ Wei Luosi，Jiao Zongxia. Research and application of visual location technology for solder paste printing based on machine vision[J]. Frontiers of Mechanical Engineering in China，2009，4(2)：184-191.

［6］ 赵 霞，袁家政，刘宏哲. 基于视觉的目标定位技术的研究进展[J]. 计算机科学，2016，43(6)：10-16.

Zhao Xia，Yuan Jiazheng，Liu Hongzhe. Advances in vision-based target location technology[J]. Computer Science，2016，43(6)：10-16(in Chinese).

［7］ Jia Yunwei，Liu Tiegen，Gao Lilan，et al. Mobile robot localization and navigation system based on monocular vision[J]. Transactions of Tianjin University，2012，25(5)：335-342.

［8］ 张子淼，王 鹏，孙长库. 单目视觉位姿测量方法及数字仿真[J]. 天津大学学报，2011，44(5)：440-444.

Zhang Zimiao，Wang Peng，Sun Changku. Position and orientation measurement method with monocular vision and digital simulation[J]. Journal of Tianjin University，2011，44(5)：440-444(in Chinese).

［9］ 许 勇，郭鹏宇，苑 云，等. 相机靶面安装误差标定方法[J]. 光学学报，2013，33(4)：139-147.

Xu Yong，Guo Pengyu，Yuan Yun，et al. Calibration method of installation error of camera sensor plane[J]. Acta Optica Sinica，2013，33(4)：139-147(in Chinese).

［10］ Forsyth D A，Munday J L，Zisserman A，et al. Projective. invariant representation using implicit algebraic curves[J]. Image and Vision Compute，1991，9(2)：130-136.

［11］ 闫 蓓，王 斌，李 媛. 基于最小二乘法的椭圆拟合改进算法[J]. 北京航空航天大学学报，2008，34(3)：295-298.

Yan Bei，Wang Bin，Li Yuan. Optimal ellipse fitting method based on least square principle[J]. Journal of Beijing University of Aeronautics and Astronautics，2008，34(3)：295-298(in Chinese).

［12］ 陈 丹，石国良. 基于视觉几何的传送带测速方法研究[J]. 仪器仪表学报，2016，37(10)：2307-2315.

Chen Dan，Shi Guoliang. Research on velocity measurement method of conveyor belt based on view geometry[J]. Chinese Journal of Scientific Instrument，2016，37(10)：2307-2315(in Chinese).

［13］ 邓明星，刘冠峰，张国英. 基于Delta并联机器人的传送带动态跟踪[J]. 机械工程与自动化，2015(1)：153-156.

Deng Mingxing，Liu Guanfeng，Zhang Guoying. Dynamic conveyor tracking for Delta robot[J]. Mechanical Engineering & Automation，2015(1)：153-156(in Chinese).

［14］ 贾丙西，刘 山，张凯祥，等. 机器人视觉伺服研究进展：视觉系统与控制策略[J]. 自动化学报，2015，41(5)：861-873.

Jia Bingxi，Liu Shan，Zhang Kaixiang，et al. Survey on robot visual servo control：Vision system and control strategies[J]. Acta Automatica Sinica，2015，41(5)：861-873(in Chinese).

［15］ 孙月海，王 兰，梅江平，等. 基于单目视觉的Delta机器人零点标定方法[J]. 天津大学学报：自然科学与工程技术版，2013，46(3)：239-243.

Sun Yuehai，Wang Lan，Mei Jiangping，et al. Zero calibration of delta robot based on monocular vision[J]. Journal of Tianjin University：Science and Technology，2013，46(3)：239-243(in Chinese).

［16］ Kruger L，Wohler C. Accurate chequerboard corner localisation for camera calibration[J]. Pattern Recognition Letter，2011，54(10)：1428-1435.

［17］ 解则晓，辛少辉，李绪勇，等. 基于单目视觉的机器人标定方法[J]. 机械工程学报，2011，47(5)：35-39.

Xie Zexiao，Xin Shaohui，Li Xuyong，et al. Method of robot calibration based on monocular vision[J]. Journal of Mechanical Engineering，2011，47(5)：35-39(in Chinese).

［18］ 朱 嘉，李醒飞，徐颖欣. 摄像机的一种主动视觉标定方法[J]. 光学学报，2010，30(5)：1297-1303.

Zhu Jia，Li Xingfei，Xu Yingxin. Camera calibration technique based on active vision[J]. Acta Optica Sinica，2010，30(5)：1297-1303(in Chinese).

［19］ 杨 强，刘冠峰. Delta机器人传送带与视觉的综合标定方法[J]. 机电工程技术，2015，44(1)：5-10.

Yang Qiang，Liu Guanfeng. Method of integrated calibration on Delta robot conveyor and vision[J]. Mechanical and Electrical Engineering Technology，2015，44(1)：5-10(in Chinese).

［20］ Zhang Z Y. A flexible new technique for camera calibration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，2000，22(11)：1330-1334.

［21］ 张文昌. Delta高速并联机器人视觉控制技术及视觉标定技术研究[D]. 天津：天津大学机械工程学院，2012.

Zhang Wenchang. Control Technique and Kinematic Calibrition of Delta Robot Based on Computer Vision[D]. Tianjin：School of Mechanical Engineering，Tianjin University，2012(in Chinese).

Dynamic Target Tracking Algorithm of a High-Speed Parallel Robot Based on Monocular Vision

Mei Jiangping，Wang Hao，Zhang Duo，Yan Han，Li Ce

(Key Laboratory of Mechanism Theory and Equipment Design，Ministry of Education，Tianjin University，Tianjin 300350，China)

Due to installation errors and other reasons，a certain angle deviation has existed between the visual system and the conveyor belt and between the robot and the conveyor belt，leading to a low-positioning accuracy of the target object in the visual system．This has made the robot end unable to catch its dynamic target accurately. Moreover，the traditional monocular vision positioning and tracking algorithm has had some problems，such as low accuracy and slow operation speed，affecting the calculation speed of the visual sorting system and reducing real-time system performance．Addressing these problems，a dynamic target tracking algorithm，based on a circular feature combining monocular vision positioning and a conveyor belt calibration is proposed．A circular target was shot by the camera without consideration of the angle of deviation between the camera coordinate system and the conveyor coordinate system，and a geometric imaging model of the straight elliptical cone，with its optical center as the apex，was constructed．On this basis，the depth information of the center of a circle in the camera coordinate system was obtained using its geometric relationship．Combining conveyor belt calibration and the Zhang Zhengyou calibration method，the ratio factor of the conveyor belt encoder and the angle deviation between the robot coordinate system and the conveyor belt coordinate system were determined．The position of the center of the circle under the robot was then tracked in real time．Finally，experiments were carried out on both the proposed algorithm and traditional algorithms．Results showed that this dynamic tracking algorithm can deduce the spatial coordinates of the center of a circle through the geometric relationship of conical projection，while avoiding solution of the traditional complex non-linear equation．The positioning accuracy and calculation speed were improved greatly．This algorithm can eliminate the angle deviation of traditional methods．The average positioning error was 0.82mm，achieving a higher positioning accuracy．The system consumed less time in the operation，the operation efficiency was effectively improved，and the rate of wrong catch and missing catch were 0.2% and 0respectively，which displayed good practical value．

monocular vision positioning；high-speed parallel robot；camera calibration；conveyor calibration；dynamic tracking

TP242

A

0493-2137(2020)02-0138-09

10.11784/tdxbz201903009

2019-03-11；

2019-05-05．

梅江平（1969—  ），男，博士，研究员.

梅江平，ppm@tju.edu.cn.

国家科技重大专项资助项目(2017ZX04013-001).

Supported by the Major National Science and Technology Program(No.2017ZX04013-001).

(责任编辑：金顺爱)