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

本发明专利技术提供一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，并建立机器人目标跟踪抓取系统，机器人目标跟踪抓取系统的RGBD相机图像获取模块得到目标的二维图像及深度图，在二维图像跟踪模块，使用基于仿射群的改进高斯重采样粒子滤波目标跟踪方法进行目标跟踪，然后在三维重构模块结合深度图进行重构估计目标三维位姿，最后在机器人位姿调整模块不断对机器人末态三维位姿与目标三维位姿进行实时计算并计算差值，并进行调整直至抓取。本发明专利技术的目标跟踪算法有效改善了粒子滤波粒子多样性匮乏的问题，有效提高了跟踪的实时性和鲁棒性，解决了抓取过程中的迟滞现象。

【技术实现步骤摘要】
一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法
本专利技术属于工业机器人自动化
，具体地涉及一种机器人目标跟踪抓取系统。
技术介绍
机器人融合视觉的技术，根据外界环境的变化智能地调整自我工作状态，增加了机器人工作的适应性，从而可以更高效的进行生产，机器人技术得以发挥其价值。近年来，三维场景分析、图像特征提取、目标视觉、轮廓分析、立体视觉等技术的发展，使得机器人能够根据用户的需求智能地动态执行任务，对运动目标识别、跟踪，得到了极为广泛的应用。目标跟踪算法按照跟踪原理分为基于对比度分析、基于匹配、基于运动检测和其他的视频目标跟踪算法。基于对比度分析又分为边缘跟踪和重心跟踪；基于匹配又可以细致分为核方法、特征匹配和概率跟踪，核方法精确表述为梯度均值漂移，特征匹配分为单特征与多特征联合两种方法，概率跟踪是研究最深的领域，已经延伸出卡尔曼滤波、粒子滤波、隐马尔科夫模型和动态贝叶斯模型等；基于运动检测则是光流法应用；其他方法包括多目标跟踪、向量机、机器学习等。卡尔曼滤波适用于先行高斯系统，经过推算便可以得出下一时刻状态量的最优估计，因此特点逐渐被应用到目标跟踪中，但目标跟踪系统都是非线性的，而后便引申出诸如扩展卡尔曼滤波及无迹卡尔曼滤波等优秀算法。粒子滤波算法的简单的采样环节导致粒子多样性丧失，一定程度上影响了算法的跟踪精度和跟踪实时性跟踪性能，进而导致机器人抓取迟滞性的存在。
技术实现思路
针对上述几何粒子滤波粒子多样性丧失导致的跟踪性能缺陷问题，为解决现有技术存在的局限和缺陷，本专利技术提供一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法及一种机器人目标跟踪抓取系统。一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，具体步骤如下：步骤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个粒子的状态初值X0i=I，第i个粒子对应的过渡矩阵初值A0i=0，其中I为单位矩阵。优选地，步骤4的初步预测具体为：利用一阶自回归模型化后的状态方程，对仿射群粒子集中的所有粒子计算先验概率密度，得到k=0初始状态的所有采样粒子，构成初始状态下的粒子集合。优选地，步骤5的重要性采样具体为：根据前一步骤获得的粒子集合，设置k=k+1，在新的k值下，对于粒子i=1，2，……N，N为粒子总数，依次从重要性概率密度中生成各自对应的采样粒子，所有新的采样粒子构成新的粒子集合，计算新的粒子集合中各粒子权重，并进行归一化，其中，新的粒子集合中粒子权重计算公式为：上式中，Xki为第k次迭代时的第i个粒子,Xk-1i为第k-1次迭代时的第i个粒子，ωki是Xki对应的权重，ωk-1i是Xk-1i对应的权重，p(yk|Xki)表示已知Xki前提下，yk的概率，p(Xki|Xk-1i)表示已知Xk-1i前提下，Xki的概率，p(Xki,yk|Xk-1i)表示已知Xk-1i前提下，Xki、yk的概率，yk为第k次迭代时的观测值；归一化粒子权重ωki%公式为：优选地，步骤6具体为：根据步骤5中归一化粒子权重后的结果，将新的粒子集合中的粒子筛选，按权重值是否超过给定阈值分为低权重粒子集合{ximin_k}和高权重粒子集合{ximax_k}，以该次迭代中高权重粒子集合中的最高权重粒子xlmax_h为均值，l∈i，h=k，以第k-1次迭代的估计方差为方差建立高斯分布，在该分布上采样，生成初步的重采样中间粒子集合{xifirst_k}，将重采样中间粒子集合{xifirst_k}与低权重粒子集合{ximin_k}进行线性组合，生成重采样粒子集合，其中每个重采样粒子对应的权重为1/N，根据重采样粒子集合进行状态估计。为实现上述目的，本专利技术还提供一种机器人目标跟踪抓取系统，机器人目标跟踪抓取系统包括RGBD相机图像获取模块、二维图像跟踪模块、三维重构模块、机器人位姿调整模块，其中，二维图像跟踪模块采用上文所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法。上述机器人目标跟踪抓取系统的工作方法具体步骤为：步骤1，RGBD相机图像获取模块得到目标的二维图像及深度图；步骤2，二维图像跟踪模块在二维图像层面使用基于仿射群的改进高斯重采样粒子滤波目标跟踪方法进行目标跟踪，得到候选区域；步骤3，结合深度图与二维图像跟踪模块得到的候选区域进行三维重构，计算RGBD相机坐标系下的目标三维位姿；步骤4，将目标三维位姿与机器人末态三维位姿输入机器人位姿调整模块，不断对机器人末态三维位姿与目标三维位姿进行实时计算差值；步骤5，判断差值是否小于阈值，若是，则控制机器人对目标进行抓取，否则调整机器人的位姿，得到新的机器人末态三维位姿，以接近目标，并返回步骤4。本专利技术中在目标跟踪算法中引入仿射群粒子，改进粒子滤波算法，融合加入改进高斯重采样环节，有效解决了几何粒子滤波算法粒子多样性匮乏的问题。在目标跟踪领域通用指标OPE、SRE、TRE上都有更好的效果，本申请算法有更好的实时性、时间鲁棒性和空间鲁棒性。快速移动、运动模糊、超出视野三种情况下本申请算法仍然表现出良好的效果，应用到机器人可以有更好的实用性，有效提升机器人跟踪抓取的成功率。附图说明图1为基于仿射群的改进高斯重采样粒子滤波目标跟踪方法流程图...

【技术保护点】
1.一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于：/n步骤1：引入仿射群粒子，建立状态方程和观测方程；/n步骤2：状态方程和观测方程进行欧拉指数离散化；/n步骤3：状态方程进行一阶自回归模型化；/n步骤4：仿射群粒子集初始化，然后进行初步预测，得到迭代次数计数器的计数值

【技术特征摘要】
1.一种基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于：
步骤1：引入仿射群粒子，建立状态方程和观测方程；
步骤2：状态方程和观测方程进行欧拉指数离散化；
步骤3：状态方程进行一阶自回归模型化；
步骤4：仿射群粒子集初始化，然后进行初步预测，得到迭代次数计数器的计数值k=0初始状态的采样粒子；
步骤5：设置k=k+1，然后进行重要性采样；
步骤6：采取改进高斯重采样算法进行重采样，然后进行状态估计；
步骤7：判断是否为最后一帧，如果是，则输出估计状态，如果不是，则返回步骤5。


2.根据权利要求1所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于，所述步骤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为过渡矩阵。


3.根据权利要求1所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于，所述步骤4中的仿射群粒子集初始化具体包括：设置迭代次数计数器的计数值k=0，设置粒子总数为N，对于粒子i=1，2，…，N，设置第i个粒子的状态初值X0i=I，第i个粒子对应的过渡矩阵初值A0i=0，其中I为单位矩阵。


4.根据权利要求3所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于，所述步骤4的初步预测具体为：
利用一阶自回归模型化后的状态方程，对仿射群粒子集中的所有粒子计算先验概率密度，得到k=0初始状态的所有采样粒子，构成初始状态下的粒子集合。


5.根据权利要求4所述的基于仿射群的改进高斯重采样粒子滤波目标跟踪方法，其特征在于，步骤5的重要性采样具体为：<...

【专利技术属性】
技术研发人员：赵光哲，陈志怀，任帆，金哲学，夏光蔚，陶永，
申请(专利权)人：北京建筑大学，北京航空航天大学，延边大学，武汉职业技术学院，宁夏工商职业技术学院，中科视元北京科技有限公司，
类型：发明
国别省市：北京;11