专利名称：基于色标块的视觉实时测量方法
技术领域：
本发明属于机器人领域中的视觉测量，具体地说是用于机器人位姿测量的方法。
背景技术：
目前比较常用的视觉测量方法主要包括双目立体视觉和基于η点透视 (Perspective-n-Point,PnP)算法的单目视觉。由于双目立体视觉在每次测量时需要对两 幅图像进行处理，并且需要进行特征点匹配，所以其实时性较差。单目视觉每次测量仅对一 幅图像进行处理，其实时性比双目立体视觉高(徐德，谭民，李原，机器人视觉测量与控制， 国防工业出版社，第130页，第140页，2008)。在采用尺寸已知的矩形色标块作为机器人的标记，特征点的提取即为矩形色标块 顶点的提取。目前比较常用的特征角点提取算法为哈里斯(Harris)角点检测算法、最小 核值相似区(Smallest Univalve Segment Assimilating Nucleus，SUSAN)角点检测法，高 斯差分(Difference-of-Gaussian，DoG)算子检测法等。上述算法在实时性较差，在色标 块一个顶点被遮挡时，不能检测出该顶点。采用先求矩形色标块四条边缘直线，然后通过直 线求交点获得特征角点的方法，可以有效地解决一个顶点被遮挡时的顶点特征提取问题。 虽然目前有很多种边缘提取算法，如罗伯特(Robert)边缘算子、坎尼(Carmy)边缘算子、 索贝尔(Sobel)边缘算子、克瑞斯(Kirsch)边缘算子和普瑞维特(Prewitt)边缘算子等， 然而这些边缘提取算法一般是对整幅图像进行处理，实时性不高(Zhang，Yang ；Rockett, P. I."The Bayesian Operating Point of the Canny Edge Detector,"IEEE Transactions on Image Processing, Vol. 15，No. 11，pp. 3409-3416，2006)。在获得边缘点后，利用霍夫 (Hough)变换提取直线，其实时性也较差。此外，目前的方法是在像素级精度边缘点的基础 上得到的直线，求交点得到的特征点精度不高，影响了基于PnP方法视觉测量的精度，且测 量结果中的姿态矩阵也不能保证是正交的。

发明内容
为了解决现有技术中利用交点得到的特征点精度不高，影响了基于PnP方法视觉 测量的精度，且测量结果中的姿态矩阵也不能保证是正交的技术问题，本发明的目的在于 提供用于机器人位姿高精度实时视觉测量、保证测量结果中的姿态矩阵的正交性的一种基 于色标块的实时视觉测量方法。为实现上述目的，本发明提供基于色标块的实时视觉测量方法解决问题的技术方 案包括步骤如下步骤Sl 在机器人的背部上方设有一个已知尺寸的色标块；步骤S2 以单摄像机采集色标块的色标图像；步骤S3 跟踪色标块的三点，所述三点是中心点、一条长边中点和一条短边中点， 确定色标块图像区域；步骤S4 对色标图像按照设定的间隔和方向扫描处理，获得具有亚像素精度的边缘占.步骤S5 将亚像素精度的色标块边缘点经最小二乘拟合，获得四条边缘直线；步骤S6 求四条边缘直线的交点，获得四个顶点的图像坐标；步骤S7 计算色标块新的中心点、新的一条长边中点和新的一条短边中点；步骤S8 利用η点透视算法，求取机器人位姿；步骤S9 利用正交迭代算法对机器人位姿进行优化，得到机器人的姿态矩阵正交 的位姿，循环测量则返回S3，单次测量则结束。本发明的有益效果本发明跟踪色标块三点确定色标块的图像区域，在此区域内 按照一定间隔扫描出色标块的四组粗略边缘点，拟合出四条边缘直线，解决了边缘直线提 取的实时性问题；本发明经过坐标变换，分别以各条边缘为纵坐标轴，按照边缘的横坐标计 算业像素边缘点，从而将二维图像平面内的亚像素边缘点求取转换为在一维空间内的亚像 素边缘点求取，既保证了亚像素边缘点的精度，又保证了算法的效率；本发明利用亚像素边 缘点拟合出精确的边缘直线，求这些直线的交点得到色标块四个顶点的高精度图像坐标， 解决了单摄像机视觉测量精度低的问题，解决了色标块一个角被遮挡时不能测量的问题； 利用PnP和OI算法计算机器人的位姿，解决了测量结果中姿态矩阵的正交性问题。本发明在色标块的一个顶点遮挡时仍然能够精确获得机器人的位姿；本发明能够 保证机器人位姿中的姿态矩阵的正交性；本发明实时性好，对于640X480像素的图像，能 够实现70次/秒测量；本发明测量精度高，在摄像机距离目标3000mm左右时的测量误差小 于10mm。因此，本发明可以大幅度提高视觉测量的可用性。


图1为单目视觉测量机器人位姿示意图。图2A为本发明基于色标块的视觉实时测量方法的流程图。图2B为本发明获得具有亚像素精度的边缘点的流程图。图3为按照一定间隔和方向快速扫描边缘点示意图。
具体实施例方式为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照 附图，对本发明进一步详细说明。请参阅图1示出单目视觉测量机器人位姿示意图，利用机器人1、色标块2、摄像机 3、网络线4和计算机5，本发明利用粘贴到机器人背部上方的色标块作为机器人的标记，针 对色标块提出了一种新的图像处理和特征提取方法，以快速获得矩形色标块四个顶点的高 精度图像坐标。如图2A示出本发明基于色标块的视觉实时测量方法的流程图，其步骤包括如下步骤Sl 在机器人1的背部上方设有一个已知尺寸的色标块2 ；步骤S2 以单摄像机3采集色标块2的色标图像；步骤S3 跟踪色标块2的三点，所述三点是中心点、一条长边中点和一条短边中 点，确定色标块2图像区域；步骤S4 对色标图像按照设定的间隔和方向扫描处理，获得具有亚像素精度的边缘占.步骤S5 将亚像素精度的色标块2的边缘点经最小二乘拟合，获得四条边缘直 线.
一入 ，步骤S6 求四条边缘直线的交点，获得四个顶点的图像坐标；步骤S7 计算色标块2新的中心点、新的一条长边中点和新的一条短边中点；步骤S8 利用η点透视(Perspective-n-Point^nP)算法，求取机器人1的位姿；步骤S9 利用正交迭代(Orthogonal Iteration, 01)算法对机器人1的位姿进行 优化，得到机器人1的姿态矩阵正交的位姿，循环测量则返回S3，单次测量则结束。如图2B示出获得具有亚像素精度边缘点的步骤如下步骤S41 在步骤S3确定的色标块图像区域内，按照设定间隔和方向快速扫描边 缘点，获得对应于色标块四条边缘的四组边缘点；步骤S42 针对四组边缘点，利用随机采样一致性(RANdom SAmple Consensus, RANSAC)方法去除偏差较大的噪声点，获得四组有效边缘点；步骤S43 针对四组有效边缘点，利用最小二乘拟合出四条边缘直线；步骤S44 针对四条边缘直线分别进行坐标变换，依次使得各条边缘为纵坐标轴；步骤S45 分别针对为纵坐标轴的各条边缘上的有效边缘点，求取具有亚像素精 度的边缘点。其中，所述亚像素精度的边缘点采用如下公式求取
χ. + χ, + 2χη σ2 ln(/n //,) σ2 ln(/n /17)μ =———!----—----—~—，
4zKX1-X0) 4(χ2 -Χ0)式中，μ为具有亚像素精度的边缘点，Xtl为原边缘上一点的横坐标，X1为原边缘上 外侧距离一个像素点的横坐标，X2为原边缘上外侧距离两个像素点的横坐标，σ为预先统 计获得的摄像机成像的近似高斯分布方差，Itl为对应于Xtl的原边缘上一点的像素灰度值， I1为对应于X1的原边缘外一点的像素灰度值，I2为对应于X2的原边缘外一点的像素灰度值。其中，所述设定间隔的范围设为10至20像素。本实施例中间隔设置为10个像素。其中，所述色标块是矩形色标块或是四边形色标块。实施例中色标块2选用了红 色，但也可以选择其他颜色。其中，所述摄像机是彩色摄像机或是黑白摄像机。请参阅图2Α和图3。在图像处理中，首先跟踪色标块2中心点Omm、一条长边中点 Pum和一条短边中点pvm。
Pvm= (Plm+P2m)/2Pum= (P2m+P3m )/ 2⑴
Omm =(P]m+p2m+P3ra+P4m )/4其中，Plffl, P2ffl, P3ffl, P4ffl是上一次获得的色标块2的四个特征顶点。根据上一幅色标图像中中心点0_、长边中点Pim和短边中点Pvm的图像坐标，向四 周扩展一定区域作为当前色标图像中色标块2所在的图像区域。在图像区域内按照10个 像素的间隔，分别按照中心点0_到长边中点Pim的方向和中心点Omm到短边中点Pvm的方向， 扫描获得对应于色标块2的四条边缘PlmP2m、P2mP3m、P3mP4m、P4mPlm上的四组边缘点{EGn，i =1,2, ...，NJ、{EG2i，i = 1，2，...，N2}、{EG3i，i = 1，2，…，N3}、{EG4i，i = 1，2，...，N4}，其 中HN3和N4分别是四组边缘点的个数，经过RANSAC去噪后得到四组有效边缘点{EG1J; j = 1,2, -,Nsl}, {EG2J, j = 1,2, -,NsJ, {EG3J, j = 1,2, ···，Ns3}、{EG4j，j = 1，2，…， Ns4I，其中Ns1、Ns2、Ns3和Ns4分别是四组有效边缘点的个数，利用有效边缘点拟合出四条边缘
自:^^ L]_、1^2 Λ Lg Λ L^ ο根据每条边缘直线1^、1^、1^、1^4方向，分别对四组有效边缘点{EGlj;j = l，2，…， NsJ、(EG2j, j = 1,2,…，NsJ、{EG3J, j = 1,2,…，Ns3}、{EG4J, j = 1,2,…，NsJ 进行旋转 变换，依次使得各条边缘直线作为变换后的纵轴。根据预先统计获得的摄像机3成像的近 似高斯分布，沿横轴即垂直于各条边缘的方向，分别求取具有亚像素精度的边缘点。以边缘直线L1和有效边缘点(EGlj = (Ulj, Vlj)，j = 1，2，-,NsJ为例，说明亚像 素精度的边缘点的求取。按照直线L1的方向进行旋转变换，使得直线L1的方向作为变换后 的纵轴。经过变换后，有效边缘点EGlj的图像坐标(Ulj，Vlj)被变换为(xljCI，ylj(l)。根据变 换后有效边缘点EGlj的图像坐标(xljCI，y1J0)以及外侧图像点的坐标，按照下式计算有效边 缘点EGlj变换后的亚像素精度的图像横坐标
权利要求
1.一种利用色标块的实时视觉测量方法，其特征在于，该方法包括步骤如下 步骤Sl 在机器人的背部上方设有一个已知尺寸的色标块；步骤S2 以单摄像机采集色标块的色标图像；步骤S3 跟踪色标块的三点，所述三点是中心点、一条长边中点和一条短边中点，确定 色标块图像区域；步骤S4:对色标图像按照设定的间隔和方向扫描处理，获得具有亚像素精度的边缘点。步骤S5 将亚像素精度的色标块边缘点经最小二乘拟合，获得四条边缘直线； 步骤S6 求四条边缘直线的交点，获得四个顶点的图像坐标； 步骤S7 计算色标块新的中心点、新的一条长边中点和新的一条短边中点； 步骤S8 利用η点透视算法，求取机器人位姿；步骤S9 利用正交迭代算法对机器人位姿进行优化，得到机器人的姿态矩阵正交的位 姿，循环测量则返回S3，单次测量则结束。
2.如权利要求1所述利用色标块的实时视觉测量方法，其特征在于，获得具有亚像素 精度的边缘点的步骤如下步骤S41 在步骤S3确定的色标块图像区域内，按照设定间隔和方向快速扫描边缘点， 获得对应于色标块四条边缘的四组边缘点；步骤S42 针对四组边缘点，利用随机采样一致性方法去除偏差较大的噪声点，获得四 组有效边缘点；步骤S43 针对四组有效边缘点，利用最小二乘拟合出四条边缘直线； 步骤S44 针对四条边缘直线分别进行坐标变换，依次使得各条边缘为纵坐标轴； 步骤S45 分别针对为纵坐标轴的各条边缘上的有效边缘点，求取具有亚像素精度的 边缘点。
3.如权利要求2所述利用色标块的实时视觉测量方法，其特征在于，所述亚像素精度 的边缘点采用如下公式求取
4.如权利要求1所述利用色标块的实时视觉测量方法，其特征在于，所述设定间隔范 围为10至20像素。
5.如权利要求1所述利用色标块的实时视觉测量方法，其特征在于，所述色标块是矩 形色标块或是四边形色标块。
6.如权利要求1所述利用色标块的实时视觉测量方法，其特征在于，所述摄像机是彩 色摄像机或是黑白摄像机。
全文摘要
本发明是基于色标块的视觉实时测量方法，步骤S1在机器人的背部上方设有已知尺寸的色标块；步骤S2以单摄像机采集色标块的色标图像；步骤S3跟踪色标块的中心点、长边中点和短边中点确定色标块图像区域；步骤S4对色标图像按照设定的间隔和方向扫描处理获得具有亚像素精度的边缘点；步骤S5将亚像素精度的色标块边缘点经最小二乘拟合获得四条边缘直线；步骤S6求四条边缘直线的交点获得四个顶点的图像坐标；步骤S7计算色标块新的中心点、新的一条长边中点和新的一条短边中点；步骤S8利用n点透视算法求取机器人位姿；步骤S9利用正交迭代算法对机器人位姿进行优化，得到机器人的姿态矩阵正交的位姿。
文档编号G01C11/00GK102128617SQ20101059289
公开日2011年7月20日 申请日期2010年12月8日 优先权日2010年12月8日
发明者任艳青, 张正涛, 徐德, 杨平, 谭民, 陈国栋 申请人:中国科学院自动化研究所