CN105196292A - 一种基于迭代变时长视觉伺服控制方法 - Google Patents

Abstract

本发明公开了一种基于变时长迭代的机械臂视觉伺服控制方法，包含以下步骤：1)以示教的方式获得一系列图片；2)得到当前图片和跟踪图片关系定义图像特征，以图像特征表达了机械臂的运动情况；3)基于机械臂运动学模型和相机模型建立视觉控制系统的相互关系模型，采用迭代前馈加反馈的控制方案；4)机械臂运行过程中出现目标物体不在视觉范围内，则此次迭代终止。本发明不要求在运动过程中目标持续可见，同时在一定程度上保证了机械臂能够精确跟踪目标图像。

Description

一种基于迭代变时长视觉伺服控制方法

技术领域

本发明涉及工业机械臂领域的视觉跟踪方法，特别是涉及一种基于迭代变时长策略的视觉伺服控制方法。

背景技术

机械臂视觉跟踪是以视觉系统作为反馈信息控制机械臂运动的方法，是机械臂研究的重要部分。视觉跟踪是目前工业机器人研究中的热点和难点，在工业机械臂执行大环境或复杂环境下任务时起到重要的作用。

机械臂视觉跟踪技术可以分为基于位置的方法和基于图像的方法。其中，基于位置的方法将视觉系统作为位置传感器使用，根据位置误差控制机械臂运动，这种方法控制器设计简单，适用于低精度跟踪，具有误差累积效应。基于图像的方法使用视觉系统反馈的图像作为反馈信息，控制机械臂运动到拍摄到与目标图像一致的场景处，这种方法精度高，但是由于基于图像的视觉控制只有一个相机，缺少深度信息，导致模型的不确定性，这样会影响系统的稳定性和收敛性，而且要求目标图像在整个运动过程中都可见。

发明内容

本发明为了克服现有技术的不足，提供了一种基于迭代变时长策略的机械臂视觉伺服控制方法，该方法在保证视觉跟踪稳定性和收敛性的同时，不要求在运动过程中目标持续可见，实现成本较低。

本发明的目的是通过以下技术方案来实现的：一种基于迭代变时长策略的机械臂视觉伺服控制方法，用于机械臂目标图像的跟踪，所述的机械臂为六自由度无冗余工业机械臂，自带单目摄像头可实时采集图像信息。该方法将跟踪任务转化到基于图像单应性定义的图像特征空间中，设计控制器在图像特征空间中进行轨迹跟踪控制，包括以下步骤：

(1)手持相机，沿跟踪轨迹拍摄目标物体的一系列图片作为参考图片，所述目标物体的一个平面上标注三个以上标记点，并且所有参考图片中均包含该平面；

(2)机械臂开始运动，由摄像头获取图像信息，根据其与步骤(1)得到的参考图片的差异，通过利用单应性关系定义图像特征，用六个和单应性矩阵相关的变量作为系统误差；

(3)将步骤(2)得到的系统误差作为控制器输入，由控制器输出信号控制机械臂运动，从而跟踪步骤1得到的一系列参考图片；所述控制器包括两部分：反馈控制器和迭代控制器，所述反馈控制器为P反馈控制器，所述迭代控制器为PID学习率迭代控制器；

(4)在机械臂运动过程中出现目标物体超出视野范围时，此次迭代结束，记迭代时间为T_k，T_k时间后，控制器的输入值、系统的误差和误差变化率均为零；

(5)迭代多次控制机械臂运动，直到拍摄图片和参考图片误差满足终止条件或者达到迭代次数，实现按照参考轨迹的机械臂视觉伺服跟踪控制。

步骤(2)中所述的单应性图像特征由公式(4)定义，公式(4)为：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}$

其中，H为单应性矩阵，(R,t)表示了机械臂的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离，K为相机内部参数矩阵。

步骤(2)中所述利用单应性关系定义图像特征具体为：依次求得广义单应性矩阵和狭义单应性矩阵；

所述广义单应性矩阵G如公式(1)所示：

$\frac{Z}{Z_r}m={\mathrm{Gm}}_r---\left(1\right)$

其中，m和m_r分别表示同一特征在当前图片和参考图片的像素点位置坐标，Z和Z_r分别表示同一特征点在当前图像坐标系和参考图像坐标系下的深度；

所述狭义单应性矩阵H如公式(2)所示：

H＝K^-1GK(2)

$K=\left[\begin{array}{ccc}{f}_u& 0& 0\\ 0& f_v& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

K为相机内部参数矩阵，f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目。

步骤(2)中所述系统误差e如公式(5)(6)(7)所示，如下：

e＝[e_te_r]^T(5)

$e_t=\frac{Z}{Z_r}m-m_r=(H-I)m_r---\left(6\right)$

e_r＝vex(H-H^T)(7)

步骤(3)中，所述P反馈控制器具体为：控制器输出和本次迭代上个时刻的系统误差e(t)成正比例关系，如公式(8)所示：

$u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)---\left(8\right)$

其中，K_fd为反馈控制器的比例系数；

所述PID学习率迭代控制器具体为：控制器输出与上次迭代的输出u_k-1(t)、系统误差e_k-1(t)和系统误差变化率关系如公式(9)所示：

$u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)---\left(9\right)$

其中，K_p为迭代控制器的P控制系数，K_d为迭代控制器的D控制系数；

所述控制器输出u_k(t)如公式(10)所示：

$u_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)---\left(10\right)$

步骤(4)迭代变时长策略如公式(11)所示，如下：

$\left\{\begin{array}{cc}{u}_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)& t\∈(0,T_k)\\ u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)& t\∈(0,T_k)\\ u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)& t\∈(0,T_k)\\ \begin{array}{ccc}{u}_k\left(t\right)=0& e_k\left(t\right)=0& {\stackrel{\·}{e}}_k\left(t\right)=0\end{array}& t\∈(T_k,T_d)\end{array}\right.---\left(11\right)$

其中，T_k为第k次的迭代时间，T_d为跟踪轨迹的期望时间。

本发明的有益效果是，本发明基于迭代变时长策略的改进图像跟踪视觉方法，在传统的基于图像视觉伺服控制方式的基础上予以改进，采用迭代的方式，克服由于深度不确定导致的系统不稳定性，而且又针对基于图像要求的目标持续可见性提出新的方法，通过变时长的方式在一定程度上克服了这样的视野约束。

附图说明

图1是本发明的流程图；

图2是机械臂物理装置图；

图3是控制过程中物体在图像中的位姿图；

图4是某次迭代终止图；

图5单应性矩阵图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

所谓迭代变时长控制策略，即机械臂多次跟踪在图像空间中的轨迹，每次跟踪受到两方面的影响，一为本次迭代的反馈误差，二为上次迭代的输入和误差，如果目标物体跑出视野外，则此次迭代结束。

本发明方法用于工业机械臂跟踪一系列目标图像的场景，首先手持相机，得到一系列的参考图片集合{I^j}，I^j表示第j幅图片。提取参考图片的特征点，根据特征点像素位置得到与参考图片的单应性矩阵，如图5所示，从而得到系统误差。采用迭代和反馈结合的控制方案，即控制器输出和本次迭代上个时刻误差有关，同时也和之前的迭代信息相关，整个控制框图如图1所示。如果某次迭代过程中目标物体跑出视野外，如图4所示，则此次迭代终止，下次迭代可以利用上次迭代终止前的信息，这样可以有效利用每次迭代的信息。具体包括以下步骤：

步骤1：手持相机，沿跟踪轨迹拍摄目标物体的一系列图片，得到参考图片集合{I_r ^j}，表示第j幅图片，最后一张图片为目标位姿，如图3所示；所述目标物体的一个平面上标注9个标记点，并且所有参考图片中均包含该平面，如图1所示；

步骤2：机械臂运行过程中，实时拍摄图片{I^j}，利用sift算法提取特征点，根据两个图片I^j和特征点像素点位置，求得单应性矩阵，如公式(1)所示，首先求得广义单应性矩阵G；

$\frac{Z}{Z_r}m={\mathrm{Gm}}_r---\left(1\right)$

其中，m和m_r分别表示同一特征在当前图片和参考图片的像素点位置坐标，Z和Z_r分别表示同一特征点在当前图像坐标系和参考图像坐标系下的深度，利用最小二乘法可以得到带有比例的广义单应性矩阵，后期采取了比例反馈控制，求得的带有比例的单应性矩阵，可以作为广义单应性矩阵，不影响结果。

根据广义单应性矩阵G求解狭义单应性矩阵H，如下式所示：

H＝K^-1GK(2)

$K=\left[\begin{array}{ccc}{f}_u& 0& 0\\ 0& f_v& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

其中，K为相机内部参数矩阵，f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目。

单应性矩阵对应的旋转和平移的关系，如下式：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}---\left(4\right)$

其中，(R,t)表示了机器人的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离。

步骤3：根据步骤2得到的单应性矩阵求得系统误差e，如公式(5)(6)(7)所示：

e＝[e_te_r]^T(5)

$e_t=\frac{Z}{Z_r}m-m_r=(H-I)m_r---\left(6\right)$

e_r＝vex(H-H^T)(7)

当且仅当e全为零时，机械臂末端相机拍摄的图片和参考图片相同。

步骤4：将步骤3得到的系统误差作为控制器输入，设计控制器得到控制器输出信号，作为机械臂输入信号，控制机械臂运动。控制器分为两个部分，分别为反馈控制器和迭代控制器；

所述的反馈控制器为P反馈控制器，具体为：控制器输出和本次迭代上个时刻的系统误差成正比例关系，如公式(8)所示：

$u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)---\left(8\right)$

其中，K_fd为反馈控制器的比例系数；

所述的迭代控制器为PID学习率迭代控制器，具体为：控制器输出与上次迭代的输出u_k-1(t)、系统误差e_k-1(t)和系统误差变化率的关系如公式(9)所示：

$u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)---\left(9\right)$

K_p为迭代控制器的P控制系数，K_d为迭代控制器的D控制系数。

系统控制器输出u_k(t)为上述两个控制器的叠加，如公式(10)所示：

$u_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)---\left(10\right)$

步骤5：在迭代过程中，目标物体超出视野时，此次迭代结束，记迭代时间为T_k，T_k时间后，系统的输入值、误差和误差变化率均为零，具体如公式(11)所示，如下：

$\left\{\begin{array}{cc}{u}_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)& t\∈(0,T_k)\\ u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)& t\∈(0,T_k)\\ u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)& t\∈(0,T_k)\\ \begin{array}{ccc}{u}_k\left(t\right)=0& e_k\left(t\right)=0& {\stackrel{\·}{e}}_k\left(t\right)=0\end{array}& t\∈(T_k,T_d)\end{array}\right.---\left(11\right)$

T_d为期望的时间。

步骤6：机械臂通过多次迭代直到拍摄图片和参考图片误差满足终止条件或者达到迭代次数，实现按照参考轨迹的机械臂视觉伺服跟踪控制。

Claims (6)

1.一种基于迭代变时长策略的机械臂视觉伺服控制方法，用于机械臂目标图像的跟踪，所述机械臂为六自由度无冗余工业机械臂，自带单目摄像头可实时采集图像信息，其特征在于，该方法包括以下步骤：

(1)手持相机，沿跟踪轨迹拍摄目标物体的一系列图片作为参考图片，所述目标物体的一个平面上标注三个以上标记点，并且所有参考图片中均包含该平面；

(2)机械臂开始运动，由摄像头获取图像信息，根据其与步骤(1)得到的参考图片的差异，通过利用单应性关系定义图像特征，用六个和单应性矩阵相关的变量作为系统误差；

(3)将步骤(2)得到的系统误差作为控制器输入，由控制器输出信号控制机械臂运动，从而跟踪步骤1得到的一系列参考图片；所述控制器包括两部分：反馈控制器和迭代控制器，所述反馈控制器为P反馈控制器，所述迭代控制器为PID学习率迭代控制器；

(4)在机械臂运动过程中出现目标物体超出视野范围时，此次迭代结束，记迭代时间为T_k，T_k时间后，控制器的输入值、系统的误差和误差变化率均为零；

(5)迭代多次控制机械臂运动，直到拍摄图片和参考图片误差满足终止条件或者达到迭代次数，实现按照参考轨迹的机械臂视觉伺服跟踪控制。

2.根据权利要求1所述一种基于迭代变时长策略的机械臂视觉伺服控制方法，其特征在于，所述步骤(2)中所述利用单应性关系定义图像特征具体为：依次求得广义单应性矩阵和狭义单应性矩阵；

所述广义单应性矩阵G如公式(1)所示：

$\frac{Z}{Z_r}m={\mathrm{Gm}}_r---\left(1\right)$

其中，m和m_r分别表示同一特征在当前图片和参考图片的像素点位置坐标，Z和Z_r分别表示同一特征点在当前图像坐标系和参考图像坐标系下的深度；

所述狭义单应性矩阵H如公式(2)所示：

H＝K^-1GK(2)

$K=\left[\begin{array}{ccc}{f}_u& 0& 0\\ 0& f_v& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

其中，K为相机内部参数矩阵，f_u、f_v分别表示焦距在像素坐标轴上对应的像素点数目。

3.根据权利要求2所述一种基于迭代变时长策略的机械臂视觉伺服控制方法，其特征在于，

所述步骤(2)中所述系统误差e如公式(5)(6)(7)所示：

e＝[e_te_r]^T(5)

$e_t=\frac{Z}{Z_r}m-m_r=(H-I)m_r---\left(6\right)$

e_r＝vex(H-H^T)(7)。

4.根据权利要求1所述一种基于迭代变时长策略的机械臂视觉伺服控制方法，其特征在于，所述步骤(2)中所述的单应性图像特征由公式(4)定义：

$H=K(R-\frac{{\mathrm{tn}}^T}{d})K^{-1}---\left(4\right)$

其中，H为单应性矩阵，(R,t)表示了机械臂的运动状态，n为相机坐标系下地面的法向量，d为相机坐标系原点与地面之间的距离，K为相机内部参数矩阵。

5.根据权利要求1所述一种基于迭代变时长策略的机械臂视觉伺服控制方法，其特征在于，所述步骤(3)中，所述P反馈控制器具体为：控制器输出和本次迭代上个时刻的系统误差e(t)成正比例关系，如公式(8)所示：

$u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)---\left(8\right)$

其中，K_fd为反馈控制器的比例系数；

所述PID学习率迭代控制器具体为：控制器输出与上次迭代的输出u_k-1(t)、系统误差e_k-1(t)和系统误差变化率关系如公式(9)所示：

$u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)---\left(9\right)$

其中，K_p为迭代控制器的P控制系数，K_d为迭代控制器的D控制系数；

所述控制器输出u_k(t)如公式(10)所示：

$u_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)---\left(10\right)$

6.根据权利要求5所述一种基于迭代变时长策略的机械臂视觉伺服控制方法，其特征在于，所述步骤(4)迭代变时长策略如公式(11)所示：

$\left\{\begin{array}{cc}{u}_k\left(t\right)=u_k^{fb}\left(t\right)+u_k^{ff}\left(t\right)& t\∈(0,T_k)\\ u_k^{fb}\left(t\right)=-K_{fd}e\left(t\right)& t\∈(0,T_k)\\ u_k^{ff}\left(t\right)=u_{k-1}\left(t\right)+K_p{e}_{k-1}\left(t\right)+K_d{\stackrel{\·}{e}}_{k-1}\left(t\right)& t\∈(0,T_k)\\ \begin{array}{ccc}{u}_k\left(t\right)=0& e_k\left(t\right)=0& {\stackrel{\·}{e}}_k\left(t\right)=0\end{array}& t\∈(T_k,T_d)\end{array}\right.---\left(11\right)$

其中，T_k为第k次的迭代时间，T_d为跟踪轨迹的期望时间。