一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法与流程

本发明属于工业机器人自动化技术领域，具体地涉及一种机器人目标跟踪抓取系统。

背景技术：

机器人融合视觉的技术，根据外界环境的变化智能地调整自我工作状态，增加了机器人工作的适应性，从而可以更高效的进行生产，机器人技术得以发挥其价值。近年来，三维场景分析、图像特征提取、目标视觉、轮廓分析、立体视觉等技术的发展，使得机器人能够根据用户的需求智能地动态执行任务，对运动目标识别、跟踪，得到了极为广泛的应用。

目标跟踪算法按照跟踪原理分为基于对比度分析、基于匹配、基于运动检测和其他的视频目标跟踪算法。基于对比度分析又分为边缘跟踪和重心跟踪；基于匹配又可以细致分为核方法、特征匹配和概率跟踪，核方法精确表述为梯度均值漂移，特征匹配分为单特征与多特征联合两种方法，概率跟踪是研究最深的领域，已经延伸出卡尔曼滤波、粒子滤波、隐马尔科夫模型和动态贝叶斯模型等；基于运动检测则是光流法应用；其他方法包括多目标跟踪、向量机、机器学习等。

卡尔曼滤波适用于先行高斯系统，经过推算便可以得出下一时刻状态量的最优估计，因此特点逐渐被应用到目标跟踪中，但目标跟踪系统都是非线性的，而后便引申出诸如扩展卡尔曼滤波及无迹卡尔曼滤波等优秀算法。粒子滤波算法的简单的采样环节导致粒子多样性丧失，一定程度上影响了算法的跟踪精度和跟踪实时性跟踪性能，进而导致机器人抓取迟滞性的存在。

技术实现要素：

针对上述几何粒子滤波粒子多样性丧失导致的跟踪性能缺陷问题，为解决现有技术存在的局限和缺陷，本发明提供一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法及一种机器人目标跟踪抓取系统。

一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，具体步骤如下：

步骤1：引入仿射群粒子，建立状态方程和观测方程；

步骤2：状态方程和观测方程进行欧拉指数离散化；

步骤3：状态方程进行一阶自回归模型化；

步骤4：仿射群粒子集初始化，然后进行初步预测，得到迭代次数计数器的计数值k=0初始状态的采样粒子；

步骤5：设置k=k+1，然后进行重要性采样；

步骤6：采取改进高斯重采样算法进行重采样，然后进行状态估计；

步骤7：判断是否为最后一帧，如果是，则输出估计状态，如果不是，则返回步骤5。

优选地，步骤1中的状态方程为

；

观测方程为

；

其中x是仿射群粒子，a是李群到李代数的映射矩阵,z=1，2，…，6，bz是第z种李群到实数的非线性变换，dz是李代数中第z种几何变换的对应矩阵，dωz表示第z种几何变换对应的状态方程过程噪声，dη表示观测方程的过程噪声，g表示李群到实数域的映射，y表示观测量，t为时间；

步骤2中，状态方程和观测方程进行欧拉指数离散化后，分别为：

其中，k为迭代次数计数器的计数值，xk、xk-1分别为第k、k-1次迭代时的仿射群粒子，a(x,t)表示离散化后李群到李代数的映射矩阵，yk为第k次迭代时的观测值，dwk、ηk分别表示欧拉指数离散化后的状态方程和观测方程对应的第k次迭代时的过程噪声，δt为迭代计数器两次计数的时间间隔；

步骤3中，状态方程一阶自回归模型化后为：

其中a是自回归模型系数，xk-2为第k-2次迭代时的仿射群粒子，ak-1为过渡矩阵。

优选地，步骤4中的仿射群粒子集初始化具体包括：设置迭代次数计数器的计数值k=0，设置粒子总数为n，对于粒子i=1，2，…，n，设置第i个粒子的状态初值x0ⁱ=i，第i个粒子对应的过渡矩阵初值a0ⁱ=0，其中i为单位矩阵。

优选地，步骤4的初步预测具体为：

利用一阶自回归模型化后的状态方程，对仿射群粒子集中的所有粒子计算先验概率密度，得到k=0初始状态的所有采样粒子，构成初始状态下的粒子集合。

优选地，步骤5的重要性采样具体为：

根据前一步骤获得的粒子集合，设置k=k+1，在新的k值下，对于粒子i=1，2，……n，n为粒子总数，依次从重要性概率密度中生成各自对应的采样粒子，所有新的采样粒子构成新的粒子集合，计算新的粒子集合中各粒子权重，并进行归一化，

其中，新的粒子集合中粒子权重计算公式为：

上式中，xkⁱ为第k次迭代时的第i个粒子,xk-1ⁱ为第k-1次迭代时的第i个粒子，ωkⁱ是xkⁱ对应的权重，ωk-1ⁱ是xk-1ⁱ对应的权重，p(yk|xkⁱ)表示已知xkⁱ前提下，yk的概率，p(xkⁱ|xk-1ⁱ)表示已知xk-1ⁱ前提下，xkⁱ的概率，p(xkⁱ,yk|xk-1ⁱ)表示已知xk-1ⁱ前提下，xkⁱ、yk的概率，yk为第k次迭代时的观测值；

归一化粒子权重ωkⁱ%公式为：

优选地，步骤6具体为：根据步骤5中归一化粒子权重后的结果，将新的粒子集合中的粒子筛选，按权重值是否超过给定阈值分为低权重粒子集合{xⁱmin_k}和高权重粒子集合{xⁱmax_k}，以该次迭代中高权重粒子集合中的最高权重粒子x^lmax_h为均值，l∈i，h=k，以第k-1次迭代的估计方差为方差建立高斯分布，在该分布上采样，生成初步的重采样中间粒子集合{xⁱfirst_k}，将重采样中间粒子集合{xⁱfirst_k}与低权重粒子集合{xⁱmin_k}进行线性组合，生成重采样粒子集合，其中每个重采样粒子对应的权重为1/n，根据重采样粒子集合进行状态估计。

为实现上述目的，本发明还提供一种机器人目标跟踪抓取系统，机器人目标跟踪抓取系统包括rgbd相机图像获取模块、二维图像跟踪模块、三维重构模块、机器人位姿调整模块，其中，二维图像跟踪模块采用上文所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法。

上述机器人目标跟踪抓取系统的工作方法具体步骤为：

步骤1，rgbd相机图像获取模块得到目标的二维图像及深度图；

步骤2，二维图像跟踪模块在二维图像层面使用基于仿射群的改进高斯重采样粒子滤波目标跟踪方法进行目标跟踪，得到候选区域；

步骤3，结合深度图与二维图像跟踪模块得到的候选区域进行三维重构，计算rgbd相机坐标系下的目标三维位姿；

步骤4，将目标三维位姿与机器人末态三维位姿输入机器人位姿调整模块，不断对机器人末态三维位姿与目标三维位姿进行实时计算差值；

步骤5，判断差值是否小于阈值，若是，则控制机器人对目标进行抓取，否则调整机器人的位姿，得到新的机器人末态三维位姿，以接近目标，并返回步骤4。

本发明中在目标跟踪算法中引入仿射群粒子，改进粒子滤波算法，融合加入改进高斯重采样环节，有效解决了几何粒子滤波算法粒子多样性匮乏的问题。在目标跟踪领域通用指标ope、sre、tre上都有更好的效果，本申请算法有更好的实时性、时间鲁棒性和空间鲁棒性。快速移动、运动模糊、超出视野三种情况下本申请算法仍然表现出良好的效果，应用到机器人可以有更好的实用性，有效提升机器人跟踪抓取的成功率。

附图说明

图1为基于仿射群的改进高斯重采样粒子滤波目标跟踪方法流程图

图2为李代数几何变换基本组成

图3为对比算法在otb2015-tb50部分数据集实验视频帧截图

图4为对比算法重叠成功率ope曲线

图5为对比算法中心位置误差ope曲线

图6为对比算法重叠成功率tre曲线

图7为对比算法中心位置误差tre曲线

图8为对比算法重叠成功率sre曲线

图9为对比算法中心位置误差sre曲线

图10为机器人目标跟踪抓取系统组成图

图11为机器人目标跟踪抓取系统的工作方法流程图。

具体实施方式

下面结合附图对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本领域普通技术人员可知，随着技术的发展和新场景的出现，本申请实施例提供的技术方案对于类似的技术问题，同样适用。

本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，以便包含一系列单元的过程、方法、系统、产品或设备不必限于那些单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它单元。

实施例1

一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，流程如图1所示，具体步骤如下：

（1）引入仿射群粒子，建立仿射群粒子的状态方程（公式（1））和观测方程（公式（2））：

（1）

（2）

上式中x是仿射群粒子，a是李群到李代数的映射矩阵,z=1，2，…，6，bz是第z种李群到实数的非线性变换，dz是李代数中第z种几何变换的对应矩阵，几何变换的具体形式如图2所示，共六种，dωz表示第z种几何变换对应的状态方程过程噪声，dη表示观测方程的过程噪声，g表示李群到实数域的映射，y表示观测量，t为时间。

（2）状态方程和观测方程根据图像逐帧采集的特点进行欧拉指数离散化，离散化后为：

（3）

（4）

其中，k为迭代次数计数器的计数值，xk、xk-1分别为第k、k-1次迭代时的仿射群粒子，a(x,t)表示离散化后李群到李代数的映射矩阵，yk为第k次迭代时的观测值，dwk、ηk分别表示欧拉指数离散化后的状态方程和观测方程对应的第k次迭代时的过程噪声，δt为迭代计数器两次计数的时间间隔。

（3）因为连续两帧图像之间有密切联系，对状态方程进行一阶自回归模型化，模型化后为：

（5）

（6）

其中，a是自回归模型系数，xk-2为第k-2次迭代时的仿射群粒子，ak-1为过渡矩阵。

（4）对仿射群粒子集进行初始化。设置迭代次数计数器的计数值k=0，设置粒子总数为n，对于粒子i=1，2，…，n，设置仿射群粒子构成的仿射群粒子集中，第i个粒子的状态初值x0ⁱ=i，第i个粒子对应的过渡矩阵初值a0ⁱ=0，其中i为单位矩阵。

（5）初步预测。利用一阶自回归模型化后的状态方程，对仿射群粒子集中的所有粒子计算先验概率密度，得到k=0的所有采样粒子，构成初始状态时的粒子集合。

（6）重要性采样

根据前一步骤获得的粒子集合，设置k=k+1，在新的k值下，对于粒子i=1，2，……n依次从重要性概率密度中生成各自对应的采样粒子，所有新的采样粒子构成新的粒子集合，计算新的粒子集合中各粒子权重，各粒子权重，并进行归一化。

粒子权重通用计算公式为：

（7）

因为仿射群粒子滤波使用了一阶自回归模型，使用p(xⁱk|xⁱk-1,yk)作为上式中分母q(xⁱk|xⁱ0：k-1，y0:k)的最佳近似，但是在这种近似下很难用解析式得到粒子权重，因此用概率p(xkⁱ,yk|xk-1ⁱ)近似p(xⁱk|xⁱk-1,yk)，p(xkⁱ,yk|xk-1ⁱ)表示已知xk-1ⁱ前提下，xkⁱ、yk的概率，最终新的粒子集合中粒子权重计算公式如（8）。

（8）

xkⁱ为第k次迭代时的第i个粒子,xk-1ⁱ为第k-1次迭代时的第i个粒子，ωkⁱ是xkⁱ对应的权重，ωk-1ⁱ是xk-1ⁱ对应的权重，yk为第k次迭代时的观测值，对于分子p(yk|xkⁱ)表示已知xkⁱ前提下，yk的概率，p(xkⁱ|xk-1ⁱ)表示已知xk-1ⁱ前提下，xkⁱ的概率，p(xkⁱ,yk|xk-1ⁱ)表示已知xk-1ⁱ前提下，xkⁱ、yk的概率。

归一化的粒子权重ωkⁱ%的计算公式如下：

（9）

（7）改进高斯重采样

根据步骤6中归一化粒子权重后的结果（公式9），将新的粒子集合中的粒子筛选，按权重值是否超过给定阈值分为低权重粒子集合{xⁱmin_k}和高权重粒子集合{xⁱmax_k}。以该次迭代中高权重粒子集合中的最高权重粒子x^lmax_h为均值，l∈i，h=k，以第k-1次迭代的估计方差为方差建立高斯分布，在该分布上采样，生成初步的重采样中间粒子集合{xⁱfirst_k}。利用重采样中间粒子集合{xⁱfirst_k}，与低权重粒子集合{xⁱmin_k}进行线性组合，生成重采样粒子集合，其中每个重采样粒子对应的权重为。

具体地，线性组合按照式（10）进行:

（10）

上式中，j=i，j=1，2，…，n，k为步长系数，l为重采样步长。

接下来根据重采样粒子集合，按照仿射变换状态量平均值计算方法求解最终的状态估计值，得到该k值下的估计状态，即该k值下的候选区域。

（8）判断是否为最后一帧，如果是，则输出估计状态，如果不是，则返回步骤（6）。

为对比本申请算法(igpf)、几何粒子滤波算法（gpf）与粒子滤波算法（pf）的跟踪性能，采用otb2015数据集中的tb-50sequences作为实验对象，这50个视频图像包含11种典型情况，它们是遮挡(occlusion，occ)，光照变化(illuminationvariation，iv)、低分辨率(lowresolution,lr)、运动模糊(motionblur，mb)、快速移动(fastmotion，fm)、超出视野(out-of-view，ov)、非刚性变形(deformation，def)、背景干扰(backgroundclutters，bc)、尺度变化(scalevariation，sv)、平面内旋转(in-planerotation，ipr)以及平面外旋转(out-of-planerotation，opr)。

评价指标采用otb平台的中心位置误差(distanceprecision,dp)和重叠成功率(overlapsuccess,os)。除此之外，算法的时间与空间鲁棒性也是重要的评价指标，因此中心位置误差和重叠成功率分别又有三种不同的评价方式，它们分别是一次性通过评估(ope)、时间鲁棒性评估(tre)和空间鲁棒性评估(sre)。

将三种算法按照上述评价方式对otb2015-tb50数据集进行了对比实验，选取部分视频帧截图如图3所示进行对比，从图中可以看出本申请算法代表的矩形框始终跟踪在目标上，未发生错位及失去目标的情况，无论在在遮挡、光暗等灰度及彩色图像跟踪性能均比几何粒子滤波与粒子滤波良好。

得到的中心位置误差曲线和重叠成功率曲线如图4-9所示，从图像可以看出本申请算法对比传统粒子滤波算法和几何粒子滤波算法不论是在tre代表的时间鲁棒性还是sre代表的空间鲁棒性都有明显提升，重叠成功率也是更高。

由图4-9也可以看出三种算法的tre值都小于ope值，表明三种算法都受初始位置设定影响，但本申请算法tre与ope差值明显要更小，表明本申请算法受初始位置影响小，更具有适应性。

除此之外，本申请对机器人抓取中影响较大的快速移动、运动模糊、超出视野三种典型情况也进行了算法对比，尽管本申请的算法在这三个特殊环境下存在性能上的波动，但综合来看本申请的算法对比粒子滤波算法与几何粒子滤波算法性能是最佳的。

实施例2

本发明另一实施例提供一种机器人目标跟踪抓取系统，如图10所示，机器人目标跟踪抓取系统包括rgbd相机图像获取模块、二维图像跟踪模块、三维重构模块、机器人位姿调整模块，其中，二维图像跟踪模块采用实施例1所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法。

实施例3

本发明另一实施例提供一种机器人目标跟踪抓取系统的工作方法，该机器人目标跟踪抓取系统包括rgbd相机图像获取模块、二维图像跟踪模块、三维重构模块、机器人位姿调整模块，其中，二维图像跟踪模块采用实施例1所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法。如图11所示，该机器人目标跟踪抓取系统的工作方法具体步骤为：

步骤（1），rgbd相机图像获取模块得到目标的二维图像及深度图；

步骤（2），二维图像跟踪模块在二维图像层面使用基于仿射群的改进高斯重采样粒子滤波目标跟踪方法进行目标跟踪，得到候选区域；

步骤（3），结合深度图与二维图像跟踪模块得到的候选区域进行三维重构，计算相机坐标系下目标三维位姿，也就是机器人最终的三维位姿；

步骤（4），将目标三维位姿与机器人末态三维位姿输入机器人位姿调整模块，对机器人末态三维位姿与目标三维位姿进行实时计算差值；

步骤（5），判断差值是否小于阈值，若是，则控制机器人对目标进行抓取，否则调整机器人的位姿，得到新的机器人末态三维位姿，以接近目标，并返回步骤（4）。

虽然上文描述了在所附权利要求书范围内的具体实施的各个方面，但是应当显而易见的是，上述具体实施的各种特征可通过各种各样的形式体现，并且上述任何特定结构和/或功能仅是例示性的。基于本公开，本领域的技术人员应当理解，本文所述的方面可以独立于任何其他方面来实现，并且这些方面中的两个或更多个可以采用各种方式组合。例如，可以使用本文阐述的任何数量的方面来实现装置和/或可以实践方法。另外，除了本文阐述的一个或多个方面之外或者不同于本文阐述的一个或多个方面，可以使用其他结构和/或功能来实现这样的装置和/或可以实践这样的方法。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。