CN109048911B - 一种基于矩形特征的机器人视觉控制方法 - Google Patents
一种基于矩形特征的机器人视觉控制方法 Download PDFInfo
- Publication number
- CN109048911B CN109048911B CN201811009651.1A CN201811009651A CN109048911B CN 109048911 B CN109048911 B CN 109048911B CN 201811009651 A CN201811009651 A CN 201811009651A CN 109048911 B CN109048911 B CN 109048911B
- Authority
- CN
- China
- Prior art keywords
- camera
- deviation
- rectangle
- formula
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000003384 imaging method Methods 0.000 claims description 55
- 239000011159 matrix material Substances 0.000 claims description 52
- 230000008859 change Effects 0.000 claims description 38
- 238000010606 normalization Methods 0.000 claims description 20
- 238000013519 translation Methods 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 abstract description 10
- 230000008569 process Effects 0.000 abstract description 5
- 238000000605 extraction Methods 0.000 abstract description 3
- 238000005286 illumination Methods 0.000 abstract description 3
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种基于矩形特征的机器人视觉控制方法,该方法以矩形为视觉特征,利用矩形的顶点、面积,以及对角线的夹角进行视觉控制;根据矩形顶点当前坐标和期望坐标的偏差,得到机器人的位置偏差,根据矩形当前面积和期望面积的偏差,以及对角线当前夹角和期望夹角的偏差,得到机器人的姿态偏差,最终实现机器人的六自由度控制;将平移运动和旋转运动进行了解耦,控制过程更加稳定可靠;解决了传统视觉控制因对点、边缘、直线等简单特征进行提取,易受目标物体的形状、纹理、噪声、光照条件等影响的问题。
Description
技术领域
本发明属于机器人控制领域,更具体地涉及一种基于矩形特征的视觉控制方法。
背景技术
随着工业领域对自动化水平要求的不断提高,机器人面临的任务也越来越复杂多样。为了提高机器人的学***,使其能够在动态不确定环境下工作,必须为机器人本体配备获取外部环境信息的各种传感器。当前,视觉传感器因其信息量大、适用范围广、非接触性等特点被广泛应用于机器人控制当中,而基于视觉的机器人控制逐渐发展成为机器人领域最活跃的研究方向之一。
基于视觉的机器人控制利用视觉信息作为反馈,间接检测机器人当前位姿或机器人相对目标物体的位姿,实现机器人的定位控制或者轨迹跟踪。因此,图像特征选择是基于视觉的机器人控制***的基础和关键。传统视觉控制过程中,点、边缘、直线是常用特征。此类特征提取简单,使用方便,但包含的特征信息有限,且易受目标物体的形状、纹理、噪声、光照条件等影响。
为了提高视觉控制的鲁棒性,研究人员探索使用更加复杂的目标特征(如四边形)进行机器人控制。Kawamura等以构建的四边形为特征,根据四边形虚拟质心当前图像坐标和期望图像坐标之间的偏差,实现户外自主移动机器人的运动控制(Kawamura H.,IwataS.,Sahashi S.,Hasegawa T..Visual attitude control using a virtual varycenterof a quadrangle that constructed from feature points for outdoor autonomousmobile robots[C].International Symposim on Micro-Nano Mechatronics and HumanScience(MHS).Nagoya,Japan.November 4-7,2012:316-318.)。然而,该方法只实现了机器人的三维姿态控制,难以实现位置的调整。刘康利用固定的合作矩形目标,通过K-Means算法进行聚类,得到矩形的四个角点特征,用PNP算法对姿态进行解算,并在三轴机器人平台上进行了验证。(刘康.基于视觉的机器人姿态测量[D].硕士学位论文,北京:北京邮电大学,2016.)。该方法中,PNP算法需要已知目标的空间坐标,真实环境中此条件不容易达到。胡敦利等在提取工件轮廓的基础上,应用Graham算法得到工件轮廓的最小外接矩形,在建立的坐标系下求解出工件的实际坐标位置,实现手机主板的抓取和放置(胡敦利,武贝贝,康鑫.基于机器视觉的手机主板定位方法.工业控制计算机,2017,30(2):58-62.)。该方法利用轮廓拟合出最小外接矩形,特征精度不高,因此***的定位精度也难以保证。
发明内容
基于上述背景,本发明的主要目的在于提供一种基于矩形特征的机器人六自由度控制方法。
为达到上述目的,本发明以矩形为视觉特征,利用矩形的顶点、面积以及两条对角线的夹角进行视觉控制。根据顶点当前坐标和期望坐标的偏差,得到机器人的位置偏差,根据当前面积和期望面积的偏差,以及对角线当前夹角和期望夹角的偏差,得到机器人的姿态偏差,最终实现机器人的六自由度控制。
该方法包括步骤如下:
步骤S0,使用一个RGB-D相机获取目标图像;
步骤S1:从所述相机获得的目标图像中提取矩形特征,离线得到所述四个顶点的期望特征,在线提取所述四个顶点的当前特征,比较当前值和期望值,得到四个顶点在焦距归一化成像平面的偏差;
步骤S2:根据S1的四个顶点坐标,得到矩形顶点的雅克比矩阵;
步骤S3:根据S1的四个顶点图像偏差,以及S2的顶点雅克比矩阵,得到摄像机沿X、Y、Z三个方向的位置偏差;
步骤S4:根据S1的四个顶点坐标,得到矩形四条边的极坐标参数,进而得到矩形在焦距归一化成像平面的面积,离线得到矩形的期望面积,在线计算矩形的当前面积,比较当前值和期望值,得到矩形的面积偏差;
步骤S5:根据S1的四个顶点坐标,以及S4的四条边的极坐标参数,得到矩形面积的雅克比矩阵;
步骤S6:根据S4所述的矩形面积偏差,以及S5的矩形面积的雅克比矩阵,得到摄像机绕X轴和绕Y轴的姿态偏差;
步骤S7:根据S1的四个顶点坐标,得到矩形两条对角线的直线方程,离线得到两条对角线的期望夹角,在线得到两条对角线的当前夹角,比较当前值和期望值,得到夹角偏差,进而得到摄像机绕Z轴的姿态偏差;
步骤S8:根据S3的沿X、Y、Z三个方向的位置偏差、S6的绕X轴和绕Y轴的姿态偏差,以及S7的绕Z轴的姿态偏差,得到机器人的运动调整量,实现机器人的六自由度控制。
进一步所述,其中步骤S1中所述矩形四个顶点在焦距归一化成像平面的偏差计算如下:
根据从RGB-D图像提取的矩形的四个顶点的图像坐标,以及摄像机内参数,得到矩形第i顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1)(i=1,2,3,4),进而得到偏差如下:
其中,(Δx1ci,Δy1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值,Δzi表示第i个顶点在笛卡尔空间的深度偏差。(x1cid,y1cid)表示第i个顶点在焦距归一化成像平面的期望坐标,(x1ci,y1ci)表示第i个顶点在焦距归一化成像平面的当前坐标,zid和zi分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。
进一步所述,其中步骤S2中所述矩形顶点的雅克比矩阵计算如下:
记点特征P在摄像机坐标系中的坐标为(xc,yc,zc),在焦距归一化成像平面上的成像点坐标为(x1c,y1c,1),则有:
将式(2)对时间求导数,改写为矩阵形式
式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系。摄像机的运动会导致点特征P在摄像机坐标系中的运动。点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为
式中:Xc=[xc,yc,zc]T是点特征P的位置向量,是点特征P的位置变化率。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度。
将式(4)代入式(3),合并同类项,并应用式(2),有
其中,Lp就是点特征P的雅克比矩阵。
式(6)中,zc是点特征P的深度,本发明通过RGB-D相机获取。(x1c,y1c)是点特征P在焦距归一化成像平面上的坐标。
进一步所述,其中步骤S3中所述的摄像机沿X、Y、Z三个方向的位置偏差计算如下:
由点特征的雅克比矩阵定义,可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系:
其中,(i=1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标变化率,(x1ci,y1ci)(i=1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标,zi(i=1,2,3,4)表示第i个顶点的当前深度,Lp4是矩形四个顶点的雅克比矩阵。
根据S1的四个顶点在焦距归一化成像平面的偏差,以及式(7)所示的矩形四个顶点的雅克比矩阵Lp4,采用基于图像的控制律:
式(8)中,cv=[cvx,cvy,cvz]T就是摄像机沿X、Y、Z三个方向上的位置偏差。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度,Tc是采样周期。是雅克比矩阵Lp4的伪逆,(Δx1ci,Δy1ci)(i=1,2,3,4)是第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值。
进一步所述,其中步骤S4中所述的矩形面积偏差计算如下:
根据从RGB-D图像提取的矩形四个顶点的图像坐标,以及摄像机内参数,得到四个顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1),进而得到矩形四条边的极坐标参数(ρj,θj)(j=1,2,3,4)。因此,第i个顶点到第j条边的距离dij为:
dij=ρj-x1cicosθj-y1cisinθj (9)
在焦距归一化成像平面上,记矩形的两个边长分别为d11和d12,则矩形面积S为:
S=d11d12 (10)
按照式(10),离线得到矩形的期望面积Sd,在线计算矩形的当前面积S,进而得到矩形的面积偏差ΔS:
ΔS=Sd-S (11)
进一步所述,其中步骤S5中所述的矩形面积雅克比矩阵计算如下:
将式(9)对时间求导,有
对四条边的极坐标方程求导,并将(5)带入式(12),整理后可得
其中,Ldij是距离dij的雅克比矩阵,vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度。
其中,zc0j是第j条边上的垂点Pi在摄像机坐标系中的深度,zcpi是第i个顶点在摄像机坐标系中的深度。本发明中,zcpi可以通过RGB-D相机获取,zc0j需要估计。
对式(10)求导,可得矩形面积的变化率为
将式(13)代入式(15),得到矩形面积变化率与摄像机运动速度的关系:
其中,Ls就是矩形面积的雅克比矩阵。
进一步所述,其中步骤S6中所述的摄像机绕X轴和绕Y轴的姿态偏差计算如下:
由式(16)可得:
式(17)中,cω=[cωx,cωy,cω′z]T就是摄像机的姿态偏差。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度,Tc是采样周期。Ls +是矩形面积雅克比矩阵Ls的伪逆,ΔSk是第k个矩形的面积变化量。
式(17)中,由于摄像机的平移运动对矩形面积有影响,ΔSk需要去掉平移引起的面积变化,由式(8)可得摄像机平移误差,将其带入式(17),可得平移误差引起的面积变化量ΔSkv,因此ΔSk计算如下:
ΔSk=ΔS-ΔSkv (18)
另外,由式(14)可知,摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离,不影响矩形的面积,因此只用式(17)中cw的前两个分量进行姿态控制,从而得到摄像机绕X轴的姿态偏差cwx和绕Y轴的姿态偏差cwy。
进一步所述,其中步骤S7中所述的摄像机绕Z轴的姿态偏差计算如下:
根据从RGB-D图像提取的矩形的四个顶点的图像坐标,得到两条对角线的斜率k1和k2,则两条对角线的夹角α为:
离线得到期望夹角αd,在线得到当前夹角α,则夹角偏差Δα即为摄像机绕Z轴的姿态偏差:
Δα=αd-α (20)
进一步所述,其中步骤S8中所述的机器人调整量ΔT如下:
由式(8)、式(17)、式(20)和手眼关系矩阵Tm可得:
其中,[vrx,vry,vrz,ωrx,ωry,ωrz]T是机器人的位置和姿态偏差。cv=[cvx,cvy,cvz]T是步骤S3得到的摄像机在X轴、Y轴、Z轴位置误差,cωx和cωy分别是步骤S6得到的摄像机绕X轴和Y轴的姿态偏差,Δα是步骤S7得到的摄像机绕Z轴的姿态偏差。
根据式(21),得到机器人的运动调整量ΔT:
其中,λp是调整系数,[vrx,vry,vrz,ωrx,ωry,ωrz]T是机器人的位置和姿态偏差。
基于上述技术方案可知,本发明具有以下有益效果:传统视觉控制使用的点、边缘、直线特征提取简单,使用方便,但包含的特征信息有限,且易受目标物体的形状、纹理、噪声、光照条件等影响。
本发明以矩形为视觉特征,利用矩形的顶点、面积以及两条对角线的夹角进行视觉控制。根据顶点当前坐标和期望坐标的偏差,得到机器人的位置偏差,根据当前面积和期望面积的偏差,以及对角线当前夹角和期望夹角的偏差,得到机器人的姿态偏差,最终实现机器人的六自由度控制。
本发明利用矩形的顶点、面积以及两条对角线的夹角实现机器人的六自由度控制,将平移运动和旋转运动进行了解耦,控制过程更加稳定可靠。相对于使用单一图像特征的传统视觉控制方法,本发明能够提高视觉***的鲁棒性,便于推广应用。
附图说明
图1为本发明的基于矩形特征的视觉控制方法的控制框图。
具体实施方式
下面结合附图对本发明的实施例作详细的说明:本实施例在以本发明技术方案为前提下进行实施,结合详细的实施方式和具体的操作过程,但本发明的保护范围不限于下述实施例。
本发明公开了一种基于矩形特征的机器人视觉控制方法,以矩形为视觉特征,利用矩形的顶点、面积以及两条对角线的夹角进行视觉控制。根据顶点当前坐标和期望坐标的偏差,得到机器人的位置偏差,根据当前面积和期望面积的偏差,以及对角线当前夹角和期望夹角的偏差,得到机器人的姿态偏差,最终实现机器人的六自由度控制。
更具体地,作为本发明的一个优选实施例,如图1示出本发明的基于矩形特征的机器人视觉控制方法的框图。在控制过程中,基于提取的矩形特征,根据矩形顶点的图像偏差和深度偏差,得到机器人的位置偏差,根据当前面积和期望面积的偏差,以及对角线当前夹角和期望夹角的偏差,得到机器人的姿态偏差,最终实现机器人的六自由度控制。该方法包括以下步骤:
第一步:从所述相机获得的目标图像中提取矩形特征,离线得到所述四个顶点的期望特征,在线提取所述四个顶点的当前特征,比较当前值和期望值,得到四个顶点在焦距归一化成像平面的偏差;
第二步:根据第一步的四个顶点坐标,得到矩形顶点的雅克比矩阵;
第三步:根据第一步的四个顶点图像偏差,以及第二步的顶点雅克比矩阵,得到摄像机沿X、Y、Z三个方向的位置偏差;
第四步:根据第一步的四个顶点坐标,得到矩形四条边的极坐标参数,进而得到矩形在焦距归一化成像平面的面积,离线得到矩形的期望面积,在线计算矩形的当前面积,比较当前值和期望值,得到矩形的面积偏差;
第五步:根据第一步的四个顶点坐标,以及第四步的四条边的极坐标参数,得到矩形面积的雅克比矩阵。
第六步:根据第四步所述的矩形面积偏差,以及第五步的矩形面积的雅克比矩阵,得到摄像机绕X轴和绕Y轴的姿态偏差;
第七步:根据第一步的四个顶点坐标,得到矩形两条对角线的直线方程,离线得到两条对角线的期望夹角,在线得到两条对角线的当前夹角,比较当前值和期望值,得到夹角偏差,进而得到摄像机绕Z轴的姿态偏差;
第八步:根据第三步的沿X、Y、Z三个方向的位置偏差、第六步的绕X轴和绕Y轴的姿态偏差,以及第七步的绕Z轴的姿态偏差,得到机器人的运动调整量,实现机器人的六自由度控制。
所述第一步,具体如下:
根据获取的目标图像,提取矩形特征,得到式(1)所示的四个顶点在焦距归一化成像平面的偏差。
其中式(1)由以下具体步骤获得:
根据从RGB-D图像提取的矩形的四个顶点的图像坐标,以及摄像机内参数,得到矩形第i个顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1)(i=1,2,3,4),进而得到偏差如下:
其中,(Δx1ci,Δy1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值,Δzi表示第i个顶点在笛卡尔空间的深度偏差。(x1cid,y1cid)表示第i个顶点在焦距归一化成像平面的期望坐标,(x1ci,y1ci)表示第i个顶点在焦距归一化成像平面的当前坐标,zid和zi分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。
所述第二步,具体如下:
根据第一步的四个顶点坐标,得到式(6)所示的矩形顶点的雅克比矩阵。
其中式(6)由以下具体步骤获得:
记点特征P在摄像机坐标系中的坐标为(xc,yc,zc),在焦距归一化成像平面上的成像点坐标为(x1c,y1c,1),则有:
将式(2)对时间求导数,改写为矩阵形式
式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系。摄像机的运动会导致点特征P在摄像机坐标系中的运动。点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为
式中:Xc=[xc,yc,zc]T是点特征P的位置向量,是点特征P的位置变化率。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度。
将式(4)代入式(3),合并同类项,并应用式(2),有
其中,Lp就是点特征P的雅克比矩阵。
式(6)中,zc是点特征P的深度,本发明通过RGB-D相机获取。(x1c,y1c)是点特征P在焦距归一化成像平面上的坐标。
所述第三步,具体如下:
根据第一步的四个顶点偏差,以及第二步的顶点雅克比矩阵,得到式(8)所示的摄像机在X、Y、Z三个方向上的位置偏差。
其中式(8)由以下具体步骤获得:
由点特征的雅克比矩阵定义,可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系:
其中,(i=1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标变化率,(x1ci,y1ci)(i=1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标,zi(i=1,2,3,4)表示第i个顶点的当前深度,Lp4是矩形四个顶点的雅克比矩阵。
根据S1的四个顶点在焦距归一化成像平面的偏差,以及式(7)所示的矩形四个顶点的雅克比矩阵Lp4,采用基于图像的控制律:
式(8)中,cv=[cvx,cvy,cvz]T就是摄像机沿X、Y、Z三个方向的位置偏差。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度,Tc是采样周期。是雅克比矩阵Lp4的伪逆,(Δx1ci,Δy1ci)(i=1,2,3,4)是第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值。
所述第四步,具体如下:
根据第一步的四个顶点坐标,得到式(9)所示的矩形四条边的极坐标参数,进而得到式(10)所示的矩形在焦距归一化成像平面的面积,最终得到式(11)所示的矩形面积偏差。
其中式(9)、式(10)、式(11)由以下具体步骤获得:
根据从RGB-D图像提取的矩形四个顶点的图像坐标,以及摄像机内参数,得到四个顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1),进而得到矩形四条边的极坐标参数(ρj,θj)(j=1,2,3,4)。因此,第i个顶点到第j条边的距离dij为:
dij=ρj-x1cicosθj-y1cisinθj (9)
在焦距归一化成像平面上,记矩形的两个边长分别为d11和d12,则矩形面积S为:
S=d11d12 (10)
按照式(10),离线得到矩形的期望面积Sd,在线计算矩形的当前面积S,进而得到矩形的面积偏差ΔS:
ΔS=Sd-S (11)
进一步所述,其中步骤S5中所述的矩形面积雅克比矩阵计算如下:
将式(9)对时间求导,有
对四条边的极坐标方程求导,并将(5)带入式(12),整理后可得
其中,Ldij是距离dij的雅克比矩阵,vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度。
其中,zc0j是第j条边上的垂点Pi在摄像机坐标系中的深度,zcpi是第i个顶点在摄像机坐标系中的深度。本发明中,zcpi可以通过RGB-D相机获取,zc0j需要估计。
对式(10)求导,可得矩形面积的变化率为
将式(13)代入式(15),得到矩形面积变化率与摄像机运动速度的关系:
其中,Ls就是矩形面积的雅克比矩阵。
进一步所述,其中步骤S6中所述的摄像机绕X轴和绕Y轴的角度偏差计算如下:
由式(16)可得:
式(17)中,cω=[cωx,cωy,cω′z]T就是摄像机的姿态偏差。vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度,Tc是采样周期。Ls +是矩形面积雅克比矩阵Ls的伪逆,ΔSk是第k个矩形的面积变化量。
式(17)中,由于摄像机的平移运动对矩形面积有影响,ΔSk需要去掉平移引起的面积变化,由式(8)可得摄像机平移误差,将其带入式(17),可得平移误差引起的面积变化量ΔSkv,因此ΔSk计算如下:
ΔSk=ΔS-ΔSkv (18)
另外,由式(14)可知,摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离,不影响矩形的面积,因此只用式(17)中cw的前两个分量进行姿态控制,从而得到摄像机绕X轴的姿态偏差cwx和绕Y轴的姿态偏差cwy。
所述第七步,具体如下:
根据第一步的四个顶点坐标,得到矩形两条对角线的直线方程,离线得到两条对角线的期望夹角,在线得到两条对角线的当前夹角,进而得到式(20)所示的摄像机绕Z轴的姿态偏差。
其中式(20)由以下具体步骤获得:
根据从RGB-D图像提取的矩形的四个顶点的图像坐标,得到两条对角线的斜率k1和k2,则两条对角线的夹角α为:
离线得到期望夹角αd,在线得到当前夹角α,则夹角偏差Δα即为摄像机绕Z轴的姿态偏差:
Δα=αd-α (20)
所述第八步,具体如下:
根据第三步的在X、Y、Z三个方向上的位置偏差、第六步的绕X轴和绕Y轴的姿态偏差,以及第七步的绕Z轴的姿态偏差,得到式(22)所示的机器人运动调整量,实现机器人的六自由度控制。
其中式(22)由以下具体步骤获得:
由式(8)、式(17)、式(20)和手眼关系矩阵Tm可得:
其中,是机器人的位置和姿态偏差。cv=[cvx,cvy,cvz]T是步骤S3得到的摄像机沿X轴、Y轴、Z轴的位置误差,cωx和cωy分别是步骤S6得到的摄像机绕X轴和Y轴的姿态偏差,Δα是步骤S7得到的摄像机绕Z轴的姿态偏差。
根据式(21),得到机器人的运动调整量ΔT:
其中,λp是调整系数,[vrx,vry,vrz,ωrx,ωry,ωrz]T是机器人的位置和姿态偏差。
以上所述的具体实施例,对本发明的目的、技术方案和有益效果进行了进一步详细说明,应理解的是,以上所述仅为本发明的具体实施例而已,并不用于限制本发明,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种基于矩形特征的机器人视觉控制方法,包括以下步骤:
步骤S0,使用一个RGB-D相机获取目标图像;
步骤S1:从所述相机获得的目标图像中提取矩形特征,离线得到四个顶点的期望特征,在线提取所述四个顶点的当前特征,比较当前值和期望值,得到四个顶点在焦距归一化成像平面的偏差;
步骤S2:根据S1的四个顶点坐标,得到矩形顶点的雅克比矩阵;
步骤S3:根据S1的四个顶点图像偏差,以及S2的顶点雅克比矩阵,得到摄像机沿X、Y、Z三个方向的位置偏差;
步骤S4:根据S1的四个顶点坐标,得到矩形四条边的极坐标参数,进而得到矩形在焦距归一化成像平面的面积,离线得到矩形的期望面积,在线计算矩形的当前面积,比较当前值和期望值,得到矩形的面积偏差;
步骤S5:根据S1的四个顶点坐标,以及S4的四条边的极坐标参数,得到矩形面积的雅克比矩阵;
步骤S6:根据S4所述的矩形面积偏差,以及S5的矩形面积的雅克比矩阵,得到摄像机绕X轴和绕Y轴的姿态偏差;
步骤S7:根据S1的四个顶点坐标,得到矩形两条对角线的直线方程,离线得到两条对角线的期望夹角,在线得到两条对角线的当前夹角,比较当前值和期望值,得到夹角偏差,进而得到摄像机绕Z轴的姿态偏差;
步骤S8:根据S3的沿X、Y、Z三个方向的位置偏差、S6的绕X轴和绕Y轴的角度偏差,以及S7的绕Z轴的角度偏差,得到机器人的运动调整量,实现机器人的六自由度控制。
2.根据权利要求1所述的基于矩形特征的机器人视觉控制方法,其中步骤S1中所述矩形四个顶点在焦距归一化成像平面的偏差计算如下:
根据从RGB-D图像提取的矩形的四个顶点的图像坐标,以及摄像机内参数,得到矩形第i顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1)(i=1,2,3,4),进而得到偏差如下:
其中,(Δx1ci,Δy1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值,Δzi表示第i个顶点在笛卡尔空间的深度偏差 ,(x1cid,y1cid)表示第i个顶点在焦距归一化成像平面的期望坐标,(x1ci,y1ci)表示第i个顶点在焦距归一化成像平面的当前坐标,zid和zi分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。
3.根据权利要求2所述的基于矩形特征的机器人视觉控制方法,其中步骤S2中所述矩形顶点的雅克比矩阵计算如下:
记点特征P在摄像机坐标系中的坐标为(xc,yc,zc),在焦距归一化成像平面上的成像点坐标为(x1c,y1c,1),则有:
将式(2)对时间求导数,改写为矩阵形式
式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系,摄像机的运动会导致点特征P在摄像机坐标系中的运动,点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为
式中:Xc=[xc,yc,zc]T是点特征P的位置向量,是点特征P的位置变化率,vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度;
将式(4)代入式(3),合并同类项,并应用式(2),有
其中,Lp就是点特征P的雅克比矩阵:
式(6)中,zc是点特征P的深度,通过RGB-D相机获取,(x1c,y1c)是点特征P在焦距归一化成像平面上的坐标。
4.根据权利要求3所述的基于矩形特征的机器人视觉控制方法,其中步骤S3中所述的摄像机沿X、Y、Z三个方向的位置偏差计算如下:
由点特征的雅克比矩阵定义,可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系:
其中,表示第i个顶点在焦距归一化成像平面的坐标变化率,(x1ci,y1ci)(i=1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标,zi(i=1,2,3,4)表示第i个顶点的当前深度,Lp4是矩形四个顶点的雅克比矩阵;
根据S1的四个顶点在焦距归一化成像平面的偏差,以及式(7)所示的矩形四个顶点的雅克比矩阵Lp4,采用基于图像的控制律:
5.根据权利要求4所述的基于矩形特征的机器人视觉控制方法,其中步骤S4中所述的矩形面积偏差计算如下:
根据从RGB-D图像提取的矩形四个顶点的图像坐标,以及摄像机内参数,得到四个顶点在焦距归一化成像平面的坐标(x1ci,y1ci,1),进而得到矩形四条边的极坐标参数(ρj,θj)(j=1,2,3,4),因此,第i个顶点到第j条边的距离dij为:
dij=ρj-x1cicosθj-y1cisinθj (9)
在焦距归一化成像平面上,记矩形的两个边长分别为d11和d12,则矩形面积S为:
S=d11d12 (10)
按照式(10),离线得到矩形的期望面积Sd,在线计算矩形的当前面积S,进而得到矩形的面积偏差ΔS:
ΔS=Sd-S (11)。
6.根据权利要求5所述的基于矩形特征的机器人视觉控制方法,其中步骤S5中所述的矩形面积雅克比矩阵计算如下:
将式(9)对时间求导,有
对四条边的极坐标方程求导,并将(5)带入式(12),整理后可得
其中,Ldij是距离dij的雅克比矩阵,vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度;
其中,zc0j是第j条边上的垂点Pi在摄像机坐标系中的深度,zcpi是第i个顶点在摄像机坐标系中的深度,zcpi通过RGB-D相机获取,zc0j需要估计;
对式(10)求导,可得矩形面积的变化率为
将式(13)代入式(15),得到矩形面积变化率与摄像机运动速度的关系:
其中,Ls就是矩形面积的雅克比矩阵。
7.根据权利要求6所述的基于矩形特征的机器人视觉控制方法,其中步骤S6中所述的摄像机绕X轴和绕Y轴的姿态偏差计算如下:
由式(16)可得:
式(17)中,cω=[cωx,cωy,cω′z]T就是摄像机的姿态偏差,vca=[vcax,vcay,vcaz]T是摄像机的平移速度,ωca=[ωcax,ωcay,ωcaz]T是摄像机的旋转速度,Tc是采样周期,Ls +是矩形面积雅克比矩阵Ls的伪逆,ΔSk是第k个矩形的面积变化量,
式(17)中,由于摄像机的平移运动对矩形面积有影响,ΔSk需要去掉平移引起的面积变化,由式(8)可得摄像机平移误差,将其带入式(17),可得平移误差引起的面积变化量ΔSkv,因此ΔSk计算如下:
ΔSk=ΔS-ΔSkv (18)
另外,由式(14)可知,摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离,不影响矩形的面积,因此只用式(17)中cw的前两个分量进行姿态控制,从而得到摄像机绕X轴的姿态偏差cwx和绕Y轴的姿态偏差cwy。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811009651.1A CN109048911B (zh) | 2018-08-31 | 2018-08-31 | 一种基于矩形特征的机器人视觉控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811009651.1A CN109048911B (zh) | 2018-08-31 | 2018-08-31 | 一种基于矩形特征的机器人视觉控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109048911A CN109048911A (zh) | 2018-12-21 |
CN109048911B true CN109048911B (zh) | 2021-08-24 |
Family
ID=64758075
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811009651.1A Active CN109048911B (zh) | 2018-08-31 | 2018-08-31 | 一种基于矩形特征的机器人视觉控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109048911B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110216685B (zh) * | 2019-03-12 | 2021-02-02 | 汕头大学 | 一种用于煤仓内壁缺陷检测的悬索并联机器人及控制方法 |
CN113211433B (zh) * | 2021-04-21 | 2022-09-20 | 山东科技大学 | 一种基于复合特征的分离式视觉伺服控制方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103913131A (zh) * | 2014-04-14 | 2014-07-09 | 大连理工大学 | 一种基于双目视觉的自由曲面法矢量测量方法 |
WO2016135658A1 (en) * | 2015-02-26 | 2016-09-01 | Khalifa University Of Science, Technology And Research (Kustar) | Parallel mechanism based automated fiber placement system |
CN106485746A (zh) * | 2016-10-17 | 2017-03-08 | 广东技术师范学院 | 基于图像无标定的视觉伺服机械手及其控制方法 |
CN107891424A (zh) * | 2017-11-10 | 2018-04-10 | 浙江科技学院 | 一种求解冗余机械臂逆运动学的有限时间神经网络优化方法 |
CN107901041A (zh) * | 2017-12-15 | 2018-04-13 | 中南大学 | 一种基于图像混合矩的机器人视觉伺服控制方法 |
CN108376411A (zh) * | 2018-01-16 | 2018-08-07 | 上海交通大学 | 一种基于双目视觉的非合作目标相对状态解算方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9086271B2 (en) * | 2012-11-09 | 2015-07-21 | Recognition Robotics, Inc. | Industrial robot system having sensor assembly |
-
2018
- 2018-08-31 CN CN201811009651.1A patent/CN109048911B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103913131A (zh) * | 2014-04-14 | 2014-07-09 | 大连理工大学 | 一种基于双目视觉的自由曲面法矢量测量方法 |
WO2016135658A1 (en) * | 2015-02-26 | 2016-09-01 | Khalifa University Of Science, Technology And Research (Kustar) | Parallel mechanism based automated fiber placement system |
CN106485746A (zh) * | 2016-10-17 | 2017-03-08 | 广东技术师范学院 | 基于图像无标定的视觉伺服机械手及其控制方法 |
CN107891424A (zh) * | 2017-11-10 | 2018-04-10 | 浙江科技学院 | 一种求解冗余机械臂逆运动学的有限时间神经网络优化方法 |
CN107901041A (zh) * | 2017-12-15 | 2018-04-13 | 中南大学 | 一种基于图像混合矩的机器人视觉伺服控制方法 |
CN108376411A (zh) * | 2018-01-16 | 2018-08-07 | 上海交通大学 | 一种基于双目视觉的非合作目标相对状态解算方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109048911A (zh) | 2018-12-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110116407B (zh) | 柔性机器人位姿测量方法及装置 | |
Qi et al. | Review on camera calibration | |
Hutchinson et al. | A tutorial on visual servo control | |
Taylor et al. | Robust vision-based pose control | |
Xu et al. | Partially decoupled image-based visual servoing using different sensitive features | |
Song et al. | CAD-based pose estimation design for random bin picking using a RGB-D camera | |
CN105091744A (zh) | 一种基于视觉传感器和激光测距仪的位姿检测装置与方法 | |
CN102848388A (zh) | 基于多传感器的服务机器人定位和抓取方法 | |
Cipolla et al. | Visually guided grasping in unstructured environments | |
CN109048911B (zh) | 一种基于矩形特征的机器人视觉控制方法 | |
Ge et al. | Online 3-D modeling of complex workpieces for the robotic spray painting with low-cost RGB-D cameras | |
Han et al. | Grasping control method of manipulator based on binocular vision combining target detection and trajectory planning | |
CN108927807A (zh) | 一种基于点特征的机器人视觉控制方法 | |
Park et al. | Global map generation using LiDAR and stereo camera for initial positioning of mobile robot | |
Xu et al. | A new monocular vision measurement method to estimate 3D positions of objects on floor | |
Peng et al. | An autonomous pose measurement method of civil aviation charging port based on cumulative natural feature data | |
CN111645067A (zh) | 一种高智能机器人环境感知方法及*** | |
CN109146957B (zh) | 一种基于三角形特征的机器人视觉控制方法 | |
Deans et al. | Terrain model registration for single cycle instrument placement | |
Qingda et al. | Workpiece posture measurement and intelligent robot grasping based on monocular vision | |
CN116100562B (zh) | 多机器人协同上下料的视觉引导方法及*** | |
Hager et al. | Tutorial tt3: A tutorial on visual servo control | |
Yu et al. | Vision-based method of kinematic calibration and image tracking of position and posture for 3-RPS parallel robot | |
LU et al. | Decoupled visual servoing control method based on point and line features | |
Fan et al. | Robot gluing localization method based on monocular vision |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |