一种视觉伺服跟踪方法、装置及系统与流程

本发明涉及视觉伺服控制技术领域，尤其是涉及一种视觉伺服跟踪方法、装置及系统。

背景技术

视觉伺服是以实现机器人的控制目的而对图像进行的自动获取和分析的过程，而针对人脸等目标物体的跟踪，通常采用的方式为基于图像的视觉伺服跟踪方法。

当前较为相似的应用有安防领域中的云台相机，可实现对目标物体的跟随，其中双目云台相机实现起来较为常见和简单，但是双目相机成本较贵，相机标定以及数据处理较为复杂，因此单目相机成为关注的热点，由于单目相机缺乏深度信息，因此无法准确获取目标物体的位置和运动信息，降低了对目标物体跟踪过程的稳定性和可靠性。

针对上述对目标物体跟踪过程的稳定性和可靠性较低的技术问题，目前尚未提出有效的解决方式。

技术实现要素：

有鉴于此，本发明的目的在于提供一种视觉伺服跟踪方法、装置及系统，以缓解现有技术中存在的稳定性低、可靠性差等的问题。

第一方面，本发明实施例提供了一种视觉伺服跟踪方法，该方法应用于视觉控制器，用于对机器人的关节角度进行控制，该方法包括：获取视频帧数据；如果视频帧数据中有目标物体的图像，提取图像的形心坐标，其中，形心坐标为图像的形心在像素平面空间中的位置坐标；计算形心坐标与像素平面空间的中心的坐标差值；根据坐标差值判断形心坐标是否位于预先设定的稳定区域；如果否，根据坐标差值计算机器人的关节角度；对关节角度进行滤波处理，生成期望关节角度；将期望关节角度发送至关节控制器，以对目标物体进行跟踪。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，根据坐标差值计算机器人的关节角度的步骤包括：获取坐标差值与关节角度差值的映射关系，计算机器人的关节角度差值，其中，映射关系为基于图像雅克比矩阵的映射关系；根据关节角度差值计算机器人的关节角度。

结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第二种可能的实施方式，其中，计算机器人的关节角度差值的步骤包括：获取图像雅克比矩阵，对图像雅克比矩阵初始化，生成初始图像雅克比矩阵；根据初始图像雅克比矩阵以及状态方程计算当前时刻的图像雅克比矩阵；基于当前时刻的图像雅克比矩阵，以及映射关系计算机器人的关节角度差值，其中，计算机器人的关节角度差值的公式为：其中，qk为k时刻机器人的关节角度，jk为k时刻的图像雅克比矩阵，δq为k-1时刻与k-2时刻机器人的关节角度差值，fk为k时刻坐标差值，tk+1为k+1时刻所对应的时间值。

结合第一方面，本发明实施例提供了第一方面的第三种可能的实施方式，其中，对关节角度进行滤波处理的步骤包括：判断获取视频帧数据的次数是否大于预先设定的次数；如果否，采用限幅滤波算法对关节角度进行滤波处理；如果是，采用限幅滤波算法，以及采用加权递推算法对关节角度进行滤波处理。

结合第一方面的第三种可能的实施方式，本发明实施例提供了第一方面的第四种可能的实施方式，其中，采用限幅滤波算法对关节角度进行滤波处理的步骤包括：计算关节角度的幅值，当幅值大于预先设置的幅值阈值时，对关节角度进行滤波处理；滤波处理的公式为：θk＝c1θk-1+c2θk-2+c3θk-3+cnθk-n，其中，θk为k时刻机器人的关节角度，c1、c2、c3和cn为第一滤波系数，n为时刻的数量。

结合第一方面的第三种可能的实施方式，本发明实施例提供了第一方面的第五种可能的实施方式，其中，采用加权递推算法对关节角度进行滤波处理的步骤包括：获取预先设置的权重值；根据权重值对关节角度进行滤波处理，其中，滤波处理的公式为：θk＝d1θk+d2θk-1+d3θk-2+d4θk-3+d5θk-4+dnθk-n，其中，θk为k时刻机器人的关节角度，d1、d2、d3、d4、d5和dn为第二滤波系数，n为时刻的数量。

结合第一方面，本发明实施例提供了第一方面的第六种可能的实施方式，其中，根据坐标差值判断形心坐标是否位于预先设定的稳定区域的步骤包括：判断坐标差值是否大于预先设定的差值阈值；如果是，确定形心坐标不在预先设定的稳定区域；如果否，确定形心坐标位于预先设定的稳定区域。

第二方面，本发明实施例还提供一种视觉伺服跟踪装置，该装置设置于视觉控制器，用于对机器人的关节角度进行控制，该装置包括：数据获取模块，用于获取视频帧数据；提取模块，用于如果视频帧数据中有目标物体的图像，提取图像的形心坐标；其中，形心坐标为图像的形心在像素平面空间中的位置坐标；第一计算模块，用于计算形心坐标与像素平面空间的中心的坐标差值；判断模块，用于根据坐标差值判断形心坐标是否位于预先设定的稳定区域；第二计算模块，用于当判断模块的判断结果为否时，根据坐标差值计算机器人的关节角度；滤波模块，用于对关节角度进行滤波处理，生成期望关节角度；发送模块，用于将期望关节角度发送至关节控制器，以对目标物体进行跟踪。

结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，第二计算模块用于获取坐标差值与关节角度差值的映射关系，计算机器人的关节角度差值，其中，映射关系为基于图像雅克比矩阵的映射关系；根据关节角度差值计算机器人的关节角度。

第三方面，本发明实施例还提供一种视觉伺服跟踪系统，该系统包括视觉控制器，以及与视觉控制器连接的关节控制器和图像采集模块；

其中，视觉控制器设置有第二方面所述的视觉伺服跟踪装置。

本发明实施例提供的视觉伺服跟踪方法、装置及系统，通过对获取的视频帧数据进行分析处理，可以判断出视频帧数据中有目标物体的图像，进而提取图像的形心坐标，并计算该形心坐标与像素平面空间的中心的坐标差值，以此来判断该形心坐标是否位于预先设定的稳定区域，当形心坐标不在稳定区域内时，则根据坐标差值计算出机器人的关节角度，对该关节角度进行滤波处理，生成期望关节角度，并发送至关节控制器，对目标物体进行跟踪，有效提高了对目标物体跟踪的稳定性和可靠性。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种视觉伺服跟踪方法的流程图；

图2为本发明实施例提供的另一种视觉伺服跟踪方法的流程图；

图3为本发明实施例提供的一种视觉伺服跟踪装置的结构示意图；

图4为本发明实施例提供的一种视觉伺服跟踪系统的结构示意图。

图标：30-数据获取模块；32-提取模块；34-第一计算模块；36-判断模块；38-第二计算模块；40-滤波模块；42-发送模块；400-视觉控制器；401-关节控制器；402-图像采集模块。

具体实施方式

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

目前，针对人脸等目标物体的跟踪常采用的方法是利用图像雅克比矩阵的求解方式，通过给定目标点在像素平面空间的运动速度，求出对应的关节角速度，从而驱动控制电机实现对目标物体的跟随，但是，图像雅克比矩阵的求解没有统一的方法，较常用的计算方法主要有：雅可比矩阵broyden在线估计方法；基于卡尔曼滤波的雅可比在线估计方法和平方根无迹卡尔曼滤波的图像雅克比估计方法。

其中，雅可比矩阵broyden在线估计方法的计算量大，且其实时性较差；基于卡尔曼滤波的雅可比在线估计方法在对目标物体进行跟踪时，容易引起机械臂末端出现震颤现象；平方根无迹卡尔曼滤波的图像雅克比估计方法常用于平面内目标物体的跟踪，对三维运动的跟踪效果不理想。

基于此，本发明实施例提供的一种视觉伺服跟踪方法、装置以及系统，下面通过具体的实施例进行描述。

为便于对本实施例进行理解，首先对本发明实施例所公开的一种视觉伺服跟踪方法进行详细介绍。

实施例一：

本发明实施例提供了一种视觉伺服跟踪方法，该方法应用于视觉控制器，其中，视觉控制器可以是机器人控制系统的视觉控制器，用于对机器人的关节角度进行控制。

参见图1所示的一种视觉伺服跟踪方法的流程图，该方法包括如下步骤：

步骤s101，获取视频帧数据；

具体的，该视频帧数据可以是机器人系统的摄像装置拍摄的视频数据，视觉控制器可以提取到该视频数据，对视频帧数据进行处理分析，进而获取到视频帧数据。

步骤s102，如果视频帧数据中有目标物体的图像，提取图像的形心坐标，其中，形心坐标为图像的形心在像素平面空间中的位置坐标；

一般的，对目标物体，如人脸等的图像进行检测时，在目标物体的图像四周会出现矩形框，该矩形框对目标物体的图像进行包围，通常规定该矩形框的中心为目标物体图像的形心，通过计算可以得出图像的形心在像素平面空间中的位置坐标，该位置坐标即为形心坐标。

步骤s103，计算形心坐标与像素平面空间的中心的坐标差值；

步骤s104，根据坐标差值判断形心坐标是否位于预先设定的稳定区域；

具体实现时，规定像素平面空间的中心所在的一定区域范围内为稳定区域，上述坐标差值通常是形心坐标与稳定区域中心点的距离，因此，步骤s104的判断过程，可以基于该距离实现，具体地，可以判断坐标差值是否大于预先设定的差值阈值，如果是，则确定该形心坐标不在稳定区域内，如果否，则确定该形心坐标位于稳定区域内。

步骤s105，如果否，根据坐标差值计算机器人的关节角度；

通常坐标值与关节角度存在一定的映射关系，因此，在计算关节角度时，可以通过该映射关系实现。

步骤s106，对关节角度进行滤波处理，生成期望关节角度；

步骤s107，将期望关节角度发送至关节控制器，以对目标物体进行跟踪。

本发明实施例提供的视觉伺服跟踪方法，通过对获取的视频帧数据进行处理分析，可以判断出视频帧数据中有目标物体的图像，进而提取该图像的形心坐标，并计算形心坐标与像素平面空间的中心的坐标差值，以此来判断该形心坐标是否位于预先设定的稳定区域，当形心坐标不在稳定区域内时，则根据坐标差值计算出机器人的关节角度，对该关节角度进行滤波处理，生成期望关节角度，并发送至关节控制器，对目标物体进行跟踪，有效提高了对目标物体跟踪的稳定性和可靠性。

在实际计算过程，上述坐标差值与关节角度之间的映射关系通常为基于图像雅克比矩阵的映射关系，因此，在上述图1的基础上，本发明实施例还提供了另一种视觉伺服跟踪方法的流程图，如图2所示，包括如下步骤：

其中，步骤s201～步骤s203的过程，与图1所示的步骤s101～步骤s103的过程一致，在此不在赘述。

步骤s204，根据坐标差值判断形心坐标是否位于预先设定的稳定区域；如果是，返回步骤s201；如果否，执行步骤s205；

具体的，如果该形心坐标位于预先设定的稳定区域，则抛弃此时获得的所有信息，回到步骤s201继续对目标物体进行跟踪。

步骤s205，获取坐标差值与关节角度差值的映射关系，计算机器人的关节角度差值；

其中，上述映射关系为基于图像雅克比矩阵的映射关系，通常可以表示为以下形式：其中，表示目标点在像素平面的坐标，通常用(u，v)表示，表示关节空间变量，通常表示为θ＝[θ1θ2]^t，其中，θ1和θ2为关节空间变量的角度变量，依据图像雅克比矩阵，则有映射关系：δy＝j*δθ；其中，δy为坐标差值，δθ为关节角度差值，j为图像雅克比矩阵，可以表示为微分的形式：

因此，依据上述关系，当获得像素平面空间中的坐标差值，并且估计出图像雅克比矩阵j时，则可求得关节角度差值，进而得出关节转动角度，从而控制机器人转到期望的角度，实现对人脸等目标物体进行跟随。

具体实现时，计算机器人关节角度差值的过程可以包括以下过程：

(1)获取图像雅克比矩阵，对图像雅克比矩阵初始化，生成初始图像雅克比矩阵；

其中，初始化过程，可以在机器人运动之初，任意做两步线性无关的动作，通过伺服控制系统可以得到两个不相关的动作增量(δq1，δq2)，依据两个图像特征变化(δf1，δf2)，可以较为准确的估计出图像雅克比矩阵的初值，即初始图像雅克比矩阵，可以表示为：j＝[δf1δf2]*[δq1δq2]^-1；

(2)根据初始图像雅克比矩阵以及状态方程计算当前时刻的图像雅克比矩阵；

具体的，状态方程为其中，c(k-1)是由k时刻关节角度偏差δθk组成的噪声输入矩阵，x(k-1)是由图像雅克比矩阵的元素组成的系统的状态矩阵，y(k-1)为坐标差值的差值，即误差值，η(k-1)、v(k-1)为噪声矩阵，其中，c(k)可以表示为：

x(k-1)可以表示为：

x(k-1)＝[j{k}(1,1)j{k}(1,2)j{k}(2,1)j{k}(2,2)]^t；

在实际计算过程中，还需要考虑误差的影响，因此，通常会定义预测误差，预测误差矩阵可以表示为：q{k}＝p{k-1}+rv{k-1}，其中，p{k-1}为估计的均方误差，rv{k-1}为噪声矩阵，如，图像观测噪声方差矩阵，具体的，均方误差可以表示为：p{k-1}＝10⁵*eye(4)，rv{k-1}＝0.5*eye(4)，其中，eye(4)为4*4的对角矩阵且对角元素为1；

噪声矩阵可以表示为：

rv(k)＝(1-d(k-1))rv(k-1)+d(k-1)[y(k-1)-c(k-1)x(k-1)][y(k-1)-c(k-1)x(k)]^t-c(k-1)q(k)c(k-1)^t，其中计算加权系数的公式为：d(k)＝(1-b)/(1-b^k)，b为取值在0到1之间的常数，例如，b可以取值为0.99；

进一步，定义增益矩阵为：k(k)＝q(k)c(k-1)^t[c(k-1)q(k)c(k-1)+rv(k)]^-1；

此时，均方误差可以表示为包含增益矩阵的形式，如：

p{k}＝(eye(4)-k{k}*c{k-1})*q{k}；

此时，状态矩阵可以表示为：

x{k}＝x{k-1}+k{k}*(δek-1-c{k-1}*x{k-1})；

由此可得到当前时刻的图像雅克比矩阵：

(3)基于当前时刻的图像雅克比矩阵，以及映射关系计算机器人的关节角度差值；

具体实现时，计算机器人的关节角度差值的公式为：

其中，qk为k时刻机器人的关节角度，jk为k时刻的图像雅克比矩阵，δq为k-1时刻与k-2时刻机器人的关节角度差值，fk为k时刻坐标差值，tk+1为k+1时刻所对应的时间值。

步骤s206，根据关节角度差值计算机器人的关节角度；

具体地，视觉控制器可以获取前一时刻的关节角度，在根据上述关节角度差值，计算出机器人当前的关节角度。

应当理解，上述关节角度差值，以及关节角度的计算过程，是本发明实施例的一种优选形式，而不是唯一的形式，具体还可以参考相关资料实现，或者以实际使用情况为准，本发明实施例对此不进行限制。

步骤s207，对关节角度进行滤波处理，生成期望关节角度；

考虑到在对目标物体进行追踪的过程会出现形心坐标位于预先设定的稳定区域的情况，并且，当形心坐标位于预先设定的稳定区域时，会重新获取视频帧数据，因此，会出现多次获取视频帧的情况，通常为了保证计算的准确性，避免出现关节抖动，通常会对关节角度进行滤波处理，具体地，本发明实施例提供的滤波过程可以包括以下步骤：

(1)判断获取视频帧数据的次数是否大于预先设定的次数；

通常，该预先设定的次数为计算初始化所需的帧数；

(2)如果否，采用限幅滤波算法对关节角度进行滤波处理；

(3)如果是，采用限幅滤波算法，以及采用加权递推算法对关节角度进行滤波处理。

具体地，采用限幅滤波算法对关节角度进行滤波处理的过程为：

计算关节角度的幅值，当幅值大于预先设置的幅值阈值，即相比前一次角度计算值的偏差较大时，对关节角度进行滤波处理，其中，计算关节角度的幅值的公式为：其中，f1为机器人关节角度θk的幅值，θ1、θ2分别为θk在不同方向上的角度变量；限幅滤波算法的公式为：θk＝c1θk-1+c2θk-2+c3θk-3+cnθk-n，其中，θk为k时刻所述机器人的关节角度，c1、c2、c3和cn为第一滤波系数，n为时刻的数量，通常，为了简化计算过程，通常选取3个第一滤波系数，此时，限幅滤波算法的公式可以表示为：θk＝0.4θk-1+0.3θk-2+0.3θk-3，其中，θk为k时刻机器人的关节角度。

进一步的，采用加权递推算法对关节角度进行滤波处理的过程包括：获取预先设置的权重值；根据权重值对关节角度进行滤波处理，其中，滤波处理的公式为：θk＝d1θk+d2θk-1+d3θk-2+d4θk-3+d5θk-4+dnθk-n，其中，θk为k时刻所述机器人的关节角度，d1、d2、d3、d4、d5和dn为第二滤波系数，n为时刻的数量，同样考虑到简化计算过程的需要，通常选取5个第二滤波系数，此时，滤波处理的公式为：θk＝0.5θk+0.2θk-1+0.1θk-2+0.1θk-3+0.1θk-4，其中，θk为k时刻机器人的关节角度。

应当理解，上述第一滤波系数和第二滤波系数的选取方式，以及，选取的大小，可以根据实际计算情况进行设置，本发明实施例对此不进行限制。

步骤s208，将期望关节角度发送至关节控制器，以对目标物体进行跟踪。

本发明实施例提供的视觉伺服跟踪方法，通过对获取的视频帧数据进行分析处理，可以判断出视频帧数据中有目标物体的图像，进而提取图像的形心坐标，并计算该形心坐标与像素平面空间的中心的坐标差值，以此来判断该形心坐标是否位于预先设定的稳定区域，当形心坐标不在稳定区域内时，则根据坐标差值计算出机器人的关节角度，对该关节角度进行滤波处理，生成期望关节角度，并发送至关节控制器，对目标物体进行跟踪，有效提高了对目标物体跟踪的稳定性和可靠性。

实施例二：

在上述实施例的基础上，本发明实施例提供了一种视觉伺服跟踪装置，该装置设置于视觉控制器上，用于对机器人的关节角度进行控制，如图3所示的一种视觉伺服跟踪装置的结构示意图，该装置包括如下模块：

数据获取模块30，用于获取视频帧数据；

提取模块32，用于如果视频帧数据中有目标物体的图像，提取图像的形心坐标；其中，形心坐标为图像的形心在像素平面空间中的位置坐标；

第一计算模块34，用于计算形心坐标与像素平面空间的中心的坐标差值；

判断模块36，用于根据坐标差值判断形心坐标是否位于预先设定的稳定区域；

第二计算模块38，用于当判断模块的判断结果为否时，根据坐标差值计算机器人的关节角度；

滤波模块40，用于对关节角度进行滤波处理，生成期望关节角度；

发送模块42，用于将期望关节角度发送至关节控制器，以对目标物体进行跟踪。

进一步的，上述第二计算模块还用于，获取坐标差值与关节角度差值的映射关系，计算机器人的关节角度差值，其中，映射关系为基于图像雅克比矩阵的映射关系；根据关节角度差值计算机器人的关节角度。

本发明实施例提供的视觉伺服跟踪装置，与上述实施例提供的视觉伺服跟踪方法具有相同的技术特征，所以也能解决相同的技术问题，达到相同的技术效果。

本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

实施例三：

在上述实施例的基础上，本发明实施例提供了一种视觉伺服跟踪系统，如图4所示的一种视觉伺服跟踪系统的结构示意图，该系统包括视觉控制器400，以及与视觉控制器400连接的关节控制器401和图像采集模块402，其中，上述视觉控制器设置有视觉伺服跟踪装置。

具体实现时，该视觉伺服跟踪系统可以是配置在机器人上的系统，图像采集模块可以在机器人控制器触发下采集视频图像，并传输至视觉控制器进行特征提取，视觉控制器可以通过实施例一所述的方法计算关节角度，并传输至关节控制器，进而实现对机器人的关节运动进行控制。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统和装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

本发明实施例所提供的进行视觉伺服跟踪方法、装置及系统的计算机程序产品，包括存储了处理器可执行的非易失的程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。