基于卡尔曼滤波器的图像雅克比在线估计

刘 伟， 于振中 ， 惠 晶

(江南大学 轻工过程先进控制教育部重点实验室，江苏 无锡214122)

在基于标定的视觉伺服中［1-2］，摄像机内外部参数等系统相关参数必须在视觉伺服之前精确标定。然而，这些系统参数的精确标定不仅费时且很难获得。无标定视觉伺服技术在只有这些参数粗略知识甚至未知的情况下也能对机械手运动实施有效控制［3-4］。

基于图像雅克比在线估计方法的无标定视觉伺服的核心问题是获得实时满足系统控制性能要求的图像雅克比估计值。图像雅克比在线估计目的不是辨识出真实的系统参数，而是能够利用估计到的图像雅克比设计满足性能要求的视觉控制器［5-6］。目前，对图像雅克比的在线估计方法已有大量的研究［7-8］，但是这些估计方法在处理图像噪声方面很薄弱。文中提出一种基于卡尔曼滤波器的图像雅克比在线估计方法，该方法对图像噪声有很强的鲁棒性且不需要关于系统参数的先验知识。

1 图像雅克比矩阵

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

机械手手爪的位置在机械手基坐标系中可表示为p = ［p1，p2，…，pn］T，手爪在图像空间中表示为f = ［f1，f2，…，fm］T。当手爪移动时，它在机器人工作空间的速率˙p 和在图像空间的速率之间有如下关系:

其中，J(p)为一个m × n 的图像雅克比矩阵。式(1)描述了f·与˙p 之间的微分关系。下面将用卡尔曼滤波器对J(p)进行在线估计。

2 卡尔曼估计

卡尔曼滤波器被广泛运用到系统状态的观测中，它对系统噪声和外部干扰有很强的鲁棒性。构建一个辅助系统，它的状态向量x(k)在时刻k 是由图像雅克比J(k)的一连串行元素构成的增广向量。

其中

是图像雅克比J(k)第i 行。式(1)的离散形式为

定义系统输出为y(k)= f(k +1)－f(k)，辅助系统可以被描述为

其中，η(k)，v(k)为状态噪声和观测噪声，在文中它们都被设为是高斯白噪声。C(k)为测量矩阵:

对于式(4)描述的辅助系统，用卡尔曼滤波器观测其状态变量简单易行。在每一步中可由以下迭代公式对图像雅克比进行合理的估计。

式中:Rη和Rv为η(k)和v(k)的协方差矩阵。

辅助系统的初始状态^x(或^J(0))可由以下步骤获得。让机械手在初始位置的附近区域移动n 次Δp1… Δpn，在图像空间中观察对应位移Δf1… Δfn。初始雅克比矩阵可由下式获得:

水利工程中堤坝是其重要的组成部分，其具有种类多，施工工艺较繁琐的特点，特别是较易形成渗漏，对堤坝的渗漏原因进行深入分析，在此同时，结合堤坝渗漏情况，选择最为适合的防治加固技术对其进行有效处理，保证堤坝防渗加固技术的应用效果。

其中，^x(0)可由^J(0)带入式(2)得到。

3 雅克比视觉伺服控制器

在有视觉信息反馈下图像雅克比可用于控制机械手移动到期望位置。由文中提出的卡尔曼估计方法获得图像雅克比的估计值后，需采用一种控制策略驱动机械手从当前位置运动到期望位置。如果机械手的当前位置和期望位置都用在图像空间中的特征向量表示，可以定义在k 时刻两者之间的误差:

其中，f*(k)为手爪在图像空间中的期望位置;fg(k)为手爪在图像空间中的当前位置。在视觉跟踪任务中，期望位置f*(k)定义为移动目标fo(k)的图像位置。那么控制策略为

式(10)中λ(k)由机械手的速率约束方程设定:

其中，Δpmax为机械手驱动器可提供的手爪最大移动速度。

整个无标定手眼协调系统的架构如图1 所示。

图1 无标定手眼协调系统Fig.1 Uncalibrated hand-eye coordination system

4 仿真与实验

通过仿真与实验验证该图像雅克比估计方法的性能和该控制方案的有效性。图2 为实验测试平台。该平台是由三自由度并联delta 机械手和一个相机组成的手眼协调系统。实验伺服任务为:驱动机械手，让其手爪跟踪一个二维随机运动的物体。机械手与相机的关系和相机参数都是未知的。

图2 实验测试平台Fig.2 Experiment testbed

4.1 仿 真

在仿真中手爪跟踪平面内运动目标的任务定义为:目标在xoy 平面做直线运动，并在运动过程中改变运动方向，机械手跟踪目标运动。仿真参数设置如下:相机的位置在机械手基坐标系中为［200，650，850］，它的光轴朝向基坐标系xoy 平面中的点［42，340，0］。相机的焦距f = 15 mm。手爪最大速度为0.8 m/s，初始位置为(320，420)。目标的运动速度为0.2 m/s，初始位置为(200，0)。为了使仿真更真实全面评价所提方法，在水平和垂直方向上对目标和手爪的图像位置叠加了三像素二维高斯噪声。机械手的跟踪轨迹和跟踪误差如图3 和图4 所示。

图3 仿真跟踪轨迹Fig.3 Curve of the simulation tracking

图4 仿真跟踪误差曲线Fig.4 Error curve of the simulation tracking

由图3 可以看出，机械手能在跟踪初期以手爪最大速度0.8 m/s 向目标接近，随后能够持续有效的对目标进行跟踪。

由图4 可以看出，跟踪开始后末端执行器与目标的误差呈急剧下降态势，在接近目标后出现小范围的震荡，随着跟踪步数的增加震荡减弱，最后收敛且误差小于10 mm。

综上所述，此种非标定方法能够实现机械手对目标的平稳跟踪，从跟踪路径上分析系统的响应速度还有待提高;从误差曲线上来看，虽然误差实现了较好的收敛，但是误差震荡较大，这也有待提高。

与其他图像雅克比在线估计方法相比，基于卡尔曼滤波器的方法对图像噪声有更强的鲁棒性。以文中方法和文献［7］提出的方法做同样的仿真实验，所有参数设置相同。仿真结果如图5 所示。

由图5 可以看出，由于图像噪声的影响手爪对目标的跟踪轨迹抖动较大。对比图3 与图5 可知，文中所提方法对图像噪声有更强的鲁棒性。

图5 对比轨迹Fig.5 Compared curves

4.2 实 验

在实验中，采用如图2 所示三自由度的并联Delta 机械手。视觉部分为一台彩色CCD 相机，图像窗口为512 × 512 像素。工作平面为200 mm ×200 mm。为了简化图像处理和对象识别，末端执行器和目标都贴上了色块。目标的初始位置为(20，125)，手爪初始位置为(80，140)。图像系统的采样频率为100 ms。目标的移动速度大约为0.1 m/s。实验所得机械手和目标运动轨迹如图6 所示。

由图6 可以看出，在跟踪过程中，机械手始终向目标靠近，并且其追踪随机运动目标的线路也较为合理，能够较好地完成跟踪任务。

图6 目标与机械手轨迹Fig.6 Tracks of the target and manipulator

5 结 语

提出基于卡尔曼滤波器的图像雅克比在线估计算法，该算法对视觉反馈噪声有很强的鲁棒性。基于这种估计方法，设计了一实现二维移动物体跟踪任务的无标定手眼系统。仿真和实验都显示了该方法对于图像雅克比矩阵在线估计的有效性。下一步工作将改进算法将其应用到其他无标定的手眼协调任务中。

［1］HU G，MacKunis W，Gans N，et al . Homography-based visual servo control with imperfect camera calibration［J］. Automatic Control，2009，54(6):1318-1324.

［2］ZHENG Yuan，PENG Silong.A practical roadside camera calibration method based on least squares optimization［J］.Intelligent Transportation Systems，2014，15(2):831-843.

［3］HAO M，SUN Zengqi.A universal state-space approach to uncalibrated model-free visual servoing［J］. Mechatronics，2012，17(5):833-846.

［4］Goncalves P J S，Mendonca L F，Sousa J M C，et al. Uncalibrated eye-to-hand visual servoing using inverse fuzzy models［J］.Fuzzy Systems，2008，16(2):341-353.

［5］Serra P，Cunha R，Hamel T，et al.Nonlinear image-based visual servo controller for the flare maneuver of fixed-wing aircraft using optical flow［J］.Control Systems Technology，2015，23(2):570-583.

［6］WANG Ying，LANG Haoxiang，De Silva C W，et al.A hybrid visual servo controller for robust grasping by wheeled mobile robots［J］.Mechatronics，2010，15(5):757-769.

［7］JIANG Ping，Bamforth L C A，FENG Zuren，et al.Indirect iterative learning control for a discrete visual servo without a camerarobot model［J］.Systems，Man，and Cybernetics，Part B:Cybernetics，2007，37(4):863-876.

［8］Chien Chern Cheah，LI Xiang，YAN Xiao，et al.Observer-based optical manipulation of biological cells with robotic tweezers［J］.Robotics，2014，30(1):68-80.