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

Abstract

本发明公开了一种基于三角形特征的机器人视觉控制方法，该方法以三角形为视觉特征，根据三角形的质心以及三条边进行视觉控制，利用质心当前信息和期望信息的偏差，得到机器人的位置偏差，利用三条边的极坐标参数以及直线雅克比矩阵，得到机器人的姿态偏差，最终实现机器人的六自由度控制；本发明根据三角形的质心和三条边实现机器人的六自由度控制，计算量少，控制过程简单。相对于使用单一图像特征的传统视觉控制方法，本发明能够提高视觉***的鲁棒性，便于推广应用。

Description

一种基于三角形特征的机器人视觉控制方法

技术领域

本发明属于机器人控制领域，更具体地涉及一种基于三角形特征的视觉控制方法。

背景技术

在机器人控制领域，视觉控制是当前的一个重要研究方向，也是目前的研究热点之一。所谓视觉控制就是根据摄像机获得的视觉信息对机器人进行的控制，它仅利用视觉传感器就能够为机器人提高丰富的工作环境信息，被国内外研究者广泛应用在机器人视觉定位、目标跟踪、视觉避障等方面。

机器人视觉控制过程中，图像特征一般是作为反馈信号直接用于控制器设计或者位姿估计，因此特征的选择与提取是视觉***的一个基本且关键的问题，会对视觉***的性能产生重要影响。传统视觉控制过程中，点、边缘、直线是常用特征。此类特征提取简单，使用方便，但包含的特征信息有限，且易受目标物体的形状、纹理、噪声、光照条件等影响。

为了提高视觉控制的鲁棒性，研究人员探索使用更加复杂的目标特征进行机器人控制。Tahri等基于球面投影模型，提出一种解耦合控制机制，该机制利用点特征和多边形特征，结合特征在球面投影的特点，推导出深度无关的交互矩阵，实现目标的伺服控制(Tahri O,Mezouar Y,Chaumette F.Decoupled Image-based Visual Servoing forcameras obeying the unified projection model[J].IEEE Transactions onRobotics.2010,26(4):684-697.)。然而，该方法主要针对球面投影模型，不适用于基于小孔成像模型的传统相机。Song等将视觉控制用到机器人的轴-孔装配中，利用目标的轮廓和颜色信息，计算位置和姿态信息,实现复杂几何外形零件的位姿对准(Song H C,Kim Y L,Song J B.Automated guidance of peg-in-hole assembly tasks for complex-shapedparts.IEEE/RSJ International Conference on Intelligent Robots andSystems.Chicago,USA,2014.4517-4522)。该方法利用了能表征目标全局特性的轮廓和颜色信息，能够提高视觉控制的可靠性和准确性，但所选特征只能适用于特定的目标对象，应用受到制约。Xu等根据不同图像特征对不同自由度的敏感性，采用点、直线以及面积等多种视觉特征，实现视觉伺服过程中位置控制和姿态控制的解耦，进而完成六自由度机器人的高精度对准。(Xu D,Lu J Y,Wang P,Zhang Z T,Liang Z Z.Image-based visualservoing with separate design for orientation and position control usingdifferent sensitive features[J],IEEE Transactions on Systems,Man andCybernetics:Systems,2017,47(8):2233-2243.)。该方法充分利用了各个视觉特征的优势，能够获得较高的控制精度，但是需要估计点特征和直线特征垂点的深度，并求取各个视觉特征的雅克比矩阵，计算过程复杂。

发明内容

基于上述背景，本发明的主要目的在于提供一种基于三角形特征的机器人六自由度控制方法。

为达到上述目的，本发明以三角形为视觉特征，利用三角形的的质心以及三条边进行视觉控制。根据质心当前信息和期望信息的偏差，得到机器人的位置偏差，根据三条边的极坐标参数以及直线雅克比矩阵，得到机器人的姿态偏差，最终实现机器人的六自由度控制。

该方法包括步骤如下：

步骤S0，使用一个RGB-D相机获取目标图像；

步骤S1：从所述相机获得的目标图像中提取三角形的三个顶点信息，并计算三角形的质心，离线得到所述质心的期望特征，在线提取所述质心的当前特征，比较当前值和期望值，得到当前质心和期望质心之间的偏差；

步骤S2：任意选择三角形的一条边，根据该边在图像中的长度，以及目标的实际空间尺寸，得到目标的当前像素当量；

步骤S3：基于小孔模型，根据S1的三个顶点的信息，得到三个顶点在焦距归一化成像平面的坐标；

步骤S4：根据S3的三个顶点的归一化坐标，得到三角形的三条边在焦距归一化成像平面上的极坐标参数，离线得到三条边的期望极坐标参数，在线计算三条边的当前极坐标参数，比较当前值和期望值，得到二者之间的偏差；

步骤S5：根据S4所述的极坐标参数，得到直线的雅克比矩阵；

步骤S6：根据S4所述的极坐标参数偏差，以及S5的直线雅克比矩阵，得到机器人的姿态偏差；

步骤S7：根据S1的质心偏差，以及S2的当前像素当量，得到机器人的位置偏差；

步骤S8：根据S6的姿态偏差和S7的位置偏差，得到机器人的运动调整量，实现机器人的六自由度控制。

进一步的说，其中步骤S1中所述质心的偏差是计算如下的：

根据从RGB-D图像提取的三角形的三个顶点的信息(u_i,v_i,d_i)(i＝1,2,3),得到三角形质心的信息：

其中，u_i是第i个顶点在U方向的图像坐标，v_i是第i个顶点在V方向的图像坐标，d_i是第i个顶点的深度，u₀是质心在U方向的图像坐标，v₀是质心在V方向的图像坐标，d₀是质心的深度。

基于质心的当前特征和期望特征，得到偏差如下：

其中，△u、△v分别表示质心在U方向和V方向的图像坐标偏差，△z表示质心在笛卡尔空间的深度偏差。(u_0d,v_0d)表示质心的期望图像坐标，(u₀,v₀)表示质心的当前图像坐标，z_0d和z_0c分别表示质心在笛卡尔空间的期望深度和当前深度。

进一步的说，其中步骤S2中所述的当前像素当量是通过下式来计算的：

t_s＝L/L₀ (3)

其中，t_s表示当前的像素当量，S表示三角形某一条边在图像中的长度，S₀表示该边的实际空间长度。

进一步的说，其中步骤S3中所述的三个顶点在焦距归一化成像平面的坐标如下：

根据三个顶点的图像坐标，以及摄像机的内参数，得到第i个顶点(u_i,v_i)(i＝1,2,3)在焦距归一化成像平面上的坐标：

进一步的说，其中步骤S4中所述的三角形的三条边的极坐标参数偏差计算如下：

以第1个顶点和第2个顶点构成的一条边为例，首先求取该边的直线方程如下：

(x-x_1c1)/(x_1c2-x_1c1)＝(y-y_1c1)/(y_1c2-y_1c1) (5)

然后求取过成像坐标系原点，且与式(5)所示直线垂直的垂线方程，进而得到式(5)所示直线的垂点坐标(x_1c0,y_1c0,1)。根据直线的极坐标参数定义，将该垂点的坐标转化为极坐标，即可得到式(5)所示直线的参数ρ_i和θ_i：

三角形剩余两条边的直线参数求解与上述过程完全相同。因此，得到三角形三条边的参数偏差如下(本发明中，直线特征参数只用到了θ,下式只列出了参数θ的偏差)：

Δθ_i＝θ_di-θ_i；i＝1,2,3 (7)

进一步的说，其中步骤S5中所述的直线特征雅克比矩阵计算如下：

利用式(6)计算的三角形三条边的极坐标参数(ρ_i,θ_i)(i＝1,2,3),基于申请人提出的直线雅克比矩阵(徐德，卢金燕。直线特征的交互矩阵求取，自动化学报，2015,41(10):1762-1771)，得到直线雅克比矩阵L_lw如下：

进一步的说，其中步骤S6中所述的机器人的姿态偏差[w_rx,w_ry,w_rz]^T如下：

根据雅克比矩阵定义，得到相机的姿态偏差：

式中：L_lw ⁺是直线交互矩阵L_lw的伪逆。

L_lw ⁺＝(L_lw ^TL_lw)^-1L_lw ^T (10)

式中，L_lw ^T是L_lw的转置。

根据式(9)和手眼关系矩阵T_m,得到机器人姿态偏差：

进一步的说，其中步骤S7中所述的机器人的位置偏差[v_rx,v_ry,v_rz]^T如下：

其中，(△u,△v,△z)是式(2)得到的三角形质心偏差,t_s是式(3)得到的像素当量。

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

其中，λ_p是调整系数，[v_rx,v_ry,v_rz]^T是式(12)计算的机器人当前的位置偏差，[w_rx,w_ry,w_rz]^T是式(11)计算的机器人当前的姿态偏差。

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

本发明以三角形为视觉特征，利用三角形的的质心以及三条边，进行视觉控制。根据质心的当前信息和期望信息的偏差，得到机器人的位置偏差，根据三条边的极坐标参数以及直线雅克比矩阵，得到机器人的姿态偏差，最终实现机器人的六自由度控制。

本发明利用信息丰富的三角形作为视觉特征，由于任意两两相交的三条直线都可以构成一个三角形，因此所选特征容易获取，使用方便。本发明根据三角形的质心和三条边实现机器人的六自由度控制，计算量少，控制过程简单。相对于使用单一图像特征的传统视觉控制方法，本发明能够提高视觉***的鲁棒性，便于推广应用。

附图说明

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

具体实施方式

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

本发明公开了一种基于三角形特征的机器人视觉控制方法，该方法以三角形为视觉特征，利用三角形的质心以及三条边进行视觉控制。根据质心当前信息和期望信息的偏差，得到机器人的位置偏差，根据三条边的极坐标参数以及直线雅克比矩阵，得到机器人的姿态偏差，最终实现机器人的六自由度控制。

更具体地，作为本发明的一个优选实施例，如图1示出本发明的基于三角形特征的机器人视觉控制方法的框图。在控制过程中，基于提取的三角形特征，先得到三角形的质心，然后根据质心的图像偏差和深度偏差，得到机器人的位置偏差，根据三条边的极坐标参数，得到直线雅克比矩阵，进而得到机器人的姿态偏差，最终实现机器人的六自由度控制。该方法包括以下步骤：

第一步：从目标图像中提取三角形的三个顶点信息，并计算三角形的质心，离线得到所述质心的期望特征，在线提取所述质心的当前特征，比较当前值和期望值，得到当前质心和期望质心之间的偏差；

第二步：任意选择三角形的一条边，根据该边在图像中的长度，以及目标的实际空间尺寸，得到目标的当前像素当量；

第三步：基于小孔模型，根据第一步提取的三个顶点信息，得到三个顶点在焦距归一化成像平面的坐标；

第四步：根据第三步计算的三个顶点的归一化坐标，得到三角形的三条边在焦距归一化成像平面上的极坐标参数，离线得到所述三条边的期望极坐标参数，在线计算三条边的当前极坐标参数，比较当前值和期望值，得到二者之间的偏差；

第五步：根据第四步计算的极坐标参数，得到直线雅克比矩阵；

第六步：根据第四步计算的极坐标参数偏差，以及第五步的直线雅克比矩阵，得到机器人的姿态偏差；

第七步：根据第一步的质心偏差，以及第二步的当前像素当量，得到机器人的位置偏差；

第八步：根据第六步的姿态偏差和第七步的位置偏差，得到机器人的运动调整量，实现机器人的六自由度控制。

所述第一步，具体如下：

根据获取的目标图像，提取三角形特征，得到式(2)所示的质心偏差。

所述第二步，具体如下：

任意选择三角形的一条边，根据该边在图像中的长度，以及目标的实际空间尺寸，得到式(3)所示的当前像素当量。

所述第三步，具体如下：

基于小孔模型，根据三角形的三个顶点坐标，得到如式(4)所示的三个顶点在焦距归一化成像平面的坐标；

所述第四步，具体如下：

根据三角形三条边的直线方程，得到式(6)所示的极坐标参数，进而得到式(7)所示的参数偏差。

所述第五步，具体如下：

基于第四步的极坐标参数，得到式(8)所示的直线雅克比矩阵。

所述第六步，具体如下：

根据第四步的极坐标参数偏差，以及第五步的直线雅克比矩阵，得到式(11)所示的机器人姿态偏差。

所述第七步，具体如下：

根据第一步的质心偏差，以及第二步的像素当量，得到式(12)所示的机器人位置偏差。

所述第八步，具体如下：

根据第六步的姿态偏差和第七步的位置偏差，得到式(13)所示的机器人运动调整量，实现机器人的六自由度控制。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于三角形特征的机器人视觉控制方法，包括以下步骤：

步骤S0，使用一个RGB-D相机获取目标图像；

步骤S1：从所述相机获得的目标图像中提取三角形特征，并计算三角形的质心，离线得到所述质心的期望特征，在线提取所述质心的当前特征，比较当前值和期望值，得到当前质心和期望质心之间的偏差；

步骤S2：任意选择三角形的一条边，根据该边在图像中的长度，以及目标的实际空间尺寸，得到目标的当前像素当量；

步骤S3：基于小孔模型，根据三角形的顶点信息，得到三个顶点在焦距归一化成像平面的坐标；

步骤S4：根据S3的三个顶点的归一化坐标，得到三角形的三条边在焦距归一化成像平面上的极坐标参数方程，离线得到三条边的期望参数，在线计算三条边的当前参数，比较当前值和期望值，得到二者之间的偏差；

步骤S5：根据S4所述的极坐标参数，得到直线的雅克比矩阵；

步骤S6：根据S4所述的极坐标参数偏差，以及S5的直线雅克比矩阵，得到机器人的姿态偏差；

步骤S7：根据S1的质心偏差，以及S2的当前像素当量，得到机器人的位置偏差；

步骤S8：根据S6的姿态偏差和S7的位置偏差，得到机器人的运动调整量，实现机器人的六自由度控制；

其中步骤S1中所述质心的偏差是计算如下的：

根据从RGB-D图像提取的三角形的三个顶点的信息(u_i,v_i,d_i)，i＝1,2,3,得到三角形质心的信息：

其中，u_i是第i个顶点在U方向的图像坐标，v_i是第i个顶点在V方向的图像坐标，d_i是第i个顶点的深度，u₀是质心在U方向的图像坐标，v₀是质心在V方向的图像坐标，z₀是质心的深度；

基于质心的当前特征和期望特征，得到偏差如下：

其中，△u、△v分别表示质心在U方向和V方向的图像坐标偏差，△z表示质心在笛卡尔空间的深度偏差；(u_0d,v_0d)表示质心的期望图像坐标，(u₀,v₀)表示质心的当前图像坐标，z_0d和z₀分别表示质心在笛卡尔空间的期望深度和当前深度；

其中步骤S2中所述的当前像素当量是通过下式来计算的：

t_s＝L/L₀ (3)

其中，t_s表示当前的像素当量，L表示三角形某一条边在图像中的长度，L₀表示该边的实际空间长度；

其中步骤S4中所述的三角形的三条边的极坐标参数偏差计算如下：

针对第1个顶点和第2个顶点构成的一条边，首先求取该边的直线方程如下：

(x-x_1c1)/(x_1c2-x_1c1)＝(y-y_1c1)/(y_1c2-y_1c1) (5)

然后求取过成像坐标系原点，且与式(5)所示直线垂直的垂线方程，进而得到式(5)所示直线的垂点坐标(x_1c0,y_1c0,1)；根据直线的极坐标参数定义，将该垂点的坐标转化为极坐标，即可得到式(5)所示直线的参数ρ_i和θ_i：

三角形剩余两条边的直线参数求解与上述过程完全相同；因此，得到三角形三条边的参数偏差如下：

Δθ_i＝θ_di-θ_i；i＝1,2,3 (7)；

其中步骤S6中所述的机器人的姿态偏差[w_rx,w_ry,w_rz]^T如下：

根据雅克比矩阵定义，得到相机的姿态偏差：

式中：L_lw ⁺是直线雅克比矩阵L_lw的伪逆；

L_lw ⁺＝(L_lw ^TL_lw)^-1L_lw ^T (10)

式中，L_lw ^T是L_lw的转置；

根据式(9)和手眼关系矩阵T_m,得到机器人姿态偏差：

其中步骤S7中所述的机器人的位置偏差[v_rx,v_ry,v_rz]^T如下：

其中，(△u,△v,△z)是式(2)得到的三角形质心偏差,t_s是式(3)得到的像素当量。

2.根据权利要求1所述的基于三角形特征的机器人视觉控制方法，其中步骤S3中所述的三个顶点在焦距归一化成像平面的坐标如下：

根据三个顶点的图像坐标(u_i,v_i)，i＝1,2,3，以及摄像机的内参数，得到第i个顶点在焦距归一化成像平面上的坐标(x_1ci,y_1ci,1)，i＝1,2,3：

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

利用式(6)计算的三角形三条边的极坐标参数(ρ_i,θ_i)，i＝1,2,3,基于现有的直线雅克比矩阵，得到直线雅克比矩阵L_lw如下：

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

其中，λ_p是调整系数，[v_rx,v_ry,v_rz]^T是式(12)计算的机器人当前的位置偏差，[w_rx,w_ry,w_rz]^T是式(11)计算的机器人当前的姿态偏差。

Images