CN110039542A - 具有速度方向控制的视觉伺服跟踪控制方法及机器人系统 - Google Patents

Abstract

本发明涉及一种具有速度方向控制的视觉伺服跟踪控制方法，包括：获取当前目标图像的特征向量s，根据期望目标图像的特征向量s_d，若||s‑s_d||小于预设值，则视觉伺服任务完成，否则计算图像特征对应的图像雅克比矩阵，利用具有速度方向调整的跟踪控制方法计算摄像头的空间速度V_c，从而驱动机械臂运动，重新获取目标物体当前图像特征，直至||s‑s_d||小于预设值；本发明对标定误差和深度误差具有鲁棒性，可以抑制目标特征在图像平面中的冗余运动，从而获得更加满意的图像平面轨迹，并且可以降低视觉伺服期间目标点离开视觉区域的风险，具有良好的实时性，为机器人视觉伺服控制问题提供了一种新的解决方案。

Description

具有速度方向控制的视觉伺服跟踪控制方法及机器人系统

技术领域

本发明涉及视觉伺服技术领域，尤其涉及一种具有速度方向控制的视觉伺服控制方法及机器人系统。

背景技术

机器人视觉已经涉及到生活中的方方面面。视觉伺服(VS) 是一种利用图像处理技术，将标记特征移动到期望位置的运动控制过程。它能够和许多学科交叉融合，例如最优化方法、滑模控制、模糊控制、鲁棒控制、切换控制、自适应控制、神经网络和强化学习等。一般来说，视觉伺服控制分为基于位置的视觉伺服(PBVS),基于图像的视觉伺服(IBVS)以及混合视觉伺服。PBVS 需要获得目标的几何模型，相机的内部参数以及观测到的图像平面特征，然后估计目标到相机的相对位姿，最大限度地减小位姿误差是PBVS系统的主要任务，因此目标的几何模型和摄像头标定精度严重影响和制约了PBVS的性能。而且PBVS是在三维空间中进行控制，很难获得满意的图像平面运动轨迹，可能会导致图像特征离开视觉区域。与PBVS系统相比，IBVS系统中图像数据直接用于控制机器人，所以IBVS系统不需要任何的位姿估计而且它对相机和手眼标定误差具有鲁棒性。

目前，在第二类的IBVS系统中较为常见的算法包括：(1)比例控制器，该算法可以使图像特征误差以指数形式收敛；(2)模型预测控制器，该算法能够处理机器人在运动过程中的系统约束，例如图像边界约束和机器人关节约束；(3)增广的基于图像视觉伺服控制器，该方法将加速度作为控制指令，提高了系统的收敛速度等；(4)卡尔曼滤波神经网络，该算法常被应用于无标定视觉伺服环境中的机械臂控制；(5)基于图像矩和亮度特征的视觉伺服控制方法，该类算法能够实现一定程度的解耦控制，但是计算过程复杂，实时性和实用性不强。以上许多方法与经典的视觉伺服控制方法相比，控制效果提升有限，在抑制图像平面轨迹冗余运动方面依然存在不足。

发明内容

(一)要解决的技术问题

为了解决现有技术上存在的问题，本发明提供了一种具有速度方向控制的视觉伺服跟踪控制方法及机器人系统，能够在继承经典视觉伺服控制器对不确定参数具有鲁棒性的基础上，优化图像平面运动轨迹，抑制其冗余运动，大大降低图像特征离开视觉区域的风险。

(二)技术方案

为了达到上述目的，本发明采用的主要技术方案包括：

一种具有速度方向控制的视觉伺服跟踪控制方法，所应用的IBVS 系统包括控制单元、机械臂和安装在机械臂执行器末端的摄像头，控制单元根据摄像头获取的图像信息提取出相应的视觉特征，进而生成控制指令驱动机械臂运动，摄像头随机械臂运动并再次获取图像，最终形成运动-反馈-运动的闭环结构，该闭环结构包括以下步骤：

101、设定摄像头和目标物体的相对位姿，获取此时目标物体的图像作为期望目标图像，并得到期望目标图像的特征向量s_d；

102、在六自由度机械臂的当前位置，获取摄像头在当前位置所采集的目标图像，并得到当前目标图像的特征向量s；

103、将所述当前目标图像的特征向量s与期望目标图像的特征向量s_d做差，获取图像特征误差s-s_d；

104、如若所述图像特征误差的范数||s-s_d||小于预设值，则完成任务；

否则，根据当前目标图像特征向量计算图像雅克比矩阵，根据图像雅克比矩阵、图像特征误差和图像的当前特征及期望特征，利用公式一计算出摄像头在笛卡尔空间中的空间速度V_c，再将空间速度V_c转换为驱动摄像头运动到下一位姿的机械臂各关节速度，并跳转至步骤102；

公式一：

其中，Λ(t)＝diag([λ₁(t)；λ₂(t)；λ₃(t)；λ₄(t)；λ₅(t)；λ₆(t)]为时变的速度比例系数，用于调整目标点的速度方向，并且满足λ₁(t)...λ₆(t)＞0，从λ₁(t)到λ₆(t)分别对应着控制律V_c中的六个元素，由此可以看到Λ(t)＞0和Λ(t)＝Λ(t)^T是Λ(t)的性质； 表示堆叠的图像雅克比矩阵估计值的伪逆，L_e＝[L_s1；L_s2；...；L_s4]∈R^2n×6，L_s1到L_sn均为图像雅克比矩阵，其中δ∈R^6×6是一个非常小的正定对称矩阵；e(t)为图像特征的误差值。

可选地，摄像头在笛卡尔空间中的空间速度表示为V_c＝ [v_x；v_y；v_z；w_x；w_y；w_z]∈R^{6 ×1}；其中，v_x、v_y和v_z表示摄像头的线速度， w_x、w_y和w_z表示摄像头的角速度。

可选地，在步骤104中计算当前目标图像雅克比矩阵的具体步骤包括：

对于在世界坐标系中的任意一目标点坐标P＝(X，Y，Z)∈R^1×3，经过投影，其对应在图像平面上的坐标为p＝(x，y)∈R^1×2，其中根据公式二可得图像目标点的像素坐标s＝ (u，v)∈R^1×2：

公式二：

根据公式三计算目标点对应的图像雅克比矩阵L_s：

公式三：

式中，f为相机焦距，ρ_u和ρ_v为像素点的长和宽，(u₀，v₀)表示主点坐标，和表示目标点像素坐标与主点坐标间的位置关系。

可选地，具有速度方向控制的视觉伺服跟踪控制方法包括：

如图7所示，为图像特征点的中心点坐标，为期望特征点的中心点坐标，为当前图像特征点的中心点坐标， 和是介于这三点之间的方向向量；v_x和v_y为机械臂末端执行器沿着水平和垂直方向的平移速度；为了让机械臂末端执行器在图像平面上的速度v能够尽可能地沿着方向向量d₀，首先，将方向向量d₃向d₀投影，投影点为s_p；为了更好地调整速度方向v，在点s_p与之间定义点s′，构成方向向量因此，方向向量d₁和d₂可以通过公式四来限制速度v的方向介于二者之间。

公式四：其中，L_xy＝[I_2×2 O_2×4]。

可选地，所述机械臂有六个自由度。

一种视觉伺服机器人系统，包括：

六自由度机械臂，设置在六自由度机械臂执行器末端的摄像头，基于上述技术方案中具有速度方向控制的视觉伺服跟踪控制方法驱动六自由度机械臂运动的控制单元和通信模块；

通信模块用于摄像头、控制单元和机械臂接收，和\或发送数据信息和控制指令。

(三)有益效果

本发明的有益效果是：本发明提供的一种具有速度方向控制的视觉伺服跟踪控制方法对于IBVS系统具有鲁棒性，同时引入具有速度方向控制的跟踪控制方法从而抑制了图像平面特征轨迹的冗余运动，并且有效的降低了在视觉伺服期间特征点离开视觉区域的风险，因此本发明方法可以完成长距离的视觉伺服任务；且该方法具有良好的实时性，为机器人视觉伺服控制等问题提供了一种新的解决思路。

附图说明

图1为本发明一实施例提供的一种具有速度方向控制的视觉伺服跟踪控制方法的流程图；

图2为本发明一实施提供的控制原理示意图；

图3为本发明一实施例提供的视觉伺服机器人系统整体结构示意图；

图4a为本发明一实施例提供在任务1中图像特征的初始位置和期望位置；

图4b为本发明一实施例提供在任务1中传统的IBVS控制器下图像特征的运动轨迹；

图4c为本发明一实施例提供在任务1中本发明方法的IBVS 控制器下图像特征的运动轨迹；

图4d为本发明一实施例提供在任务1中传统的IBVS控制器下机械臂关节角度变化轨迹；

图4e为本发明一实施例提供在任务1中本发明方法的IBVS 控制器下机械臂关节角度变化轨迹；

图4f为本发明一实施例提供在任务1中传统的IBVS控制器下图像特征误差的变化轨迹；

图4g为本发明一实施例提供在任务1中本发明方法的IBVS 控制器下图像特征误差的变化轨迹；

图5a为本发明一实施例提供在任务2中图像特征的初始位置和期望位置；

图5b为本发明一实施例提供在任务2中传统的IBVS控制器下图像特征的运动轨迹；

图5c为本发明一实施例提供在任务2中本发明方法的IBVS 控制器下图像特征的运动轨迹；

图5d为本发明一实施例提供在任务2中传统的IBVS控制器下机械臂关节角度变化轨迹；

图5e为本发明一实施例提供在任务2中本发明方法的IBVS 控制器下机械臂关节角度变化轨迹；

图5f为本发明一实施例提供在任务1中传统的IBVS控制器下图像特征误差的变化轨迹；

图5g为本发明一实施例提供在任务1中本发明方法的IBVS 控制器下图像特征误差的变化轨迹；

图6a为本发明一实施例提供在任务3中图像特征的初始位置和期望位置；

图6b为本发明一实施例提供在任务3中传统的IBVS控制器下图像特征的运动轨迹；

图6c为本发明一实施例提供在任务3中本发明方法的IBVS 控制器下图像特征的运动轨迹；

图6d为本发明一实施例提供在任务3中传统的IBVS控制器下机械臂关节角度的变化轨迹；

图6e为本发明一实施例提供在任务3中本发明方法的IBVS 控制器下机械臂关节角度的变化轨迹；

图6f为本发明一实施例提供在任务1中传统的IBVS控制器下图像特征误差的变化轨迹；

图6g为本发明一实施例提供在任务1中本发明方法的IBVS 控制器下图像特征误差的变化轨迹；

图7为本发明中提供的速度方向边界向量示意图。

【附图标记说明】

1：计算机；2：机械臂控制箱；3：六自由度机械臂；4：摄像头；5：目标物体；6：工作台。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

实施例一

在本实施例中将IBVS系统应用视觉伺服机器人控制系统，如图 3所示，举例来说视觉伺服机器人系统包括：控制单元，机械臂3、安装在机械臂末端的摄像头4，以及用于数据通信的通信模块；在工作台6上放置有目标物体5，视觉伺服机器人系统的控制单元根据摄像头4拍摄的图像发送控制指令，以使机械臂3带动摄像头4运到目标物体5附近完成任务；

举例来说，在本实施例中主控制单元包括用于数据计算和处理的计算机1和安装在机械臂上3的机械臂控制箱2，在本实施例中机械臂具有六个自由度，本实施例仅用于举例说明，机械臂也可以具有适合其任务需求的自由度；其中机械控制箱2内设置有控制机械臂各关节执行器运动的控制器件；摄像头4随着械臂3末端的执行器运动，摄像头4获取目标物体5的图像特征信息，再将图像特征信息通过通信模块传递回计算机1和机械臂控制箱2，形成运动-反馈-运动循环。

实施例二

在实施例一的机构的基础上本实施例提供了一种具有速度方向控制的视觉伺服跟踪控制方法，如图1所示，具体包括以下步骤：

101、设定摄像头和目标物体的相对位姿，获取此时目标物体的图像作为期望目标图像，并得到期望目标图像的特征向量s_d；

102、在六自由度机械臂的当前位置，获取摄像头在当前位置所采集的目标图像，并得到当前目标图像的特征向量s；

103、将所述当前目标图像的特征向量s与期望目标图像的特征向量s_d做差，获取图像特征误差s-s_d；

104、如若所述图像特征误差的范数||s-s_d||小于预设值，则完成任务；否则，根据当前目标图像特征向量计算图像雅克比矩阵，根据图像雅克比矩阵、图像特征误差和图像的当前特征及期望特征，利用公式一计算出摄像头在笛卡尔空间中的空间速度V_c，再将空间速度V_c转换为驱动摄像头运动到下一位姿的机械臂各关节速度，并跳转至步骤102；

举例来说，步骤104中根据当前目标图像特征向量计算出图像雅可比矩阵的具体计算步骤是：

根据相机的中心投影模型成像原理，对于世界坐标系中的目标点坐标P＝(X，Y，Z)∈R^1×3，经过投影，其对应在图像平面上的坐标为p＝(x，y)∈R^1×2，其中根据公式1可得图像目标点的像素坐标s＝(u，v)∈R^1×2：

公式1：

式中，f为相机焦距，ρ_u和ρ_v为像素点的长和宽，(u₀，v₀)表示相机的主点坐标。由此，摄像头的空间速度V_c与图像特征的关系根据下述公式2获得：

公式2：

公式2中的L_s即为图像特征点s对应的图像雅克比矩阵，根据下述公式3即可计算获得：

公式3：

其中，和表示目标点像素坐标与主点坐标间的位置关系。

举例来说，公式2中机械臂末端执行器在笛卡尔空间中的速度V_c表示为V_c＝[v_x；v_y；v_z；w_x；w_y；w_z]∈R^6×1；其中，v_x、v_y和v_z表示摄像头的线速度，w_x、w_y和w_z表示摄像头的角速度。

进一步地，如图2所示并根据公式5，计算摄像头在笛卡尔空间中的速度V_c的步骤如下：

公式4：e(t)＝s_c-s^*；

其中，e(t)为当前和期望图像特征间的误差，s_c＝[s₁；s₂；...；s_n] ∈R^2n×1为n个图像特征点像素坐标，为对应的期望像素坐标。

根据公式2，求取公式4的导数得到公式5，进而得到公式6：

公式5：

公式6：

其中，Λ(t)＝diag([λ₁(t)；λ₂(t)；λ₃(t)；λ₄(t)；λ₅(t)；λ₆(t)]为时变的速度比例系数，用于调整目标点的速度方向，并且满足λ₁(t)...λ₆(t)＞0，从λ₁(t)到λ₆(t)分别对应着控制律V_c中的六个元素，由此可以看到Λ(t)＞0和Λ(t)＝Λ(t)^T是Λ(t)的性质； 表示堆叠的图像雅克比矩阵估计值的伪逆，L_e＝L_s＝[L_s1；L_s2；...；L_sn]∈R^2n×6，L_s1到L_sn均为图像雅克比矩阵，其中δ∈R^6×6是一个非常小的正定对称矩阵；e(t)为图像特征的误差值。

速度方向控制的核心部分Λ(t)通过公式7获得：

公式7：其中，L_xy＝[I_2×2 O_2×4]。

如图7所示，为图像特征点的中心点坐标，为期望特征点的中心点坐标，为当前图像特征点的中心点坐标， 和是介于这三点之间的方向向量。v_x和v_y为机械臂末端执行器沿着水平和垂直方向的平移速度。为了让机械臂末端执行器在图像平面上的速度v能够尽可能地沿着方向向量d₀，首先，将方向向量d₃向d₀投影，投影点为s_p。为了更好地调整速度方向v，在点s_p与之间定义点s′，构成方向向量因此，方向向量d₁和d₂可以通过公式四来限制速度v的方向介于二者之间。

本实施例中，由图像特征作为反馈信息，由多目标最优化方向控制模型和图像特征误差信号求得控制器时变参数Λ(t)，进而获得摄像头在笛卡尔空间中的空间速度V_c，并根据空间速度更新机械臂各关节的角度，直至机械臂到达期望位姿，完成视觉伺服任务。

仿真实验

将实施例二方法应用于仿真实验中，并与传统IBVS方法进行对比，设置了任务1、任务2和任务3共三个视觉伺服任务，其中图4b、图5b和图6b分别为传统IBVS在执行任务1、任务2和任务3时的图像特征轨迹；图4c、图5c和图6c为本发明方法在执行任务1、任务2和任务3时的图像特征轨迹；本发明方法有效的抑制了图像特征在图像平面轨迹的冗余运动，且比传统的方法具有更好的图像特征轨迹，从而可以降低图像特征点离开视觉区域的风险，因此可以有效完成长距离的视觉伺服任务。

图4a、图5a和图6a为传统IBVS方法和本发明方法在执行三个视觉伺服任务时的初始图像特征位置和期望位置图。图4d、图 5d和图6d分别为传统IBVS在执行任务1、任务2和任务3时的机械臂关节角度随时间变化的实验图；图4e、图5e和图6e为本发明方法在执行任务1、任务2和任务3时的图像特征误差随时间变化的实验图，在任务3中传统IBVS特征点离开了视觉区域没有完成视觉伺服任务，而本发明方法完成了视觉伺服任务3。

图4f、图5f和图6f分别为传统IBVS方法在执行任务1、任务2和任务3时的图像特征误差变化轨迹图；图4g、图5g和图 6g分别为本发明方法在执行任务1、任务2和任务3时的图像特征误差变化轨迹图；可以看出本发明方法比传统IBVS具有更好的性能。

本发明方法对IBVS系统具有鲁棒性，同时具有实时速度方向控制的效果，从而抑制了图像平面特征轨迹的冗余运动，并且有效地降低了在执行视觉伺服任务期间特征点离开视觉区域的风险，因此本发明方法可以完成长距离的视觉伺服任务；且该方法具有良好的实时性，为机器人视觉伺服控制等问题提供了一种新的解决思路。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (6)

1.一种具有速度方向控制的视觉伺服跟踪控制方法，所应用的IBVS系统包括控制单元、机械臂和安装在机械臂执行器末端的摄像头，控制单元根据摄像头获取的图像信息提取出相应的视觉特征，进而生成控制指令驱动机械臂运动，摄像头随机械臂运动并再次获取图像，最终形成运动-反馈-运动的闭环结构，其特征在于，该闭环结构包括以下步骤：

101、设定摄像头和目标物体的相对位姿，获取此时目标物体的图像作为期望目标图像，并得到期望目标图像的特征向量s_d；

102、在六自由度机械臂的当前位置，获取摄像头在当前位置所采集的目标图像，并得到当前目标图像的特征向量s；

103、将所述当前目标图像的特征向量s与期望目标图像的特征向量s_d做差，获取图像特征误差s-s_d；

104、如若所述图像特征误差的范数||s-s_d||小于预设值，则完成任务；

否则，根据当前目标图像特征向量计算图像雅克比矩阵，根据图像雅克比矩阵、图像特征误差和图像的当前特征及期望特征，利用公式一计算出摄像头在笛卡尔空间中的空间速度V_c，再将空间速度V_c转换为驱动摄像头运动到下一位姿的机械臂各关节速度，并跳转至步骤102；

公式一：

其中，Λ(t)＝diag([λ₁(t)；λ₂(t)；λ₃(t)；λ₄(t)；λ₅(t)；λ₆(t)]为时变的速度比例系数，用于调整目标点的速度方向，并且满足λ₁(t)...λ₆(t)＞0，从λ₁(t)到λ₆(t)分别对应着控制律V_c中的六个元素，由此可以看到Λ(t)＞0和Λ(t)＝Λ(t)^T是Λ(t)的性质； 表示堆叠的图像雅克比矩阵估计值的伪逆，L_e＝[L_s1；L_s2；...；L_sn]∈R^2n×6，L_s1到L_sn均为图像雅克比矩阵，其中δ∈R^6×6是一个非常小的正定对称矩阵；e(t)为图像特征的误差值。

2.如权利要求1所述的方法，其特征在于，

摄像头在笛卡尔空间中的空间速度表示为V_c＝[v_x；v_y；v_z；w_x；w_y；w_z]∈R^6×1；其中，v_x、v_y和v_z表示摄像头的线速度，w_x、w_y和w_z表示摄像头的角速度。

3.如权利要求1所述的方法，其特征在于，在步骤104中计算当前目标图像雅克比矩阵的具体步骤包括：

对于在世界坐标系中的任意一目标点坐标P＝(X，Y，Z)∈R^1×3，经过投影，其对应在图像平面上的坐标为p＝(x，y)∈R^1×2，其中

根据公式二可得图像目标点的像素坐标s＝(u，v)∈R^1×2：

公式二：

根据公式三计算目标点对应的图像雅克比矩阵L_s：

公式三：

式中，f为相机焦距，ρ_u和ρ_v为像素点的长和宽，(u₀，v₀)表示主点坐标，和表示目标点像素坐标与主点坐标间的位置关系。

4.如权利要求1所述的方法，其特征在于，具有速度方向控制的视觉伺服跟踪控制方法包括：

为图像特征点的中心点坐标，为期望特征点的中心点坐标，为当前图像特征点的中心点坐标，

和是介于这三点之间的方向向量；

v_x和v_y为机械臂末端执行器沿着水平和垂直方向的平移速度；

将方向向量d₃向d₀投影，投影点为s_p；

在点s_p与之间定义点s′，构成方向向量

方向向量d₁和d₂根据通过公式四来限制速度v的方向介于二者之间；

公式四：

其中，L_xy＝[I_2×2 0_2×4]。

5.如权利要求1所述的方法，其特征在于，所述机械臂有六个自由度。

6.一种视觉伺服机器人系统，其特征在于，包括：

六自由度机械臂，设置在六自由度机械臂执行器末端的摄像头，基于如权利要求1-5任一项中的具有速度方向控制的视觉伺服跟踪控制方法驱动六自由度机械臂运动的控制单元和通信模块；

通信模块用于摄像头、控制单元和机械臂接收，和\或发送数据信息和控制指令。