中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
指挥控制与仿真

指挥控制与仿真 >

2021 , Vol. 43 >Issue 4: 117 - 122

DOI: https://doi.org/10.3969/j.issn.1673-3819.2021.04.021

工程实践

改进强跟踪粒子滤波及其在紧组合导航中的应用

  • 王聘 1 ,
  • 田敬 2 ,
  • 吴鹏 2 ,
  • 王洋洋 2
展开
  • 1.海军装备部驻连云港地区军事代表室, 江苏 连云港 222061
  • 2.江苏自动化研究所, 江苏 连云港 222061

王聘(1993—),男,助理工程师,研究方向为组合导航与武器控制。

田敬(1990—), 女, 工程师。

Copy editor: 许韦韦

收稿日期: 2020-12-06

  修回日期: 2021-04-07

  网络出版日期: 2022-05-20

收起

Improved Strong Tracking Particle Filter and Its Application in the Tightly-coupled Integrated Navigation

  • WANG Pin 1 ,
  • TIAN Jing 2 ,
  • WU Peng 2 ,
  • WANG Yang-yang 2
Expand
  • 1. Military Representative Office of the Naval Equipment Department in Lianyungang, Lianyungang 222061
  • 2. Jiangsu Automation Research Institute, Lianyungang 222061, China

Received date: 2020-12-06

  Revised date: 2021-04-07

  Online published: 2022-05-20

Fold

摘要

如何提高粒子滤波中采样效率和算法对突变状态的跟踪能力,是粒子滤波算法研究和应用中的重点。将强跟踪滤波算法与粒子滤波算法相结合,提出了改进强跟踪粒子滤波(Improved Strong Tracking Particle Filter, ISTPF)算法,该算法通过强跟踪滤波生成一个修正项,并以此修正粒子滤波中的粒子样本,大大提高了采样效率,解决了传统强跟踪粒子滤波算法中粒子退化和采样效率低的问题。最后将ISTPF算法应用于紧组合导航模型中,仿真可得位置误差小于0.3 m,速度误差小于0.05 m/s,姿态误差小于0.05°,证明采用ISTPF算法进行紧组合导航解算,具有较高的精度,此方法具有可行性和优越性。

关键词： 强跟踪滤波; 粒子滤波; 紧组合导航; 误差估计

本文引用格式

王聘 , 田敬 , 吴鹏 , 王洋洋 . 改进强跟踪粒子滤波及其在紧组合导航中的应用[J]. 指挥控制与仿真, 2021 , 43(4) : 117 -122 . DOI: 10.3969/j.issn.1673-3819.2021.04.021

Abstract

How to improve the sampling efficiency of particle filter and the capability of an algorithm to track an abrupt change state are the key to the research and application of particle filter. This paper combines the strong tracking filter with the particle filter, and proposes the improved strong tracking particle filter (ISTPF) algorithm. The ISTPF algorithm uses strong tracking Kalman filter to generate a correction term and uses it to correct the sample particles during particle filter, greatly improving sampling efficiency and resolving the problems of particle degeneracy and low sampling efficiency in the traditional strong tracking particle filtering algorithms. Finally, the ISTPF algorithm was applied to tightly-coupled integrated navigation model, the simulation results show that the position error is less than 0.3 m, the velocity error is less than 0.05 m/s and the attitude error is less than 0.05°, so which shows that ISTPF algorithm has higher precision, and it is feasible and superior in the tightly-coupled integrated navigation.

Key words： strong tracking filter; particle filter; tightly-coupled integrated navigation; error estimation

粒子滤波算法是解决非线性/非高斯动态系统状态估计问题最主要的方法,并成功应用于多目标跟踪[1]、故障诊断、飞行器姿态估计[2]、机器人导航[3]以及信号处理[4]等众多领域。但是,粒子退化问题,以及由其所导致的对突变状态的追踪失效是目前粒子滤波所面临的主要问题,为此,学者们进行了大量研究并提出了多种改进方法: EPF[5]、UPF[6]等,虽然这些算法能够在一定程度上解决粒子退化的问题,但对于带有突变状态的系统,还是会产生较大的误差。针对带有突变状态的系统,周东华提出了强跟踪滤波STPF算法[7],在卡尔曼滤波框架下引入次优渐消因子,提高了处理突变系统时的滤波精度,但仍不能够较好地处理似然函数位于状态空间尾部的情况。
针对上述问题,本文提出了改进强跟踪粒子滤波算法ISTPF,该算法将强跟踪算法与粒子滤波算法相结合,实现高效采样和强跟踪功能。该算法既具有较强处理突变系统的能力,又能够很好地解决似然函数位于状态空间尾部的问题,实现了高效采样粒子滤波。最后将ISTPF算法应用到紧组合导航模型中,仿真结果证实ISTPF算法能够得出更精确的导航结果,具有一定的可行性及优越性。

1 改进强跟踪粒子滤波算法

改进强跟踪粒子滤波算法通过运行一次强跟踪滤波,从而得出一个修正项并用于修正粒子滤波中粒子样本,将粒子推向高似然区域[8],即得出一个新的重要性概率密度函数,然后从中抽样,该过程将当前的量测信息考虑了进去,故所抽取的粒子样本更接近后验分布,最终的计算精度得到提高。
考虑如下的非线性系统:
系统方程:xt=ft(xt-1)+vt-1
量测方程:zt=ht(xt)+wt
其中xt和zt分别为状态向量和量测向量,f(·)和h(·)是已知的连续非线性函数,vt-1和wt分别为统计特性已知的系统噪声和量测噪声,滤波的最终目的就是根据所有量测值z= z 1 z 2 … z t递推求出当前时刻状态值xt。
在改进强跟踪滤波算法中,根据重要性概率密度函数q(xt|xt-1,zt)抽取一定数量粒子x= x 1 x 1 … x n,并结合后验概率密度函数p(zt| x t i)得出对应粒子权重 π t i,计算如下
π t i∝ π t - 1 i p ( z t | x t i ) P ( x t i | x t - 1 i ) q ( x t i | x t - 1 i , z t )
进而对粒子进行加权求得状态估计值
x ˙ t= ∑ i = 1 N π i t x i t
由此可见,重要性概率密度函数至关重要,它代表了粒子分布,同时影响着各个粒子的权重。在改进强跟踪粒子滤波中,对重要性概率密度函数进行改进,通过运行一步强跟踪EKF,获得一个修正项
Ct∝Ktγt
其中,Kt为t时刻滤波增益,γt为t时刻新息。此修正项考虑了量测信息,在减弱老数据对当前滤波值影响的基础上,使得粒子滤波在抽样阶段便权衡了量测信息,所抽取粒子更加接近真实值。
求出修正项Ct后,通过更新系统方程(1)便可得到修正后的状态转移方程:
xt=f(xt-1)+Ct+vt-1
因此,将重要性概率密度函数定义为pv(xt-f(xt-1)-Ct),从中抽取粒子并计算出其权重,得出状态估计值。经此重要性概率密度函数所抽取粒子能够更好地遍布整个函数,且更多地集中在高似然区域,有效地描述了系统函数,同时高似然区域粒子所占权重更大,最终经这些高质量粒子加权所得状态估计值便更为准确。
由上可见,给定状态初始值x1和状态方差初始值P1,便可递推求出相应时刻状态估值 x ˙ t。

2 紧组合导航模型

紧组合模型是将卫星导航设备接收机的伪距测量值和伪距率测量值,与利用惯性导航输出计算出的相应伪距、伪距率的值进行比较,得到的差值形成滤波器(ISTPF)的测量输入值,经组合导航滤波器,生成系统的误差估计值,这种估计值可在每次测量更新后对惯导系统进行修正,以提高惯性导航的精度。紧组合导航具体过程如图1所示。
图1 紧组合模型示意图
1) 求解载体位置速度和姿态。
根据惯性器件(陀螺和加计)测得的数据按照导航方程(7)解算出载体的速度,进而得出载体的位置;按照姿态更新方程(8)解算出载体的姿态[9-10]。
地固系下的导航方程:
v ˙ e e= C b e fb-2 ω i e e× v e e+ge
姿态更新的四元数算法
Q=cos Δ β 2+ Δ β δ βsin Δ β 2
其中, v e e为地固系载体速度, v ˙ e e为地固系载体速度导数, C b e为载体系到地固系转移矩阵,fb为加速度计的测量值, ω i e e为地球自转角速度,ge为重力加速度,Δβ为陀螺在采样周期内测得角度增量。
2)求解位置速度和姿态修正量
根据导航方程可以推导出的地固系下误差状态方程(将陀螺零偏、加计零偏、接收机钟差、钟差漂移误差作为待估计值)为式(9)
δ r ˙ δ v ˙ δ φ ˙ δ ω ˙ δ f ˙ δ t ˙ u δ t ˙ r u= F S I N S C b e 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 C b e 0 9 × 1 0 9 × 1 0 3 × 9 ( - 1 / τ ω ) 3 × 3 0 3 × 3 0 3 × 1 0 3 × 1 0 3 × 9 0 3 × 3 ( - 1 / τ f ) 3 × 3 0 3 × 1 0 3 × 1 0 1 × 9 0 1 × 3 0 1 × 3 0 1 0 1 × 9 0 1 × 3 0 1 × 3 0 - 1 / τ t r u δ r δ v δ φ δ ω δ f δ t u δ t r u+v
FSINS= - ( Ω i e e ) 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 × 3 ( F f ) 3 × 3 N 3 × 3 - 2 ( Ω i e e ) 3 × 3
其中,δr为位置误差向量,δv为速度误差向量,δφ为失准角向量,I3×3为3×3的单位对角阵,Ff为( f x f y f z)T的反对称阵,Ff= 0 - f z f y f z 0 - f x - f y f x 0, Ω i e e为地球自转角速度, ω i e e= 0 0 ω i e的反对称矩阵 Ω i e e= 0 - ω i e 0 ω i e 0 0 0 0 0,N为与重力矢量有关的系数阵,δω为陀螺漂移随机分量,δf为加速度计零偏的随机分量,δtu为接收机钟差,δtru为钟差漂移误差,这里陀螺零偏和加计零偏使用一阶马尔科夫过程进行描述为
δ ω ˙ = d i a g ( - 1 / τ ω ) δ ω + ε ω δ f ˙ = d i a g ( - 1 / τ f ) δ f + ε f
接收机钟差和钟差漂移误差采用以下模型进行描述:
δ t ˙ u = δ t r u + ε t u δ t ˙ r u = d i a g ( - 1 / τ t r u ) δ t r u + ε t r u
结合卫星导航设备接收机测得的伪距、伪距率和惯导设备解算出的载体位置速度,以及星历信息(卫星位置和速度),可得滤波的量测方程为式(13)
R 1 I - R 1 R 2 I - R 2 ︙ d R 1 I - d R 1 d R 2 I - d R 2 ︙= x I - x 1 s R 1 I y I - y 1 s R 1 I z I - z 1 s R 1 I 0 1 × 3 0 1 × 9 - 1 0 x I - x 2 s R 2 I y I - y 2 s R 2 I z I - z 2 s R 2 I 0 1 × 3 0 1 × 9 - 1 0 ︙ ︙ ︙ ︙ ︙ 0 1 × 3 x I - x 1 s R 1 I y I - y 2 s R 1 I z I - z 1 s R 1 I 0 1 × 9 0 - 1 0 1 × 3 x I - x 2 s R 2 I y I - y 2 s R 2 I z I - z 2 s R 2 I 0 1 × 9 0 - 1 ︙ ︙ ︙ ︙ ︙ δ r δ v δ φ δ ω δ f δ t u δ t r u+w
其中,Ri、dRi分别为卫星导航设备所得第i颗卫星的伪距和伪距率, x I y I z I、 V x I V y I V z I分别为惯导设备解算所得载体位置和速率, x i s y i s z i s、 V i s x V i s y V i s z分别为第i颗卫星的位置和速率。
用卫星导航设备接收机提供的星历数据、惯导设备计算得到的载体位置和速度,计算出卫星相应于载体的伪距RI和伪距率 R ˙ I,把RI和 R ˙ I与卫星导航设备测量的伪距RG和伪距率 R ˙ G相比较作为滤波器的量测值,即采用ISTPF滤波器,以公式(9)作为系统方程、公式(13)作为量测方程进行滤波计算,得出状态误差的估计值。
3)载体位置速度和姿态修正
将2)中滤波所得状态误差的估计值按照式(14)和式(15)来修正1)中SINS解算出的载体状态。
直接用滤波值修正载体位置和速度:
x y z V x V y V z= x y z V x V y V z- δ x δ y δ z δ V x δ V y δ V z
通过修正方向余弦矩阵修正载体的姿态:
C b e=RxRzRy C b e
其中,
Rx= 1 0 0 0 c o s δ φ x s i n δ φ x 0 - s i n δ φ x c o s δ φ xRy= c o s δ φ y 0 - s i n δ φ y 0 1 0 s i n δ φ y 0 c o s δ φ yRz= c o s δ φ z s i n δ φ z 0 - s i n δ φ z c o s δ φ z 0 0 0 1
通过上述步骤,则完成了紧组合导航一个周期的解算。

3 仿真及分析

滤波初值选取及相关参数设定:状态初值X=017×1,状态方差P中位置取10 m、速度取0.1 m/s、姿态取0.3°、陀螺漂移20°/h、加计零漂3 mg、接收机钟差0.5 m、接收机钟漂0.05 m/s,系统噪声方差Q中加计噪声3 mg、陀螺噪声2°/h、接收机钟差噪声0.5 m、接收机钟漂噪声0.05 m/s,量测噪声R中伪距测量噪声7 m、伪距率测量噪声1 m/s。ISTPF滤波中粒子数设定为N=1 000,利用计算机进行仿真,仿真结果如下。

3.1 不同滚转角速度下组合导航仿真

初始位置为纬度L=34.25°、经度λ=108.97°、高度h=100.44 m,初始速度为VN=7.5 m/s、VU=0 m/s、VE=12.990 4 m/s,初始姿态为偏航角ψ=60°、俯仰角θ=30°。即载体在初始位置处以15 m/s的速度及30°的俯仰角朝着北偏东60°方向做匀速直线运动,图2~4为滚转角速度ωroll=720°/s情况下载体位置、速度、姿态的误差(紧组合导航所得结果与给定真值之差),表1为不同滚转角速度下载体位置、速度、姿态误差的统计值。
图2 ωroll=720°/s情况下载体位置误差图
图3 ωroll=720°/s情况下载体速度误差图
图4 ωroll=720°/s情况下载体姿态误差图
表1 不同滚转角速度下9个参数误差的均值和方差
ωroll/(°/s) 参数
L/m λ/m h/m VN/(m/s) VU/(m/s) VE/(m/s) ψ/(°) θ/(°) γ/(°)
0 均值 0.097 0.162 0.088 0.001 0.049 0.003 0.046 0.002 0.002
方差 0.003 0.009 0.002 6×10-7 2×10-5 4×10-7 5×10-4 2×10-6 0
720 均值 0.096 0.162 0.088 0.004 0.049 0.002 0.026 0.001 0.001
方差 0.003 0.009 0.003 2×10-6 2×10-5 1×10-6 2×10-4 1×10-6 0
1 800 均值 0.097 0.162 0.088 0.004 0.046 0.002 0.024 0.002 0.002
方差 0.003 0.009 0.003 2×10-6 1×10-5 7×10-7 2×10-4 2×10-6 0

3.2 不同加速度下组合导航仿真

初始位置为纬度L=34.25°、经度λ=108.97°、高度h=100.44 m,初始速度为VN=0、VU=0、VE=0,初始姿态为偏航角ψ=60°、俯仰角θ=30°、滚转角γ=0°。即载体在初始位置处以不同的加速度及30°的仰角朝着北偏东60°方向由静止做匀加速直线运动,且偏航角和俯仰角保持不变,滚转角速度保持不变ωroll=720°/s,图5~7为加速度a=0.4 m/s2情况下载体的位置、速度、姿态误差,表2为不同加速度情况下载体位置、速度、姿态误差的统计值。
图5 a=0.4 m/s2情况下载体位置误差图
图6 a=0.4 m/s2情况下载体速度误差图
图7 a=0.4 m/s2情况下载体姿态误差图
表2 不同加速度情况下9个参数误差的均值和方差
a/(m/s) 参数
L/m λ/m h/m VN/(m/s) VU/(m/s) VE/(m/s) ψ/(°) θ/(°) γ/(°)
0.2 均值 0.097 0.162 0.088 0.004 0.049 0.001 0.022 0.003 0.002
方差 0.003 0.009 0.003 6×10-7 2×10-5 8×10-7 2×10-4 3×10-6 0
0.6 均值 0.097 0.162 0.088 0.005 0.050 0.002 0.022 0.006 0.013
方差 0.003 0.009 0.003 1×10-6 2×10-5 9×10-7 2×10-4 8×10-6 0
1.0 均值 0.096 0.163 0.102 0.003 0.019 0.002 0.020 0.011 0.034
方差 0.003 0.009 0.003 2×10-6 4×10-6 1×10-6 1×10-4 2×10-5 0
由图2~4及表1可以直观地看出,当载体做匀速运动时,在给定滚转角速度范围内,紧组合导航解算结果误差均较小,且不同滚转角速度对解算精度影响差别不大,位置误差不超过0.3 m,速度误差不超过0.05 m/s,姿态误差不超过0.05°;由图5~7及表2可以得出,当载体做加速运动时,在给定的加速度范围内,紧组合导航解算结果误差均较小,不同加速度对组合导航位置、速度及姿态的精度影响差别不大。位置误差不超过0.3 m,速度误差不超过0.02 m/s,姿态误差不超过0.05°。

4 结束语

本文将强跟踪滤波算法与粒子滤波算法相结合,采用通过强跟踪滤波产生修正项,并以此修正粒子滤波中粒子样本的思想,提出了改进强跟踪粒子滤波算法,提高了算法的精度,并将ISTPF滤波器应用于车辆紧组合导航中,得出较高的导航精度,且所得误差方差较小,即该算法具有较高的稳定性,能够满足导航定位定姿的要求,具有较强的实用性及优越性。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
李天成, 范红旗, 孙树栋. 粒子滤波理论、方法及其在多目标跟踪中的应用[J]. 自动化学报, 2015, 41(12):1981-2002.

[2]
B Jia, M Xin, Y Cheng. Sparse Gauss-Hermite Quadrature Filter with Application to Spacecraft Attitude Estimation[J]. Journal of Guidance Control and Dynamics, 2011, 34 (2):367-379.

DOI

[3]
M. M. Atia, J. Georgy, M. J. Korenberg, A. Noureldin. Real-Time Implementation of Mixture Particle Filter for 3D RISS/GPS Integrated Navigation Solution[J]. Electronic Letters, 2010, 46 (15):1083-1084.

DOI

[4]
B. N. M. Lask, M. Bolic, R.A. Goubran. Particle Filter Enhancement of Speech Spectral Amplitudes[J]. IEEE Transactions on Audio, Speech, and Language Processing, 2010, 18 (8):2155-2167.

DOI

[5]
Christophe Blanc, Laurent Trassoudaine, Jean Gallice. EKF and Particle Filter Track to Track Fusion: A Quantitative Comparison from Radar/Lidar Obstacle Tracks[J]. In Proceedings of 8th International Conference on Information Fusion, Philadelphia,PA, 2005, 7(2):1303-1310.

[6]
洪晋夫, 刘锦昌. 约束无迹粒子滤波及其在汽车导航中的应用[J]. 火力与指挥控制, 2019, 44(1):77-81.

[7]
Zhou Dong-Hua, Xi Yu-Geng, Zhang Zhong-Jun. A Suboptimal Multiple Fading Extended Kalman Filter[J]. Acta Automatica Sinica, 1991, 17(6): 689-695.

[8]
薛丽, 高社生, 赵岩. 权值自适应调整Unscented粒子滤波及其在组合导航中的应用[J]. 中国惯性技术学报, 2012, 20(4):459-463.

[9]
张红梅. 非线性滤波及其在姿态确定和导航中的应用[D]. 哈尔滨: 哈尔滨工业大学, 2004.

[10]
秦永元. 惯性导航[M]. 北京: 科学出版社, 2014.

Options
文章导航

/

〈 〉