自适应卡尔曼滤波图像雅克比估计

孙冬雪， 杨宏韬， 刘克平

(长春工业大学 电气与电子工程学院， 吉林 长春 130012)

自适应卡尔曼滤波图像雅克比估计

孙冬雪， 杨宏韬， 刘克平*

(长春工业大学 电气与电子工程学院， 吉林 长春 130012)

基于自适应卡尔曼滤波实现了图像雅克比矩阵的递归估计，以及过程噪声和测量噪声的在线估计和调节。补偿了图像采集、传输和处理带来的时延，进行了仿真验证。

图像雅克比矩阵； 自适应卡尔曼滤波； 时延补偿

0 引 言

图像雅克比矩阵估计是视觉伺服领域最重要的研究内容之一。在机器人系统中，图像空间与机器人工作空间的非线性微分变化关系就是利用图像雅克比矩阵表示的，其关系如下[1]：

由于机器人关节位置是随时间变化的，由此可得图像雅克比矩阵是时变的，因此需要对其在线计算或估计。

传统方法一般利用Broyden估计器对图像雅克比矩阵进行估计[2-4]。其基本原理是利用机器人每一步的运动变化求取相应的图像特征误差变化，从而估计各个时刻的图像雅克比矩阵的值。此类方法从最初的静态Broyden更新法改进为动态Broyden更新法，在一定范围内提高了机器人跟踪的稳定性和收敛速度；后来发展为带遗忘因子动态Broyden更新法，此方法在图像雅克比矩阵的估计过程中，利用遗忘因子减少了旧数据的权重，从而减小了旧数据对系统稳定性的影响。上述方法不仅都需要精确的机器人及摄像机参数，而且也没有考虑噪声和时延对系统的影响，因此，当相机到达期望位置后，不可避免地存在较为明显的震荡现象。

为了克服Broyden方法存在的问题，近年来,对图像雅克比矩阵在线估计方法的研究也吸引了国内外许多学者的关注。文献[5]使用递推最小二乘法，在系统存在噪声时对图像雅克比矩阵进行在线估计；文献[6]通过跟踪一个点在图像特征空间的运动，用最小二乘法估计当前时刻的图像雅克比矩阵;文献[7]在没有考虑系统时延和噪声的情况下，利用目标点期望位置和实际位置的差值定义方差最小化函数，并由此估计图像雅克比矩阵；文献[8]则控制机器人在工作空间中产生几次微小的运动，对于每次微小运动在图像特征空间都会有相应的变化，通过两者之间的比值得到局部图像雅克比矩阵的动态估计值；文献[9]将图像雅克比矩阵作为系统的状态观测量，然后利用Kalman-Bucy滤波算法对其进行估计，这种研究方法在近年来也得到了广泛的应用，并取得了较好的成果。

然而，上述研究方法都是假设系统没有时延且已知过程噪声协方差阵Q和测量噪声协方差阵R等模型参数。但是在机器人视觉伺服控制中，难以获取精确的过程噪声协方差阵Q和测量噪声协方差阵R等模型参数，特别是当系统还同时存在由图像采集、传输和处理过程带来的时延情况下，采集的滤波参数不仅难以保证系统稳定最优的估计结果，而且可能导致系统发散[10]。

针对文献[5-9]的不足，文中提出了一种自适应卡尔曼滤波补偿算法。该方法不但能够对滤波参数进行在线估计和调节，还能利用自适应卡尔曼滤波基本方程，通过图像雅克比矩阵的视觉伺服系统状态模型估计当前时刻的图像雅克比矩阵，进而预测目标特征。最后通过仿真验证了机器人跟踪过程中的误差可以利用文中所提方法得到有效的改善，从而更进一步表明了该方法适用于解决图像雅克比矩阵的估计问题。

1 图像雅克比矩阵

图像雅克比矩阵是视觉伺服的一个核心概念，需要用它将视觉空间反馈信息映射到机械手控制空间中对机械手运动实施控制，使其到达期望状态。因此，图像雅克比矩阵的求取是视觉伺服研究的一个重要内容。

机器人在运动过程中，末端执行器与目标对象的相对位置一定会发生变化。虽然这种变化与摄像机安装位置或目标对象的运动无关，但是能通过图像特征变化表现出来。所以，两者之间必然存在着一一对应的映射关系。如果想从图像特征的变化得到机器人的动作规律，就要明确这种映射关系。因此，在基于图像的视觉伺服控制系统中，图像雅克比矩阵常被用来表示这种映射关系。

式(1)是目前使用最多的图像雅克比矩阵模型。在基于图像的视觉伺服控制系统中，因为摄像机的位置变化是由机器人各关节运动引起的，所以式(1)的变化关系可以转移到机器人的关节空间上，此时，式(1)的映射关系可变为：

J2=J1Jr也称为图像雅克比矩阵。单从定义上看，它表示了图像空间与机器人关节空间的联系。虽然式(1)与式(3)定义图像雅克比矩阵表达形式不同，但是机器人各关节运动规律的计算都可以从图像特征变化求取图像雅克比矩阵的逆得到。

2 自适应卡尔曼滤波补偿算法

2.1 自适应卡尔曼滤波算法

主要有两种噪声干扰存在于未知的机器人视觉伺服系统的图像雅克比矩阵估计中：

1)过程噪声η(k)是由系统的成像模型变化导致的；

2)测量噪声v(k)是由系统的图像采集与处理导致的。

自适应卡尔曼滤波的基本思想是针对未知或不确定的系统噪声统计特性(Q,R)问题，利用观测数据进行滤波，与此同时，对其进行在线修正。从而减小滤波误差，防止滤波器发散[11]。将式(1)离散化为:

为实现对J(q)矩阵各个元素的估计，定义mn×1维的状态向量为[12]：

与此同时，将式(5)定义为系统状态，而系统输出定义为图像特征变化的差值，定义如下：

得到如下状态方程：

式中：η(k)、v(k)----分别为过程噪声序列和图像观测噪声序列，其统计特性如下：

据此可以设计出自适应卡尔曼滤波的递推算法：

式中：Pk----状态估计误差方差阵。

2.2 自适应卡尔曼滤波补偿器设计

受系统时延的影响，在实际控制过程中,当前时刻特征点的状态不能直接得到。假设系统采样周期为T，由图像采集、传输和处理等过程引起的时延为hT。从而只能得到从初始0时刻到k-h时刻的机械手末端在图像空间的位置fi(i=0,1，…,k-h)[13-14]。根据式(7)可以得到系统的状态方程和观测方程，从k-h+1时刻到k时刻的图像雅克比矩阵可以用自适应卡尔曼滤波估计出来。

在考虑时延情况时,视觉伺服系统的状态方程和观测方程为：

加时延补偿后的自适应卡尔曼滤波算法如下：

将视觉伺服系统用式(13)和式(14)的自适应卡尔曼滤波补偿算法进行h次预测，从k-h+1时刻到k时刻的图像雅克比矩阵就能估计出来。

3 仿真实验研究

通过一个执行器末端带有摄像机的机器人系统来验证所给出的基于自适应卡尔曼滤波补偿算法估计图像雅克比矩阵的有效性，该系统完成由摄像机引导机器人沿环形轨迹运动的任务。

设被跟踪目标的运动方程为:

假设机器人起始关节角位于(0°,90°)处，采样周期为50 ms，粗略估计J(q)的初值为

自适应卡尔曼滤波的初始参数取值如下：过程噪声是均值为0.1，方差为0.5的高斯白噪声，测量噪声均值的初值为r=[0.01;0.01]，初始方差阵Rv=0.5diag{1,1}，遗忘因子b=0.65。状态估计误差方差阵P的初值可取P(0)=105I4。假定系统时延τ是两个采样周期。

采用卡尔曼滤波方法的图像雅克比矩阵估计实验结果如图1～图3所示。

图1 轨迹跟踪曲线(Kalman)

图2 跟踪误差曲线(Kalman)

图3 关节角运动轨迹(Kalman)

采用自适应卡尔曼滤波补偿算法的图像雅克比矩阵估计的实验结果如图4～图6所示。

由图1和图4可以看出，机械手在跟踪初期能够快速接近目标，并且对椭圆轨迹进行比较精确的跟踪。从图2和图5中可以看出，两种方法在k=8左右迅速接近目标轨迹；由图2可以看出,在k=20左右系统达到稳定状态；在k=70～120之间系统出现震荡；当k>120时，系统才逐渐归于稳定。采用自适应卡尔曼滤波补偿算法比卡尔曼滤波算法在跟踪过程中的轨迹曲线更平滑；而且跟踪轨迹在k=30左右几乎没有误差，当k>100时，系统无明显震荡，体现了较好的跟踪性能。

图4 轨迹跟踪曲线(加时延与自适应)

图5 跟踪误差曲线(加时延与自适应)

图6 关节角运动轨迹(加时延与自适应)

跟踪误差及时间对比见表1。

表1 跟踪误差及时间对比表

利用表1中的数据可以对比两种估计图像雅克比矩阵的方法，从而得出利用自适应卡尔曼滤波补偿算法估计图像雅克比矩阵的机器人在X轴和Y轴的跟踪误差减小。因此，采用自适应卡尔曼滤波补偿算法估计图像雅克比矩阵的方法更具有实用性。

4 结 语

提出了基于自适应卡尔曼滤波补偿算法的图像雅克比矩阵估计方法。在噪声和时延系统中，该方法可以有效地估计图像雅克比矩阵，并应用于机器人视觉伺服控制中。仿真实验证明了所给出的自适应卡尔曼滤波补偿算法可以准确地估计图像雅克比矩阵，并且降低了跟踪过程中X、Y轴方向上的跟踪误差，减小了目标跟踪过程中的震荡，从而提高了机器人系统的跟踪精度。

[1] 高振东,苏剑波.带有时延补偿的图像雅克比矩阵估计方法[J].控制理论与应用,2009,26(1):23-27.

[2] Shademan A, Farahmand A M, Ja Gersand M. Robust jacobian estimation for uncalibrated visual servoing[C]//IEEE International Conference on Robotics and Automation. [S.l.]: IEEE,2010:5564-5569.

[3] 曾祥进,黄心汉,王敏.基于Broyden在线图像雅可比矩阵辨识的视觉伺服[J].华中科技大学学报：自然科学版,2008,36(9):17-20.

[4] Piepmeier J A, Mcmurray G V, Lipkin H. A dynamic jacobian estimation method for uncalibrated visual servoing[C]//Ieee/asme International Conference on Advanced Intelligent Mechatronics. Proceedings. [S.l.]: IEEE,1999:944-949.

[5] Piepmeier J A, Mcmurray G V, Lipkin H. Uncalibrated dynamic visual servoing [J]. Robotics & Automation IEEE Transactions on,2004,20(1):143-147.

[6] Yoshimi B H, Allen P K. Alignment using an uncalibrated camera system [J]. IEEE Transactions on Robotics & Automation,1995,11(4):516-521.

[7] 项龙江,司秉玉,薛定宇,等.模型无关的无定标视觉伺服控制[J].机器人,2003,25(5):424-427.

[8] 郭振民,陈善本,吴林.一种基于图象的无标定视觉伺服方法的研究[J].哈尔滨工业大学学报,2002,34(3):294-296.

[9] Qian J, Su J. Online estimation of image Jacobian matrix by Kalman-Bucy filter for uncalibrated stereo vision feedback[C]// IEEE International Conference on Robotics and Automation, 2002. Proceedings. ICRA. [S.l.]: IEEE Xplore,2002:562-567.

[10] 刘文芳,邴志刚,卢胜利,等.带时延补偿的图像雅克比矩阵在线估计方法[J].计算机工程与应用,2010,46(21):181-184.

[11] 辛菁,白蕾,刘丁.基于自适应kalman滤波的机器人6DOF无标定视觉定位[J].系统仿真学报,2014,26(3):586-591.

[12] 徐鹏，杨宏韬，李岩，等.基于H∞粒子滤波算法的视觉伺服雅可比矩阵在线辨识[J].广西大学学报:自然科学版，2017，42(3):1051-1057.

[13] 王新梅,魏武,刘玮,等.鲁棒卡尔曼滤波下的图像雅可比矩阵带时延补偿的估计[J].控制理论与应用,2015,32(8):1052-1057.

[14] 张显伟，吴忠伟，张凤海，等.移动机器人模糊滑模轨迹跟踪控制[J].长春工业大学学报：自然科学版，2013,34(4)：373-378.

ImagejacobianmatrixestimationbasedonadaptiveKalmanfilter

SUN Dongxue, YANG Hongtao, LIU Keping*

(School of Electrical & Electronic Engineering, Changchun University of Technology, Changchun 130012， China)

An adaptive Kalman filter is applied to realize the recursive estimation of the image Jacobian matrix. The process and measurement noise are estimated and adjusted on-line compensate the delay brought by image acquisition, transmission and processing. Simulation results indicate the method is feasible.

image jacobian matrix; adaptive Kalman filter; time-delay compensation.

2017-07-22

吉林省科技厅基金资助项目(201505190009JH)； 吉林省教育厅基金资助项目(2016338)

孙冬雪(1991-)，女，汉族，吉林松原人，长春工业大学硕士研究生，主要从事机器人视觉伺服方向研究,E-mail:sundongxue1121@163.com. *通讯作者：刘克平(1971-)，男，汉族，吉林长春人，长春工业大学教授，博士生导师，主要从事复杂系统建模、优化与控制等方向研究,E-mail:liukeping@ccut.edu.cn.

10.15923/j.cnki.cn22-1382/t.2017.5.07

TP 241

A

1674-1374(2017)05-0447-06