CN105527980A - 双目视觉系统目标跟踪控制方法 - Google Patents

Abstract

本发明涉及多自由度双目视觉系统的运动学建模及目标跟踪控制方法，针对多自由度双目视觉系统目标跟踪控制任务，提出了一种的基于虚拟移动关节的目标跟踪控制方法。该方法在双目视觉系统的多自由度臂中引入了一个虚拟的移动关节，通过增加1个虚拟移动关节变量d，替换掉双目视觉算法中图像空间的4个参数(u_l,v_l)和(u_r,v_r)，将n自由度双目视觉系统的逆运动学求解转化为n+1自由度机械臂的逆运动学求解，进而利用传统的D-H法进行逆运动学求解。本发明解决了多自由度双目视觉系统的目标跟踪控制算法问题。本算法具有计算量小，求解简单的优点，成功应用于一个3自由度的双目视觉系统的目标跟踪控制任务。

Description

双目视觉系统目标跟踪控制方法

技术领域

本发明涉及机器人领域多自由度双目视觉系统的运动控制问题，尤其适用于行星车多自由度双目视觉系统对目标的跟踪控制。

背景技术

双目视觉测量在机器人领域广泛应用。在航天工程领域，行星车多自由度双目视觉系统是一个应用典型，可用于对目标进行搜索和跟踪。一般是将双目视觉系统安装在多自由度臂末端的平台上，通过多自由度臂扩展双目视觉相机的跟踪范围。行星车多自由度双目视觉系统的目标跟踪任务可由如图1所示的模型表示，包括三个组成部分：多自由度臂、双目视觉相机(包括左眼和右眼)和目标。图中{0}、和{T}分别表示多自由度臂的基坐标系、左眼相机坐标系和目标坐标系，{i}(i＝1，2，…，n)表示多自由度臂各关节坐标系。对于行星车多自由度双目视觉系统的目标跟踪任务，一般选取目标物上的一特征点作为跟踪目标点，由于相机镜头边缘畸变的问题，因此希望该目标成像点能始终位于相机图像的中心(本算法以目标成像点位于左眼相机图像的中心为例)。

多自由度双目视觉系统的目标跟踪控制方法需要建立运动学模型：一是正运动学模型，即已知多自由度臂各关节{i}的位置参数，求得目标坐标系{T}在基坐标系{0}中的位姿参数，由此可获得目标的位置信息；二是逆运动学，即已知目标坐标系{T}在基坐标系{0}中的期望位姿参数，求得相应的多自由度臂各关节{i}的位置参数，由此可用于对多自由度臂进行控制，调整相机运动，实现对目标跟踪。

目前，常规的运动学算法在求解逆运动学时，需要联合基坐标系下正运动学方程和图像坐标系下的控制目标方程(目标点在相机坐标系下X、Y值为0)，求解困难，计算繁琐。

发明内容

本发明的目的在于提供计算方式简便的双目视觉系统目标跟踪控制方法。

为解决上述技术问题，本发明提供了一种双目视觉系统目标跟踪控制方法，包括以下步骤：

S1、通过正运动学取得目标点在基坐标系中的位置；

S2、根据目标点在基坐标系中的位置，通过逆运动学取得多自由度机械臂各关节期望位置；

S3、根据多自由度机械臂各关节期望位置以及多自由度机械臂当前位置计算出多自由度机械臂各关节的控制角度；

其中，在求解所述逆运动学中，引入虚拟移动变量d和虚拟坐标系{C_l’}，虚拟坐标系{C_l’}从基坐标系沿光轴方向移动d个单位，使虚拟坐标系{C_l’}的原点与目标点重合，通过虚拟移动关节变量d和虚拟坐标系{C_l’}利用D-H法计算出多自由度机械臂各关节的期望位置。

其中，D-H法为Denavit和Hartenberg于1955年提出了一种为关节链中的每一杆件建立坐标系的矩阵方法，这种方法在机器人的每个连杆上都固定一个坐标系，然后用4×4的齐次变换矩阵来描述相邻两连杆的空间关系。通过依次变换可最终推导出末端执行器相对于基坐标系的位姿，从而建立机器人的运动学方程。

作为优选的，目标点在基坐标系中的位置通过D-H法计算出。

作为优选的，目标点在基坐标系中的位置的运动学模型为

$P_T^0(x_0,y_0,z_0)=T_1^0\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_n^{n-1}\left({\mathrm{\θ}}_n\right)\·T_{c_l}^N\·P_T^{c_l}(x_{c_l},y_{c_l},z_{c_l}).$

作为优选的，目标点在基坐标系中的位置为其中，

$x_{c_l}=\frac{b(u_l-u_0)}{u_l-u_r},y_{c_l}=\frac{{\mathrm{bk}}_x(v_l-v_0)}{k_y(u_l-u_0)},z_{c_l}=\frac{{\mathrm{bk}}_x}{u_l-u_r}.$

作为优选的，逆运动学的求解为n+1自由度机械臂的逆运动学求解。

作为优选的，虚拟移动关节变量d的位置参数为d＝f(x₀,y₀,z₀)。

作为优选的，多自由度机械臂各关节的位置参数为θ_i＝f(x₀,y₀,z₀)，(i＝1,2,…,n)。

作为优选的，正运动学的正运动学方程为

$P_T^B(x,y,z)=T_1^B\left({\mathrm{\θ}}_1\right)\·\frac{1}{2}T\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_i^{i-1}\left({\mathrm{\θ}}_i\right)\·P_c^i\left(d\right).$

作为优选的，通过正运动学方程求出多自由度机械臂位于目标位置时的多自由度机械臂各关节的位置参数θ’_i，求出关节控制量Δθ。

作为优选的，目标点处于运动状态时，双目视觉系统能够根据关节控制量Δθ控制多自由度机械臂移动，直至关节控制量Δθ＝0。

本发明相比现有的双目视觉系统目标跟踪控制方法，利用虚拟移动关节变量d，替换双目视觉系统目标跟踪控制方法的参数(u_l,v_l)和(u_r,v_r)，将n自由度双目视觉系统的逆运动学求解转化为n+1自由度机械臂的逆运动学求解，这样便可以使用传统的D-H法进行求解，具有算法巧妙、且计算量小的特点。

附图说明

图1为本发明的正运动学算法模型示意图。

图2为本发明的逆运动学算法模型示意图。

图3为本发明的目标跟踪算法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。

在本发明的实施例中，对图1中所示的目标点位置信息，采用齐次变换可以建立运动学模型，该运动学模型为

$P_T^0=T_{C_l}^0\·P_T^{C_l}---\left(1\right)$

令目标点在基坐标系下的位置为在左眼相机坐标系下的位置为多自由度臂的各关节位置为θ_i(i＝1，2，…，n)，则(1)式可写成

$P_T^0(x_0,y_0,z_0)=T_1^0\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_n^{n-1}\left({\mathrm{\θ}}_n\right)\·T_{C_l}^n\·P_T^{C_l}(x_{C_l},y_{C_l},z_{C_l})---\left(2\right)$

式中，为多自由度臂的连杆变换，i＝1，2，…，n；为左眼相机相对多自由度臂末关节{n}的位姿。

目标点在左眼相机坐标系下的位置可以通过双目视觉模型求解，如以下各式所示：

$x_{C_l}=\frac{b(u_l-u_0)}{u_l-u_r}---\left(3\right)$

$y_{C_i}=\frac{{\mathrm{bk}}_x(v_l-v_0)}{k_y(u_l-u_r)}---\left(4\right)$

$z_{C_l}=\frac{{\mathrm{bk}}_x}{u_l-u_r}---\left(5\right)$

式中k_x,k_y,u₀,v₀为相机内参数，通过标定得到；b为左右眼相机光轴的间距，且有(u_l，v_l)和(u_r，v_r)分别为目标点在左右眼相机的图像坐标。

因而，可由(2)式求得目标点在基坐标系下的位置此即为多自由度双目视觉系统的正运动学。

如图2所示，在左眼相机光轴方向虚设一个移动关节，左眼相机坐标系{C_l}可以沿相机光轴向前移动距离d，变为坐标系{C'_l}，则d为关节变量。当坐标系{C'_l}原点移动至与目标点(坐标系{T}的原点)重合时，即为满足目标点位于左眼相机图像的中心。实现让右眼相机光轴通过目标点。

因为则(2)式可写为

$P_T^0(x_0,y_0,z_0)=T_1^0\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_n^{n-1}\left({\mathrm{\θ}}_n\right)\·P_T^n---\left(9\right)$

由前述算法坐标系{C'_l}原点移动至与坐标系{T}的原点重合时，有

$P_T^n=P_c^n\left(d\right)---\left(10\right)$

所以(9)式可写为

$P_T^B(x,y,z)=T_1^B\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_i^{i-1}\left({\mathrm{\θ}}_i\right)\·P_c^i\left(d\right)---\left(11\right)$

(11)式即为基于虚拟移动关节的多自由度双目视觉系统的正运动学方程。

基于虚拟移动关节的逆运动学算法基本思想是：将“目标点在左眼相机的成像点位于图像中心”这一图像坐标系几何关系，转化为“左眼相机光轴通过目标点”这一基坐标系几何关系。同时将“左眼相机光轴通过目标点”这一控制目标通过引入一个虚拟移动关节实现，从而在逆运动学求解时，通过增加1个虚拟移动关节变量d，替换掉双目视觉算法中图像空间的4个参数(u_l，v_l)和(u_r，v_r)。

据此，根据(11)式，可以将n自由度双目视觉系统的逆运动学求解转化为n+1自由度机械臂的逆运动学求解，这样便可利用传统的D-H法进行求解。即已知目标点在基坐标系{B}中的位置(x₀,y₀,z₀)，求得相应多自由度臂各关节及虚拟移动关节的位置参数：

θ_i＝f(x₀,y₀,z₀)，i＝1，2，…，n(12)

d＝f(x₀,y₀,z₀)(13)

其中多自由度臂各关节的Δθ为真正所需求解的控制参数，虚拟移动关节的d值只作为中间量用于计算。此算法可有效减少求解的难度和计算量。

常规算法(2)式在工程应用中，当已知多自由度臂的各关节位置(由关节位置传感器获得)和目标点在左右眼相机的图像坐标(u_l，v_l)和(u_r，v_r)(由相机图像处理获得)，就可以直接进行正运动学求解，但却难以求解逆运动学。而基于虚拟移动关节算法中虚拟变量d无法测量，因此(11)式实际上无法真正用于求解运动学正解，但(11)式引入了1个虚拟变量d，替代了(2)式中的双目视觉模型所需的4个参数，可有效地利用D-H法进行逆运动学求解。因此，常规算法用于求解正运动学，基于虚拟移动关节算法用于求解逆运动学，两者结合则解决了多自由度双目视觉系统的目标跟踪控制算法问题。

根据上述模型及算法，可以给出多自由度双目视觉系统对目标跟踪的控制算法的流程图，具体实施流程如图3所示，系统先读取当前时刻t时各关节的θ_i值，以及左眼相机图像的坐标值(u_l,v_l)、(u_r,v_r)，代入上方(2)式中，求出当前时刻t时的正解(x₀,y₀,z₀)，再代入(11)式中求出期望位置下的θ_i值，求取出关节控制量Δθ。若此时目标处于运动状态，本系统会对在每一个采样周期Δt下重复上述步骤求取出关节控制量Δθ，直至Δθ＝0，即目标物静止时，停止上述步骤。

本领域的普通技术人员可以理解，上述实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，例如将本实施例中的以左眼相机作为基准替换为以右眼相机作为基准替换等有效替换均属于本发明的保护范围，不偏离本发明的精神和范围。

Claims (10)

1.一种双目视觉系统目标跟踪控制方法，其特征在于，包括以下步骤：

S1、通过正运动学取得目标点在基坐标系中的位置；

S2、根据所述目标点在基坐标系中的位置，通过逆运动学取得多自由度机械臂各关节期望位置；

S3、根据所述多自由度机械臂各关节期望位置和所述多自由度机械臂当前位置计算出所述多自由度机械臂各关节的控制角度；

其中，在求解所述逆运动学中，引入虚拟移动变量d和虚拟坐标系{C_l’}，所述虚拟坐标系从所述基坐标系沿光轴方向移动所述d个单位，使所述虚拟坐标系{C_l’}的原点与目标点重合，通过所述虚拟移动关节变量d和所述虚拟坐标系{C_l’}利用D-H法计算出多自由度机械臂各关节的期望位置。

2.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述目标点在基坐标系中的位置通过D-H法计算出。

3.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述目标点在基坐标系中的位置的运动学模型为

$P_T^0(x_0,y_0,z_0)=T_1^0\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_n^{n-1}\left({\mathrm{\θ}}_n\right)\·T_{c_l}^N\·P_T^{c_l}(x_{c_l},y_{c_l},z_{c_l}).$

4.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述目标点在基坐标系中的位置为其中，

$x_{c_l}=\frac{b(u_l-u_0)}{u_l-u_r},y_{c_l}=\frac{{\mathrm{bk}}_x(v_l-v_0)}{k_y(u_l-u_0)},z_{c_l}=\frac{{\mathrm{bk}}_x}{u_l-u_r}.$

5.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述逆运动学的求解为n+1自由度机械臂的逆运动学求解。

6.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述虚拟移动关节变量d的位置参数为d＝f(x₀，y₀，z₀)。

7.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述多自由度机械臂各关节的位置参数为θ_i＝f(x₀，y₀，z₀)，(i＝1,2,…,n)。

8.根据权利要求1所述的双目视觉系统目标跟踪控制方法，其特征在于，所述正运动学的正运动学方程为

$P_T^B(x,y,z)=T_1^B\left({\mathrm{\θ}}_1\right)\·T_2^1\left({\mathrm{\θ}}_2\right)\·\mathrm{...}\·T_i^{i-1}\left({\mathrm{\θ}}_i\right)\·P_c^i\left(d\right).$

9.根据权利要求8所述的双目视觉系统目标跟踪控制方法，其特征在于，通过所述正运动学方程求出所述多自由度机械臂位于目标位置时的所述多自由度机械臂各关节的位置参数θ’_I，求出关节控制量Δθ。

10.根据权利要求9所述的双目视觉系统目标跟踪控制方法，其特征在于，所述目标点处于运动状态时，双目视觉系统能够根据所述关节控制量Δθ控制所述多自由度机械臂移动，直至所述关节控制量Δθ＝0。