一种简化的发射系下SINS/GPS/CNS组合导航系统无迹卡尔曼滤波算法

潘加亮，熊智，王丽娜，郁丰，赵慧，林爱军

(1.南京航空航天大学 自动化学院，江苏 南京210016;2.北京航天自动控制研究所，北京100854;3.南京航空航天大学 航天学院，江苏 南京210016)

0 引言

构建发射惯性系下的捷联惯导系统/导航星全球定位系统/天文寻航系统(SINS/GPS/CNS)多信息组合导航系统是实现未来高超、空天高动态飞行器高精度导航的有效手段之一［1-2］，而发射惯性系下组合导航系统的核心技术是多信息融合处理方法。针对未来高超、空天飞行器飞行轨迹高动态性和导航系统状态强非线性等特点，传统的扩展卡尔曼滤波(EKF)算法因系统模型线性化展开的要求而会影响估计的精度［3］。为此，Julier 等提出的无损卡尔曼滤波(UKF)方法可以有效解决模型线性化展开所带来的模型不准确这一问题［4-9］，其核心思想是通过利用一系列近似高斯分布的采样点，通过无迹变换(UT)进行状态和误差协方差阵的递推和更新，不需要对系统非线性方程进行线性化，因而可以有效避免引入线性化误差，提高导航精度。

但对于高阶扩维的多组合导航系统，由于组合系统状态维数过高，传统的UKF 算法存在大量采样粒子计算过程，递推滤波计算步骤繁琐、运算复杂等问题，影响系统的实时性，使其不便于工程实现。为此，针对发射惯性系下SINS/GPS/CNS 组合导航系统状态模型非线性而观测模型线性的特点，本文设计了一种简化UKF 算法(SUKF)直接进行导航参数的估计，在算法模型等效原则下可以简化计算步骤，降低计算的复杂度，提高算法的实时性，同时SUKF算法由于继承了UKF 算法无需状态模型一阶线性化展开的优点，能获得比EKF 算法更高精度的导航结果，十分适合算法的工程应用。

1 发射系下组合导航系统SUKF 算法

1.1 组合导航系统数学模型

本文选取发射惯性系作为导航坐标系，SINS/GPS/CNS 组合导航系统相关解算方程如下:

式中:q 为姿态四元数(包括一维标量部分q0和三维矢量部分q1、q2、q3);p 为发射惯性系下的位置值;v 为发射惯性系下的速度值;g 为地球的万有引力;为发射惯性系相对于载体系的姿态矩阵;为发射惯性系相对于地心惯性系的姿态矩阵;fc为加速度实际测量值;fr为加速度计随机游走误差;ωc为角速度实际测量值;ωr为陀螺随机游走误差;ωε为陀螺测量噪声;ωn、fn分别为陀螺和加速度计随机游走驱动噪声。

考虑到姿态的变化主要反映在四元数的矢量部分，同时为了减少滤波器的计算量，本文选取方程(1)式中的姿态四元数三维矢量部分(q1、q2、q3)、三维位置矢量p、三维速度矢量v、三维陀螺随机游走误差ωr、三维加速度计随机游走误差fr作为状态量构建滤波器状态方程，状态量定义:X =［q1q2q3pxpypzvxvyvzωrxωryωrzfrxfryfrz］T，系统白噪声矢量为

W=［ωεxωεyωεzωnxωnyωnzfnxfnyfnz］T.

选取GPS 和星敏感器分别输出的位置信息和姿态信息(矢量部分)作为观测量，则有观测方程:

式中:qc，13为姿态观测量的矢量部分;qI，13为惯导系统姿态参数的矢量部分;qε，13为姿态量测噪声;pc为位置观测量;pI为惯导系统的位置参数;pε为位置量测噪声;H=［I6×606×9］;V 为量测噪声阵。

由上述分析，可以获得发射系下组合导航系统的状态方程和量测方程一般形式表示为

式中:状态量X 维数为15 维;系统噪声向量W 为9 维;系统量测噪声向量V 为6 维。

1.2 SUKF 算法实现方案

发射惯性系下的SINS/GPS/CNS 组合导航系统状态方程具有非线性，基于UT 的UKF 滤波算法是处理非线性问题的一种有效方法［10-14］，传统UKF算法需要将系统噪声和量测噪声都增广为系统状态，不失一般性，假设系统状态维数为n 维，系统状态噪声阵为w 维，系统量测噪声阵为v 维，则增广后的系统状态维数将由原来的n 维扩展为n +w +v维，扩维后的系统状态向量表示如下:

在我国计划生育的变迁过程中，政策专家发挥了不可替代的作用。传统媒体报道进一步放大了某些代表学者的声音，甚至使之成为一类政策主张的“符号”；新媒体则赋予了更多专家以话语权，进一步拓展了他们的话语空间，使得专家学者共同体的话语影响力得到强化。

式中:Xa表示扩维后的系统状态向量;χa为扩维后的状态对应的采样点向量;^xa0表示扩维后系统状态初始均值;Pa0为增广后的状态向量初始协方差阵;P0为原状态向量初始协方差阵;Q 为系统噪声阵;R为量测噪声阵。

从扩维后的系统状态向量可以看出，如果系统自身的状态噪声和量测噪声维数较高，则通过相应的状态量增广处理后，进一步增大了状态维数，从而较大增加了滤波算法运算量，如以本文研究的发射惯性系组合导航系统数学模型为例，按照传统UKF算法状态扩维后，其状态维数将由15 维增加到30 维，从而极大增加了算法的计算量。

为此，在系统状态自身维数较高的情况下，为减少UKF 算法计算量，结合发射系下组合导航系统量测方程线性的特点，本文设计了一种降维的SUKF实现算法，主要对传统UKF 算法中^zk/k-1、Pzz和Pxz的计算方法进行简化，具体简化部分对应的简化原理如下:

1.2.1 初始化

基于发射惯性系下系统状态方程非线性而量测方程线性这一特点，将系统噪声增广为状态向量，而不把量测噪声增广为状态向量，这样可以有效降低扩维后系统状态的维数，设简化后的UKF 算法的增广状态向量记为Xa，相应的采样点向量记为χa，则可得

1.2.2 系统量测方程

由传统的UKF 算法公式，则有

基于发射惯性系下SINS/GPS/CNS 组合导航系统线性量测方程，将(4)式代入(5)式、(6)式得

1.2.3 系统估计误差方差阵

基于传统UKF 算法模型有

式中:R 表示系统量测噪声阵;PA1k/k-1表示由Pk/k-1的前六行与前六列构成的分块矩阵;表示由Pk/k-1的前十五行与前六列构成的分块矩阵，

根据上述分析，传统UKF 算法中的计算式(6)式、(9)式和(10)式可分别由(8)式、(11)式和(12)式代替，简化了矩阵运算维数，从而大大降低了计算的复杂度。由此可以得到改进后的SUKF 算法基本计算流程如图1所示，其中n 为增广后的系统状态向量维数，λ =α2(n +k)-n，α 决定采样点距均值的远近程度，通常被赋一个较小的正值，本文取α2=0.002，k≥0 保证方差阵的半正定性，本文取0，β 用于包含状态量分布的高阶成分信息，本文取2.

图1 SUKF 算法计算流程Fig.1 Calculation procedure of simplified unscented Kalman filter algorithm

2 仿真实验与分析

为有效验证本文算法的性能，在MATLAB 2009Ra 环境下，对分别采用UKF、SUKF 和EKF 滤波方法的发射惯性系下的SINS/GPS/CNS 组合导航系统性能进行对比分析，同时还将UKF 与SUKF 的计算量进行了对比分析。

2.1 仿真条件

导弹发射初始经度、纬度、高度为:118°、32°、0 m，初始航向角为90°，发射时间为2014年6月1日0 时0 分0 秒，飞行时间为600 s;捷联解算周期为0.02 s，滤波周期为1 s;仿真中设置捷联惯性传感器仿真参数为:陀螺随机游走驱动噪声0.2°/h，陀螺白噪声0.2°/h，加速度计随机游走驱动噪声0.000 1 g，加速度计白噪声0.000 1 g;卫星接收机和星敏感器仿真参数为:卫星导航位置误差15 m，星光跟踪仪误差21″.

2.2 仿真分析

2.2.1 标准弹道飞行航迹

为有效验证本文算法性能，设计了一条标准导弹飞行轨迹如图2所示。仿真误差曲线如下所示，图3为采用UKF、SUKF 与EKF 算法求解得到的姿态误差曲线对比图，图4为采用UKF、SUKF 与EKF算法求解得到的位置误差曲线对比图，图5为采用UKF、SUKF 与EKF 算法求解得到的速度误差曲线对比图。并根据相应的仿真数据，可以获得各类导航参数的均方误差RMS 统计结果如表1所示。

图2 导弹航迹图Fig.2 Missile track

图3 姿态误差曲线对比图Fig.3 Attitude error curves

同时估计误差协方差阵曲线如图6所示，由于姿态、位置和速度的三轴误差参数规律大体一致，在此给出滚动角误差、Y 轴位置误差和Z 轴速度误差对应的估计误差协方差参数。从图6中可以看出，达到稳态时，采用UKF、SUKF 与EKF 算法求解得到的滚动角估计误差协方差参数为2.362 rad，2.362 rad，3.774 rad;采用UKF、SUKF 与EKF 算法求解得到的Y 轴位置估计误差协方差参数为3.038 m，3.038 m，4.166 m;采用UKF、SUKF 与EKF 算法求解得到的Z 轴速度估计误差协方差参数为0.078 m/s，0.078 m/s，0.206 m/s.

从以上误差曲线对比图、误差RMS 统计表以及估计误差协方差参数可以看出，SUKF 继承了UKF 精度高这一优点，其滤波精度较EKF 有显著的提升。

图4 位置误差曲线对比图Fig.4 Position error curves

图5 速度误差曲线对比图Fig.5 Velocity error curves

对SUKF 和UKF 算法的计算量进行对比分析，如表2所示。

从表2中还可以看出在同样的仿真计算条件下，SUKF 算法的计算时间大约比传统UKF 算法减少了18%，有效降低了计算的复杂度。

表1 组合导航系统UKF、SUKF 与EKF 算法误差对比表Tab.1 UKF，SUKF and EKF RMS errors of integrated navigation system

表2 算法计算量分析(低动态)Tab.2 Calculated amount (low dynamic range)

图6 估计误差协方差参数(低动态)Fig.6 Estimation error covariance matrix parameters(low dynamic range)

2.2.2 高动态飞行航迹

为有效验证本文算法的性能，还设计了一条高动态环境下导弹飞行轨迹如图7所示，包含了高速姿态机动等过程。仿真误差曲线如下所示，图8为采用UKF、SUKF 与EKF 算法求解得到的姿态误差曲线对比图，图9为采用UKF、SUKF 与EKF 算法求解得到的位置误差曲线对比图，图10 为采用UKF、SUKF 与EKF 算法求解得到的速度误差曲线对比图。并根据相应的仿真数据，可以获得各类导航参数的均方误差RMS 统计结果，如表3所示。

图7 导弹航迹图(高动态)Fig.7 Missile track(high dynamic range)

图8 姿态误差曲线对比图(高动态)Fig.8 Attitude error curves(high dynamic range)

估计误差协方差阵曲线如图11 所示，从图11中可以看出，达到稳态时，采用UKF、SUKF 与EKF算法求解得到的滚动角估计误差协方差参数为2.362 rad，2.362 rad，3.773 rad;采用UKF、SUKF 与EKF 算法求解得到的Y 轴位置估计误差协方差参数为3.028 m，3.028 m，4.158 m;采用UKF、SUKF 与EKF 算法求解得到的Z 轴速度估计误差协方差参数为0.079 m/s，0.079 m/s，0.206 m/s.

表3 组合导航系统UKF、SUKF 与EKF 算法误差对比表(高动态)Tab.3 UKF，SUKF and EKF RMS errors of integrated navigation system

图9 位置误差曲线对比图(高动态)Fig.9 Position error curves(high dynamic range)

从以上误差曲线对比图、误差RMS 统计表以及估计误差协方差参数可以看出，在高动态环境下，SUKF 依然具备UKF 精度高这一优点，其滤波精度较EKF 有显著的提升。

对SUKF 和UKF 算法的计算量进行对比分析如表4所示。

从表4中还可以看出，在高动态的仿真计算条件下，SUKF 算法的计算时间大约比传统UKF 算法减少了17.7%，有效降低了计算的复杂度。

图10 速度误差曲线对比图(高动态)Fig.10 Velocity error curves(high dynamic range)

表4 算法计算量分析(高动态)Tab.4 Calculated amount (high dynamic range)

3 结论

弹载等高动态环境下组合导航系统状态方程具有强非线性，且各状态相互耦合影响，传统的EKF算法由于需要对模型进行线性化展开从而影响导航系统精度，而传统的UKF 算法又因组合导航系统维数过大而存在滤波过程计算复杂和计算量大的不足，不利于工程实现。为此，本文针对发射惯性系下SINS/GPS/CNS 组合导航系统状态模型非线性而观测模型线性的特点，设计了一种SUKF 算法对组合导航系统状态参数直接进行估计，从而简化了UKF算法计算步骤，降低了计算的复杂度。算法仿真结果表明SUKF 算法具有比EKF 算法更高的导航精度，同时也有效减少了传统UKF 算法的计算量，从而为解决高动态环境下非线性滤波估计问题提供了一种有效的方法。

图11 估计误差协方差参数(高动态)Fig.11 Estimation error covariance matrix parameters(high dynamic range)

References)

［1］Wang R，Xiong Z，Liu J，et al. SINS/GPS/CNS information fusion system based on improved Huber filter with classified adaptive factors for high-speed UAVs［C］∥Proceeding of the 2012 IEEE/ION Position，Location and Navigation Symposium (PLANS).Myrtle Beach，South Cardina:IEEE，2012:441 -446.

［2］Hu H D，Huang X L.SINS/CNS/GPS integrated navigation algorithm based on UKF［J］. Journal of Systems Engineering and Electronics，2010，21(1):102 -109.

［3］江晓东，谢京稳，郭军海. 基于UKF 的再入弹道高精度估计方法研究［J］. 航天控制，2011，29(3):28 -32.JIANG Xiao-dong，XIE Jing-wen，GUO Jun-hai.The precision estimation method of reentry target trajectory based on the UKF［J］.Aerospace Control，2011，29(3):28 -32.(in Chinese)

［4］Wang Q T，Xiao D. The research and application of robust UKF algorithm for GPS/SINS integrated system［J］. Journal of Convergence Information Technology，2011，6(6):202 -211.

［5］Majeed M，Kar I N. Aerodynamic parameter estimation using adaptive unscented Kalman filter［J］. Aircraft Engineering and Aerospace Technology，2013，85(4):267 -279.

［6］罗楠，许录平，张华. 基于UKF 和信息融合的航天器自主导航方法［J］. 中国空间科学技术，2012，32(2):1 -9.LUO Nan，XU Lu-ping，ZHANG Hua. Method of autonomous celestial navigation based on UKF and information fusion［J］. Chinese Space Science and Technology，2012，32(2):1 -9.(in Chinese)

［7］Zhang H T. Unscented Kalman filter and its nonlinear application for tracking a moving target［J］. Optik-International Journal for Light and Electron Optics，2013，13(3):4468 -4471.

［8］Kolås S，Foss B A，Schei T S. Constrained nonlinear state estimation based on the UKF approach［J］. Computers and Chemical Engineering，2009，33(8):1386 -1401.

［9］周丕森，鲍其莲. 组合导航系统UKF 滤波算法设计［J］. 上海交通大学学报，2009，43(3):389 -392.ZHOU Pei-sen，BAO Qi-lian. Design of filter in micro-integrated navigation system［J］. Journal of Shanghai Jiaotong University，2009，43(3):389 -392.(in Chinese)

［10］Luo Z，Fang H J. Modified state prediction algorithm based on UKF［J］. Journal of Systems Engineering and Electronics，2013，24(1):135 -140.

［11］Liu J，Ma J. Pulsar/CNS integrated navigation based on federated UKF［J］. Journal of Systems Engineering and Electronics，2010，21(4):675 -681.

［12］Ge Z X，Yang Y M，Zheng H. A new UKF based fault detection method in non-linear systems［J］. International Journal of Plant Engineering and Management，2006，11(3):179 -183.

［13］Rhudy M，Gu Y，Jason V，et al. Evaluation of matrix square root operations for UKF within a UAV GPS/INS sensor fusion application［J］. International Journal of Navigation and Observation，2011:1 -11.

［14］Nowak T，Eidloth A. Dynamic multipath mitigation applying unscented Kalman filters in local positioning systems［J］. International Journal of Microwave and Wireless Technologies，2011，3(3):365 -372.