CN110989341B - 一种约束辅助粒子滤波方法及目标跟踪方法 - Google Patents
一种约束辅助粒子滤波方法及目标跟踪方法 Download PDFInfo
- Publication number
- CN110989341B CN110989341B CN201911114940.2A CN201911114940A CN110989341B CN 110989341 B CN110989341 B CN 110989341B CN 201911114940 A CN201911114940 A CN 201911114940A CN 110989341 B CN110989341 B CN 110989341B
- Authority
- CN
- China
- Prior art keywords
- target
- prior probability
- covariance
- value
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 121
- 239000002245 particle Substances 0.000 title claims abstract description 91
- 238000001914 filtration Methods 0.000 title claims abstract description 63
- 238000009826 distribution Methods 0.000 claims abstract description 50
- 230000008569 process Effects 0.000 claims abstract description 31
- 238000012937 correction Methods 0.000 claims abstract description 23
- 238000004364 calculation method Methods 0.000 claims abstract description 19
- 238000005315 distribution function Methods 0.000 claims abstract description 10
- 239000013598 vector Substances 0.000 claims description 25
- 238000005457 optimization Methods 0.000 claims description 15
- 230000008901 benefit Effects 0.000 abstract description 6
- 230000000875 corresponding effect Effects 0.000 description 26
- 238000004422 calculation algorithm Methods 0.000 description 17
- 238000005259 measurement Methods 0.000 description 12
- 230000002452 interceptive effect Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 230000003190 augmentative effect Effects 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 238000000342 Monte Carlo simulation Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 238000012952 Resampling Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 238000005303 weighing Methods 0.000 description 2
- 241000197727 Euscorpius alpha Species 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012552 review Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Radar Systems Or Details Thereof (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了一种约束辅助粒子滤波方法,包括以下步骤:将非线性状态约束信息融入到建议分布函数的修正计算,进行近似求解目标状态的后验分布,得到后验概率分布;通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;利用先验概率进行粒子滤波以获取第一均值和第一协方差值,利用修正先验概率进行粒子滤波以获取第二均值和第二协方差值;对第一均值和第二均值、第一协方差值和第二协方差值计算加权,获取后验概率的均值和协方差,完成粒子滤波过程。本发明由于在利用约束条件优化修正先验概率的过程中引入了当前观测信息及运动特征,其在跟踪机动目标上具有优势。相应地,本发明还提供了一种目标跟踪方法。
Description
技术领域
本发明涉及目标跟踪技术领域,具体而言,涉及一种约束辅助粒子滤波方法及目标跟踪方法。
背景技术
纯方位跟踪能够利用传感器测得的被噪声污染的方位数据估计目标当前的运动参数,如位置、速率等,隐蔽性好,安全性高,已经广泛应用于雷达、航空航天、计算机视觉等众多领域。由于其高度的非线性和固有的不完全可测性,以及可能的信号时延问题,几十年来一直是跟踪领域的研究重点之一。
经过大量检索发现一些典型的现有技术,如专利申请号为201510763029.X的专利公开了一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置,该专利能够提高粒子滤波的准确性和实时性,从而解决非线性高斯环境下目标机动带来模型不确定情况下的快速目标跟踪问题。又如申请号为2014510457534.7的专利公开了一种基于建筑物规则结构信息的室内定位方法,该专利在行走轨迹复杂的条件下,仍能准确估计航向,抑制了航向漂移,提高了长时间室内定位的精确度。又如申请号为201910301095.3的专利公开了一种基于几何先验信息下的室内机器人快速重定位的方法,该快速重定位方法鲁棒性高,实时性好,能够有效解决大地图之下室内机器人无法快速重新定位以及定位困难的问题
可见,如何目标跟踪,其实际应用中的亟待处理的实际问题(提高目标跟踪的精度以及鲁棒性等)还有很多未提出具体的解决方案。
发明内容
为了克服现有技术的不足提供了一种约束辅助粒子滤波方法及目标跟踪方法,本发明的具体技术方案如下:
一种约束辅助粒子滤波方法,包括以下步骤:
步骤1,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布;
步骤2,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;
步骤3,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值;
步骤4,分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程并更新时间。
可选的,在步骤1,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布的方法包括以下步骤:
步骤1a,基于动态方程xk+1=fk(xk)+ek和zk+1=Hk+1(xk+1)+vk+1,将非线性状态约束信息设为其中是k时刻的状态向量,通过动态转移矩阵Ak从k时刻递推到k+1时刻,高斯噪声ek~N(0,Qk)是观测噪声向量,Hk是传感器测量矩阵,vk~N(0,Rk)是传感器测量噪声参数λ>0,用来权衡过程噪声和测量噪声,符号向量表示满足约束条件的状态所在的可行区域;
步骤1c,根据后验概率分布,得到状态xk的修正似然函数为pliknew(zk|xk)=p(zk|xk)pg(yk|xk)以及修正先验概率pnew(xk|xk-1)=p(xk|xk-1)pg(yk|xk),其中pg(·)是满足非线性状态约束条件区域的指示函数,该指示函数为
可选的,在步骤2中,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率的方法包括以下步骤:
步骤2b,给出阈值ε,其中0≦ε≦1;
可选的,在步骤3中,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值的方法包括如下步骤:
步骤3a,取先验概率和修正先验概率的加权和作为混合建议密度p(·)=(1-α)p0(·)+αpnew(·);
可选的,在步骤4,分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程的方法包括如下步骤:
本发明还提供一种目标跟踪方法,包括如下步骤:
步骤1,在K=0时,初始化粒子,并给出过程噪声X0和测量噪声P0;
步骤2,通过最小二乘方法计算初始位置矢量;
步骤3,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布;
步骤4,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;
步骤5,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值;
步骤6,分别对步骤5获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程并更新时间;
步骤7,利用步骤6得到的后验概率对目标状态进行估计,以获取目标状态估计值;
步骤8,输出目标状态估计值,以实现对目标状态的跟踪。
可选的,在步骤2中,通过最小二乘方法计算初始位置矢量根据如下公式实现: 以及其中,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的均值,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的协方差,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak对应的协方差,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak与bk对应的协方差。
本发明所取得的有益效果包括:
1、由于约束辅助粒子滤波器(CAPF)在利用约束条件优化修正先验概率的过程中引入了当前观测信息及运动特征,遍历搜索可行域内满足最小误差的最优解,其在跟踪机动目标上具有优势;
2、本发明所述的一种约束辅助粒子滤波方法(CAPF)在跟踪结果及统计误差分析性能上均显示了较好的改进,在稀疏观测、纯方位测量的场景中,在跟踪精度、噪声影响及后验分布方面,相比较于交互多模型Rao-Blackwellized粒子滤波(IMMRBPF),CAPF目标跟踪具有更高的滤波精度、更强的鲁棒性、后验分布更加接近真实分布。
附图说明
从以下结合附图的描述可以进一步理解本发明,将重点放在示出实施例的原理上。
图1是本发明实施例之一中一种约束辅助粒子滤波方法的流程示意图;
图2是本发明实施例之一中不同滤波方法的一次迭代运算的滤波结果比较示意图;
图3是本发明实施例之一中不同滤波方法的均方根误差估计比较示意图;
图4是本发明实施例之一中不同滤波方法的时间比较示意图;
图5是本发明实施例之一中一种目标跟踪方法的流程示意图;
图6是本发明实施例之一中IMMRBPF及CAPF两种滤波方法在跟踪轨迹与真实轨迹对比示意图;
图7是本发明实施例之一中IMMRBPF及CAPF两种滤波方法100次蒙特卡洛仿真计算的统计均方根误差(RMSE)对比示意图;
图8是本发明实施例之一中IMMRBPF及CAPF两种滤波方法在X轴方向上与运动目标的真实位置矢量的偏差比较示意图;
图9是本发明实施例之一中IMMRBPF及CAPF两种滤波方法在Y轴方向上与运动目标的真实位置矢量的偏差比较示意图;
图10是本发明实施例之一中一次迭代计算过程中,运动目标在X轴方向上运动矢量的后验概率分布变化示意图;
图11是本发明实施例之一中一次迭代计算过程中,运动目标在Y轴方向上运动矢量的后验概率分布变化示意图。
具体实施方式
为了使得本发明的目的、技术方案及优点更加清楚明白,以下结合其实施例,对本发明进行进一步详细说明;应当理解,此处所描述的具体实施例仅用于解释本发明,并不用于限定本发明。对于本领域技术人员而言,在查阅以下详细描述之后,本实施例的其它***、方法和/或特征将变得显而易见。旨在所有此类附加的***、方法、特征和优点都包括在本说明书内、包括在本发明的范围内,并且受所附权利要求书的保护。在以下详细描述描述了所公开的实施例的另外的特征,并且这些特征根据以下将详细描述将是显而易见的。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件;在本发明的描述中,需要理解的是,若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或组件必须具有特定的方位、以特定的方位构造和操作,因此附图中描述位置关系的用语仅用于示例性说明,不能理解为对本专利的限制,对于本领域的普通技术人员而言,可以根据具体情况理解上述术语的具体含义。
本发明为一种约束辅助粒子滤波方法及目标跟踪方法,根据附图所示讲述以下实施例:
实施例一:
纯方位跟踪能够利用传感器测得的被噪声污染的方位数据估计目标当前的运动参数,如位置、速率等,隐蔽性好,安全性高,已经广泛应用于雷达、航空航天、计算机视觉等众多领域。由于其高度的非线性和固有的不完全可测性,以及可能的信号时延问题,几十年来一直是跟踪领域的研究重点之一。交互运动多模型算法IMM细化状态空间,与非线性滤波算法相结合,在滤波过程中,通过模型概率的变动实现自适应的变结构,是在均方误差意义下的最优估计。然而,该滤波方法较多依赖建立模型的准确性,由于机动目标运动的不确定性,精确模型估计难以建立,导致跟踪机动目标的实时性能较差,且随着模型越多,计算量越大。H∞滤波器能够在***不确定的情况下将响应和误差限定在一定范围,基本思想是最小化“最坏情况”下的代价函数,滤波过程中更加依赖测量而非先验状态,所以对于模型误差鲁棒性更好,但也因此引入高频噪声。
最优估计器卡尔曼滤波器通过递归得到最小二乘解,但对于非线性***,滤波性能下降。对此,为了解决复杂***中的非线性滤波问题,研究者们提出了大量改进的卡尔曼滤波器算法,最为典型的为扩展卡尔曼滤波器(EKF),基本思想是通过泰勒级数一阶线性化非线性测量函数,忽略了高阶信息,通常滤波精度不高,在目标机动发生运动时导致发散。另一典型线性非线性估计算法是无迹卡尔曼滤波器(UKF)、该算法将泰勒扩展到二阶,通过选择一些高斯点来近似非线性分布,可以将精度提高到三阶。但是对于非高斯分布不太适用。近年来,基于蒙特卡洛逼近的粒子滤波器(PF)能够有效处理非线性估计问题,发展飞速,其基本思想是通过寻找一组在状态空间中传播的带权值的序列Monte Carlo样本对后验概率进行逼近,以样本均值代替积分运算,从而获得状态的最小方差估计。粒子滤波能够有效消除线性化所带来的误差,然而,随着目标状态维数的增加,粒子维数和计算量也增加,导致粒子退化严重。传统的粒子滤波器未考虑当前时刻的观测信息,从而在目标发生突然机动时不能够有效估计跟踪。
为了得到更好的跟踪效果,滤波方法需要建立更精确的运动模型及自适应非线性滤波算法。但是,对于机动目标跟踪问题,精确的运动模型很难建立;并且自适应非线性滤波需要能够处理目标的突然机动;另外,非线性与线性部分之间的动态变化需要关联。这些都是非线性滤波领域有待改进的难题所在。
本发明提出一种约束辅助粒子滤波(CAPF)方法用于纯方位机动目标跟踪。通过最小二乘估计运动目标的初始位置,将非线性状态约束信息融入到建议分布函数的修正计算,通过凸优化方法确定满足非线性状态约束的可行区域的中心点,搜索全局最优值逼近修正先验概率,从而修正似然函数,将采样粒子限制在可行域内,选择高似然概率的粒子传播到下一迭代过程。该数值优化理论上能够收敛。同时融合基于先验概率分布的滤波更新结果,自适应地权衡过去信息与当前测量对目标运动的影响。得到最终的估计结果。通过仿真实验,比较不同滤波算法对一维强非线性空间模型的估计情形,证明了提出算法的优越性。同时,对纯方位机动目标跟踪场景进行仿真模拟,与多模型RB粒子滤波算法进行比较,实验结果表明不同噪声情况下,约束辅助粒子滤波(CAPF)方法可以更好地提高目标跟踪的精确性与鲁棒性。
本实施例提出一种约束辅助粒子滤波方法,如图1所示,包括以下步骤:
步骤1,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布;
步骤2,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;
步骤3,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值;
步骤4,分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程并更新时间。
为了提高滤波精度,以对统计噪声进行量化约束,其中在步骤1,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布的方法包括以下步骤:
步骤1a,基于动态方程xk+1=fk(xk)+ek和zk+1=Hk+1(xk+1)+vk+1,将非线性状态约束信息设为其中是k时刻的状态向量,通过动态转移矩阵Ak从k时刻递推到k+1时刻,高斯噪声ek~N(0,Qk)是观测噪声向量,Hk是传感器测量矩阵,vk~N(0,Rk)是传感器测量噪声参数λ>0,用来权衡过程噪声和测量噪声,符号向量表示满足约束条件的状态所在的可行区域;
步骤1c,根据后验概率分布,得到状态xk的修正似然函数为pliknew(zk|xk)=p(zk|xk)pg(yk|xk)以及修正先验概率pnew(xk|xk-1)=p(xk|xk-1)pg(yk|xk),其中pg(·)是满足非线性状态约束条件区域的指示函数,该指示函数为
步骤1d,滤波算法的目标是在贝叶斯滤波框架下,通过观测序列矢量建立后验分布的最大概率(MAP)。从计算的角度,对数函数具有单调递增特性,采用后验概率分布的自然对数函数,比直接计算后验概率分布更为方便。采用后验概率分布的自然对数函数,计算后验概率分布,可以得到最大后验概率估计为其中log表示自然对数。
在贝叶斯框架下,由对偶原理可知,最大后验概率等价于最小-logp(xk|xk-1)(i=1,…,N)。数学上,使最小化-logp(xk|xk-1)的点定义为可行域的中心点。为了确定每个修正先验概率的中心点,计算满足非线性状态约束的状态预测,扩展至二阶以近似中心点。本文所提的约束粒子滤波算法(CAPF)采用非线性规划方法近似全局最小值。障碍函数选为自然对数,即选择障碍函数以满足约束条件按,自然对数能够得到较快的收敛速度[13]。其基本思想是:保持每一个迭代点xk是可行域的内点,可对于不满足约束区域的点,当迭代点靠近边界时,增广目标函数值骤然增大,以示“惩罚”,并阻止迭代点穿越边界。
其中,在步骤2中,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率的方法包括以下步骤:
将增广目标函数的最优解的解析表达式求出来,然后对罚参数γ→0取极限得到原问题的极小点。一般来说,对于较复杂的问题,只能用数值方法来求子问题的近似全局极小点。假设已经选择。可以拟牛顿方法求式通过一步迭代得到解近似为其中α*是步长,di是搜索方向,
步骤2b,给出阈值ε,其中0≦ε≦1;
其中,在步骤3中,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值的方法包括如下步骤:
步骤3a,取先验概率和修正先验概率的加权和作为混合建议密度p(·)=(1-α)p0(·)+αpnew(·);
其中,在步骤4,分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程的方法包括如下步骤:
本实施例所述的一种约束辅助粒子滤波方法CAPF在利用最小二乘估计目标初始位置时引入了当前观测信息,通过雅可比计算引入时间间隔、航向角差、速度矢量等运动特征对位置矢量的影响;设计非线性状态约束条件的数学表达式子,建立运动目标的最大后验概率目标函数,通过数值优化方法遍历搜索可行域的最优解以近似逼近修正先验概率的均值,通过加权计算建立先验信息、当前观测信息、非线性状态约束信息及运动特征的自适应融合结构,该方法能够满足计算的收敛性,具有良好的准确性和鲁棒性。
本实施例采用如下单变量模型进行测试,如下式所示:
其中,过程噪声观测噪声这里,N(m,σ2)表示均值为m,方差为σ2的高斯分布;IID表示独立同分布。该单变量模型常用作序列蒙特卡洛方法的模拟验证。由于该模型为多模型不确定性,过程噪声取为伽马噪声,即vk~gamm(2,3),观测噪声取为同样的高斯噪声。每个粒子滤波器取200个粒子,进行100次蒙特卡洛仿真。样本集的均值作为滤波输出,即
该一维算法的误差取为绝对值偏差,其通过以下公式计算:
如图2、图3以及图4所示,相对来说GPF,EKF算法的偏差较大,这是由于GPF未考虑当前观测信息,而EKF取泰勒一阶级数进行线性化处理,忽略了高阶信息,导致此两种算法精度较差。而UKF计算能够扩展到二阶泰勒级数,提高计算精度。相应地,UPF在某些强机动点不够稳定。约束辅助粒子滤波方法(CAPF)在计算精度和鲁棒性方面表现最优,主要原因是该方法遍历搜索全局最优解,从而能够快速收敛,及时跟上机动运动目标。在实施例中,EKF(Extended Kalman Filter),即扩展卡尔曼滤波器,UKF(Unscented Kalman Filter),即无迹卡尔曼滤波器,PF(Particle Filter),即粒子滤波器,EPF(Extended ParticleFilter),即扩展粒子滤波器,UPF(Unscented Particle Filter),即无迹粒子滤波器,CAPF(Control Auxiliary Particle Filter),即约束辅助粒子滤波器,APF(AuxiliaryParticle Filter),即辅助粒子滤波器。
下表给出了不同算法的RMSE统计信息,统计估计偏差的均值、方差及偏差最大值与滤波轨迹相符合。
实施例二:
与实施例所述的一种约束辅助粒子滤波方法相对应的,本发明还提供一种目标跟踪方法,如图5所示,包括如下步骤:
步骤1,在K=0时,初始化粒子,并给出过程噪声X0和测量噪声P0;
步骤2,通过最小二乘方法计算初始位置矢量;
步骤3,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布;
步骤4,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;
步骤5,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值;
步骤6,分别对步骤5获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程并更新时间;
步骤7,利用步骤6得到的后验概率对目标状态进行估计,以获取目标状态估计值;
步骤8,输出目标状态估计值,以实现对目标状态的跟踪。
可选的,在步骤2中,通过最小二乘方法计算初始位置矢量根据如下公式实现: 以及其中,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的均值,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的协方差,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak对应的协方差,表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak与bk对应的协方差。
图6、7、8以及9给出了交互多模型Rao-Blackwellized粒子滤波器跟约束辅助粒子滤波器的目标跟踪对比。图6为交互多模型Rao-Blackwellized粒子滤波器跟约束辅助粒子滤波器的跟踪轨迹与真实轨迹对比,图7为交互多模型Rao-Blackwellized粒子滤波器跟约束辅助粒子滤波器的100次蒙特卡洛仿真计算的统计均方根误差(RMSE)对比,图8分别是两种滤波器交互多模型Rao-Blackwellized粒子滤波器跟约束辅助粒子滤波器在x、y轴方向上与运动目标的真实位置矢量的偏差比较,其中横轴是目标真实位置,纵轴是滤波估计结果,对角线对应采样时刻的估计值越接近x=y,说明估计值越准确。结合图6、7、8以及9可以知道,在运动目标突然发生机动运动及采样间隔T较大两个阶段,约束辅助粒子滤波器(CAPF)的跟踪效果明显优于交互多模型Rao-Blackwellized粒子滤波(IMMRBPF)。
下面两份表格给出了采用控制变量法给出了统计过程噪声及观测噪声对交互多模型Rao-Blackwellized粒子滤波(IMMRBPF)以及约束辅助粒子滤波器(CAPF)的影响。
保持观测噪声不变,取为σe=0.0015,过程噪声分别取为σv=0.05mrad,σv=0.1mrad,σv=0.5mrad三组,统计比较如下表所示:
保持过程噪声不变,取为σv=0.2,测量噪声分别取为σe=0.0015,σe=0.005σe=0.01三组,统计比较如下表所示:
比较分析统计意义上的均方跟误差的均值、方差及偏离目标位置矢量的最大值,可知相比交互多模型Rao-Blackwellized粒子滤波(IMMRBPF),约束辅助粒子滤波器(CAPF)的跟踪效果对过程噪声及观测噪声均表现出较强鲁棒性,其非线性状态约束的优化作用明显。
由目标运动参数及运动轨迹知,相对z轴上运动速度为0。图10以及图11给出了一次迭代计算过程中,运动目标在x、y轴方向上运动矢量的后验概率分布变化。可知,约束辅助粒子滤波器(CAPF)遍历搜索可行域最优解,逼近真实后验分布。由于约束辅助粒子滤波器(CAPF)在利用约束条件优化修正先验概率的过程中引入了当前观测信息及运动特征,遍历搜索可行域内满足最小误差的最优解,其在跟踪机动目标上具有优势。
本发明提出了一种约束辅助粒子滤波方法(CAPF)及目标跟踪方法,通过一系列优化算法来近似计算满足约束条件伪测量似然函数产生建议分布。采用交叉二乘法确定目标位置,引入当前观测空时运动信息;采用最大后验概率法建立目标函数,选取1范数的自然对数作为障碍函数,采用凸优化(内点法)方法遍历搜索满足约束条件的全局最优值近似估计修正先验概率的中心点;与先验概率加权融合,作为重要性采样的建议分布函数。从而,自适应地权衡过去先验信息及当前观测信息对状态估计的影响。在一维强非线性、非高斯噪声的状态模型跟踪仿真模拟实验中,相比于EKF,UKF,GPF,EPF,UPF算法,本发明所述的一种约束辅助粒子滤波方法(CAPF)在跟踪结果及统计误差分析性能上均显示了较好的改进;在稀疏观测、纯方位测量的场景中,在跟踪精度、噪声影响及后验分布方面,相比较于交互多模型Rao-Blackwellized粒子滤波(IMMRBPF),CAPF目标跟踪具有更高的滤波精度、更强的鲁棒性、后验分布更加接近真实分布。
综上所述,本发明公开的一种约束辅助粒子滤波方法及目标跟踪方法,所产生的有益技术效果包括:
1、由于约束辅助粒子滤波器(CAPF)在利用约束条件优化修正先验概率的过程中引入了当前观测信息及运动特征,遍历搜索可行域内满足最小误差的最优解,其在跟踪机动目标上具有优势;
2、本发明所述的一种约束辅助粒子滤波方法(CAPF)在跟踪结果及统计误差分析性能上均显示了较好的改进,在稀疏观测、纯方位测量的场景中,在跟踪精度、噪声影响及后验分布方面,相比较于交互多模型Rao-Blackwellized粒子滤波(IMMRBPF),CAPF目标跟踪具有更高的滤波精度、更强的鲁棒性、后验分布更加接近真实分布。
虽然上面已经参考各种实施例描述了本发明,但是应当理解,在不脱离本发明的范围的情况下,可以进行许多改变和修改。也就是说上面讨论的方法、***和设备是示例,各种配置可以适当地省略、替换或添加各种过程或组件。例如,在替代配置中,可以以与所描述的顺序不同的顺序执行方法和/或可以添加、省略和/或组合各种部件。而且,关于某些配置描述的特征可以以各种其他配置组合,如可以以类似的方式组合配置的不同方面和元素。此外,随着技术发展其中的元素可以更新,即许多元素是示例,并不限制本发明公开或权利要求的范围。
在说明书中给出了具体细节以提供对包括实现的示例性配置的透彻理解。然而,可以在没有这些具体细节的情况下实践配置,例如已经示出了众所周知的电路、过程、算法、结构和技术而没有不必要的细节,以避免模糊配置。该描述仅提供示例配置,并且不限制权利要求的范围,适用性或配置。相反,前面对配置的描述将为本领域技术人员提供用于实现所描述的技术的使能描述。在不脱离本发明公开的精神或范围的情况下,可以对元件的功能和布置进行各种改变。
综上,其旨在上述详细描述被认为是例示性的而非限制性的,并且应当理解,以下权利要求(包括所有等同物)旨在限定本发明的精神和范围。以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后,技术人员可以对本发明作各种改动或修改,这些等效变化和修饰同样落入本发明权利要求所限定的范围。
Claims (1)
1.一种目标跟踪方法,其特征在于,包括如下步骤:
步骤1,在K=0时,初始化粒子,并给出过程噪声X0和测量噪声P0;
步骤2,通过最小二乘方法计算初始位置矢量;
步骤3,将非线性状态约束信息融入到建议分布函数的修正计算,应用数值优化方法进行近似求解目标状态的后验分布,得到后验概率分布;
步骤4,通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率;
步骤5,选取先验概率和修正先验概率的加权和作为混合建议密度,利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值,利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值;
步骤6,分别对步骤5获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理,获取后验概率的均值和协方差,完成粒子滤波过程并更新时间;
步骤7,利用步骤6得到的后验概率对目标状态进行估计,以获取目标状态估计值;
步骤8,输出目标状态估计值,以实现对目标状态的跟踪;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911114940.2A CN110989341B (zh) | 2019-11-14 | 2019-11-14 | 一种约束辅助粒子滤波方法及目标跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911114940.2A CN110989341B (zh) | 2019-11-14 | 2019-11-14 | 一种约束辅助粒子滤波方法及目标跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110989341A CN110989341A (zh) | 2020-04-10 |
CN110989341B true CN110989341B (zh) | 2022-08-26 |
Family
ID=70084475
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911114940.2A Active CN110989341B (zh) | 2019-11-14 | 2019-11-14 | 一种约束辅助粒子滤波方法及目标跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110989341B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111524163B (zh) * | 2020-04-16 | 2024-06-25 | 南京卓宇智能科技有限公司 | 一种基于连续扩展卡尔曼滤波的目标跟踪方法 |
CN111707997B (zh) * | 2020-06-03 | 2022-05-27 | 南京慧尔视智能科技有限公司 | 雷达目标跟踪方法、装置、电子设备及存储介质 |
CN112949216B (zh) * | 2021-02-03 | 2023-07-14 | 中国空气动力研究与发展中心高速空气动力研究所 | 一种基于混合性能函数的在线寻峰数据处理方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103955600A (zh) * | 2014-04-03 | 2014-07-30 | 深圳大学 | 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置 |
CN105447574A (zh) * | 2015-11-10 | 2016-03-30 | 深圳大学 | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 |
-
2019
- 2019-11-14 CN CN201911114940.2A patent/CN110989341B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103955600A (zh) * | 2014-04-03 | 2014-07-30 | 深圳大学 | 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置 |
CN105447574A (zh) * | 2015-11-10 | 2016-03-30 | 深圳大学 | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 |
Non-Patent Citations (2)
Title |
---|
The Constrained Extended Kalman Particle Filter;Hongwei Zhang等;《2018 14th IEEE International Conference on Signal Processing (ICSP)》;20190228;第99-102页 * |
一种约束扩展卡尔曼粒子滤波器;***;《东莞理工学报》;20181031;第25卷(第5期);第10-16页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110989341A (zh) | 2020-04-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111985093B (zh) | 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法 | |
CN110989341B (zh) | 一种约束辅助粒子滤波方法及目标跟踪方法 | |
CN108759833B (zh) | 一种基于先验地图的智能车辆定位方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN103389094B (zh) | 一种改进的粒子滤波方法 | |
CN111291471B (zh) | 一种基于l1正则无迹变换的约束多模型滤波方法 | |
US7277047B1 (en) | Reduced state estimation with biased measurements | |
Liu et al. | Adaptive Gaussian sum squared-root cubature Kalman filter with split-merge scheme for state estimation | |
CN115204212A (zh) | 一种基于stm-pmbm滤波算法的多目标跟踪方法 | |
CN116047498A (zh) | 基于最大相关熵扩展卡尔曼滤波的机动目标跟踪方法 | |
Pelka et al. | Introduction, discussion and evaluation of recursive Bayesian filters for linear and nonlinear filtering problems in indoor localization | |
Bai et al. | A Robust Generalized $ t $ Distribution-Based Kalman Filter | |
KR101502721B1 (ko) | 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치 | |
Qian et al. | An INS/DVL integrated navigation filtering method against complex underwater environment | |
CN111291319A (zh) | 一种应用于非高斯噪声环境下的移动机器人状态估计方法 | |
CN114608585A (zh) | 一种移动机器人同步定位与建图方法及装置 | |
Fernandes et al. | GNSS/MEMS-INS integration for drone navigation using EKF on lie groups | |
CN114139109A (zh) | 一种目标跟踪方法、***、设备、介质及数据处理终端 | |
CN111262556B (zh) | 一种同时估计未知高斯测量噪声统计量的多目标跟踪方法 | |
CN111340853B (zh) | 基于ospa迭代的多传感器gmphd自适应融合方法 | |
Fariña et al. | Sensor fusion algorithm selection for an autonomous wheelchair based on EKF/UKF comparison | |
Havangi | An adaptive particle filter based on PSO and fuzzy inference system for nonlinear state systems | |
Lai et al. | Adaptive fuzzy neural network-aided progressive Gaussian approximate filter for GPS/INS integration navigation | |
Chen et al. | Multiple similarity measure-based maximum correntropy criterion Kalman filter with adaptive kernel width for GPS/INS integration navigation | |
Blasch et al. | Comparison of stochastic integration filter with the unscented Kalman filter for maneuvering targets |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |