CN112734823A - 一种基于图像的视觉伺服的雅可比矩阵深度估计方法 - Google Patents

Abstract

本发明涉及一种基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，包括如下步骤：S1、确定选择的状态的数量；S2、根据选择的状态的数量和图像的视觉伺服IBVS，建立基于卡尔曼滤波器的观测器模型；S3、根据建立的基于卡尔曼滤波器的观测器模型获得基于图像的视觉伺服IBVS的雅可比矩阵未知深度的估计信息；所述状态的数量包括：单一状态和多状态；提取雅可比矩阵的部分深度信息作为基于卡尔曼滤波器的观测器模型的状态向量。本发明提供的信息估计方法从图像雅可比矩阵中提取深度信息或深度信息以及二维图像点坐标作为卡尔曼滤波器的状态向量，大大提高了准确率；同时，通过六自由度机械臂的仿真和实验结果验证了该方法的有效性。

Description

一种基于图像的视觉伺服的雅可比矩阵深度估计方法

技术领域

本发明属于视觉伺服技术领域，尤其涉及一种基于图像的视觉伺服的雅可比矩阵深度估计方法。

背景技术

如今，视觉伺服已经涉及到我们生活各个方面，尤其在工业自动化领域得到了广泛应用。视觉伺服是通过视觉传感器接收处理图像信号，根据所得到的信息让机械臂系统做出进一步的控制或者自适应的调整行为。近年来，对于视觉伺服的研究越来越受到机械臂等各个领域研究人员的关注，研究方向也开始与许多学科相结合，如优化方法、滑模、模糊控制、鲁棒控制、开关控制、自适应控制、神经网络、强化学习等。根据反馈差值函数的区别可将机器人的视觉伺服系统分为两种：基于位置的视觉伺服(PBVS)与基于图像的视觉伺服(IBVS)。

在IBVS中，采用图像雅可比矩阵建立起图像空间与机械臂操作空间的映射关系。但是在图像雅可比矩阵中包含特征点相对于相机坐标系的深度信息，它不能直接测量。

发明内容

(一)要解决的技术问题

为了解决现有技术的上述问题，本发明提供一种基于图像的视觉伺服的雅可比矩阵深度估计方法。

(二)技术方案

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

一种基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，包括如下步骤：

S1、确定选择的状态的数量；

S2、根据选择的状态的数量和图像的视觉伺服IBVS，建立基于卡尔曼滤波器的观测器模型；

S3、根据建立的基于卡尔曼滤波器的观测器模型获得基于图像的视觉伺服IBVS的雅可比矩阵未知深度的估计信息；

所述状态的数量包括：单一状态和多状态；

提取雅可比矩阵的部分深度信息作为基于卡尔曼滤波器的观测器模型的状态向量。

优选地，当状态的数量为单一状态下的点特征深度估计时，所述方法中的基于卡尔曼滤波器的观测器模型为：

u(k)＝C(k)s(k)+V(k)

其中：

A(k)是单位矩阵，s(k)＝[1/Z_k]是系统在时间k的状态向量；u(k)＝[x(t_k+Δt)-x(t_k)，y(t_k+Δt)-y(t_k)是模型的观测向量；

和W(k)分别是过程噪声与观测噪声；

和W(k)的协方差分别是Q和R。

优选地，假定过程噪声和观察噪声均为高斯白噪声，针对使用点特征的情况还包括一种基于卡尔曼滤波器的观测器结构为深度估计的递归卡尔曼滤波模型：

状态向量预测：

协方差预测：

卡尔曼滤波器增益矩阵：

状态向量更新：

s_k|k＝s_k|k-1+K_k[u_k-C_ks_k|_k-1]

协方差更新：

P_k|k＝I-K_kC_k

其中Q是过程噪声协方差，R是观测噪声协方差，P是状态估计误差协方差。

优选地，当状态的数量为单一状态下的线特征深度估计时，

所述方法还包括：深度信息s(k)＝[1/Z_k]是模型状态向量，而u(k)＝[ρ(t_k+Δt)-ρ(t_k)，θ(t_k+Δt)-θ(t_k)]是模型的观察向量，模型建立过程如下：

其中，θ和ρ分别为线特征在极坐标下的坐标表示；v_x,v_y,v_z为机械臂末端执行器在x、y、z方向的线速度，w_x,w_y,w_z为机械臂末端执行器在x、y、z方向的角速度；W(k)为系统的测量误差。

优选地，当状态的数量为单一状态下的线特征深度估计时，所述方法中的基于卡尔曼滤波器的观测器模型为：

其中s(k)与u(k)分别为系统的状态向量与观测向量；θ和ρ分别为线特征在极坐标下的坐标表示，tk表示第k个时间段的起始时间点；Φ(k)与W(k)分别为系统的过程噪声与观测噪声；vx,vy,vz为机械臂末端执行器在x、y、z方向的线速度，wx,wy,wz为机械臂末端执行器在x、y、z方向的角速度。

优选地，当状态的数量为多状态下的点特征深度估计时，所述方法还包括：深度信息1/Z和二维图像平面点(x，y)的坐标值是模型状态变量s(k)＝[x_k，y_k，1/Z_k]，并且模型的观测向量与状态向量相同，u(k)＝[x_k，y_k，1/Z_k]；在获得连续系统的状态空间表达式之后，使用前向差分法离散化连续系统以获得离散数学模型，然后使用卡尔曼滤波器实时估计系统深度的模型建立过程如下：

使用前向差分法离散化连续系统：

其中xk、yk为第k个采样时刻特征点在图像平面的坐标值；同理，xk+1、yk+1为第k+1个采样时刻特征点在图像平面的坐标值；vxk,vyk,vzk为机械臂末端执行器在第k个采样时刻x、y、z方向的线速度，wxk,wyk,wzk为机械臂末端执行器在第k个采样时刻x、y、z方向的角速度；T为系统的采样周期；Z为待估计的特征点的深度信息。

优选地，视觉伺服系统模型为：

其中C(k)是单位矩阵，m(k)是系统离散化产生的误差；

φ(k)是Tm(k)与系统过程噪声之和；

W(k)是系统的观测噪声；T是系统的采样周期。

优选地，基于点特征的图像的视觉伺服IBVS的相机速度与图像特征运动之间的关系：

其中：

Lx是图像雅可比矩阵；

其中，x、y为图像特征点在二维图像平面的坐标值，Z为特征点的深度信息；vc与wc分别为机械臂末端执行器的线速度与角速度。

优选地，基于点特征的图像的视觉伺服IBVS的相机速度与图像特征运动之间的关系为：

其中，θ和ρ分别为线特征在极坐标下的坐标表示；vc与wc分别为机械臂末端执行器的线速度与角速度。

(三)有益效果

本发明的有益效果是：本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法具有以下有益效果：

从图像雅可比矩阵中提取深度信息或深度信息以及二维图像点坐标作为卡尔曼滤波器的状态向量，大大提高了准确率；同时，通过六自由度机械臂的仿真和实验结果验证了该方法的有效性。所提出的方法带来了更好的相机轨迹并明显减少了冗余运动。完成视觉伺服任务所需的时间也大大减少了。

附图说明

图1为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的方法流程图；

图2为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法中线特征的图像平面投影示意图；

图3为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机轨迹图；

图4为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机速度曲线图；

图5为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的特征误差图；

图6为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机轨迹图；

图7为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机速度曲线图；

图8为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机位置和角度图；

图9为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的实验的相机轨迹图；

图10为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的相机速度曲线图；

图11为本发明提供的一种基于图像的视觉伺服的雅可比矩阵深度估计方法的实施例中的特征误差变化曲线图。

具体实施方式

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

如图1所示：本实施例中公开了一种基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，包括如下步骤：

S1、确定选择的状态的数量；

S2、根据选择的状态的数量和图像的视觉伺服IBVS，建立基于卡尔曼滤波器的观测器模型；

S3、根据建立的基于卡尔曼滤波器的观测器模型获得基于图像的视觉伺服IBVS的雅可比矩阵未知深度的估计信息；

所述状态的数量包括：单一状态和多状态；

提取雅可比矩阵的部分深度信息作为基于卡尔曼滤波器的观测器模型的状态向量。

具体地，关于本实施例中的基于图像的视觉伺服系统如下所示述：

基于点特征的IBVS

对于世界坐标系中坐标为P＝(X，Y，Z)∈R^1×3的3D点，将其投影到常规针孔相机模型的像平面中，并将其归一化为平面点坐标p＝(x，y)∈R^1×2。

可以得到：

其中，α是像素尺寸的比率，f是焦距，图像平面的主点坐标是(u₀，v₀)，(u，v)是以像素为单位表示的图像点的坐标。在这种情况下，p＝(x，y)是像平面的坐标，它是视觉伺服控制系统的输入信号。

通过对公式(1)求时间导数，并将三维点的速度与末端执行器的空间速度相关联，我们得到：

v_c＝[v_x，v_y，v_z]^T和w_c＝[w_x，w_y，w_z]^T分别是摄像机的线速度矢量和角速度矢量。可以找到相机速度与图像特征运动之间的关系：

其中：

Lx是图像雅可比矩阵，其中特征点相对于相机坐标系的深度信息Z无法直接测量。因此，使用IBVS的控制方案必须估计深度信息的值。

基于线特征的IBVS

线在归一化图像平面上的映射仍然是线，并且设像素点的坐标是(x，y)，则线性映射关系表达式为：

x cosθ+y sinθ＝ρ (5)

如图2所示，ρ表示从图像的平面原点到直线的距离，θ表示直线的方向。

图像平面的线特征坐标值和照相机运动之间的关系可以从现有工作中得到：

其中：

ax+by+cz+d＝0是直线所在平面的方程。当该线大致垂直于相机的光轴时，相机运动速率和图像线特征坐标变化之间的转换关系可以近似表示为：

基于单一状态模型下的点特征深度估计

与现有的使用卡尔曼滤波器估计雅可比矩阵的方法不同，本实施例中提取了雅可比矩阵中的深度信息，而不是提取整个雅可比矩阵作为卡尔曼滤波器的状态向量。根据公式(3)，可以得到：

系统中特征点的位置信息[x，y]可以直接测量。如果通过对时间求导[x，y]获得

则测量信号中的误差信息将被放大，从而导致得到的观测向量

的误差很大。为了解决

难以测量的问题，我们在极短的一个采样时间内(系统的一个采样时间)同时对公式(10)的两侧进行积分，将观测向量由

换为可以直接测量得到的特征点的位置坐标信息。

其中ti是每个积分周期的开始时间，N是总的时间段数。以第一个积分间隔为例：

在很短的时间内，可以认为深度信息是不变的，上述公式可以转换为：

由于深度的变化是连续且缓慢的，因此能够认为一个积分时间段中的深度值近似相等。则公式(13)是合理的。因此，本实施例中考虑的视觉伺服控制系统通常可以使用以下的数学模型进行表示：

u(k)＝C(k)s(k)+V(k)

(14)

其中：

A(k)是单位矩阵,s(k)＝[1/Z_k]是系统在时间k的状态向量。u(k)＝[x(t_k+Δt)-x(t_k)，y(t_k+Δt)-y(t_k)]是系统的观测向量。

和W(k)分别是过程噪声与观测噪声。

和W(k)的协方差分别是Q和R。

其中假定过程噪声和观察噪声均为高斯白噪声。

基于以上的数学模型得到深度估计的递归卡尔曼滤波模型。

状态向量预测：

协方差预测：

卡尔曼滤波器增益矩阵：

状态向量更新：

s_k|k＝s_k|k-1+K_k[u_k-C_ks_k|k-1] (20)

协方差更新：

P_k|k＝I-K_kC_k (21)

其中Q是过程噪声协方差，R是观测噪声协方差，P是状态估计误差协方差。五个卡尔曼滤波器的递归估计公式是一个递归循环过程，包括两个步骤的预测和更新。(17)和(18)是状态变量和状态估计误差的协方差的预测估计。

然后，(19)、(20)和(21)是对卡尔曼增益，状态变量和状态估计误差协方差的校正。

基于单一状态模型下的线特征深度估计

考虑线特征大致垂直于相机光轴的情况。深度信息s(k)＝[1/Z_k]是系统状态向量，而u(k)＝[ρ(t_k+Δt)-ρ(t_k)，θ(t_k+Δt)-θ(t_k)]是系统的观察向量。推导公式(9)可得出以下公式：

可以得到线特征下的视觉伺服系统的数学模型为：

与点特征相同，针对使用线特征的情况设计了一种新的基于卡尔曼滤波器的观测器结构，该结构使用线特征视觉伺服模型来实时估计雅可比矩阵中的深度信息。

基于多状态模型下的点特征深度估计

与基于单状态数学模型的卡尔曼滤波器不同，该部分重新构建了系统的数学模型，选择了更多的状态来构建状态向量，并提高了估计的准确性。由于系统状态方程中的系数矩阵A(k)需要实时采样，因此无法获得A(k)的明确表达式，也无法求解Riccati方程。本实施例中选择离散化系统，而不是对连续系统使用卡尔曼滤波器。深度信息1/Z和二维图像平面点(x，y)的坐标值是系统状态变量s(k)＝[x_k，y_k，1/Z_k]，并且系统的观测向量与状态向量相同，u(k)＝[x_k，y_k，1/Z_k]。在获得连续系统的状态空间表达式之后，使用前向差分法离散化连续系统以获得离散数学模型。然后使用卡尔曼滤波器实时估计系统深度。具体的模型求解过程如下：

使用前向差分法离散化连续系统：

然后，可以得到视觉伺服系统模型为：

其中C(k)是单位矩阵，m(k)是系统离散化产生的误差。φ(k)是Tm(k)与系统过程噪声之和，W(k)是系统的观测噪声。T是系统的采样周期。一般而言，采样周期T越小，系统的离散程度越高。但是，考虑到两个采样之间的差异太小，调节效果会减弱，所以采样T不应太小。根据经验，在4-20毫秒之间选择T更合适。与基于单状态模型的点特征深度估计相同，这一部分设计了基于多状态视觉伺服模型的卡尔曼滤波器结构，用于估计雅可比矩阵中的深度信息。与单状态方案相比，该方案可以通过使用更多的状态量和观测量来获得更准确的估计结果，但是计算量较大。

为了验证所提方法的有效性，在6自由度机械臂设备上进行了一些仿真和实验。并将仿真与传统方法(一种新的非线性降阶观测器结构方法)中的现有工作进行比较。为了评估该方法的实用性，本实施例中使用Matalb中的机器人视觉工具箱进行仿真，然后选择点特征和线特征两种图像特征进行仿真验证。由于仿真只是代表理想情况，因此还在6自由度机械手进行了物理实验。

这些仿真使用6自由度Puma560机器人。对于使用点特征的情况，选择的四个点在笛卡尔空间中为一个正方形，其相对于摄影机框架的初始坐标为(0.01，0.01，1.17)，(0.01，0.01，1.17)，(0.01，0.01，1.17)，(0.01，0.01，1.17)。像素平面中目标的坐标值为(264.9，171.4)，(285.1，219)，(332.8，198.9)，(312.6，151.2)。在使用传统的恒定深度值方法时，我们在系统中设置z＝0.4(这样可以获得更好的仿真结果)对于使用卡尔曼滤波器的两种新方法，过程噪声协方差和观察噪声协方差为Q＝1e-2和R＝0.5I2×2。模糊控制器的设计采用第四部分中提出的方法。

仿真结果如图3-5所示。VS任务期间四种方法的相机轨迹分别如图3所示。可以表明，与经典方法和非线性降阶观测器结构方法相比，两种新方法的图像二维平面的轨迹更直。图4给出了VS任务期间四种方法的相机速度曲线。图5给出了四种方法的特征误差曲线。与传统方法和基于非线性降阶观测器的方法相比，所提出的新方法收敛时间短，并且收敛速度快。相机速度和特征误差的曲线更平滑。两种基于卡尔曼滤波器的新方法的结果是相似的，但是从仿真图来看，比传统方法和基于非线性观测器的方法要好得多。

图3：四种方法的相机轨迹，图(a)是经典的基于点的IBVS方法，图(b)是非线性降阶观测器方法，图(c)是基于单状态模型的卡尔曼滤波方法，图(d)提出的基于多状态的卡尔曼滤波方法，绿色三角形和红色矩形分别代表初始位置和期望位置。

图4：四种方法的相机速度曲线，图(a)是基于经典点特征下的IBVS方法，图(b)是非线性降阶观测器方法，图(c)是基于单状态模型的卡尔曼滤波方法，图(d)是基于多状态模型的卡尔曼滤波方法。

图5：四种方法的特征误差，图(a)是经典的基于点的IBVS方法，图(b)是非线性降阶观测器方法，图(c)是基于单状态模型的卡尔曼滤波方法，图(d)是基于多状态模型的卡尔曼滤波方法。

本实施例中还比较了使用线特征下的仿真结果。由于非线性降阶观测器方法不能用于线特征，因此使用线特征进行仿真的这一部分只与传统方法进行比较。同时，由于两种提出的新方法在点特征的仿真比较中具有相似的结果，因此本部分仅选择基于单状态的卡尔曼滤波器与传统方法进行比较。卡尔曼滤波器的设计和模糊控制器的设计规则与点特征下的设计相同。在视觉伺服任务中选择了三条线作为图像特征。仿真结果如图6-8所示。VS任务期间两种方法的相机轨迹在图6中给出。图7显示了两种方法的相机速度曲线。图8给出了两种方法的相机运动的位置和角度变化。通过对仿真图的分析，可以得出，与传统方法相比，新方法的相机的轨迹的冗余运动更少，收敛时间也缩短了近1/3。同时，相机的速度变化更加平稳光滑，可以有效减少速度变化过快对机械臂的损害。与传统方法相比，该方法有明显的改进。

图6：两种方法的相机轨迹，图(a)是经典的基于线特征的IBVS方法，图(b)是基于单状态模型下的卡尔曼滤波方法，“紫色点”和“红色点”分别代表初始位置和期望位置。

图7：两种方法的相机速度曲线，图(a)是经典的基于线特征的IBVS方法，图(b)是基于单状态模型下的卡尔曼滤波方法。

图8：两种方法的相机位置和角度，图(a)是经典的基于线特征的IBVS方法，图(b)是基于单状态模型下的卡尔曼滤波方法。

实验是在6自由度机械手系统上完成的。使用Denso6轴机器人，Denso 6轴机器人由Denso UP6242G和Quanser开放式体系结构控制模块组成，该模块具有工业相机系统的所有功能，并连接到QUARC。控制器以1kHz的频率驱动每台电机运行。相机以每秒40帧的采样率采样，然后将图像数据实时传输到图像处理器。相机的分辨率为374×240像素。

在实验过程中，选择图像特征为点特征。二维图像平面中初始点的坐标为(57.21，189.69)，(89.63，188.15)，(87.74，155.29)，(55.50，157.09)，目标点坐标为(341.4，90.57)，(395.2，88.62)，(392.7，33.87)，(339，35.68)。在使用两种新方法的实验中，参数λ被选为由6个模糊控制器实时估计的数据组成的矩阵。传统方法中将λ选择为0.15(可以得到较好的控制效果)。两种新方法中的卡尔曼滤波器是根据3.1的介绍设计的。

图9(a)中的黑点和红点分别表示目标点的初始位置和期望位置。经典控制器和非线性降阶观测器方法下的相机轨迹如图9(b-c)所示。图9(d)是实施例中所提出的基于单状态模型的卡尔曼滤波方法下的相机轨迹，图9(e)是实施例中所提出的基于多状态模型的卡尔曼滤波方法的相机轨迹。图10显示了四种方法的相机速度曲线。图11中给出了四种方法下的特征误差变化曲线。根据四种方法下的点特征轨迹和相机速度曲线，可以得到四种方法都可以成功完成视觉伺服任务。但是显然，在两种新方法的控制下，可以得到更少冗余运动的轨迹。而且它具有较短的收敛时间，收敛时间几乎减小到传统方法的1/4。

图9：实验的相机轨迹：(a)实验的初始状态。(b)经典方法下的相机轨迹。(c)非线性降阶观测器方法下的相机轨迹。(d)实施例中提出的基于单一状态模型的卡尔曼滤波方法下的相机轨迹。(e)提出的基于多状态模型的卡尔曼滤波方法下的相机轨迹。

图10：四种方法的相机速度曲线，图(a)是经典的基于点的IBVS方法，图(b)是非线性降阶观测器方法，图(c)是基于单一状态模型的卡尔曼滤波方法，(d)是实施例中提出的基于多状态模型的卡尔曼滤波方法。

图11：四种方法的特征误差变化曲线，图(a)是经典的基于点的IBVS方法，图(b)是非线性降阶观测器方法，图(c)是基于单一状态模型的卡尔曼滤波方法，(d)是实施例中提出的基于多状态模型的卡尔曼滤波方法。

以上结合具体实施例描述了本发明的技术原理，这些描述只是为了解释本发明的原理，不能以任何方式解释为对本发明保护范围的限制。基于此处解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。

Claims (9)

1.一种基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，包括如下步骤：

S1、确定选择的状态的数量；

S2、根据选择的状态的数量和图像的视觉伺服IBVS，建立基于卡尔曼滤波器的观测器模型；

S3、根据建立的基于卡尔曼滤波器的观测器模型获得基于图像的视觉伺服IBVS的雅可比矩阵未知深度的估计信息；

所述状态的数量包括：单一状态和多状态；

提取雅可比矩阵的部分深度信息作为基于卡尔曼滤波器的观测器模型的状态向量。

2.根据权利要求1所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，当状态的数量为单一状态下的点特征深度估计时，所述方法中的基于卡尔曼滤波器的观测器模型为：

u(k)＝C(k)s(k)+V(k)

其中：

A(k)是单位矩阵，s(k)＝[1/Z_k]是系统在时间k的状态向量；u(k)＝[x(t_k+Δt)-x(t_k)，y(t_k+Δt)-y(t_k)]是模型的观测向量；

和W(k)分别是过程噪声与观测噪声；

和W(k)的协方差分别是Q和R。

3.根据权利要求2所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，假定过程噪声和观察噪声均为高斯白噪声，针对使用点特征的情况还包括一种基于卡尔曼滤波器的观测器结构为深度估计的递归卡尔曼滤波模型：

状态向量预测：

协方差预测：

卡尔曼滤波器增益矩阵：

状态向量更新：

s_k|k＝s_k|k-1+K_k[u_k-C_ks_k|k-1]

协方差更新：

P_k|k＝I-K_kC_k

其中Q是过程噪声协方差，R是观测噪声协方差，P是状态估计误差协方差。

4.根据权利要求1所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，当状态的数量为单一状态下的线特征深度估计时，所述方法还包括：深度信息s(k)＝[1/Z_k]是模型状态向量，而u(k)＝[ρ(t_k+Δt)-ρ(t_k)，θ(t_k+Δt)-θ(t_k)]是模型的观察向量，模型建立过程如下：

其中，θ和ρ分别为线特征在极坐标下的坐标表示；v_x，v_y，v_z为机械臂末端执行器在x、y、z方向的线速度，w_x，w_y，w_z为机械臂末端执行器在x、y、z方向的角速度；W(k)为系统的测量误差。

5.根据权利要求4所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，当状态的数量为单一状态下的线特征深度估计时，所述方法中的基于卡尔曼滤波器的观测器模型为：

其中s(k)与u(k)分别为系统的状态向量与观测向量；θ和ρ分别为线特征在极坐标下的坐标表示；t_k表示第k个时间段的起始时间点；Φ(k)与W(k)分别为系统的过程噪声与观测噪声；v_x,v_y,v_z为机械臂末端执行器在x、y、z方向的线速度，w_x,w_y,w_z为机械臂末端执行器在x、y、z方向的角速度。

6.根据权利要求1所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，当状态的数量为多状态下的点特征深度估计时，所述方法还包括：

深度信息1/Z和二维图像平面点(x，y)的坐标值是模型状态变量s(k)＝[x_k，y_k，1/Z_k]，并且模型的观测向量与状态向量相同，u(k)＝[x_k，y_k，1/Z_k]；在获得连续系统的状态空间表达式之后，使用前向差分法离散化连续系统以获得离散数学模型；然后使用卡尔曼滤波器实时估计系统深度的模型建立过程如下：

使用前向差分法离散化连续系统：

其中，x_k、y_k为第k个采样时刻特征点在图像平面的坐标值；同理，x_k+1、y_k+1为第k+1个采样时刻特征点在图像平面的坐标值；v_xk,v_yk,v_zk为机械臂末端执行器在第k个采样时刻x、y、z方向的线速度，w_xk,w_yk,w_zk为机械臂末端执行器在第k个采样时刻x、y、z方向的角速度；T为系统的采样周期；Z为待估计的特征点的深度信息。

7.根据权利要求6所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，

视觉伺服系统模型为：

其中C(k)是单位矩阵，m(k)是系统离散化产生的误差；

φ(k)是Tm(k)与系统过程噪声之和；

W(k)是系统的观测噪声；T是系统的采样周期。

8.根据权利要求1所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，基于点特征的图像的视觉伺服IBVS的相机速度与图像特征运动之间的关系：

其中：

Lx是图像雅可比矩阵；

其中x、y为图像特征点在二维图像平面的坐标值，Z为特征点的深度信息；v_c与w_c分别为机械臂末端执行器的线速度与角速度。

9.根据权利要求1所述的基于图像的视觉伺服的雅可比矩阵深度的信息估计方法，其特征在于，基于点特征的图像的视觉伺服IBVS的相机速度与图像特征运动之间的关系为：

其中，θ和ρ分别为线特征在极坐标下的坐标表示；v_c与w_c分别为机械臂末端执行器的线速度与角速度。

Images