基于视觉伺服的机械臂目标导引系统、方法与流程

本发明属于视觉伺服领域，具体涉及一种基于视觉伺服的机械臂目标导引系统、方法。

背景技术：

随着机器人技术的发展，机器人在感知、思维、效应方面逐渐发展成全面模拟人的机器系统，承担着越来越复杂和多样性的任务。传统的机器人在感知、识别、推理、规划等方面功能比较单一，有较强的局限性，只适应于特定环境下的特定作业。视觉伺服控制系统充分利用计算机技术和图像处理技术的发展，基于视觉信息进行反馈，经视觉信号处理后，能更精确、灵活、自主地对机器人进行控制，从而具有非特定环境下的作业能力。

视觉伺服控制在技术上包括基于图像的、基于位置的、基于2.5d的视觉伺服控制方法，另外还有无标定视觉伺服控制等；在结构上包括eye-in-hand方式和eye-to-hand方式等。eye-in-hand相机安装在机械臂上，随机器人一起运动，缺陷是：目标的成像有时会在图像边缘甚至脱离视野，存在遮挡问题，尤其在靠近目标时目标的成像大小有时不合理，过大或者过小。eye-to-hand一般是相机固定在工作空间内，同时观测目标和机器人，缺点是感知空间范围固定，也存在目标特征遮挡现象。

针对eye-in-hand方式和eye-to-hand方式的缺陷，本发明设计了一种新型的视觉伺服配置eye-in-vehicle。该系统由移动机器人和机械臂系统两部分构成。

技术实现要素：

为了解决现有技术中的上述问题，即为了解决视觉伺服中目标成像存在遮挡、脱离视野的问题，本发明第一方面，提出了一种基于视觉伺服的机械臂目标导引系统，该导引系统包括移动机器人和机械臂系统；所述移动机器人包括pan-tilt相机平台、一个差动式移动小车；所述机械臂系统包括机械臂本体、机械臂控制系统；所述机械臂本体可移动一端的端部设置有标志物；

所述pan-tilt相机平台固设于所述差动式移动小车；

所述的差动式移动小车可以围绕机械臂本体移动，以通过pan-tilt相机平台获取包含被抓取目标标志物和机械臂本体上标志物的图像。

在一些优选的实施例中，所述机械臂本体上设置的标志物与所述被抓取目标上设置的标志物一致。

在一些优选的实施例中，所述标志物为矩形，该矩形上设置有四个标志点，四个标志点分别关于该矩形长度方向、宽度方向的中线对称。

本发明的第二方面，提出了一种基于视觉伺服的机械臂目标导引方法，基于上述的基于视觉伺服的机械臂目标导引系统，包括以下步骤：

步骤s10，通过pan-tilt相机平台获取包括第一标志物、第二标志物的图像，提取该图像中各标志物对应图像的特征点，并计算第一标志物、第二标志物的相对位姿；所述第一标志物为机械臂本体上设置的标志物，所述第二标志物为目标物体上设置的标志物；

步骤s20，基于所述相对位姿，使用度量函数获取pan-tilt相机平台的最佳观测位置；

步骤s30，基于pan-tilt相机平台的相机的水平和垂直视野区域约束，将所述差动式移动小车移动至所述的最佳观测位置；基于pan-tilt相机平台的相机的水平和垂直视野区域约束，以及所述相对位姿，控制机械臂本体的功能部位向目标物体移动。

在一些优选的实施例中，步骤s10“提取该图像中各标志物对应图像的特征点，并计算第一标志物、第二标志物的相对位姿”，其方法为：分别计算第一标志物特征点坐标系与pan-tilt相机平台坐标系之间的位姿关系以及第二标志物特征点坐标系与pan-tilt相机平台坐标系之间的位姿关系，进而求解第一标志物与第二标志物之间的相对位姿。

在一些优选的实施例中，步骤s20中“使用度量函数获取pan-tilt相机平台的最佳观测位置”，度量函数的最小值为pan-tilt相机平台的最佳观测位置，度量函数的计算方法为：

其中，l函数是pan-tilt相机平台与目标相对位置的约束函数，θ1为被抓取目标标志物的特征中心点与相机的夹角，θ2为机械臂标志物的特征中心点与相机的夹角，t为预设的余弦阈值，s函数是成像面积的约束，k1为s函数的权重系数，s为标志物的成像面积，sd为预设期望面积，h是透视畸变函数，k2为h函数的权重系数，a1,a2,a3,a4为被抓取目标的标志物特征点之间的距离，b1,b2,b3,b4为机械臂标志物特征点之间的距离，p函数是成像距离图像边缘的约束，vmin为左上边特征点的v坐标，vmax为最下边特征点的v坐标，umin为最左边特征点的u坐标，umax为左右边特征点的u坐标，γ为特征点距离边缘的阈值。

在一些优选的实施例中，步骤s30中“基于pan-tilt相机平台的相机的水平和垂直视野区域约束，将所述差动式移动小车移动至所述的最佳观测位置”其方法为：根据pan-tilt相机平台的水平和垂直视野区域约束的区域，将两个约束区域进行合并，获取差动式小车可以移动的区域，在可移动区域范围内，小车按照预设的规则移动。

在一些优选的实施例中，步骤s30中“基于pan-tilt相机平台的相机的水平和垂直视野区域约束，以及所述相对位姿，控制机械臂本体的功能部位向目标物体移动”，其方法为：所述的相对位姿为所述差动式移动小车移动至所述的最佳观测位置后，重新计算得到第一标志物与第二标志物的相对位姿。

本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序应用由处理器加载并执行上述的基于视觉伺服的机械臂目标导引方法。

本发明的第四方面，提出了一种处理设置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；所述程序适用于由处理器加载并执行上述的基于视觉伺服的机械臂目标导引方法。

本发明的有益效果：

本发明能够避免传统的视觉伺服eye-in-hand和eye-to-hand中常出现的特征点被遮挡、目标成像大小不合适、目标脱离相机视野等情况。本发明中的机械臂和被操作目标设置有标志物，pan-tilt相机平台通过同时观测两个标志物确定机械臂与操作目标之间的相对位姿关系，通过无线通讯传送给机械臂所述相对位姿信息，机械臂系统完成趋近或者装配任务。在机械臂操作的过程中，移动平台能够与机械臂进行协调运动。移动平台会根据相机当前的观测效果做出最佳观测位置估计，并移动到该位置，保证两个标志物在相机中具有良好的观测效果。

附图说明

通过阅读参照以下附图所做的对非限制性实施例所做的详细描述，本申请的其他特征、目的和优点将会变得更明显。

图1是本发明一种实施例的基于视觉伺服的机械臂目标导引方法的移动机器人与机械臂系统交互流程示意图；

图2是本发明一种实施例的基于视觉伺服的机械臂目标导引系统的eye-in-vehicle配置示例图；

图3本发明一种实施例的基于视觉伺服的机械臂目标导引系统的移动机器人示例图；

图4本发明一种实施例的基于视觉伺服的机械臂目标导引方法的pan-tilt相机平台的水平视野区域约束的示例图；

图5本发明一种实施例的基于视觉伺服的机械臂目标导引方法的pan-tilt相机平台的水平和垂直视野区域约束区域重叠下的小车逆时针移动区域示例图；

图6本发明一种实施例的基于视觉伺服的机械臂目标导引方法的pan-tilt相机平台的水平和垂直视野区域约束区域重叠下的小车顺时针移动区域示例图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明的一种基于视觉伺服的机械臂目标导引系统，其特征在于，该导引系统包括移动机器人和机械臂系统；所述移动机器人包括pan-tilt相机平台、一个差动式移动小车；所述机械臂系统包括机械臂本体、机械臂控制系统；所述机械臂本体可移动一端的端部设置有标志物；

所述pan-tilt相机平台固设于所述差动式移动小车；

所述的差动式移动小车可以围绕机械臂本体移动，以通过pan-tilt相机平台获取包含被抓取目标标志物和机械臂本体上标志物的图像。

为了更清晰地对本发明基于视觉伺服的机械臂目标导引系统进行说明，下面结合附图对本发明系统实施例中各系统展开详述。

本发明一种实施例的基于视觉伺服的机械臂目标导引系统，包括：移动机器人和机械臂系统；移动机器人由pan-tilt相机平台和一个差动式移动小车组成。

图2为本发明eye-in-vehicle配置的一个示意图，机械臂系统包括机械臂本体、机械臂控制系统，机械臂本体和被抓取目标上均有标志物，分别为标志物1和标志物2，机械臂本体上设置的标志物与所述被抓取目标上设置的标志物一致，所述标志物为矩形，该矩形上设置有四个标志点，四个标志点分别关于该矩形长度方向、宽度方向的中线对称，用来计算它们之间的相对位姿关系。移动机器人通过pan-tilt相机平台获取标志物的图像。

其中，移动机器人如图3所示，pan-tilt相机平台由俯仰、偏转步进电机和相机组成。pan-tilt相机平台固设于所述差动式移动小车，俯仰步进电机和俯仰步进电机编码器位于相机同水平侧，偏转步进电机和偏转步进电机编码器位于相机下方，步进电机编码器位于步进电机的末端。

本发明第二实施例的一种基于视觉伺服的机械臂目标导引方法，基于上述的基于视觉伺服的机械臂目标导引系统，包括以下步骤：

步骤s10，通过pan-tilt相机平台获取包括第一标志物、第二标志物的图像，提取该图像中各标记物对应图像的特征点，并计算第一标志物、第二标志物的相对位姿；所述第一标志物为机械臂本体上设置的标志物，所述第二标志物为目标物体上设置的标志物；

步骤s20，基于所述相对位姿，使用度量函数获取pan-tilt相机平台的最佳观测位置；

步骤s30，基于pan-tilt相机平台的相机的水平和垂直视野区域约束，将所述差动式移动小车移动至所述的最佳观测位置；基于pan-tilt相机平台的相机的水平和垂直视野区域约束，以及所述相对位姿，控制机械臂本体的功能部位向目标物体移动。

为了更清晰地对本发明基于视觉伺服的机械臂目标导引方法进行说明，下面结合附图1对本发明方法一种实施例中各步骤进行展开详述。

步骤s10，通过pan-tilt相机平台获取包括第一标志物、第二标志物的图像，提取该图像中各标记物对应图像的特征点，并计算第一标志物、第二标志物的相对位姿；所述第一标志物为机械臂本体上设置的标志物，所述第二标志物为目标物体上设置的标志物。

在本实施例中，pan-tilt相机平台的相机获取机械臂本体与被抓取目标的标志物图像，获取所述标志物图像的上下左右四个特征点，基于特征点建立坐标系，根据特征点与相机坐标系计算所述标志物图像的相对位姿。

p-n-p方法，即从图像的n个特征点求解摄像时刻物体相对于相机的位姿(位置和姿态)，在目标跟踪、零件装配、物体识别、视觉导航等许多相面有着广阔的应用。在本实施例中选用p-n-p方法计算相对位姿。p-n-p方法在特征点共面的情况下需要四个点，在非共面的情况下需要六个点，本实施例中标志物图像都是在共面的情况下，选取机械臂本体上的四个特征点和被操作的目标对象上上的四个特征点。使用p-n-p方法可以分别计算出机械臂本体特征点坐标系与pan-tilt相机平台坐标系之间的关系，以及被操作目标特征点坐标系与pan-tilt相机平台坐标系之间的关系，进而可以求出机械臂本体与物体之间的相对位姿。

步骤s20，基于所述相对位姿，使用度量函数获取pan-tilt相机平台的最佳观测位置。

在本实施例中，基于步骤s10获取的相对位姿，通过度量函数，即位置、视野、成像距离边缘等约束，在条件约束下求取最佳观测位置。

其中，位置约束函数，保证pan-tilt相机平台位于目标的前方；成像面积约束使两个目标的成像面积接近预设的期望面积；透视畸变函数，保证成像不会有太大的透视畸变；成像距离图像边缘的约束，防止特征点太靠近图像边缘，出离相机视野。在这4个函数的约束下能有效的解决eye-in-hand和eye-to-hand中常出现的特征点被遮挡、目标成像大小不合适、目标脱离相机视野等情况。

度量函数计算公式如公式(1)所示：

其中，l函数是pan-tilt相机平台与目标相对位置的约束函数，θ1为被抓取目标标志物的特征中心点与相机的夹角，θ2为机械臂标志物的特征中心点与相机的夹角，t为预设的余弦阈值，当夹角θ1或者θ2的余弦值小于该阈值该函数值会迅速增加，反之会等于0，s函数是成像面积的约束，k1为s函数的权重系数，s为标志物的成像面积，sd为预设期望面积，h是透视畸变函数，k2为h函数的权重系数，a1,a2,a3,a4为被抓取目标的标志物特征点之间的距离，b1,b2,b3,b4为机械臂标志物特征点之间的距离，p函数是成像距离图像边缘的约束，vmin为左上边特征点的v坐标，vmax为最下边特征点的v坐标，umin为最左边特征点的u坐标，umax为左右边特征点的u坐标，γ为特征点距离边缘的阈值。

该度量函数f(x,y)由4个部分组成，在分别介绍这4个部分前，我们先定义两个函数，如公式(2)(3)所示：

其中，g1(m,n)函数为当m小于n时，g1(m,n)函数值迅速增加，反之为0；g2(m,n)函数为当m大于n时，g2(m,n)函数值迅速增加，反之为0，m为自变量，根据与n的大小关系执行分段函数的不同部分，n为自变量，在此处相当于函数输入的一个阈值。

视野区域约束函数如公式(4)所示：

p(umin,umax,vmin,vmax,γ)＝g1(umin,γ)+g1(vmin,γ)+g2(umax,γ)+g2(vmax,γ)(4)

pan-tilt相机平台位置约束函数如公式(5)所示：

l(cos(θ1),cos(θ2),t)＝g1(cos(θ1),t)+g1(cos(θ2),t)(5)

当pan-tilt相机平台越位于一个目标的正前方时，θ越接近于0，当偏离正前方越远时，θ越大，cosθ也越小。当cosθ小于t时，l函数的值不再为0，开始迅速增长。

透视畸变度量函数如公式(6)所示：

当对边的比值越接近于1时，该函数值越接近于0。

面积度量函数如公式(7)所示：

s(s,sd)＝(s/sd-1)²+(sd/s-1)²(7)

面积度量函数通过移动机器人与标志物之间的距离来求得标志物图像的合理成像面积。基于以上4个函数求解度量函数，当度量函数值最小时为最佳观测位置。

同时基于所述标志物图像的相对位姿，进而得出被操作目标和机械臂本体之间的相对位姿，通过无线通信系统，移动机器人将相对位姿信息传送给机械臂控制系统。

步骤s30，基于pan-tilt相机平台的相机的水平和垂直视野区域约束，将所述差动式移动小车移动至所述的最佳观测位置；基于pan-tilt相机平台的相机的水平和垂直视野区域约束，以及所述相对位姿，控制机械臂本体的功能部位向目标物体移动。

在本实施例中，找到最优观测位置后，移动机器人开始进行路径规划，约束条件为视野区域约束，也就是说路径规划经过的地方都是使用pan-tilt相机可以完整地观测到那两个标志物的地方，在这些地方当pan-tilt相机平台控制收敛后都可将这两个标志物保持在视野内防止移动机器人运动时，经过这种位置：在该位置由于pan-tilt相机平台距离两个目标过近，无法使两个标志物在一幅图像中完整地成像。

移动规划路径的目的是为了防止移动机器人在从当前位置走向期望位置时，防止移动机器人经过这样一个位置：在该位置，无论pan-tilt平台怎样运动，相机都无法完整地观测到两个标志物。

基于相机的水平视角和垂直视角，结合相机光心所在高度的水平面可以确定水平视野约束区域和垂直视野约束区域。图4为水平视野约束区域，它是移动机器人运动平面的俯视图，由一个长方形和一个半椭圆组合而成，相机在该组合区域内，无法观测到完整的目标。h1-h4为长方形的四个顶点。其中h3、h4为椭圆的短轴，a为椭圆的长半轴长度，b为椭圆的短半轴长度，c为长方形短边的长度。

图5为水平视野约束区域和垂直视野约束区域结合。图5中v1-v4点内的区域为垂直视野约束区域，由一个矩形区域组成。相机在该组合区域内，无法在垂直方向观测到完整的目标。r1-r8为水平视野约束区域和垂直视野约束区域图形分割成的区域。移动机器人在这些区域内运动，当小车位于ri区域时，目标位置在rj区域时，其中(i≤j),我们可以采用如下规则控制小车行走。

规则：当小车可以直线走向目标位置时，小车走直线。反之，按着下表1走。

表1

其中，v1、v3为垂直视野约束区域的顶点，tp为移动机器人当前的坐标点与椭圆的切线交点，h1、h2为水平视野约束区域中矩形的顶点。

规则如下：如果小车无法按照直线走向目标点，当目标在r1区域时，小车先走向v1点进入r2区域。然后判断小车能否按照直线走向目标点，当无法按照直线走向目标点时，小车先走向v3区域进入r3。在r3区域中，小车前进的方向始终指向切点tp。其余的情况以此类推。当小车可以沿直线走向目标点时，小车沿直线走向目标点，不受这个表的规则影响。

当小车位于rj区域，目标位置位于ri时，只需将图5进行对称，如图6所示，然后依旧按着表1走即可。

在移动机器人运动的过程中，pan-tilt相机平台根据差动式移动小车的速度和角速度计算相应的控制率，根据所述的控制率跟踪机械臂本体与被抓取目标的标志物图像。采用的控制律如公式(8)(9)所示：

其中，α、β分别为当前偏转电机和俯仰电机的转角，ωα、ωβ为当前偏转电机和俯仰电机的转角的控制率，为自适应控制中被估计的量，r为预设值用来更新(u，v)为被跟踪特征点的坐标，eu、ev为u、v的误差，ωr为移动机器人运动的角速度，k1、k2为增益系数，它控制eu误差减小的快慢。

αx、αy为相机的固定内参数，vr为移动机器人移动的线速度。

自适应律如公式(10)所示：

其中，为的导数。

机械臂根据相机从最佳观测位置拍摄的图像，可以通过p-n-p方法计算物体与被操作物体的相对位姿关系，从而控制机械臂趋近物体，进行操作路径规划，一边操作物体，一边防止出现物体与机械臂之间的标志物出现遮挡的现象。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适用于由处理器加载并实现上述的基于视觉伺服的机械臂目标导引方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的基于视觉伺服的机械臂目标导引方法。

所述技术领域的技术人员可以清楚的了解到，未描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考签署方法实例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。