一种组合导航的直接法滤波算法

闻 新，纪 龙，赵建新

（南京航空航天大学 航天学院，南京 210016）

1 引言

组合导航系统以惯性导航系统 （inertial navigation system，INS）辅助全球定位系统 （global positioning system，GPS）接收机增强系统抗干扰能力，保证组合后的导航精度高于各系统单独工作的精度［1-2］。组合系统直接法滤波的系统状态方程为非线性方程［2］，这给滤波器设计带来一定困难。针对该问题，部分学者在工作点处作线性化后采用扩展卡尔曼滤波 （extended Kalman filter，EKF）进行状态估计，但当INS精度较低或载体处于高动态环境时，EKF算法参数估计精度不高甚至会发生滤波发散；另有学者在假设系统噪声为白噪声前提下，采用无迹卡尔曼滤波 （unscented Kalman particle filter，UPF）直接对非线性系统方程进行滤波估计，但这一方法仅在系统噪声为白噪声时成立。

粒子滤波 （particle filter，PF）在处理非线性、非高斯系统的参数估计问题上具有独特优势，对组合导航这类复杂问题的求解可获得较高精度［3-5］。本文提出了基于 UPF的INS/GPS组合导航系统直接法滤波状态估计，以INS的导航参数及平台误差角作为系统状态，惯导力学编排方程和姿态误差方程作为状态方程，GPS的输出作为观测值，采用UPF直接进行组合导航系统参数解算。基于这一方案的系统状态方程包含9个参数，减少了陀螺仪及加速度计漂移等状态变量，降低了状态变量维数易于粒子滤波实现。

2 INS/GPS组合导航系统滤波结构

本文INS/GPS组合导航系统基于闭环组合方式采用直接法滤波，INS和GPS分别对载体运动参数进行测量，将INS和GPS测得的参数分别送入UKF滤波器进行集中解算，从而获得系统状态的最优估计并作为组合导航系统的输出，滤波输出对INS系统进行状态反馈修正。这种组合系统还能够实现GPS信号无效时的容错性，其结构如图1所示。

图1 INS/GPS组合导航系统的直接法UPF滤波结构图

3 基于UPF的INS/GPS组合导航系统直接法滤波算法

导航坐标系选为东北天地理坐标系，则惯导系统的力学编排方程和姿态误差方程如式 （1）和式 （2）所示。

式中，vE、vN、vU为东、北、天向速度，L、λ、h为经度、纬度和高度，φE、φN和φU为东、北、天向姿态误差角，系统以这9个参数作为状态值。其中，fE、fU、fN为加速度计输出的东北天向比力观测值，δvE、δvN、δh和δL在实际计算中可以由INS和GPS的输出差值近似获得，εbE、εbN和εbU为陀螺的东、北、天向常值漂移。

3.1 系统状态方程

为简化系统模型并提高滤波计算效率，本文仅对东、北、天向位置、速度和姿态误差建模，不对加速度计和陀螺进行误差建模，则INS/GPS组合导航系统状态向量为

根据选取的系统状态向量x，结合式 （1）、式（2）和式 （3），可列写出INS/GPS组合导航系统的状态方程为

式 中，f［·］为 非 线 性 连 续 函 数，W（t）＝［wgE，wgN，wgU］T为系统噪声。

3.2 观测方程

以GPS输出的速度信息vEGPS、vNGPS、vUGPS和位置信息LGPS、λGPS、hGPS为观测值Z，即

据前面所选取的系统状态x和观测值Z，可获得组合导航系统的观测方程

式中，Zk和xk为Z和x在k时刻的值，H（·）为线性函数，Vk＝ ［VVE，VVN，VVU，VL，Vλ，Vh］T为观测噪声，强度为Rk。

3.3 UPF滤波算法解算过程

基于个人计算机的解算过程要采用离散的系统方程，而INS/GPS组合导航系统状态方程为微分方程。因此，需要对式 （4）进行离散化。离散化后的系统状态方程

式中，F（·）为非线性离散函数，Wk为系统噪声，噪声方差强度为Qk。其中

为减小系统离散化误差，仿真过程中取前三项以减小离散化误差。并且有

式中，Δt为滤波更新周期，＝Q。状态方程和观测噪声如下

利用上面推导出的INS/GPS组合导航系统离散方程，结合UKF滤波法的递推更新过程，得出基于UPF算法的组合导航系统滤波过程如下

1）初始化：k＝0

其中，＝x（t）t＝k。

2）重要性密度函数，k＝1，2，…，T，使用UKF更新粒子

其中na＝nx＋nw＋nv为扩展后系统状态的维数，且，通过该式对状态向量进行扩展。

（2）对每个采样点，i＝1，2，…，N，应用UKF算法得到，，从而得到简易分布函数

（3）从建议分布中抽取粒子：

（4）计算粒子权值并归一化

3）重采样

对 ｛，｝ ，i＝1，2，…，N重采样，得到新的粒子集合 ｛，｝ ，权值＝1/N，i＝1，2，…，N。

4）结果输出

根据组合导航系统的离散状态方程式 （7）和观测方程式 （6），只须设定滤波初值，就可获得第k时刻的系统状态最优估计值xk，k＝1，2，3…。

4 仿真分析

本文主要研究INS/GPS组合导航数据融合算法的精度，仿真期间保证系统在组合模式下工作。INS/GPS组合导航系统的初始参数和仿真参数如表1和表2所示。仿真选取典型的飞机机动轨迹，该轨迹包含滑跑、加速拉起、爬高、改平、平飞、预转弯、转弯、减速飞行、低头、俯冲等机动动作，仿真时长为3 600s，其轨迹如图2所示。

表1 INS/GPS组合导航系统初始参数设置

表2 INS/GPS组合导航系统仿真参数设置

图2 飞行轨迹仿真

在上述条件下，分别对INS/GPS组合导航系统的直接法UPF和UKF进行了仿真研究，并对仿真结果进行比较分析。为保证组合导航系统直接法滤波的精度并控制滤波计算量，滤波粒子数N取为1 000，并取α＝1，β＝0，k＝2。由于篇幅有限，且东向和北向导航结果具有相似性，本文仅对东向 （经度）、天向 （高度）的位置和速度误差进行数值计算分析，其仿真结果如图3、图4所示。

从图3和图4可以看出，应用UKF组合导航系统经度误差可控制在±13m以内，高度误差在±15m以内，速度误差在±0.6m/s以内。而应用UPF的组合导航系统经纬度误差控制在±6m以内，高度误差在±10m以内，速度误差在±0.5 m/s以内。通过上述两种滤波计算结果的比较，说明了基于UPF滤波的直接法INS/GPS组合导航系统可以获得更高的导航精度，并能更准确地反映真实状态的演变情况。

图3 基于UPF和UKF算法的经度和高度位置误差

图4 基于UPF和UKF算法的东向和天向速度误差

5 结论

直接法UPF滤波在组合导航系统状态结算过程中不含截断误差，滤波过程避免了惯导力学编排方程的许多重复计算，降低了算法冗余度。同时，闭环结构的滤波方法在GPS信号突然失效时系统可实现有效切换。仿真结果表明UPF算法与UKF算法相比，具有更高的估计效率和估计精度，解算过程具有较强的鲁棒性。

［1］全伟，刘百奇，宫晓琳.惯性/天文/卫星组合导航技术［M］.北京：国防工业出版社，2011.

［2］刘建业，曾庆化，赵伟.导航系统理论与应用［M］.西安：西北工业大学出版社，2010.

［3］秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理［M］.西安：西北工业大学出版社，1998.

［4］程水英，张剑云.粒子滤波评述［J］.宇航学报，2008，29（4）：1099－1112.

［5］赵琳，王小旭，丁继成.组合导航系统非线性滤波算法综述［J］.中国惯性技术学报，2009，17（1）：46－54.

［6］杨波，秦永元，柴艳.UKF在INS/GPS直接法卡尔曼滤波中的应用［J］.传感技术学报，2007，20（4）：842－846.

［7］汪秋婷，胡修林.基于UKF的新型北斗/SINS组合系统直接法卡尔曼滤波［J］.系统工程与电子技术，2010，32（2）：376－379.

［8］BENGTSSON T，SNYDER C，NYCHKA D.Toward a Nonlinear Ensemble Filter for High Dimensional Systems［J］.Journal of Geophysical Research，2003，108（D24）：8775－8785.

［9］VAN DER MERWE R，DOUCET A，DE FREITAS N，etal.The Unscented Particle Filter［EB/OL］.［2013－02－16］.http：//automatica.dei.unipd.it/tl＿files/utenti/lucaschenato/Classes/PSC10 ＿11/UPF ＿slides.pdf.

［10］VAN DER MERWE R，DOUCET A，DE FREITAS N，eta1.The Unscented Particle Filter Technical Report［R］.Cambridge：Department of Engineering，University of Cambridge，2000.