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

王 聘，田 敬，吴 鹏，王洋洋

（1.海军装备部驻连云港地区军事代表室，江苏 连云港 222061；2.江苏自动化研究所，江苏 连云港 222061）

粒子滤波算法是解决非线性／非高斯动态系统状态估计问题最主要的方法，并成功应用于多目标跟踪［1］、故障诊断、飞行器姿态估计［2］、机器人导航［3］以及信号处理［4］等众多领域。但是，粒子退化问题，以及由其所导致的对突变状态的追踪失效是目前粒子滤波所面临的主要问题，为此，学者们进行了大量研究并提出了多种改进方法：EPF［5］、UPF［6］等，虽然这些算法能够在一定程度上解决粒子退化的问题，但对于带有突变状态的系统，还是会产生较大的误差。针对带有突变状态的系统，周东华提出了强跟踪滤波STPF算法［7］，在卡尔曼滤波框架下引入次优渐消因子，提高了处理突变系统时的滤波精度，但仍不能够较好地处理似然函数位于状态空间尾部的情况。

针对上述问题，本文提出了改进强跟踪粒子滤波算法ISTPF，该算法将强跟踪算法与粒子滤波算法相结合，实现高效采样和强跟踪功能。该算法既具有较强处理突变系统的能力，又能够很好地解决似然函数位于状态空间尾部的问题，实现了高效采样粒子滤波。最后将ISTPF算法应用到紧组合导航模型中，仿真结果证实ISTPF算法能够得出更精确的导航结果，具有一定的可行性及优越性。

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

改进强跟踪粒子滤波算法通过运行一次强跟踪滤波，从而得出一个修正项并用于修正粒子滤波中粒子样本，将粒子推向高似然区域［8］，即得出一个新的重要性概率密度函数，然后从中抽样，该过程将当前的量测信息考虑了进去，故所抽取的粒子样本更接近后验分布，最终的计算精度得到提高。

考虑如下的非线性系统：

其中xt和zt分别为状态向量和量测向量，f（·）和h（·）是已知的连续非线性函数，vt－1和wt分别为统计特性已知的系统噪声和量测噪声，滤波的最终目的就是根据所有量测值z＝{z1z2…zt}递推求出当前时刻状态值xt。

进而对粒子进行加权求得状态估计值

由此可见，重要性概率密度函数至关重要，它代表了粒子分布，同时影响着各个粒子的权重。在改进强跟踪粒子滤波中，对重要性概率密度函数进行改进，通过运行一步强跟踪EKF，获得一个修正项

其中，Kt为t时刻滤波增益，γt为t时刻新息。此修正项考虑了量测信息，在减弱老数据对当前滤波值影响的基础上，使得粒子滤波在抽样阶段便权衡了量测信息，所抽取粒子更加接近真实值。

求出修正项Ct后，通过更新系统方程（1）便可得到修正后的状态转移方程：

因此，将重要性概率密度函数定义为pv（xtf（xt－1）－Ct），从中抽取粒子并计算出其权重，得出状态估计值。经此重要性概率密度函数所抽取粒子能够更好地遍布整个函数，且更多地集中在高似然区域，有效地描述了系统函数，同时高似然区域粒子所占权重更大，最终经这些高质量粒子加权所得状态估计值便更为准确。

由上可见，给定状态初始值x1和状态方差初始值P1，便可递推求出相应时刻状态估值＾xt。

2 紧组合导航模型

紧组合模型是将卫星导航设备接收机的伪距测量值和伪距率测量值，与利用惯性导航输出计算出的相应伪距、伪距率的值进行比较，得到的差值形成滤波器（ISTPF）的测量输入值，经组合导航滤波器，生成系统的误差估计值，这种估计值可在每次测量更新后对惯导系统进行修正，以提高惯性导航的精度。紧组合导航具体过程如图1所示。

图1 紧组合模型示意图

1）求解载体位置速度和姿态。

根据惯性器件（陀螺和加计）测得的数据按照导航方程（7）解算出载体的速度，进而得出载体的位置；按照姿态更新方程（8）解算出载体的姿态［9-10］。

地固系下的导航方程：

姿态更新的四元数算法

2）求解位置速度和姿态修正量

根据导航方程可以推导出的地固系下误差状态方程（将陀螺零偏、加计零偏、接收机钟差、钟差漂移误差作为待估计值）为式（9）

接收机钟差和钟差漂移误差采用以下模型进行描述：

结合卫星导航设备接收机测得的伪距、伪距率和惯导设备解算出的载体位置速度，以及星历信息（卫星位置和速度），可得滤波的量测方程为式（13）

其中，Ri、d Ri分别为卫星导航设备所得第i颗卫星的伪距和伪距率，(xIyIzI)、 (VxIVyIVzI)分别为惯导设备解算所得载体位置和速率， (xis yis zis)、(VisxVisyVisz)分别为第i颗卫星的位置和速率。

用卫星导航设备接收机提供的星历数据、惯导设备计算得到的载体位置和速度，计算出卫星相应于载体的伪距RI和伪距率˙RI，把RI和˙RI与卫星导航设备测量的伪距RG和伪距率˙RG相比较作为滤波器的量测值，即采用ISTPF滤波器，以公式（9）作为系统方程、公式（13）作为量测方程进行滤波计算，得出状态误差的估计值。

3）载体位置速度和姿态修正

将2）中滤波所得状态误差的估计值按照式（14）和式（15）来修正1）中SINS解算出的载体状态。

直接用滤波值修正载体位置和速度：

通过修正方向余弦矩阵修正载体的姿态：

其中，

通过上述步骤，则完成了紧组合导航一个周期的解算。

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为不同滚转角速度下载体位置、速度、姿态误差的统计值。

表1 不同滚转角速度下9个参数误差的均值和方差

图2 ωroll＝720°／s情况下载体位置误差图

图3 ωroll＝720°／s情况下载体速度误差图

图4 ωroll＝720°／s情况下载体姿态误差图

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为不同加速度情况下载体位置、速度、姿态误差的统计值。

图6 a＝0.4 m／s2情况下载体速度误差图

图7 a＝0.4 m／s2情况下载体姿态误差图

表2 不同加速度情况下9个参数误差的均值和方差

由图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滤波器应用于车辆紧组合导航中，得出较高的导航精度，且所得误差方差较小，即该算法具有较高的稳定性，能够满足导航定位定姿的要求，具有较强的实用性及优越性。