基于前馈径向基网络的动态抓取参数估计方法

王建强，黄开启，苏建华

1）江西理工大学电气工程与自动化学院，江西赣州 341000；2）中国科学院自动化研究所复杂系统管理与控制国家重点实验室，北京 100190

抓取运动的目标物体是机器人领域中的难题之一，在航天应用及工业生产等场合有广泛的应用需求.为保证抓取可靠性和稳定性，机器人需通过视觉实时跟踪运动目标并估计出目标的运动参数.近年来，主流的目标跟踪方法包括增量视觉跟踪（incremental visual tracking，IVT）［1］和循环结构核（circulant structure kernel，CSK）［2］的生成式搜索算法、跟踪学习检测（tracking learning detection，TLD）算法［3］和核相关滤波（kernel correlation filter，KCF）［4］的判别式模型算法等.现有的运动参数估计方法有mean shift 算法［5］、Cam shift 算法［6］、粒子滤波［7］和卡尔曼滤波（Kalman filter，KF）等.Mean shift 算法和Com shift 算法需要较大的目标与背景的对比度，通过图中颜色直方图分布确定目标运动状态.KF是将理论预测与实际观测融合得到更准确的估计，但传统KF 运动估计方法仅适于统计特性已知的白噪声［8］，对统计特性未知的有色噪声存在很大的估计误差.JULIER 等［9］将 KF 推广到非线性空间，将连续的非线性方程线性化和离散化，提出扩展卡尔曼滤波（extended Kalman filter，EKF），但存在如下缺点：①非线性模型的雅可比矩阵计算量大且难解出；②随着时间延长，标称状态会偏离系统实际状态，造成估计精度降低；③若系统的误差传播函数不能用线性函数逼近，则会导致滤波器发散.JULIER等［10］将近似概率分布代替近似非线性函数提出了无迹卡尔曼滤波（unscented Kalman filter，UKF），采用unscented非线性变换进行模型的状态与误差协方差的递推和更新，但是若UKF系统状态的后验概率密度是非高斯，则滤波效果很差［11］.CHOUKROUN 等［12］利用哈密顿算子将四元数状态方程改写成线性形式，提出四元数卡尔曼滤波（quaternion Kalman filter，QKF），但该方法需要频繁计算四元数与误差修正罗德里格斯参数之间的切换［13］，以保证四元数维度与滤波方差维度相同［14］.

在抓取运动目标的研究中，安旭文［15］将机器人的控制过程分为跟踪预测和捕捉两个阶段.在跟踪预测阶段，机器人处理图像噪声并用改进的最大类间方差法（Otsu's method）进行目标检测，而在捕捉阶段则使用基于速度补偿的视场控制算法.但该方法缺少观测量，且状态量对模型的依赖较大；此外，该方法使用的粒子滤波对非线性非高斯的系统的滤波效果不好，无法满足实时性要求.

针对机器人对二维平面中随机运动物体的运动参数估计问题，通过前馈径向基函数（radial basis function，RBF）［16］神经网络预测目标运动采样时间，迭代优化求解最小损失函数来拟合非线性系统［17］.将神经网络的时间预测结果代入KF 的状态方程，规划机器人的最优抓取路径.相比传统的机器人拦截式抓取方法［18-20］，本研究首次提出KCF与参数预估结合的机器人抓取方法，提高了抓取成功率.该方法绕开了非线性运动预测计算量大且不易求得奇异矩阵的难点，对目标的突变有更好地适应性，基于前馈RBF 的KF 运动预测抓取方法与EKF、QKF和UKF 方法相比，提高了抓取的成功率，与前馈感知机网络的KF 方法相比，提高了目标跟踪的精度与抓取的效率.

1 系统概述

本研究提出的机器人抓取运动目标的工作流程如图1所示.通过KCF算法提取目标特征，将相机提取到的信息分为平面信息与深度信息.平面信息抽取第1 帧制作目标模板，第2 帧开始进行模板目标匹配，从而得到目标物体在平面上的实时位置，将其与深度信息结合，并通过机器人手-眼坐标变换，可获得目标物体在机器人坐标系下的三维坐标.然后，将此时末端手爪与目标的运动速度，以及两者之间的距离输入到训练好的神经网络模型中，可以预测KF 状态方程中的采样时间.通过基于前馈神经网络的KF 预估下一采样目标的位置，可以实时跟踪并抓取目标.

1.1 KCF跟踪算法

KCF算法的思想是将所有输入图像特征的循环矩阵回归到傅里叶空间并对其进行对角化运算，此过程不需要目标外观的硬阈值样本，因此具有计算量小、检测准确性高的特点，满足对实时性要求较高的目标跟踪需求.

KCF 算法的目标跟踪过程如图2所示，具体检测过程为：①在第1 帧给定的目标位置提取图像块，训练后得到相关滤波器.在后续的每一帧中，根据前一帧的位置提取新的图像块，用于目标检测.②提取图像块的方向梯度直方图（histogram of oriented gradient，HOG）特征，并用余弦窗口平滑边缘.③采用离散傅里叶变换（discrete Fourier transform，DFT）执行相关滤波操作.④采用离散傅里叶逆变换（inverse DFT，IDFT）得到置信图，置信图最大值所对应的坐标位置即为目标的新位置，并由此估计位置训练和更新相关滤波器.其中，⊙为卷积操作符.

2.2 卡尔曼运动估计算法

本研究主要针对的是待抓取的物体为非匀速、任意方向运动的目标.首先，判断机器人末端手爪与目标物体两者中心点的距离，如图3红色虚线所示.然后，根据两帧之间的时间和红点的位移算出目标瞬时速度与加速度来判断物体的运动方向.

图3 机器人末端运动与目标物体运动关系图Fig.3 Relationship between manipulator end motion and target object motion

预判抓取时间有利于让机器人在设定的边界条件下到达抓取点.若物体速度超过机器人抓取允许的最大值，则需延长估计KF状态方程的采样时间.为使物体运动轨迹与手爪轨迹交汇于某点，需根据物体当前的速度和加速度计算机器人末端的速度和加速度.由于物体的速度与方向是随机的，为避免因目标运动突变而导致的预测失效，需以适当的频率在机器人抓取路径上均匀插值.

假设物体以恒定加速度直线运动，若物体到达抓取点xf所需的时间（亦称抓取时间）为tf，则

其中，xm、vm和am分别是当前物体的位置、速度和加速度.

假设物体在x轴上的捕捉距离D=xf-xm，若vm＞ 0且则最小捕捉时间为

若am很小，则tf可通过假设恒定速度轨迹来计算，由式（1）可得xf=xm+vmtf，由xf=xm+vmtf和D=xf-xm可得通过规划机器人的运动轨迹，可使手爪经过时间tf时准确到达抓取点.

在数字图像采集中，由于相邻两帧的采集时间间隔非常短，且物体的运动距离也非常短，因此在理想状态下，可假定目标在短时间内做匀速运动，故用位置与速度可表达运动状态向量.用一个4维向量 (sx，k，sy，k，vx，k，vy，k)表示目标在x和y方向的位移和速度在k时刻的状态，则运动方程为

其中，sx，k和sy，k分别为k时刻目标中心在x方向和y方向的位移；vx，k和vy，k分别为k时刻目标在x方向和y方向的速度；Δt=tk-tk-1.

本研究建立的KF状态方程为

其中，ω为4 × 1维的系统噪声向量；Δt为KF方程的采样时间.

KCF 在采样周期内返回k时刻图像目标中心点位置 (sx，k，sy，k)，故观测方程的前半部分是一个 2 × 2维的对角矩阵，即

其中，v为 2 × 1 维的观测噪声向量；分别为k时刻目标中心在x方向和y方向的位移观测值.

通过KCF 算法检测出目标中心的位置(sx，k-1，sy，k-1），用倒数第 1、14 和 27 帧中目标的位移差除以两帧之间的时间差，计算出速度vx，k和vy，k，并组成系统初始状态以初始化KF方程.将观测向量带入KF状态方程并进行更新状态，得到不同时刻运动目标的位置和速度的估计值.利用改进的卡尔曼滤波器预测目标运动的位置和速度.将式（5）的观测向量代入状态方程，实现滤波器状态更新.经过这样的预估-校正过程，用卡尔曼滤波器估计出目标在未来某一时刻的位置，根据该位置规划机器人的运动轨迹和速度，并通过夹爪的开合判断是否抓取成功.

2 基于前馈径向基网络的运动参数预测与抓取

在目标速度随机变化情况下，为实现机器人稳定抓取，本研究提出一种基于前馈RBF神经网络动态预测采样时间Δt的卡尔曼滤波算法.图4给出机器人运动与相机采样周期对应关系的示例.其中，黑色实心点是机器人每一轮运动和相机采样的起始位置；绿色空心点是需要预测的目标位置；相机采样周期是30 ms，机器人每次运动的时间与它运动的距离和速度有关.机器人在其一个运动周期内，将不接受上位机的指令，需等到下一个运动周期的到来才能接受指令，故相机采样周期与机器人运动周期的差值（即KF的采样时间）是动态变化的.

图4 机器人运动周期与相机的采样周期（单位：ms）Fig.4 Robot arm movement and camera sampling period(unit:ms)

常用的感知机神经网络通过权连接输入层与隐藏层，RBF神经网络将输入矢量直接映射到以RBF为隐单元的“基”所构成的隐空间上.输入到输出的映射是非线性的，隐含层到输出的映射是线性的，在隐含层，把低维度线性不可分向量映射到高维度成线性可分向量.通过在高纬度可调参数（权值和阈值）与输出之间关系得到线性方程组可求得网络的权值.RBF 神经网络的优点有：①提高训练速度与学习效率；②克服了在低维度陷入局部极小值的问题；③收敛速度比前一个网络快，输入数据与所选中心点中起主要作用的是与输入数据很近的点，且局部逼近简化计算量，局部逼近比全局逼近学习速度更快.图5是神经网络网络在系统中对输入聚类和训练输出的信息流程.

图5 系统框架及其RBF网络信息流图Fig.5 RBF network information flow diagram and its system framework

RBF神经网络的激活函数为

其中，xp是第p个输入样本的坐标；ci是第i个中心点的坐标；σ是函数的宽度参数，用于控制函数的径向作用范围.

RBF神经网络的输出为

其中，n为输出的样本数；h为隐含层的结点数；wi为第i个输出神经元的权重.在本研究中，y即采样时间Δt，代入KF的状态方程得

利用最小二乘法计算隐藏层神经元与输出层之间的连接权值wi，并设cmax为所选取中心点之间的最大距离，则

其中，P为聚类中心的个数.

深度神经网络虽然适合用来进行时序建模，但其体系结构缺乏直观的高阶时空架构.繁杂的训练过程和中央处理器（central processing unit，CPU）资源的大量占用导致机器人抓取过程中的实时性降低.

3 实验与分析

为验证本研究方法的优越性，选取两种神经网络预测KF 的采样时间，并在自建的数据集上用相同数据集进行训练，对比分析两者的抓取效果.实验的硬件配置为Intel Core i7-10875 2.3 GHz CPU，16 Gbytes 内 存 ， GDDR6 显 卡 ， 操 作 系 统 为Ubuntu16.04，输入图像分辨率为1 280 × 960像素，使用Python 语言在pycharm 中搭建手眼机器人的操作系统.

3.1 实验结果与分析

基于本研究的抓取稳定与高效性的评估方法，分别比较感知机与RBF神经网络预测时间参数，结合KF 估计运动参数，在UR5 机器人进行追踪并抓取机动目标实验，分析不同算法的性能.实验所用UR5 机器人实物如图6（a）.机器人首先对在单方向、非匀速的状态下的目标物体进行抓取，即由操作者随机拉着目标做变速直线运动（图6（b）），然后对如图6（c）中任意方向下的目标物体进行抓取.

图6 UR5机器人实物图与试验机器人单方向非匀速运动模式Fig. 6 The physical map of the UR5 manipulator and the one-direction non-uniform motion mode of the test robot

图7为采用EKF、UKF和QKF算法抓取非匀速且随机运动的物体时的过程曲线.由图7可见，采用EKF、UKF和QKF算法进行参数预估的方法，目标抓取成功率很低，大多数情况下是失败的.

图7 采用（a）EKF、（b）UKF和（c）QKF预测算法对非匀速随机运动物体的抓取过程Fig.7 The grasping process of non-uniform random moving objects predicted by(a)EKF,(b)UKF,and(c)QKF

将平面抓取运动分解到x轴与y轴.图8（a）中，采用感知机神经网络，手爪与目标在x轴方向自开始抓取点起，后面的最大距离不超过10 cm，这是由于当机器人结束一轮运动周期时，物体突然加速，UR5机器人的最大加速度设为0.35 m/s2.如果突然施加的瞬时加速度超过当前运动速度，机器人会在下一轮运动周期由两者位置、速度与加速度的差值，通过神经网络前馈时间预测的KF 加大预测量再次进行抓取.在后面的跟踪抓取中，机器人依据物体移动的轨迹实时规划自己的路径，直至抓取成功.图8（b）是在RBF 神经网络预测采样时间参数下的情况，机器人手爪和目标在x轴上的坐标变化情况.由图可见，前馈RBF 神经网络的KF 预测使UR5在开始抓取点后追踪过程中与目标距离最大不超过5 cm.可见，感知机经网络预测跟踪算法虽然在时间预测上对于非线性的运动有很好的适应性，但是在某些点上还是会有较大落后.相比之下，采用RBF神经网络的路径规划，在跟踪过程会更平滑，机器人运动路径规划中的插值也更均匀，此过程对于目标下一步运动状态发生突变而能及时做出调整.图8（c）和（d）显示了抓取过程中y轴上的跟踪误差.由图8（c）和（d）可见，在时长4 s的抓取过程中，RBF神经网络预测方法抓取到目标所用的时间比感知机方法少0.8 s，抓取效率提高了20%.对于目标物体在加速或减速（曲线的转折处代表速率发生了大突变）情况下的目标跟踪，两种神经网络比传统的位置伺服方法都更具灵活性，这表明神经网络对于KF 状态方程的时间预测更具优越性.RBF神经网络预测方法对于单方向上变速运动的灵敏性更好，能在采样周期内规划好跟踪的最佳路径，达到更稳定跟踪和更高效抓取的效果.

图8 感知机神经网络和RBF神经网络在x轴和y轴抓取变速直线运动物体的过程Fig.8 The x axisand y axis grabbing process of change speed straightline motion objectby perceptronandRBF neuralnetworks

图9为分别采用前馈感知机神经网络和RBF神经网络的KF 参数预估算法抓取非匀速任意方向运动的物体（图7（c））时的过程曲线.由图9可见，采用感知机神经网络方法预测出现了跟踪不良的情况，在急加速或急转弯的情况下，跟踪甚至会产生较大的误差，导致误抓取从而降低了抓取的效率.

图9 感知机神经网络和RBF神经网络在x轴和y轴抓取非匀速运动物体的过程Fig.9 The x axis and y axis grabbing process of non-uniform movement motion object by perceptron and RBF neural networks

图10为前馈神经网络KF对任意方向做非匀速运动的目标进行抓取的3 维仿真结果.由图10 可见，前馈RBF 的KF 预测对目标的预测在运动速度突变或方向突变的情况下也能及时做出调整并稳定跟踪，其对机动目标运动的预判性，及对机器人跟踪路径上预测点插值的均匀性，对一些急速变化的目标也能紧紧跟随，精准抓取.在时间预测与路径规划上，基于RBF神经网络改进的卡尔曼滤波规划的跟踪路径及抓取都优于感知机神经网络时间预测的卡尔曼滤波.

图10 采用感知机神经网络和径向基神经网络进行抓取的3维仿真结果Fig.10 Three-dimensional grasping process by using perceptron and RBF network networks

结 语

本研究提出一种新的运动参数估计方法，设计了基于径向基函数神经网络动态规划卡尔曼滤波采样时间的方案，并结合视觉伺服控制器规划UR5机器人运动路径、速度与加速度，在保证抓取效率的同时，大幅提高了机器人抓取的精度.通过自建数据库训练RBF 网络模型，并提升了模型的泛化能力.机器人抓取动态目标的实验结果，验证了本研究方法比起传统卡尔曼滤波算法能更有效地提高跟踪的稳定性、抓取成功率和抓取效率.

未来计划采用上述多种典型方法进行多种组合方式进行方案的测试，并结合3维空间跟踪算法与机器人的视觉伺服控制器，实现机器人在3维空间的动态抓取.