CN110989341B - 一种约束辅助粒子滤波方法及目标跟踪方法 - Google Patents

一种约束辅助粒子滤波方法及目标跟踪方法 Download PDF

Info

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
prior probability
target
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
Application number
CN201911114940.2A
Other languages
English (en)
Other versions
CN110989341A (zh
Inventor
张宏伟
张小虎
杨夏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN201911114940.2A priority Critical patent/CN110989341B/zh
Publication of CN110989341A publication Critical patent/CN110989341A/zh
Application granted granted Critical
Publication of CN110989341B publication Critical patent/CN110989341B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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，将非线性状态约束信息设为
Figure BDA0002273771960000021
其中
Figure BDA0002273771960000022
是k时刻的状态向量，通过动态转移矩阵Ak从k时刻递推到k+1时刻，高斯噪声ek～N(0,Qk)是观测噪声向量，Hk是传感器测量矩阵，vk～N(0,Rk)是传感器测量噪声参数λ＞0,用来权衡过程噪声和测量噪声,符号向量
Figure BDA0002273771960000031
表示满足约束条件的状态所在的可行区域；
步骤1b,将非线性状态约束信息作为先验知识，应用数值优化方法进行近似求解目标状态的后验分布，得到后验概率分布为
Figure BDA0002273771960000032
步骤1c，根据后验概率分布，得到状态xk的修正似然函数为pliknew(zk|xk)＝p(zk|xk)pg(yk|xk)以及修正先验概率pnew(xk|xk-1)＝p(xk|xk-1)pg(yk|xk)，其中pg(·)是满足非线性状态约束条件区域的指示函数，该指示函数为
Figure BDA0002273771960000033
步骤1d,根据步骤1c得到的修正似然函数以及修正先验概率，得到目标状态在K时刻的全后验分布为
Figure BDA0002273771960000034
步骤1d，采用后验概率分布的自然对数函数，计算后验概率分布，得到最大后验概率估计为
Figure BDA0002273771960000035
其中log表示自然对数。
可选的，在步骤2中，通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率的方法包括以下步骤：
步骤2a,选择障碍函数
Figure BDA0002273771960000036
建立目标函数，以满足约束条件p(ek)＝0,
Figure BDA0002273771960000037
并建立增广目标函数
Figure BDA0002273771960000041
并将该增广目标函数记为fo,其中
Figure BDA0002273771960000042
分别是
Figure BDA0002273771960000043
在点
Figure BDA0002273771960000044
的雅可比和海森矩阵；
步骤2b,给出阈值ε，其中0≦ε≦1；
步骤2c，利用公式
Figure BDA00022737719600000415
计算搜索方向，其中di表示搜索方向；
步骤2d,利用回溯线搜索方法以及公式
Figure BDA0002273771960000045
计算步长α*；
步骤2e,利用公式
Figure BDA0002273771960000046
进行更新迭代,得到修正先验概率
Figure BDA0002273771960000047
可选的，在步骤3中，选取先验概率和修正先验概率的加权和作为混合建议密度，利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值，利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值的方法包括如下步骤：
步骤3a，取先验概率和修正先验概率的加权和作为混合建议密度p(·)＝(1-α)p0(·)+αpnew(·)；
步骤3b,利用公式
Figure BDA0002273771960000048
Figure BDA0002273771960000049
以及
Figure BDA00022737719600000410
获取与目标状态对应的第一均值
Figure BDA00022737719600000411
和第一协方差值
Figure BDA00022737719600000412
与目标状态对应的第二均值
Figure BDA00022737719600000413
和第二协方差值
Figure BDA00022737719600000414
其中，zk表示当前观测；hk(·)非线性观测函数。
可选的，在步骤4，分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理，获取后验概率的均值和协方差，完成粒子滤波过程的方法包括如下步骤：
步骤4a,从混合建议密度中采用粒子集
Figure BDA0002273771960000051
预测目标轨迹
Figure BDA0002273771960000052
利用公式p(·)＝(1-α)p0(·)+αpnew(·)、
Figure BDA0002273771960000053
以及
Figure BDA0002273771960000054
进行加权并归一化处理；
步骤4b,重采样，根据重要性权值
Figure BDA00022737719600000510
的大小分别增多或减少
Figure BDA0002273771960000055
根据修正先验概率
Figure BDA0002273771960000056
以近似得到N个随机样本
Figure BDA0002273771960000057
步骤4c,利用公式
Figure BDA0002273771960000058
计算获取后验概率的均值
Figure BDA0002273771960000059
和方差pk。
本发明还提供一种目标跟踪方法，包括如下步骤：
步骤1，在K＝0时，初始化粒子，并给出过程噪声X0和测量噪声P0；
步骤2，通过最小二乘方法计算初始位置矢量；
步骤3，将非线性状态约束信息融入到建议分布函数的修正计算，应用数值优化方法进行近似求解目标状态的后验分布，得到后验概率分布；
步骤4，通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率；
步骤5，选取先验概率和修正先验概率的加权和作为混合建议密度，利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值，利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值；
步骤6，分别对步骤5获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理，获取后验概率的均值和协方差，完成粒子滤波过程并更新时间；
步骤7，利用步骤6得到的后验概率对目标状态进行估计，以获取目标状态估计值；
步骤8，输出目标状态估计值，以实现对目标状态的跟踪。
可选的，在步骤2中，通过最小二乘方法计算初始位置矢量根据如下公式实现：
Figure BDA0002273771960000061
Figure BDA0002273771960000062
Figure BDA0002273771960000063
以及
Figure BDA0002273771960000064
其中，
Figure BDA0002273771960000065
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的均值，
Figure BDA0002273771960000066
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的协方差，
Figure BDA0002273771960000067
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak对应的协方差，
Figure BDA0002273771960000068
表示修正先验概率密度函数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，将非线性状态约束信息设为
Figure BDA0002273771960000111
其中
Figure BDA0002273771960000112
是k时刻的状态向量，通过动态转移矩阵Ak从k时刻递推到k+1时刻，高斯噪声ek～N(0,Qk)是观测噪声向量，Hk是传感器测量矩阵，vk～N(0,Rk)是传感器测量噪声参数λ＞0,用来权衡过程噪声和测量噪声,符号向量
Figure BDA0002273771960000113
表示满足约束条件的状态所在的可行区域；
符号向量
Figure BDA0002273771960000114
表示满足约束条件的状态所在的可行区域，如下公式所示：
Figure BDA0002273771960000115
步骤1b,将非线性状态约束信息作为先验知识，只影响目标的位置矢量，故而可以应用数值优化方法进行近似求解目标状态的后验分布，得到后验概率分布为
Figure BDA0002273771960000121
步骤1c，根据后验概率分布，得到状态xk的修正似然函数为pliknew(zk|xk)＝p(zk|xk)pg(yk|xk)以及修正先验概率pnew(xk|xk-1)＝p(xk|xk-1)pg(yk|xk)，其中pg(·)是满足非线性状态约束条件区域的指示函数，该指示函数为
Figure BDA0002273771960000122
步骤1d,根据步骤1c得到的修正似然函数以及修正先验概率，得到目标状态在K时刻的全后验分布为
Figure BDA0002273771960000123
步骤1d，滤波算法的目标是在贝叶斯滤波框架下，通过观测序列矢量建立后验分布的最大概率(MAP)。从计算的角度，对数函数具有单调递增特性，采用后验概率分布的自然对数函数，比直接计算后验概率分布更为方便。采用后验概率分布的自然对数函数，计算后验概率分布，可以得到最大后验概率估计为
Figure BDA0002273771960000124
其中log表示自然对数。
在贝叶斯框架下，由对偶原理可知，最大后验概率
Figure BDA0002273771960000125
等价于最小-logp(xk|xk-1)(i＝1,…,N)。数学上，使最小化-logp(xk|xk-1)的点
Figure BDA0002273771960000126
定义为可行域的中心点。为了确定每个修正先验概率的中心点，计算满足非线性状态约束的状态预测，扩展至二阶以近似中心点。本文所提的约束粒子滤波算法(CAPF)采用非线性规划方法近似全局最小值。障碍函数选为自然对数，即
Figure BDA0002273771960000127
选择障碍函数
Figure BDA0002273771960000131
以满足约束条件按，自然对数能够得到较快的收敛速度[13]。其基本思想是：保持每一个迭代点xk是可行域
Figure BDA0002273771960000132
的内点，可对于不满足约束区域
Figure BDA0002273771960000133
的点，当迭代点靠近边界时，增广目标函数值骤然增大，以示“惩罚”，并阻止迭代点穿越边界。
其中，在步骤2中，通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率的方法包括以下步骤：
步骤2a,选择障碍函数
Figure BDA0002273771960000134
建立目标函数，以满足约束条件p(ek)＝0,
Figure BDA0002273771960000135
并建立增广目标函数
Figure BDA0002273771960000136
并将该增广目标函数记为fo,其中
Figure BDA0002273771960000137
分别是
Figure BDA0002273771960000138
在点
Figure BDA0002273771960000139
的雅可比和海森矩阵。
将增广目标函数的最优解的解析表达式求出来，然后对罚参数γ→0取极限得到原问题的极小点。一般来说，对于较复杂的问题，只能用数值方法来求子问题的近似全局极小点。假设
Figure BDA00022737719600001310
已经选择。可以拟牛顿方法求式
Figure BDA00022737719600001311
通过一步迭代得到解近似为
Figure BDA00022737719600001312
其中α*是步长，di是搜索方向，
Figure BDA00022737719600001314
在本实施例中，初始位置矢量x0的选取，采用交叉二乘定位法。当起始点在约束区域，即
Figure BDA00022737719600001313
步长取为1，即α*＝1。否则，采用内点法(IPM)近似求解最优解，先用进退法确定高低高区间，再进行一维搜索。
步骤2b,给出阈值ε，其中0≦ε≦1；
步骤2c，利用公式
Figure BDA00022737719600001411
计算搜索方向，其中di表示搜索方向；
步骤2d,利用回溯线搜索方法以及公式
Figure BDA0002273771960000141
计算步长α*；
步骤2e,利用公式
Figure BDA0002273771960000142
进行更新迭代,得到修正先验概率
Figure BDA0002273771960000143
其中，在步骤3中，选取先验概率和修正先验概率的加权和作为混合建议密度，利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值，利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值的方法包括如下步骤：
步骤3a，取先验概率和修正先验概率的加权和作为混合建议密度p(·)＝(1-α)p0(·)+αpnew(·)；
步骤3b,利用公式
Figure BDA0002273771960000144
Figure BDA0002273771960000145
以及
Figure BDA0002273771960000146
获取与目标状态对应的第一均值
Figure BDA0002273771960000147
和第一协方差值
Figure BDA0002273771960000148
与目标状态对应的第二均值
Figure BDA0002273771960000149
和第二协方差值
Figure BDA00022737719600001410
其中，zk表示当前观测；hk(·)非线性观测函数。
其中，在步骤4，分别对步骤3获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理，获取后验概率的均值和协方差，完成粒子滤波过程的方法包括如下步骤：
步骤4a,从混合建议密度中采用粒子集
Figure BDA0002273771960000151
预测目标轨迹
Figure BDA0002273771960000152
利用公式p(·)＝(1-α)p0(·)+αpnew(·)、
Figure BDA0002273771960000153
以及
Figure BDA0002273771960000154
进行加权并归一化处理；
步骤4b,重采样，根据重要性权值
Figure BDA0002273771960000155
的大小分别增多或减少
Figure BDA0002273771960000156
根据修正先验概率
Figure BDA0002273771960000157
以近似得到N个随机样本
Figure BDA0002273771960000158
步骤4c,利用公式
Figure BDA0002273771960000159
计算获取后验概率的均值
Figure BDA00022737719600001510
和方差pk。
本实施例所述的一种约束辅助粒子滤波方法CAPF在利用最小二乘估计目标初始位置时引入了当前观测信息，通过雅可比计算引入时间间隔、航向角差、速度矢量等运动特征对位置矢量的影响；设计非线性状态约束条件的数学表达式子，建立运动目标的最大后验概率目标函数，通过数值优化方法遍历搜索可行域的最优解以近似逼近修正先验概率的均值，通过加权计算建立先验信息、当前观测信息、非线性状态约束信息及运动特征的自适应融合结构，该方法能够满足计算的收敛性，具有良好的准确性和鲁棒性。
本实施例采用如下单变量模型进行测试，如下式所示：
Figure BDA00022737719600001511
Figure BDA00022737719600001512
其中，过程噪声
Figure BDA00022737719600001513
观测噪声
Figure BDA00022737719600001514
这里，N(m,σ2)表示均值为m，方差为σ2的高斯分布；IID表示独立同分布。该单变量模型常用作序列蒙特卡洛方法的模拟验证。由于该模型为多模型不确定性，过程噪声取为伽马噪声，即vk～gamm(2,3)，观测噪声取为同样的高斯噪声。每个粒子滤波器取200个粒子，进行100次蒙特卡洛仿真。样本集的均值作为滤波输出，即
Figure BDA0002273771960000161
该一维算法的误差取为绝对值偏差，其通过以下公式计算：
Figure BDA0002273771960000162
如图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统计信息，统计估计偏差的均值、方差及偏差最大值与滤波轨迹相符合。
Figure BDA0002273771960000163
Figure BDA0002273771960000171
实施例二：
与实施例所述的一种约束辅助粒子滤波方法相对应的，本发明还提供一种目标跟踪方法，如图5所示，包括如下步骤：
步骤1，在K＝0时，初始化粒子，并给出过程噪声X0和测量噪声P0；
步骤2，通过最小二乘方法计算初始位置矢量；
步骤3，将非线性状态约束信息融入到建议分布函数的修正计算，应用数值优化方法进行近似求解目标状态的后验分布，得到后验概率分布；
步骤4，通过计算可行域的中心来近似得到满足非线性状态约束的修正先验概率；
步骤5，选取先验概率和修正先验概率的加权和作为混合建议密度，利用先验概率密度函数进行粒子滤波以获取与目标状态对应的第一均值和第一协方差值，利用修正先验概率密度函数进行粒子滤波以获取与目标状态对应的第二均值和第二协方差值；
步骤6，分别对步骤5获得的第一均值和第二均值、第一协方差值和第二协方差值计算加权并归一化处理，获取后验概率的均值和协方差，完成粒子滤波过程并更新时间；
步骤7，利用步骤6得到的后验概率对目标状态进行估计，以获取目标状态估计值；
步骤8，输出目标状态估计值，以实现对目标状态的跟踪。
可选的，在步骤2中，通过最小二乘方法计算初始位置矢量根据如下公式实现：
Figure BDA0002273771960000181
Figure BDA0002273771960000182
Figure BDA0002273771960000183
以及
Figure BDA0002273771960000184
其中，
Figure BDA0002273771960000185
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的均值，
Figure BDA0002273771960000186
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的协方差，
Figure BDA0002273771960000187
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak对应的协方差，
Figure BDA0002273771960000188
表示修正先验概率密度函数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三组，统计比较如下表所示：
Figure BDA0002273771960000191
Figure BDA0002273771960000192
Figure BDA0002273771960000193
保持过程噪声不变，取为σv＝0.2，测量噪声分别取为σe＝0.0015，σe＝0.005σe＝0.01三组，统计比较如下表所示：
Figure BDA0002273771960000201
Figure BDA0002273771960000202
Figure BDA0002273771960000203
比较分析统计意义上的均方跟误差的均值、方差及偏离目标位置矢量的最大值，可知相比交互多模型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，输出目标状态估计值，以实现对目标状态的跟踪；
其中，在步骤2中，通过最小二乘方法计算初始位置矢量根据如下公式实现：
Figure FDA0003722588450000011
Figure FDA0003722588450000012
Figure FDA0003722588450000013
以及
Figure FDA0003722588450000021
其中，
Figure FDA0003722588450000022
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的均值，
Figure FDA0003722588450000023
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量bk对应的协方差，
Figure FDA0003722588450000024
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak对应的协方差，
Figure FDA0003722588450000025
表示修正先验概率密度函数p1,m,k(·)中目标的状态分量ak与bk对应的协方差。
CN201911114940.2A 2019-11-14 2019-11-14 一种约束辅助粒子滤波方法及目标跟踪方法 Active CN110989341B (zh)

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 (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111707997B (zh) * 2020-06-03 2022-05-27 南京慧尔视智能科技有限公司 雷达目标跟踪方法、装置、电子设备及存储介质
CN112949216B (zh) * 2021-02-03 2023-07-14 中国空气动力研究与发展中心高速空气动力研究所 一种基于混合性能函数的在线寻峰数据处理方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103955600A (zh) * 2014-04-03 2014-07-30 深圳大学 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置
CN105447574A (zh) * 2015-11-10 2016-03-30 深圳大学 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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) 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN111291471B (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
CN110989341B (zh) 一种约束辅助粒子滤波方法及目标跟踪方法
Yu et al. A SLAM algorithm based on adaptive cubature kalman filter
Liu et al. Adaptive Gaussian sum squared-root cubature Kalman filter with split-merge scheme for state estimation
CN110706265A (zh) 一种改进srckf强跟踪滤波的机动目标跟踪方法
Bai et al. A Robust Generalized $ t $ Distribution-Based Kalman Filter
CN115204212A (zh) 一种基于stm-pmbm滤波算法的多目标跟踪方法
Qian et al. An INS/DVL integrated navigation filtering method against complex underwater environment
KR101502721B1 (ko) 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치
CN114139109A (zh) 一种目标跟踪方法、系统、设备、介质及数据处理终端
Fariña et al. Sensor fusion algorithm selection for an autonomous wheelchair based on EKF/UKF comparison
CN111291319A (zh) 一种应用于非高斯噪声环境下的移动机器人状态估计方法
Wang et al. A line-of-sight rate estimation method for roll-pitch gimballed infrared seeker
Fernandes et al. Gnss/mems-ins integration for drone navigation using ekf on lie groups
Havangi An adaptive particle filter based on PSO and fuzzy inference system for nonlinear state systems
CN114608585A (zh) 一种移动机器人同步定位与建图方法及装置
Lai et al. Adaptive fuzzy neural network-aided progressive Gaussian approximate filter for GPS/INS integration navigation
Blasch et al. Comparison of stochastic integration filter with the unscented Kalman filter for maneuvering targets
Chen et al. Multiple similarity measure-based maximum correntropy criterion Kalman filter with adaptive kernel width for GPS/INS integration navigation
CN117784114B (zh) 异常噪声下基于混合熵的不规则扩展目标跟踪方法
Xin et al. Nonlinear filtering methods for the state estimation of dynamic systems and applications in navigation
RU2631766C1 (ru) ТРЕХМЕРНЫЙ АДАПТИВНЫЙ α-β ФИЛЬТР
Zhang et al. A robust particle filter for ambiguous updates of underwater terrain-aided navigation

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