基于自适应无迹卡尔曼滤波的采摘机械手视觉伺服控制方法

梁喜凤，彭明，路杰，秦超

（中国计量大学机电工程学院，杭州310018）

0 引 言

视觉伺服技术是机器人的关键技术，在采摘机器人中应用也越来越广泛[1-2]。图像雅克比矩阵将视觉空间信息反馈到机械手运动空间，是基于图像的无标定视觉伺服控制方法的核心内容，其估计精度直接影响系统的视觉伺服控制精度[3]。对于图像雅克比矩阵的在线估计，国内外学者已经进行了大量研究。Yoshi和HOSODA 等提出了使用最小二乘法在线估计图像雅克比矩阵[4-5]。Piepmeier 提出了动态拟牛顿法估计图像雅克比矩阵，实现无标定手眼协调[6-7]。Matthew 提出了使用卡尔曼滤波以及自适应卡尔曼滤波的方法估计图像雅克比矩阵，实现对静态目标的定位和动态目标的跟踪[8-10]。Sang 等将鲁棒控制器与卡尔曼滤波方法结合起来形成鲁棒卡尔曼滤波，从而实现图像雅克比矩阵的在线估计[11-12]。哈尔滨工业大学机器人研究所李牧提出“眼在手”无标定机器人视觉伺服控制算法，通过将非线性目标函数最小化,采用迭代最小二乘法提高系统的稳定性，以视觉信息跟踪动态图像[13-14]。赵清杰等研究了基于粒子滤波估计图像雅克比矩阵的方法，实验证明该方法比卡尔曼滤波具有更高的估计精度和更强的鲁棒性[15-16]。辛菁等在噪声的统计特性不完全已知的情况下，设计自适应Kalman 滤波器能使6 自由度机器人到达期望的位置，且定位精度高[17-18]。孙冬雪提出了自适应卡尔曼滤波算法估计图像雅克比矩阵，进行过程噪声和测量噪声的在线估计和调节，补偿了图像采集、传输和处理带来的延时[19]。以上图像雅克比矩阵估计方法均以卡尔曼滤波法为基础，将机器人手眼关系近似线性化，且在视觉伺服过程中将时变的系统噪声协方差矩阵设为常数矩阵[20-22]。在系统噪声协方差矩阵选取恰当的情况下，上述方法均能很好地反映机器人手眼映射关系，实现视觉伺服任务。然而在实际视觉伺服过程中，系统噪声矩阵会发生变化，从而会影响视觉伺服控制精度，增加视觉伺服过程所用时间。

为克服未知统计特性的系统噪声对采摘机械手图像雅克比矩阵估计精度的影响，提高视觉伺服控制精度，减少视觉伺服过程所用时间，本文在KF 算法以及UKF 算法估计图像雅克比矩阵的基础上，对系统过程噪声协方差矩阵引入自适应噪声统计估计器，还原非线性的手眼映射关系，提出AUKF 算法，利用该算法估计图像雅可比矩阵并构建视觉伺服控制系统，以7 自由度番茄串采摘机械手为对象搭建试验平台，进行AUKF 算法可行性验证。

1 图像雅克比矩阵

基于图像的视觉伺服控制方法是通过相机采集目标，经过图像处理后提取目标图像特征，与机械手末端期望图像特征进行比较，经过图像雅克比矩阵转化为机械手末端空间位姿变化量。机械手末端位姿变化量再经过机械手雅克比矩阵，转化为各关节变化量，通过积分得到确定的关节角度增量，从而驱动机械手向固定目标运动。图像雅克比矩阵描述高度非线性的机器人手眼关系，即图像特征变化量与机械手末端位姿变化量之间的关系。利用图像雅克比矩阵的反馈信息，映射到机器人控制系

统中，可对机器人运动进行控制。其数学模型可表示为

2 基于AUKF算法估计图像雅克比矩阵

在传统KF 算法以及UKF 算法估计图像雅可比矩阵过程[20-22]中，将过程噪声方差矩阵Qk和系统噪声qk设为常数矩阵，并且在视觉伺服过程中保持不变。为适应时变的系统过程噪声统计特性，本文利用系统状态一步预测值与状态更新值之差构造噪声估计递推器，对UKF 算法中的系统噪声qk以及系统噪声协方差矩阵Qk进行自适应估计，构造AUKF 算法。定义图像雅克比矩阵的观测量作为系统状态输入，机械手末端运动所引起的图像特征变化为系统输出，设置滤波次数为100，AUKF 算法估计图像雅可比矩阵流程如图1所示。qk和Qk由自适应噪声统计估计器递推获得

图1 AUKF算法估计图像雅克比矩阵流程图Fig.1 Flow chart of estimating image Jacobian matrix with AUKF

在图像雅可比矩阵估计方法中，KF法的时间复杂度为O( N×n2)，空间复杂度为O(n3)。UKF 法使用UT 变换求取一步预测值，其时间复杂度为O(2M×N×n2)，空间复杂度为O(n3)。AUKF法在UKF法的基础上引入了自适应噪声估计器，并为qk与Qk额外分配存储空间，故其时间复杂度为O(2M×N×2n2)，空间复杂度大于O(n3)。其中M为Sigma点个数，N为总采样次数，n为预测协方差阵维数。因此，AUKF 法的时间复杂度与空间复杂度均高于传统KF法与UKF法。

3 采摘机械手视觉控制器模型

3.1 图像特征选取

本文机械手的采摘对象为果实串，与单个果实采摘仅以果实质心的图像特征作为视觉伺服系统输入信息不同[23-25]，果实串采摘时夹持与切割目标点均在果梗上，因此选取机械手末端执行器上2 点即切割点与夹持点作为期望特征点，果梗上2 点即采摘点与夹持点作为目标特征点。利用相机参数矩阵将目标特征点与期望特征点在相机坐标系下的三维坐标变换为像素坐标系下的坐标，变换过程为

其中ρw,ρh为每个像素点各自的宽和高，(u0,v0)为图像平面与光轴的交点，为空间点P 在相机坐标系下的坐标为空间点P 在像素坐标系中的坐标。目标点在像素平面成像坐标如图2所示。

图2 期望特征与目标特征像素坐标Fig.2 Desired feature and target feature pixel coordinates

图2 中，选取期望特征点与目标特征点像素坐标差u1,v1,u2,v2,u3,u4作为图像特征误差Δf，6 个图像特征误差计算方法为

式中u1为左目相机中夹持点与被夹持点横坐标差，像素；v1为左、右相机中夹持点与被夹持点纵坐标差。u2为左目相机中夹持点与被夹持点横坐标差；v2为左、右相机中切割点与被切割点纵坐标差；u3为左目相机中切割点与被切割点横坐标差；u4为右目相机中切割点与被切割点横坐标差。

视觉伺服的任务是使机械手末端2 点与果梗上2 个目标点重合，即图像特征误差趋近于0。

3.2 视觉控制器模型

视觉控制器模型将图像特征变化速度通过图像雅克比矩阵转化为机械手末端三维空间运动速度[26]，输入图像特征变化速度，输出机械手末端空间运动速度，其表达式为

其中J-1为图像雅克比矩阵逆。

期望图像特征fd为机械手末端夹持点与采摘点在相机成像平面的坐标，实际图像特征f为目标点在成像平面的坐标，Δe 为二者特征之差。本文利用P 控制方法建立视觉控制器模型，即：

4 仿真试验

4.1 机械手视觉伺服仿真系统

基于AUKF 算法设计视觉伺服控制系统，通过Robotic Toolbox 工具箱以及Machine Visual Toolbox 工具箱建立机械手模型与相机模型，利用Simulink 模块构建视觉伺服控制系统。视觉伺服控制流程如图3所示。

图3 视觉伺服控制流程图Fig.3 Flow chart of visual servo control system

视觉伺服系统根据输入的实际图像特征估计图像雅可比矩阵J(r)，经过求逆后得到图像雅可比矩阵的逆。期望图像特征为末端执行器上切割点与夹持点在相机平面内的成像坐标，在视觉伺服过程中为定值。实际图像特征与期望图像特征的差值为图像特征误差，经过计算后将图像特征变化量转化为机械手末端空间速率，其中k 为比例系数，经多次试验论证后，取最佳值为0.1。J(q)经过求伪逆后，与机械手末端空间速率相乘得出关节角速率，即q·= J+(q)· r·。经过积分器运算后得到Δq 作为正运动学的输入，输出为末端期望空间坐标P。同时q又输入到机械手雅可比矩阵模块，更新关节角度，即在下一次视觉伺服过程中，关节角度为(q + Δq )，在视觉伺服时间内，该循环始终存在，循环判断条件为是否达到设置的仿真时间。

4.2 仿真参数设置

为验证AUKF 算法估计图像雅克比矩阵的可行性，进行视觉伺服控制系统仿真试验，使机械手在视觉伺服系统控制下，从初始位置向目标点运动。仿真试验中，双目相机模型焦距均为0.008 m，像素尺寸均为（1e-0.5,1e-0.5）(单位:m/像素)，图像横轴和纵轴的分辨率均为1 280和1 024，(单位:像素), 图像中心像素坐标为（512，512）( 单 位: 像 素)。 机 械 手 关 节 角 度 初 始 值 为q =[0,0.4,0,0,0,pi/2,0 ]，在机械手初始状态下，空间中点P1(1.055,0.4,0.15)、P2(1.05,0.45,0.2)位于相机视野范围内，故取P1、P2两点作为目标点（果梗上被夹持点与被切割点）。目标点在相机中的初始图像特征误差为[u1,v1,u2,v2,u3,u4]=[110,120,100,50,45,20](单 位:pixels)。仿真时间设置为10 s，使用变步长求解器，求解器类型为4 阶龙格-库塔。视觉伺服过程结束后，在系统输出模块读取最终图像特征误差、末端执行器与目标位置误差以及响应时间。

4.3 仿真结果与分析

基于KF 法、UKF 算法、AUKF 算法估计图像雅克比矩阵构建的视觉伺服控制系统图像特征误差仿真图像以及最终图像特征误差如图4所示。

由图4 可知，机械手末端上的特征点与目标特征点在像素平面上的坐标差呈衰减趋势，最终实现机械手末端向目标靠近。由表1 可知，基于AUKF 算法的图像特征误差平均值为4.978 像素，相比于KF 算法（11.4985 像素）与UKF 算法（6.6175 像素），分别减小了54.44%与24.78%。基于AUKF 算法的视觉伺服控制系统仿真结果的6 个图像特征误差值均小于KF 算法和UKF 算法的视觉伺服控制系统。

机械手末端夹持点与目标三维坐标差初始值：[ΔX,ΔY,ΔZ] =[0.01,-1.45,0.75](单位:m)，视觉伺服控制系统运行过程中机械手末端执行器上夹持点与目标点的位置误差变化如图5示。

图4 图像特征误差变化过程Fig.4 Changing progress of image feature errors

图5 末端执行器位置与目标位置误差Fig.5 Changing progress errors between end-effector position and target position

由图5 可知，在视觉伺服过程中，末端执行器与目标点之间的三维坐标差呈衰减趋势。由表2 可知，基于AUKF 算法的视觉伺服控制系统中，末端执行器最终位置与目标距离为4.19 mm。基于KF 算法和UKF 算法的视觉伺服控制系统，末端执行器最终位置与目标距离误差分别为20.68 和21.33 mm，AUKF 算法相比于KF 算法与UKF 法构建的视觉伺服控制系统距离误差分别减小了79.74% 与80.36%。基于KF 算法、UKF 算法以及AUKF 算法的视觉伺服控制系统响应时间分别为2 s、1.5 s 以及1.2 s，AUKF 算法相比于KF 算法与UKF 算法，响应时间分别减少了40%与20%。结果表明，采用AUKF 算法估计的图像雅克比矩阵，能够克服未知统计特性的系统过程噪声，使图像雅克比矩阵估计更加准确，响应时间更短。

表1 图像特征误差仿真结果Table1 Simulation results of image features error pixels

表2 位置误差仿真结果Table2 Simulation results of position errors mm

5 验证试验

5.1 机械手视觉伺服系统试验平台

为验证基于AUKF图像雅可比矩阵估计算法的视觉伺服控制系统在实际采摘中的可行性，评价视觉伺服控制精度，搭建了7 自由度番茄串采摘机械手视觉伺服系统试验平台，如图6所示。

图6 采摘机械手视觉伺服系统试验平台Fig.6 Picking manipulator visual servo system test platform

视觉伺服系统以PC 机作为上位机，操作系统为Win7(64bit)，处理器为Intel Core i5-4200M 2.5GHz。双目相机为COMS 彩色相机，型号为DC-USB300-B，灵敏度为1.0V/lux-sec550 nm，光谱响应范围为400～1030 nm，像素尺寸为3.2 μm×3.2 μm，动态范围大于61 db，信噪比为43 db，采用650 nm 低通滤光片。相机像素设置为640+480 像素，采用USB 数据线与上位机通信。上位机开发采用软触发方式控制相机采集图像，相机自定义数据为256 字节块数据区域。双目相机平行放置于末端执行器两侧，间距为15 cm。下位机是以ARM 公司生产的STM32F103系列芯片为核心的微控制器，通过单脉冲模式控制步进电机驱动器。

5.2 试验设计

5.2.1 试验指标

1）采摘成功率

采摘成功率计算如下：

其中η 为采摘成功率，n 为试验成功次数，M 为总试验次数，30次。

2）图像特征误差平均值

在相机成像平面内，读取视觉伺服结束后的被切割点与被夹持的成像坐标，与末端执行器上的切割点与夹持点成像坐标作差得到图像特征误差。所有采摘成功试验的图像特征误差平均值为

3）单次平均采摘时间

视觉伺服过程中，末端执行器与目标距离误差不断减小，当果梗位于末端执行器上夹爪夹持范围内时认为到达采摘点位置，视觉伺服过程结束，试验用夹爪夹持范围为17.7、16.0和28.0 mm。利用计时器记录视觉伺服过程开始到结束所有时间，即为单次采摘时间，统计所有采摘成功试验的采摘时间平均值为：

5.2.2 试验方案

分别基于KF、UKF、AUKF 图像雅可比矩阵估计算法的视觉系统进行静态与动态采摘试验。 静态采摘试验时，将番茄果实串悬挂于移动平台并保持静止不动，机械手在相对于目标的任意初始位置开始向目标点运动实现采摘。动态采摘试验时，控制移动平台做小范围的往复摆动，使番茄串产生不确定的振动。移动平台电机驱动器位于移动平台丝杆末端，电机驱动器型号为HBS57，频率为1 500 Hz，移动范围为-0.02～0.02 m。

每种方法的静态与动态采摘试验各30 次，每次试验均保持机械手初始位姿相同，统计静态与动态条件下采摘成功率、图像特征误差平均值以及单次平均采摘时间。

5.3 试验流程

利用MATLAB 编写的上位机控制界面对双目相机采集到的图像进行处理以及目标特征提取。为将果梗从复杂的背景中提取出来，利用支持向量机法分离出目标果梗图像[27-28]。分离出的果梗图像经过灰度化、阈值分割、形态学开运算等处理，得到番茄串果梗的二值图像。通过连通组件标记算法找到所有的连通区域，并分别标记，对每个连通区域运用计算几何距算法得到质心。将果梗质心点作为被夹持点，根据番茄串垂直生长特性以及末端上夹持点与切割点在图像上的距离，提取被夹持点上方一点作为被切割点。由于双目相机固定于机械手末端，故夹持点与切割点图像坐标为定值，在成像平面上垂直距离为80 像素，试验前预先标定好。通过视觉伺服控制算法将目标在相机平面的图像特征变化量转化为关节角度变化量。上位机将关节角度变化量对应的脉冲数与频率通过串口传送到下位机。步进电机驱动器把控制器发出的脉冲信号转化为步进电机的角位移，从而驱动机械手关节运动，具体试验流程如图7 所示。

图7试验过程流程图Fig.7 Flow chart of experiment progress

5.4 采摘试验结果与分析

视觉伺服过程结束后，基于KF、UKF 与AUKF 3 种图像雅克比矩阵估计算法的视觉伺服系统采摘失败次数与成功率如表3所示。

表3 采摘试验成功率Table 3 Success rate of picking experiment

从表3 可知，基于AUKF 算法的视觉伺服系统静态采摘试验成功率为90%，动态采摘试验成功率为83%，相比于KF 算法与UKF 算法，静态采摘试验成功率分别提高了17 与10 个百分点，动态采摘试验成功率分别提高了16 与10 个百分点。采摘试验失败的原因是图像雅克比矩阵估计不准确，导致机械手末端执行器运动到相机视野之外。除去试验失败的数据，基于3 种图像雅克比矩阵估计方法的视觉伺服系统最终图像特征误差平均值如表4 所示。

表4 图像特征误差平均值Table 4 Average value of image feature error pixels

由表4 可知，基于AUKF 算法的视觉伺服系统静态采摘的最终图像特征误差平均值相比于基于KF 算法与UKF 算法估计图像雅克比矩阵的视觉伺服系统分别减小了70.39%，80.20%，68.89%，80.20%，70.39%，68.89%与49.84%，67.84%，51.56%，67.84%，49.84%，51.56%，动态采摘的最终图像特征误差平均值相比于基于KF 算法与UKF 算法的视觉伺服系统分别减小了50.94%，54.53%，74.32%，54.53%，50.94%，74.32% 与30.42%，37.14%，50.19%，37.14%，30.42%，50.19%。

3 种图像雅克比矩阵估计方法的视觉伺服系统响应时间如表5所示。

表5 单次采摘平均时间Table 5 Average value of picking time s

由表5可知，基于AUKF算法的视觉伺服系统在目标静态条件和动态条件平均用时分别为18和22 s，相比于KF算法与UKF算法，目标静态条件下用时分别减小了10与6 s，减小的百分比为35.71%与25%。目标动态条件下用时分别减小了12与8 s，减小的百分比为35.29%与26.67%。

6 结 论

1）本文在KF 算法与UKF 算法估计图像雅克比矩阵的基础上，引入自适应系统噪声估计器，提出的AUKF算法估计图像雅克比矩阵，能够实现时变过程噪声适应系统的变化，使图像雅克比矩阵估计更加准确。

2）视觉伺服控制系统仿真试验结果表明，在系统噪声统计特性未知的情况下，基于AUKF 算法的视觉伺服控制系统图像特征误差平均值为4.978 像素，相比于KF算法与UKF 算法，分别减小了54.44%与24.78%；机械手末端与目标最终距离为4.19 mm，相比于KF 算法与UKF 算法的视觉伺服控制系统分别减小了79.74%与80.36%。因此AUKF 算法能抑制未知统计特性的系统过程噪声对图像雅克比矩阵估计精度的影响。

3）番茄串采摘机械手静态采摘试验时，基于AUKF算法的视觉伺服系统与基于KF 算法和UKF 算法视觉伺服系统相比，最终图像特征误差平均值分别减小了68.89%～80.20%和49.84%～67.84%，采摘时间分别减小了35.71%与25%；动态采摘时，最终图像特征误差平均值分别减小了50.94%～74.32%和30.42%～50.19%，采摘时间分别减少了35.29%与26.67%。基于AUKF 算法的视觉伺服系统静态和动态采摘试验成功率分别为90%和83%，相比于KF 算法与UKF 算法，分别提高了17 与10和16与10个百分点。