基于单目视觉的工业机器人末端轨迹测量方法

No.11Nov.2020
第11期2020年11月组合机床与自动化加工技术
Modular Machine Tool & Automatic Manufacturing Techninue
文章编号：1001 -2265(2020)11 -0060 -05
DOI # 10.13462/s. .nki. mmtamt. 2020.11.014
基于单目视觉的工业机器人末端轨迹测量方法
罗 哉,蔡泽亮，江文松，杨 力
(中国计量大学计量测试工程学院，杭州310018)
摘要：针对工业机器人末端轨迹测量精度不足问题，设计一套基于单目视觉的工业机器人末端轨迹 测量系统。

该测量系统以I 固EKTS4MCL 固80M/C 工业相机采集图像，以PC 平台作为上位机,C ++ 语言和OpenC V 库编写测量程序，棋盘格靶标作为末端实时位姿测量靶标。

当工业机器人运动时， 测量系统能对固定于机器人末端的棋盘格标靶进行实时拍摄。

通过VulcanfL PCIe 采集卡实时读 取和处理图像。

利用该视觉测量系统，对机器人末端轨迹进行精确测量，结果表明：基于单目视觉 的工业机器人末端轨迹测量系统的位移精度达到0.315mm ，旋转精度达到0.365。

，说明该文方法能 精确地动态测量机器人的末端轨迹。

关键词：单目视觉；工业机器人;位姿测量；OpenCV 中图分类号:TH165 :TG659 文献标识码:A
Endoscopic Attittde Measpremedt Mettod for Industrial Robot Basee on Monocclar Vision LUO Zag CAI Zvliang , JIANG Wen-1ona , YANG Li
(College of Metrology & Measurement Engineering & China JiPang University & Hangzhou 310018 & China ) Abstract : Aiming at the tnsufaci.ent accuracy of the end trajectory measurement of industial Tobots , a set
of end eajectory measurement system for industial robote based on monocular vision was designed. This
measurement system uses I-TEK TS4MCL-180M/C bidusteal camera to collect images & uses PC platform
as host computer , C ++ language and OpenCV library to write measurement program , checterboard target as end real-time poshre measurement target. When the induseial robot moves & the measurement system can take a real-time shot of the checterboard target fixed at the end of the robot. Read and proces s images in realimewih Vulcan-CL PCIecap)urecards.Thevision measuremen)sysem isused )o accuraely
measure)heend raeec)ory of)herobo .Theresulsshow)ha))heend raeec)ory measuremen)sysem of)he
indusrialrobo)based on monocularvision achievesadisplacemen)accuracy of 0. 315mm and aro aion ac ­curacy of 0. 365 °, indicating that the method in this paper can accurately measure the robot dynamically end hrack.
Key words : monocular vision ； induseial robot ； eajectory measurement ； OpenCV
0引言
视觉测量技术是通过机器视觉技术将三维空间中 的目标投影到二维图像内，然后通过数学模型将其重
新还原为三维信息的技术，能完成空间物体的几何尺 寸、位置信息及姿态信息等的测量⑴。

位置和姿态简 称为位姿,通常用6个自由度来表示，即两个坐标系之 间的3个旋转量和3个平移量⑵。

目前，位姿测量已 经广泛地应用于航空航天、目标跟踪、增强现实、自动 导航、视觉伺服控制和工业制造等领域［3'5］。

工业制造
中基于模型的姿态估计常用于工业机器人定位,工业 机器人定位精度将影响到后续生产安全、产品质量等 一系列问题，因此机器视觉测量工业机器人轨迹具有
重要意义。

基于模型的单目视觉定位方法是基于模型的定位
方法中的一种，为了方便图像处理，通常采用较为简单 的几何特征构成模型，如点、直线、二次曲线等。

根据 几何特征不同，可将基于模型的定位方法分为基于点
特征定位,基于直线特征定位和基于曲线特征定位三 类［6］o 现有方法中,部分算法测量精度高，但在计算速 度上存在不足;部分算法具有极高的图像处理速度，但 处理结果精度难以满足要求。

文中主要采用共面4点的点特征定位方法，基于 共面4点的空间位置关系求解出物体在相机坐标系下 的位姿。

文中算法简单有效、易于实现、精度高，具有 一定的应用价值。

收稿日期：2020-01 -02;修回日期：2020-02-18
*基金项目：国家自然科学基金(51927811,51675499);国家重点研发计划项目(2017YFF0206306, ZLJC 1703 - 6,2019YFB2004900);浙江省自然科
学基金(LQ20E050016)
作者简介:罗哉(1979—),男&四川遂宁人冲国计量大学教授&博士&研究方向为精密测量技术，(E-mail ) luozai@cjlu. 。

2020年11月罗哉，等：基于单目视觉的工业机器人末端轨迹测量方法-61-
1单目视觉测量模型
在单目量系统中，拍到的图片由
集卡发送到PC端后，表示为矩阵形式，矩阵中各元
素的值为图度，矩阵中各元素的行和列信息即为
图素的坐标,通常以此矩阵为依据建立像素坐标
系。

图上角的作为像素坐，横
向与纵向分别为像素坐标的Q轴与”轴。

在立像素
坐标后，还需要建立图像物理坐标系，以元尺寸
为，以图像中心为。

，还需要建立相机坐
系及物体世界坐标系。

系统棋盘格标靶为目标物，通量棋盘格
靶姿来间接得到与之的工业机器人姿
信息,进而通到工业机器人轨迹信息。

小孔成型，坐关系如图1所示。

图中。

9为相机光心,0。

为相机图像像素坐标原点，O m为相
机图像主点。

摄像机坐标系中,c、e轴分
别平行于图像物理坐标0,-3的34轴和像素坐标
O-Q”的Q、8轴;'轴为摄像机光轴，垂直于图像平
面。

假设空间中在坐标系中坐标为(C，
e，')T，在世界坐标系下坐标为(c，e，z”)T，则二
者关系可由以下公式表示：
(C,
=7(
)”+8
1'丿
、'”丿
(1)式中，7为世界坐标系与坐标系的旋转矩阵，是3x3的交矩阵;8为世界坐标系与坐标系的平移向量，是3x1的向量。

相机
图1相机成像模型图
靶坐标系的可由世界坐标系原在坐标系下的坐标表示，即假设世界坐标系原坐标为"，其在坐标系下的坐标可表示为8= 7)0,0，1* t+8；同，靶标在坐标系下的姿态可由世界坐系坐系的转矩阵表示，即假设世界坐标系c”，e,z”轴上向量变换坐系坐标轴的向量分别为71,72,73,7j=7[1,0, 0]T,72=7[0,1,0]T,73=7[0,0,1*T,步7= [71,72,73]=79'
一组向量在两个坐标系下变换时,各个向量的模不变，各向量之间的角度，世界坐标系下C, e,z”轴上的向量变换到坐标系，其
换矩阵也可表示为靶的姿态。

已通到，取标靶上距离相等的，4个点作为特征点,其在世界坐标系下的坐标为(C”-e”-Z”,)，(z=1,2,3,4"，可根据靶标特征点实际间距得到；在相机坐标系下的坐标为;(c,e,'), (i-1,2,3,4)；在像素坐标系下坐标为a(Q-8,),(i= 1,2,3,4)。

面即可求解目标物相对于相机的初始位置/和姿态V
2位解算法
根据征在图中的素坐及其在世界坐
系下的坐标，要到世界坐标与坐标的
和姿态，需要求征点在坐标系的坐标o 2.1约P4P解析
面目征点分方形4；~；
处，世界坐标系原点位于正方形中心点；处,C轴、e轴分别与&&、&&2重合，如图2所示。

图2空间特征点透视关系示意图若正方形边长为=,征点；~；4在世界坐标系下坐标分别为；”-(槡2=,0,0),;”-(0,槡2=,0),；”=(-槡槡=,0,0),;=(0,-槡2=,0)。

设特征点
；~；4与坐；在化面上投分别为P1~P4、P0,可通素方到各在相机坐标下的坐标p,(3,4-1"。

令-O c P i|，空间特征点P,的深度记为"+-O C P<\，空间特征点；的相机坐标记为；=(3,4“,)，则3="叫/—,
4“-"4/—,z“-。

由于4个特征点；~；4方形的4,则任意三点与相机光心0,组成的三棱锥体积都相等,可表达式:_
V1=0X-(OX y IX)|/6=///V/(6M1-2-3)
V=0X-(OX Y IX)|/6=///V/(6-1-2-4)
'V=OX-(IX y OX)|/6=///V/(6-1-3-4)■[4=-(0。

；3Y0°P4)|/6=///V4/(6-2-3-4)
(2)其中,
V1=131(42-43)+41(3-3)+343-3/
V=31(42-44)+41(3-3)+344-342
V=31(43-44)+41(3-3)+344-343
V=3(43-44)+42(3-33)+344-343(3)棱锥体积[=[-[-[,可知/与/、/、/之间关系为
：
・62・组合机床与自动化加工技术第11期
—=—2化—=—3化—=L化(4)
2——V41&3——V41&4——V41!"
又由空间约束|&&21二=得：
=2=(-31)2+(4*-4*)2+(—-—1"2(5)式(4)、式(5)可征点；(心1〜4)的深度信息"为:
—=__________________-/5—=____________________ ](32V3-31V4)2+(42V3-41V4)2+(V3-V4)2
(6)
根据深度信息—可得特征点P(，=1〜4)的相机坐标为：
'3V5
X
*二--/槡
(32V3-31V4)2+(42V3-41V4)2+(V3-V4)'
4-=
y*=
槡]32V3-31V4)2+(42V3-41V4)2+(V3-V4)'—=____________________V—=_____________________ '](32V3-31V4)2+(42V3-41V4)2+(V3-V4)'
(7)
根据上述推导可知靶标上4征点P1~；4的世界坐标；+与相机坐标；*已可求得。

2.2绝对定向问题求解
绝向是指由一组空间点在间直角坐标系下的三维坐标，求解这两个坐标系之间的变换关系[7]o
设其中坐标系下的三维点集为.P1,5,0, P”1,的坐标系下的点集为.A,A,0,A”/异之间有如下刚体变换关系：
A=75+8(8)求解绝向是根据对应点坐求解
转矩阵7与平移向量t的过程，即可将绝对定向为约束最优化问题：
J m C n"II75+t-A A I2
〔s.I7T7=!
(9)
针对上述化，已多式解法)8池2*,各
基，的基于SVD分解的，为：__
集质心坐标分别为P与A,则：
5+"5,A=+家(10)对两组点集去中心化：_
P=Pi-P，A=A-A(11)然后，计算协方差矩阵：
"=占"(5-P)(Ai-A)T(12)
”#1
求得协方差"后对其进行SVD奇异值分解,(-, >,?)=SVD(""，所果中，-,?为3X3正交矩阵,>为3x3矩阵。

由可得式9中的最优满足：
{7=argmax"(7")⑴)
二A_7p
即所转矩阵的转与协方矩阵矩阵的迹应为最大，而移矩阵则由所求转矩阵得到。

结合SVD奇异值分解结果,可得：
7=UV T(14)可求出每图片的7,8,通多张图片7,8的计算,即可得到不同图片间棋盘格标靶的位姿化。

3系统搭建
3.1位移精度标定系统
设计了高精度二维移动平台与旋转平台，用于本文方法的精度。

二维
AeroTech公司的高精度二维气
移动，其重复位移精度为
0.01%m。

位移系统中，棋格标靶
维上
在工作上的标靶进
行拍摄，通馈位移数据
与程移数据得到单目：
量系统的移精度。

位移精度标
定系统实物图如图4所示。

3.2旋转精度标定系统
旋转精度标定实验系统(图5)
由工业相机、高精度旋转平台、PC上位机、相机固定、棋格标靶等组成。

旋转
Accoetech ROND-
COM S4柱度测量转平
台，其重复旋转精度为0.1°
转精度系统中几固
上&棋格靶
转，并随平台旋
转&上通拍转
前后靶标的图像得到数
图5旋转精度标定系统设计据，程后得到测得的标靶旋转角度，将
量结果与高精度旋转所显示旋转角度结果得到程量的旋转角度精度。

4位姿解算流程
根据上述，工业机器人末端位姿测量系统的具体测量步骤：
(1)将工作拍摄位置，调整好相机焦距等相关参数。

(2)拍图片，友标定算法对标定图片进行，获。

2020年11月罗哉"等：基于单目视觉的工业机器人末端轨迹测量方法・63・
(3"获拍摄的标靶图片，并图片进行预。

(4"在图像中选取4征，并得到特征点之间距离与化平面中的坐标P1~P4。

(5"根据特征点间距到其在世界坐标系中的坐标；，并根据式(7)求得其在相机坐标系中的对应坐标；。

(6)到征在世界坐系与坐系的坐标后，上述求解绝向方到两坐标集之间的位移旋转关系矩阵7,8。

流程图如图6所示。

謹
*
w
图9y轴位移精度数据折线图
0.051——,——|----,——,——,——,
0.045-|—•—谢测量误差~
004.|T-剧年药测量误差|
图6机械臂末端轨迹求解流程图
5
3
5
2
5
1
5
03
0.0
02
0.
01
0.0
00
O
o.
o.
o.
糊
兆
删
®
5实验与数据分析
为上述量精度及，设计了位移精度量及转精度量'
5.1位验
为得到试验系统的重复位移测量精度，搭建了重
%24681012141618202224
移动距离
图10Z轴位移精度数据折线图
由数据分析可得,c、e、z轴最佳测量环境下测量平均分别为0.023mm,0.023mm,0.022mm。

5.2角度精度测量实验
同理，为到系统的重复转量精度，搭建了如图5所示的重复转角测量精度实验平台。

c、e、z 各轴每转动2。

为间隔，每拍摄5张原始图像，经过计到测量结果及其表1。

复移精度,图4
所示。

将棋盘格标靶：于
AecTechABL1000高精度气浮
维上，气浮
,拍靶图,
根据气 维的反馈得
到当前与靶标的相对位
移作为真值，中靶标每移
动2mm为,
拍24的图,每拍10图像作为数据源，将10图的均值作为测量值与真值进行&图像及数据如图7所示。

对图7中数据进一步处理,得到图8。

24681012141618202224
移动距离
图8X轴位移精度数据折线图
表1角度精度测量数据
转轴"轴/(°,轴/(。

)2轴/(。

)
真值7.1-3.210.3测量均值7.2-3.310.4
误差值0.10.10.1
真值9.1-1.28.3测量均值9.2-1.38.4
误差值0.10.10.1
真值11.1 1.2 6.3测量均值11.3 1.4 6.5
误差值0.20.20.2
真值13.1 3.2 4.3测量均值13.3 3.5 4.5
误差值0.20.30.2
真值15.1 5.2 2.3测量均值15.3 5.5 2.5
误差值0.20.30.2
真值17.17.20.3测量均值17.37.50.5值0.20.30.2
由表1数据分析可得,"、,、2三轴测量平均误差分别为0.167。

,0.217。

,0.167。

同理可得e、z轴测量数据与分析如图9、图10所示。

-64-组合机床与自动化加工技术第11期
5.3机械臂末端位姿测量实验
设计了基于棋盘格靶标的机械臂末端位子测量实验，将高精度棋盘格靶械臂末端，通部的单目工业拍摄靶标图像，通图像的解算得到机械臂在图像位置时的位姿。

6轴工业机器人作为
测量，lek公司
TS4MCLB80M/C工业相机，其分辨
率为2048x2048像素，最高为
180fps@10tap,距为
25mm的可调光圈镜头，其工作距离图11位姿测量实为500mm o测量果如图11~图13验系统设计图
所示。

图12机械臂运动轨迹测量实验原图
15-11.6
图13机运动验图
由上述可知，本系统在量环境中能够成测量机械臂运动轨迹的功能，并精度保
为量地分析轨迹测量的测量精度，定义合成E为：
E=2E+E；+E；(15)其中,E3，E r，E2分别为机械臂末端在3、y、z轴的位移量均值。

由式(15)可求量环境中测量合成误差为0.315mm，达到工作位姿测量需求。

6结束语
本文设计了基于单目相机与棋盘格靶标的工业机器人姿测量系统，实现工业机器人■爪具的外部位姿测量；果表明：该系统能够高效、确工业机器人姿进行评判，测量系统设计，其好,可广泛工业机器人外部姿测量。

[参考文献]
[I]王中宇，李亚茹,郝仁杰，等.基征的单目视觉位姿测
量算法[J].红外与激光工程，2019,48(5)#169-176.
[2]Jia Hou,Hade n g Li，Zhencng Zheng,et al.Distortion cor­
rection for imaging on non-planar suCace using freeform lens [J].Optics Communications,2011,285(6)：986-991. [3]Edward Rosten,Rohan Loveland.Camera distortion8611-1x1-
ibction using the plumb-1ine constraint and minimalhough entropy[J].Machine Vision and Applications,2011,22
(1)：77-85.
[4]汪启跃，王中宇.基于单目视觉的航天器位姿测量)J].应
用光学,2017,38(2)：250-255.
[5]王强，程红，杨-，等.全局图像配准的目标快速定位方
法[J].红外与激光工程,2015,44(S1)：225-229.
[6]秦娟，王挺，刘.计目[M].北京：
国防工业出版社,2016.
[7]鑫.基的位姿估计与点目标运动测量方法研究
[D].长沙：国科学技术大学,2015.
[8]Arun K S,Huang T S,Blostein S D.Least squares fitting of
two3-D point sets[ J].IEEE Trans Pattern Anal Mach In­tell,1987(5):698-700.
[9]Horn B K P.Closed-form solution of absolute orientation u­
sing quaternions[J].Int.J.Opt.Soo.Amer.AD,1987, 4(4)：629-642
[10]Horn B K P,Hildren H M,Negidaripour S.Closed液c so-
luhon of absolute orientation using orthonormci1”6匚(06[J].
Inh J.Oph Soo.Amer.AD,1988,5(7)：1127-1135.
[II]Kanatani K.Analysis of3-D rotation fitting[J].Pattern Analysis&Machine Intelligence IEEE Transactions on, 1994,16(5)：543-549.
[12]Umeyama S.Least-squares estimation of transformation pa­
rameters between two point pxterns[J].IEEE Trans pi t a-nai mach intel i,1991,13(4)：376-380.
(编辑李秀敏)
(上接第59页)
[13]Huang J,Zhu L M.Feedrate scheduling for interpolation of
parametric tool path using the sine series representation of jerk profile[J].Proceedings of the Institution of Mechani­cal Engineers Part B Journal of Engineering Manufacture, 2017,231(13)：2359-2371.
(编
辑李秀敏)。