CN109048911B - 一种基于矩形特征的机器人视觉控制方法 - Google Patents

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顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)(i＝1,2,3,4)，进而得到偏差如下：

其中，(Δx_1ci,Δy_1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值，Δz_i表示第i个顶点在笛卡尔空间的深度偏差。(x_1cid,y_1cid)表示第i个顶点在焦距归一化成像平面的期望坐标，(x_1ci,y_1ci)表示第i个顶点在焦距归一化成像平面的当前坐标，z_id和z_i分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。

进一步所述，其中步骤S2中所述矩形顶点的雅克比矩阵计算如下：

记点特征P在摄像机坐标系中的坐标为(x_c,y_c,z_c)，在焦距归一化成像平面上的成像点坐标为(x_1c,y_1c,1)，则有：

将式(2)对时间求导数，改写为矩阵形式

其中

是点特征P在焦距归一化平面的坐标变化率，

是点特征P在摄像机坐标系的坐标变化率。

式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系。摄像机的运动会导致点特征P在摄像机坐标系中的运动。点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为

式中：X_c＝[x_c,y_c,z_c]^T是点特征P的位置向量，

是点特征P的位置变化率。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度。

将式(4)代入式(3)，合并同类项，并应用式(2)，有

其中，L_p就是点特征P的雅克比矩阵。

式(6)中，z_c是点特征P的深度，本发明通过RGB-D相机获取。(x_1c,y_1c)是点特征P在焦距归一化成像平面上的坐标。

进一步所述，其中步骤S3中所述的摄像机沿X、Y、Z三个方向的位置偏差计算如下：

由点特征的雅克比矩阵定义，可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系：

其中，

(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标变化率，(x_1ci,y_1ci)(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标，z_i(i＝1,2,3,4)表示第i个顶点的当前深度，L_p4是矩形四个顶点的雅克比矩阵。

根据S1的四个顶点在焦距归一化成像平面的偏差，以及式(7)所示的矩形四个顶点的雅克比矩阵L_p4，采用基于图像的控制律：

式(8)中，cv＝[cv_x,cv_y,cv_z]^T就是摄像机沿X、Y、Z三个方向上的位置偏差。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期。

是雅克比矩阵L_p4的伪逆，(Δx_1ci,Δy_1ci)(i＝1,2,3,4)是第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值。

进一步所述，其中步骤S4中所述的矩形面积偏差计算如下：

根据从RGB-D图像提取的矩形四个顶点的图像坐标，以及摄像机内参数，得到四个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)，进而得到矩形四条边的极坐标参数(ρ_j,θ_j)(j＝1,2,3,4)。因此，第i个顶点到第j条边的距离d_ij为：

d_ij＝ρ_j-x_1cicosθ_j-y_1cisinθ_j (9)

在焦距归一化成像平面上，记矩形的两个边长分别为d₁₁和d₁₂，则矩形面积S为：

S＝d₁₁d₁₂ (10)

按照式(10),离线得到矩形的期望面积S_d，在线计算矩形的当前面积S，进而得到矩形的面积偏差ΔS：

ΔS＝S_d-S (11)

进一步所述，其中步骤S5中所述的矩形面积雅克比矩阵计算如下：

将式(9)对时间求导，有

其中，

第i个顶点到第j条边的距离d_ij的变化率，

分别是第j条边的极径ρ_j和极角θ_j的变化率。

(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci)的变化率。

对四条边的极坐标方程求导，并将(5)带入式(12),整理后可得

其中，L_dij是距离d_ij的雅克比矩阵，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度。

其中，z_c0j是第j条边上的垂点P_i在摄像机坐标系中的深度，z_cpi是第i个顶点在摄像机坐标系中的深度。本发明中，z_cpi可以通过RGB-D相机获取，z_c0j需要估计。

对式(10)求导，可得矩形面积的变化率为

将式(13)代入式(15)，得到矩形面积变化率与摄像机运动速度的关系：

其中，L_s就是矩形面积的雅克比矩阵。

进一步所述，其中步骤S6中所述的摄像机绕X轴和绕Y轴的姿态偏差计算如下：

由式(16)可得：

式(17)中，cω＝[cω_x,cω_y,cω′_z]^T就是摄像机的姿态偏差。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期。L_s ⁺是矩形面积雅克比矩阵L_s的伪逆,ΔS_k是第k个矩形的面积变化量。

式(17)中，由于摄像机的平移运动对矩形面积有影响，ΔS_k需要去掉平移引起的面积变化，由式(8)可得摄像机平移误差，将其带入式(17)，可得平移误差引起的面积变化量ΔS_kv,因此ΔS_k计算如下：

ΔS_k＝ΔS-ΔS_kv (18)

另外，由式(14)可知，摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离，不影响矩形的面积，因此只用式(17)中cw的前两个分量进行姿态控制，从而得到摄像机绕X轴的姿态偏差cw_x和绕Y轴的姿态偏差cw_y。

进一步所述，其中步骤S7中所述的摄像机绕Z轴的姿态偏差计算如下：

根据从RGB-D图像提取的矩形的四个顶点的图像坐标，得到两条对角线的斜率k₁和k₂,则两条对角线的夹角α为：

离线得到期望夹角α_d,在线得到当前夹角α，则夹角偏差Δα即为摄像机绕Z轴的姿态偏差：

Δα＝α_d-α (20)

进一步所述，其中步骤S8中所述的机器人调整量ΔT如下：

由式(8)、式(17)、式(20)和手眼关系矩阵T_m可得：

其中，[v_rx,v_ry,v_rz,ω_rx,ω_ry,ω_rz]^T是机器人的位置和姿态偏差。cv＝[cv_x,cv_y,cv_z]^T是步骤S3得到的摄像机在X轴、Y轴、Z轴位置误差，cω_x和cω_y分别是步骤S6得到的摄像机绕X轴和Y轴的姿态偏差,Δα是步骤S7得到的摄像机绕Z轴的姿态偏差。

根据式(21),得到机器人的运动调整量ΔT：

其中，λ_p是调整系数，[v_rx,v_ry,v_rz,ω_rx,ω_ry,ω_rz]^T是机器人的位置和姿态偏差。

基于上述技术方案可知，本发明具有以下有益效果：传统视觉控制使用的点、边缘、直线特征提取简单，使用方便，但包含的特征信息有限，且易受目标物体的形状、纹理、噪声、光照条件等影响。

本发明以矩形为视觉特征，利用矩形的顶点、面积以及两条对角线的夹角进行视觉控制。根据顶点当前坐标和期望坐标的偏差，得到机器人的位置偏差，根据当前面积和期望面积的偏差，以及对角线当前夹角和期望夹角的偏差，得到机器人的姿态偏差，最终实现机器人的六自由度控制。

本发明利用矩形的顶点、面积以及两条对角线的夹角实现机器人的六自由度控制，将平移运动和旋转运动进行了解耦，控制过程更加稳定可靠。相对于使用单一图像特征的传统视觉控制方法，本发明能够提高视觉系统的鲁棒性，便于推广应用。

附图说明

图1为本发明的基于矩形特征的视觉控制方法的控制框图。

具体实施方式

下面结合附图对本发明的实施例作详细的说明：本实施例在以本发明技术方案为前提下进行实施，结合详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。

本发明公开了一种基于矩形特征的机器人视觉控制方法，以矩形为视觉特征，利用矩形的顶点、面积以及两条对角线的夹角进行视觉控制。根据顶点当前坐标和期望坐标的偏差，得到机器人的位置偏差，根据当前面积和期望面积的偏差，以及对角线当前夹角和期望夹角的偏差，得到机器人的姿态偏差，最终实现机器人的六自由度控制。

更具体地，作为本发明的一个优选实施例，如图1示出本发明的基于矩形特征的机器人视觉控制方法的框图。在控制过程中，基于提取的矩形特征，根据矩形顶点的图像偏差和深度偏差，得到机器人的位置偏差，根据当前面积和期望面积的偏差，以及对角线当前夹角和期望夹角的偏差，得到机器人的姿态偏差，最终实现机器人的六自由度控制。该方法包括以下步骤：

第一步：从所述相机获得的目标图像中提取矩形特征，离线得到所述四个顶点的期望特征，在线提取所述四个顶点的当前特征，比较当前值和期望值，得到四个顶点在焦距归一化成像平面的偏差；

第二步：根据第一步的四个顶点坐标，得到矩形顶点的雅克比矩阵；

第三步：根据第一步的四个顶点图像偏差，以及第二步的顶点雅克比矩阵，得到摄像机沿X、Y、Z三个方向的位置偏差；

第四步：根据第一步的四个顶点坐标，得到矩形四条边的极坐标参数，进而得到矩形在焦距归一化成像平面的面积，离线得到矩形的期望面积，在线计算矩形的当前面积，比较当前值和期望值，得到矩形的面积偏差；

第五步：根据第一步的四个顶点坐标，以及第四步的四条边的极坐标参数，得到矩形面积的雅克比矩阵。

第六步：根据第四步所述的矩形面积偏差，以及第五步的矩形面积的雅克比矩阵，得到摄像机绕X轴和绕Y轴的姿态偏差；

第七步：根据第一步的四个顶点坐标，得到矩形两条对角线的直线方程，离线得到两条对角线的期望夹角，在线得到两条对角线的当前夹角，比较当前值和期望值，得到夹角偏差，进而得到摄像机绕Z轴的姿态偏差；

第八步：根据第三步的沿X、Y、Z三个方向的位置偏差、第六步的绕X轴和绕Y轴的姿态偏差，以及第七步的绕Z轴的姿态偏差，得到机器人的运动调整量，实现机器人的六自由度控制。

所述第一步，具体如下：

根据获取的目标图像，提取矩形特征，得到式(1)所示的四个顶点在焦距归一化成像平面的偏差。

其中式(1)由以下具体步骤获得：

根据从RGB-D图像提取的矩形的四个顶点的图像坐标，以及摄像机内参数，得到矩形第i个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)(i＝1,2,3,4)，进而得到偏差如下：

其中，(Δx_1ci,Δy_1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值，Δz_i表示第i个顶点在笛卡尔空间的深度偏差。(x_1cid,y_1cid)表示第i个顶点在焦距归一化成像平面的期望坐标，(x_1ci,y_1ci)表示第i个顶点在焦距归一化成像平面的当前坐标，z_id和z_i分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。

所述第二步，具体如下：

根据第一步的四个顶点坐标，得到式(6)所示的矩形顶点的雅克比矩阵。

其中式(6)由以下具体步骤获得：

记点特征P在摄像机坐标系中的坐标为(x_c,y_c,z_c)，在焦距归一化成像平面上的成像点坐标为(x_1c,y_1c,1)，则有：

将式(2)对时间求导数，改写为矩阵形式

其中

是点特征P在焦距归一化平面的坐标变化率，

是点特征P在摄像机坐标系的坐标变化率。

式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系。摄像机的运动会导致点特征P在摄像机坐标系中的运动。点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为

式中：X_c＝[x_c,y_c,z_c]^T是点特征P的位置向量，

是点特征P的位置变化率。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度。

将式(4)代入式(3)，合并同类项，并应用式(2)，有

其中，L_p就是点特征P的雅克比矩阵。

式(6)中，z_c是点特征P的深度，本发明通过RGB-D相机获取。(x_1c,y_1c)是点特征P在焦距归一化成像平面上的坐标。

所述第三步，具体如下：

根据第一步的四个顶点偏差，以及第二步的顶点雅克比矩阵，得到式(8)所示的摄像机在X、Y、Z三个方向上的位置偏差。

其中式(8)由以下具体步骤获得：

由点特征的雅克比矩阵定义，可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系：

其中，

(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标变化率，(x_1ci,y_1ci)(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标，z_i(i＝1,2,3,4)表示第i个顶点的当前深度，L_p4是矩形四个顶点的雅克比矩阵。

根据S1的四个顶点在焦距归一化成像平面的偏差，以及式(7)所示的矩形四个顶点的雅克比矩阵L_p4，采用基于图像的控制律：

式(8)中，cv＝[cv_x,cv_y,cv_z]^T就是摄像机沿X、Y、Z三个方向的位置偏差。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期。

是雅克比矩阵L_p4的伪逆，(Δx_1ci,Δy_1ci)(i＝1,2,3,4)是第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值。

所述第四步，具体如下：

根据第一步的四个顶点坐标，得到式(9)所示的矩形四条边的极坐标参数，进而得到式(10)所示的矩形在焦距归一化成像平面的面积，最终得到式(11)所示的矩形面积偏差。

其中式(9)、式(10)、式(11)由以下具体步骤获得：

根据从RGB-D图像提取的矩形四个顶点的图像坐标，以及摄像机内参数，得到四个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)，进而得到矩形四条边的极坐标参数(ρ_j,θ_j)(j＝1,2,3,4)。因此，第i个顶点到第j条边的距离d_ij为：

d_ij＝ρ_j-x_1cicosθ_j-y_1cisinθ_j (9)

在焦距归一化成像平面上，记矩形的两个边长分别为d₁₁和d₁₂，则矩形面积S为：

S＝d₁₁d₁₂ (10)

按照式(10),离线得到矩形的期望面积S_d，在线计算矩形的当前面积S，进而得到矩形的面积偏差ΔS：

ΔS＝S_d-S (11)

进一步所述，其中步骤S5中所述的矩形面积雅克比矩阵计算如下：

将式(9)对时间求导，有

其中，

第i个顶点到第j条边的距离d_ij的变化率，

分别是第j条边的极径ρ_j和极角θ_j的变化率。

(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci)的变化率。

对四条边的极坐标方程求导，并将(5)带入式(12),整理后可得

其中，L_dij是距离d_ij的雅克比矩阵，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度。

其中，z_c0j是第j条边上的垂点P_i在摄像机坐标系中的深度，z_cpi是第i个顶点在摄像机坐标系中的深度。本发明中，z_cpi可以通过RGB-D相机获取，z_c0j需要估计。

对式(10)求导，可得矩形面积的变化率为

将式(13)代入式(15)，得到矩形面积变化率与摄像机运动速度的关系：

其中，L_s就是矩形面积的雅克比矩阵。

进一步所述，其中步骤S6中所述的摄像机绕X轴和绕Y轴的角度偏差计算如下：

由式(16)可得：

式(17)中，cω＝[cω_x,cω_y,cω′_z]^T就是摄像机的姿态偏差。v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期。L_s ⁺是矩形面积雅克比矩阵L_s的伪逆,ΔS_k是第k个矩形的面积变化量。

式(17)中，由于摄像机的平移运动对矩形面积有影响，ΔS_k需要去掉平移引起的面积变化，由式(8)可得摄像机平移误差，将其带入式(17)，可得平移误差引起的面积变化量ΔS_kv,因此ΔS_k计算如下：

ΔS_k＝ΔS-ΔS_kv (18)

另外，由式(14)可知，摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离，不影响矩形的面积，因此只用式(17)中cw的前两个分量进行姿态控制，从而得到摄像机绕X轴的姿态偏差cw_x和绕Y轴的姿态偏差cw_y。

所述第七步，具体如下：

根据第一步的四个顶点坐标，得到矩形两条对角线的直线方程，离线得到两条对角线的期望夹角，在线得到两条对角线的当前夹角，进而得到式(20)所示的摄像机绕Z轴的姿态偏差。

其中式(20)由以下具体步骤获得：

根据从RGB-D图像提取的矩形的四个顶点的图像坐标，得到两条对角线的斜率k₁和k₂,则两条对角线的夹角α为：

离线得到期望夹角α_d,在线得到当前夹角α，则夹角偏差Δα即为摄像机绕Z轴的姿态偏差：

Δα＝α_d-α (20)

所述第八步，具体如下：

根据第三步的在X、Y、Z三个方向上的位置偏差、第六步的绕X轴和绕Y轴的姿态偏差，以及第七步的绕Z轴的姿态偏差，得到式(22)所示的机器人运动调整量，实现机器人的六自由度控制。

其中式(22)由以下具体步骤获得：

由式(8)、式(17)、式(20)和手眼关系矩阵T_m可得：

其中，

是机器人的位置和姿态偏差。cv＝[cv_x,cv_y,cv_z]^T是步骤S3得到的摄像机沿X轴、Y轴、Z轴的位置误差，cω_x和cω_y分别是步骤S6得到的摄像机绕X轴和Y轴的姿态偏差,Δα是步骤S7得到的摄像机绕Z轴的姿态偏差。

根据式(21),得到机器人的运动调整量ΔT：

其中，λ_p是调整系数，[v_rx,v_ry,v_rz,ω_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顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)(i＝1,2,3,4)，进而得到偏差如下：

其中，(Δx_1ci,Δy_1ci)表示第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值，Δz_i表示第i个顶点在笛卡尔空间的深度偏差 ，(x_1cid,y_1cid)表示第i个顶点在焦距归一化成像平面的期望坐标，(x_1ci,y_1ci)表示第i个顶点在焦距归一化成像平面的当前坐标，z_id和z_i分别表示第i个顶点在笛卡尔空间的期望深度和当前深度。

3.根据权利要求2所述的基于矩形特征的机器人视觉控制方法，其中步骤S2中所述矩形顶点的雅克比矩阵计算如下：

记点特征P在摄像机坐标系中的坐标为(x_c,y_c,z_c)，在焦距归一化成像平面上的成像点坐标为(x_1c,y_1c,1)，则有：

将式(2)对时间求导数，改写为矩阵形式

其中

是点特征P在焦距归一化平面的坐标变化率，

是点特征P在摄像机坐标系的坐标变化率；

式(3)为点特征P在笛卡儿空间的平移运动速度与投影到成像平面空间的运动速度之间的关系，摄像机的运动会导致点特征P在摄像机坐标系中的运动，点特征P在摄像机坐标系中的运动速度与摄像机在笛卡儿空间运动速度之间的关系为

式中：X_c＝[x_c,y_c,z_c]^T是点特征P的位置向量，

是点特征P的位置变化率，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度；

将式(4)代入式(3)，合并同类项，并应用式(2)，有

其中，L_p就是点特征P的雅克比矩阵：

式(6)中，z_c是点特征P的深度，通过RGB-D相机获取，(x_1c,y_1c)是点特征P在焦距归一化成像平面上的坐标。

4.根据权利要求3所述的基于矩形特征的机器人视觉控制方法，其中步骤S3中所述的摄像机沿X、Y、Z三个方向的位置偏差计算如下：

由点特征的雅克比矩阵定义，可得四个顶点在焦距归一化平面的坐标变化率与摄像机运动速度之间满足关系：

其中，

表示第i个顶点在焦距归一化成像平面的坐标变化率，(x_1ci,y_1ci)(i＝1,2,3,4)表示第i个顶点在焦距归一化成像平面的坐标，z_i(i＝1,2,3,4)表示第i个顶点的当前深度，L_p4是矩形四个顶点的雅克比矩阵；

根据S1的四个顶点在焦距归一化成像平面的偏差，以及式(7)所示的矩形四个顶点的雅克比矩阵L_p4，采用基于图像的控制律：

式(8)中，cv＝[cv_x,cv_y,cv_z]^T就是摄像机沿X、Y、Z三个方向上的位置偏差，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期，

是雅克比矩阵L_p4的伪逆，(Δx_1ci,Δy_1ci)(i＝1,2,3,4)是第i个顶点在焦距归一化成像平面的期望坐标与当前坐标的差值。

5.根据权利要求4所述的基于矩形特征的机器人视觉控制方法，其中步骤S4中所述的矩形面积偏差计算如下：

根据从RGB-D图像提取的矩形四个顶点的图像坐标，以及摄像机内参数，得到四个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci,1)，进而得到矩形四条边的极坐标参数(ρ_j,θ_j)(j＝1,2,3,4)，因此，第i个顶点到第j条边的距离d_ij为：

d_ij＝ρ_j-x_1cicosθ_j-y_1cisinθ_j (9)

在焦距归一化成像平面上，记矩形的两个边长分别为d₁₁和d₁₂，则矩形面积S为：

S＝d₁₁d₁₂ (10)

按照式(10),离线得到矩形的期望面积S_d，在线计算矩形的当前面积S，进而得到矩形的面积偏差ΔS：

ΔS＝S_d-S (11)。

6.根据权利要求5所述的基于矩形特征的机器人视觉控制方法，其中步骤S5中所述的矩形面积雅克比矩阵计算如下：

将式(9)对时间求导，有

其中，

第i个顶点到第j条边的距离d_ij的变化率，

分别是第j条边的极径ρ_j和极角θ_j的变化率，

表示第i个顶点在焦距归一化成像平面的坐标(x_1ci,y_1ci)的变化率；

对四条边的极坐标方程求导，并将(5)带入式(12),整理后可得

其中，L_dij是距离d_ij的雅克比矩阵，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度；

其中，z_c0j是第j条边上的垂点P_i在摄像机坐标系中的深度，z_cpi是第i个顶点在摄像机坐标系中的深度，z_cpi通过RGB-D相机获取，z_c0j需要估计；

对式(10)求导，可得矩形面积的变化率为

将式(13)代入式(15)，得到矩形面积变化率与摄像机运动速度的关系：

其中，L_s就是矩形面积的雅克比矩阵。

7.根据权利要求6所述的基于矩形特征的机器人视觉控制方法，其中步骤S6中所述的摄像机绕X轴和绕Y轴的姿态偏差计算如下：

由式(16)可得：

式(17)中，cω＝[cω_x,cω_y,cω′_z]^T就是摄像机的姿态偏差，v_ca＝[v_cax,v_cay,v_caz]^T是摄像机的平移速度，ω_ca＝[ω_cax,ω_cay,ω_caz]^T是摄像机的旋转速度,T_c是采样周期，L_s ⁺是矩形面积雅克比矩阵L_s的伪逆,ΔS_k是第k个矩形的面积变化量，

式(17)中，由于摄像机的平移运动对矩形面积有影响，ΔS_k需要去掉平移引起的面积变化，由式(8)可得摄像机平移误差，将其带入式(17)，可得平移误差引起的面积变化量ΔS_kv,因此ΔS_k计算如下：

ΔS_k＝ΔS-ΔS_kv (18)

另外，由式(14)可知，摄像机绕Z轴的旋转运动不改变第i个特征点到第j条直线的距离，不影响矩形的面积，因此只用式(17)中cw的前两个分量进行姿态控制，从而得到摄像机绕X轴的姿态偏差cw_x和绕Y轴的姿态偏差cw_y。

8.根据权利要求7所述的基于矩形特征的机器人视觉控制方法，其中步骤S7中所述的摄像机绕Z轴的姿态偏差计算如下：

根据从RGB-D图像提取的矩形的四个顶点的图像坐标，得到两条对角线的斜率k₁和k₂,则两条对角线的夹角α为：

离线得到期望夹角α_d,在线得到当前夹角α，则夹角偏差Δα即为摄像机绕Z轴的姿态偏差：

Δα＝α_d-α (20)。

9.根据权利要求8所述的基于矩形特征的机器人视觉控制方法，其中步骤S8中所述的机器人调整量△T如下：

由式(8)、式(17)、式(20)和手眼关系矩阵T_m可得：

其中，[v_rx,v_ry,v_rz,ω_rx,ω_ry,ω_rz]^T是机器人的位置和姿态偏差，cv＝[cv_x,cv_y,cv_z]^T是步骤S3得到的摄像机在X轴、Y轴、Z轴位置误差，cω_x和cω_y分别是步骤S6得到的摄像机绕X轴和Y轴的姿态偏差,Δα是步骤S7得到的摄像机绕Z轴的姿态偏差；

根据式(21),得到机器人的运动调整量ΔT：

其中，λ_p是调整系数，[v_rx,v_ry,v_rz,ω_rx,ω_ry,ω_rz]^T是机器人的位置和姿态偏差。

Images