CN109945895A - 基于渐消平滑变结构滤波的惯性导航初始对准方法 - Google Patents

Abstract

本发明提出了一种基于渐消平滑变结构滤波的惯性导航初始对准方法，具体步骤为：根据捷联惯性导航系统的速度误差方程、姿态误差方程，以速度误差为量测量，建立大方位失准角下的非线性连续系统模型；将非线性连续系统模型离散化，形成非线性离散系统模型；根据建立的非线性离散系统模型构建用于初始对准的渐消平滑变结构滤波模型，并求解滤波模型中的渐消因子；将惯性元件采集到的导航参数代入渐消平滑变结构滤波模型中，完成惯性导航的初始对准。本发明能提供高精度的初始姿态信息，提高了惯性导航初始对准的精度。

Description

基于渐消平滑变结构滤波的惯性导航初始对准方法

技术领域

本发明属于制导技术，具体为一种基于渐消平滑变结构滤波的惯性导航初始对准方法。

背景技术

近些年惯性导航的民用领域逐渐扩大，在民用飞机，船舶，汽车运输，以及农业播种，施肥等领域都发挥着重要作用。惯性导航系统采取推算式导航方式，即通过对惯性测量组件(Inertial Navigation System，IMU)测量的加速度、角速度信号的连续积分运算，获得运载器的位置、速度、姿态信息。因此在惯导系统开始工作之前，需要为惯导系统提供运载器初始位置、速度、姿态信息。获得准确导航的一个关键因素就是在惯导系统开始工作前对其进行初始对准，即向惯导系统提供准确的运载器位置、速度和姿态信息。因为任何初始信息中的误差将随着惯导系统工作时间增加产生输出误差。此外，由于捷联算法结构缘故，这些初始误差在很短的时间内将引起惯导系统输出误差的快速积累，最终有可能导致最终输出的位置、速度、姿态信息变得误差巨大。所以初始对准是惯性导航系统的关键技术之一，初始对准的精度直接关系到惯导系统的工作精度。

惯性导航系统主要分为平台式惯性导航系统(Gimbaled INS，GINS)和捷联式惯性导航系统(Strap-down INS，SINS)两种类型。SINS具有系统结构简单、可靠性高、体积小、重量轻、成本低、易维修等优点，已成为惯性技术中的一个主要发展方向，在许多应用领域中已经取代了GINS。捷联式惯性导航系统摒弃了稳定平台和常平架系统，而将陀螺仪和加速度计直接与载体固连的惯性导航系统。在捷联惯导系统中经常采用卡尔曼滤波(KalmanFilter，KF)技术，但是该技术要求能够提供系统精确的状态方程和量测方程，并且系统噪声和量测噪声是均值为零、协方差已知的高斯噪声，否则难以满足要求，甚至发散。针对此问题，不同的研究者提出了很多滤波算法如扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、正交卡尔曼滤波算法(Quadrature Kalman Filter，QKF)、容积卡尔曼滤波(Cubature Kalman Filter，CKF)，但仍存在诸多不足，如：模型误差作为过程噪声来处理，且假设为高斯白噪声，这与惯性导航系统的实际噪声情况并不相符，系统模型鲁棒性差。

发明内容

本发明的目的在于提出了一种基于渐消平滑变结构滤波的惯性导航初始对准方法，以解决现有捷联惯导系统滤波发散，稳定性差的问题。

实现本发明的技术解决方案为：一种基于渐消平滑变结构滤波的惯性导航初始对准方法，具体步骤为：

步骤1、根据捷联惯性导航系统的速度误差方程、姿态误差方程，以速度误差为量测量，建立大方位失准角下的非线性连续系统模型；

步骤2、将步骤1中的非线性连续系统模型离散化，形成非线性离散系统模型；

步骤3、根据步骤2中建立的非线性离散系统模型构建用于初始对准的渐消平滑变结构滤波模型，并求解滤波模型中的渐消因子；

步骤4、将惯性元件采集到的导航参数代入渐消平滑变结构滤波模型中，完成惯性导航的初始对准。

优选地，步骤3构建的用于初始对准的渐消平滑变结构滤波模型具体为：

渐消平滑变结构滤波的非线性状态方程和非线性量测方程为：

式中，f(·)为状态转移函数；h(·)为量测转移函数；x_k为系统状态；u_k是系统的相对输入；w_k是系统噪声；z_k+1是量测值；x_k+1为状态值，v_k+1是量测噪声；

先验状态估计方程和协方差方程分别为：

式中，为预测先验状态估计，P_k+1|k为先验状态估计协方差，为前一刻的状态估计，P_k|k为前一时刻的状态估计方差，Q_k为系统噪声协方差阵，λ_k为渐消因子，为状态矩阵；

先验量测估计方程和量测误差变量方程分别为：

式中，为量测估计方程，e_z,k+1|k为量测误差变量。

后验状态估计方程和增益方程分别为：

式中，为后验状态估计，K_K+1为增益，为状态矩阵；为量测矩阵；A_k+1＝|e_z,k+1|k|+γ|e_z,k|k|；e_z,k+1|k为先验量测误差变量；e_z,k+1|k+1为后验量测误差变量；e_z,k|k为量测误差变量；ψ为平滑有界层的宽度；γ为记忆或收敛速度因子，0<γ<1；

ψ_i为给定的噪声上限值；

新息方差方程以及平滑有界层矩阵方程分别为：

式中，S_k+1为新息方差，ψ_k+1为平滑有界层矩阵，R_k为量测噪声协方差阵；

后验状态估计方差方程、后验量测估计方程和后验量测误差变量方程分别为：

式中：P_k+1|k+1为后验状态估计方差，为后验量测估计。

本发明与现有技术相比，其显著优点为：

(1)本发明的渐消平滑变结构滤波模型在模型不确定、量测噪声非高斯白噪声的情形下，能够提供高精度的初始姿态信息，提高了惯性导航初始对准的精度；

(2)本发明构建的渐消平滑变结构滤波模型，具有较好的稳定性和鲁棒性；

(3)通本发明过引入渐消因子对协方差预报方程进行调整，能抑制滤波发散，滤波效果更佳。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1是基于渐消平滑变结构滤波的惯性导航初始对准方法的流程图。

具体实施方式

如图1所示，基于渐消平滑变结构滤波的惯性导航初始对准方法，具体步骤为：

步骤1、根据捷联惯性导航系统的速度误差方程、姿态角误差方程，以导航系速度为量测量，建立大方位失准角下的非线性连续系统模型，具体为：

以地理坐标系作为导航坐标系n，由惯性导航系统解算的位置建立的坐标系为计算坐标系g，以载体实际位置为载体坐标系b，姿态和速度误差误差分别为：

式中，φ是姿态误差矢量；δv是速度误差矢量；是导航坐标系到计算坐标系的转换矩阵；是载体坐标系到计算坐标系的方向余弦矩阵，每个元素表示为C’_ij(i＝1,2,3；j＝1,2,3)，是载体坐标系到导航坐标系的方向余弦矩阵，每个元素表示为C_ij i＝1,2,3；j＝1,2,3)，为加速度计的实际输出；δf^b为加速度计误差；为陀螺测量误差；为解算出的数学平台旋转角速度；为地球旋转角速度；为解算出的数学平台相对于地球的旋转角速度；为对应的的计算误差；δg为重力加速度的计算误差；[·]^T表示矩阵转置。

设静基座下的v＝0，忽略重力加速度的计算误差δg，则：加速度计误差δf^b主要来源于常值零偏和零均值白噪声陀螺量测误差主要来源于常值漂移ε^b和零均值白噪声加速度计输出在计算坐标系上的投影为其中f_x、f_y、f_z为投影在x、y、z轴的分量。

将上述速度误差方程、姿态误差方程详细展开组成初始对准非线性模型方程，具体形式如下：

式中，惯导系统的俯仰、横滚和航向的姿态误差角分别为下标N、E、U分别表示北向、东向和天向分量；L为载体所在的地理纬度；东向、北向的速度误差为δV_E、δV_N；系统噪声向量为其中为北向和东向的速度误差的系统噪声；为俯仰、横滚和航向的姿态误差角的系统噪声。取两个水平速度误差δV_E和δV_N为观测量。在系统精度和可观测性基础上，考虑和等不可观状态，对设计的滤波影响不大，因而可以忽略，因此构建惯性导航系统的非线性误差模型为：

式中，状态向量f(X)、G(X)分别具体为：

式中，Z＝[Z₁ Z₂]^T＝[δV_N δV_E]^T为观测矢量，H＝[I_2×2 O_2×3]为观测矩阵，v为观测方程的量测噪声。

步骤2、将步骤1中的非线性连续系统模型离散化，形成非线性离散系统模型，具体过程为：

步骤1中构建的非线性连续系统模型是非线性连续的，将非线性连续系统模型通过四阶龙格库塔方法进行离散化处理，即以采样周期T_f作为滤波周期，并以T_f为步长将非线性连续系统模型进行离散化后，非线性离散系统模型为：

式中，w_k和v_k分别表示k时刻离散的系统噪声和量测噪声，并且w_k和v_k满足：

E[·]表示均值，Cov[·]表示协方差，Q_k为离散系统噪声w_k的方差强度阵，R_k为离散量测噪声v_k的方差强度阵，具体为：R_k＝R/T_f，R为连续系统噪声v的方差强度阵；T_f为滤波周期；δ_kj为Kronecker-δ函数。

步骤3、根据步骤2中建立的非线性离散系统模型构建用于初始对准的渐消平滑变结构滤波模型，并求解滤波模型中的渐消因子，渐消平滑变结构滤波模型具体为：

渐消平滑变结构滤波的非线性状态方程和非线性量测方程为：

式中，f(·)为状态转移函数；h(·)为量测转移函数；x_k为系统状态；u_k是系统的相对输入；w_k是系统噪声；z_k+1是量测值；x_k+1为状态值v_k+1是量测噪声。

在忽略系统噪声的情况下，采用非线性状态方程预测先验状态估计和协方差方程为：

式中，预测先验状态估计，P_k+1|k为先验状态估计协方差，为前一刻的状态估计，P_k|k为前一时刻的状态估计方差，Q_k为系统噪声协方差阵，λ_k为渐消因子，为状态矩阵。

在忽略量测噪声的情况下，采用非线性量测方程预测先验量测估计，并计算量测误差变量：

式中，为量测估计方程，e_z,k+1|k为量测误差变量。

然后计算渐消平滑变结构滤波器的增益用来改善系统后验状态估计方程分别为：

式中，为后验状态估计

对于渐消平滑变结构滤波器来说，能够有效估计系统状态的前提是：|e_z,k+1|k+1|＜|e_z,k|k|即前一周期的误差大于后一周期的估计误差，系统误差是收敛的。基于上述前提，得到滤波器的增益方程为：

式中，K_K+1为增益，为状态矩阵，为量测矩阵，A_k+1＝|e_z,k+1|k|+γ|e_z,k|k|，e_z,k+1|k为先验量测误差变量，e_z,k+1|k+1为后验量测误差变量，e_z,k|k为量测误差变量，ψ为平滑有界层的宽度，γ为记忆或收敛速度因子，0<γ<1；

ψ_i为给定的噪声上限值；

然后，计算系统的新息方差以及平滑有界层矩阵方程分别为：

式中，S_k+1为新息方差，ψ_k+1为平滑有界层矩阵，R_k为量测噪声协方差阵。

最后计算后验状态估计方差、更新后的量测估计和其对应的后验量测误差变量方程分别为：

式中：P_k+1|k+1为后验状态估计方差，为后验量测估计。

求解滤波模型中的渐消因子：

渐消平滑变结构滤波器就是采用遗忘因子限制平滑变结构滤波器的记忆长度，充分利用现时的测量数据，加重现时数据在状态估计中的作用，从而避免了滤波发散。

由于渐消平滑变结构滤波在计算P_k+1|k时乘了渐消因子λ_k，因此需要对其求解，渐消因子λ_k是基于新息向量的统计特性来确定的，滤波器中每一步预的新息向量为：

V_k+1＝z_k+1-H_k+1x_k+1|k

在滤波最优的情况下，其新息序列协方差阵为：

新息序列自相关函数：

式中，E[·]是对[·]取均值符号；P_k+1|k为系统协方差；K_k+1为滤波增益；H_k+1为量测矩阵；

通过开窗法得到新息序列协方差阵的估计值为：

式中，V₀为k＝0时刻新息序列向量；λ_k-1为k-1时刻渐消因子；

如果残差自相关函数等于零，即残差序列不相关或者说残差序列保持相互正交，则K_k+1是最优的。实际情况是，由于模型误差，实际的残差方差阵C_o(k+1)与计算出的理论值不一样，因此，残差自相关函数不一定等于0。基于以上情况，可以实时地调整滤波增益阵K_k+1，强迫残差序列保持相互正交，即使C_o(k+1)等于零成立。根据上述分析，求解得到渐消因子λ_k：

式中，max{·}表示取{·}中最大的值；trace代表矩阵的迹

步骤4、将惯性元件采集到的导航参数代入渐消平滑变结构滤波模型中，使平台进入导航工作状态时的对准精度符合惯导要求，完成惯性导航的初始对准。

本发明中提出了一种新的基于渐消平滑变结构滤波模型，其基本思想是基于变结构和滑模控制概念，采用变结构的增益，使得预测状态趋近于系统真实轨迹，因此是一种“预测-校正”的估计器，其对于建模的不确定性和给定上限但无法建模的扰动噪声等引起的初始对准精度下降问题，具有较好的稳定性和鲁棒性。同时采用渐消因子限制平滑变结构滤波器的记忆长度，充分利用现时的测量数据，加重现时数据在状态估计中的作用，从而避免了滤波发散。

Claims (7)

1.一种基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，具体步骤为：

步骤1、根据捷联惯性导航系统的速度误差方程、姿态误差方程，以速度误差为量测量，建立大方位失准角下的非线性连续系统模型；

步骤2、将步骤1中的非线性连续系统模型离散化，形成非线性离散系统模型；

步骤3、根据步骤2中建立的非线性离散系统模型构建用于初始对准的渐消平滑变结构滤波模型，并求解滤波模型中的渐消因子；

步骤4、将惯性元件采集到的导航参数代入渐消平滑变结构滤波模型中，完成惯性导航的初始对准。

2.根据权利要求1所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，步骤1中速度误差方程、姿态误差方程为：

式中，φ是姿态误差矢量；δv是速度误差矢量；是导航坐标系到计算坐标系的转换矩阵是载体坐标系到计算坐标系的方向余弦矩阵，每个元素表示为C′_ij，i＝1,2,3；j＝1,2,3；是载体坐标系到导航坐标系的方向余弦矩阵，每个元素表示为C_ij，i＝1,2,3；j＝1,2,3；为加速度计的实际输出；δf^b为加速度计误差；为陀螺测量误差；为解算出的数学平台旋转角速度；为地球旋转角速度；为解算出的数学平台相对于地球的旋转角速度；为对应的计算误差；δg为重力加速度的计算误差；[·]^T表示矩阵转置。

3.根据权利要求1所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，步骤1中建立的大方位失准角下的非线性连续系统模型为：

式中，状态向量f(X)、G(X)分别具体为：

式中，Z＝[Z₁ Z₂]^T＝[δV_N δV_E]^T为观测矢量，L为载体所在的地理纬度，H＝[I_2×2 O_2×3]为观测矩阵，v为观测方程的量测噪声；分别为惯导系统俯仰、横滚和航向的姿态误差角，下标N、E、U分别表示北向、东向和天向分量；δV_E为东向速度误差，δV_N为北向速度误差，为系统噪声向量，为北向和东向的速度误差的系统噪声；为俯仰、横滚和航向的姿态误差角的系统噪声。

4.根据权利要求1所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，步骤2中形成的非线性离散系统模型为：

式中，w_k和v_k分别表示k时刻离散的系统噪声和量测噪声，并且w_k和v_k满足：

E[·]表示均值，Cov[·]表示协方差，Q_k为离散系统噪声w_k的方差强度阵，R_k为离散量测噪声v_k的方差强度阵，具体为：R_k＝R/T_f，R为连续系统噪声v的方差强度阵；T_f为滤波周期；δ_kj为Kronecker-δ函数。

5.根据权利要求1所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，步骤3构建的用于初始对准的渐消平滑变结构滤波模型具体为：

渐消平滑变结构滤波的非线性状态方程和非线性量测方程为：

式中，f(·)为状态转移函数；h(·)为量测转移函数；x_k为系统状态；u_k是系统的相对输入；w_k是系统噪声；z_k+1是量测值；x_k+1为状态值，v_k+1是量测噪声；

先验状态估计方程和协方差方程分别为：

式中，为预测先验状态估计，P_k+1|k为先验状态估计协方差，为前一刻的状态估计，P_k|k为前一时刻的状态估计方差，Q_k为系统噪声协方差阵，λ_k为渐消因子，为状态矩阵；

先验量测估计方程和量测误差变量方程分别为：

式中，为量测估计方程，e_z,k+1|k为量测误差变量。

后验状态估计方程和增益方程分别为：

式中，为后验状态估计，K_K+1为增益，为状态矩阵；为量测矩阵；A_k+1＝|e_z,k+1|k|+γ|e_z,k|k|；e_z,k+1|k为先验量测误差变量；e_z,k+1|k+1为后验量测误差变量；e_z,k|k为量测误差变量；ψ为平滑有界层的宽度；γ为记忆或收敛速度因子，0<γ<1；

ψ_i为给定的噪声上限值；

新息方差方程以及平滑有界层矩阵方程分别为：

式中，S_k+1为新息方差，ψ_k+1为平滑有界层矩阵，R_k为量测噪声协方差阵；

后验状态估计方差方程、后验量测估计方程和后验量测误差变量方程分别为：

式中：P_k+1|k+1为后验状态估计方差，为后验量测估计。

6.根据权利要求1所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，步骤3中渐消因子λ_k的计算公式为：

式中，max{·}表示取{·}中最大的值；trace代表矩阵的迹，为k+1时刻新息序列协方差阵的估计值，C₀(k+1)为k+1时刻新息序列协方差阵。

7.根据权利要求6所述的基于渐消平滑变结构滤波的惯性导航初始对准方法，其特征在于，新息序列协方差阵的估计值具体为：

V₀为k＝0时刻新息序列向量；λ_k-1为k-1时刻渐消因子；V_k+1为k+1时刻新息序列向量，k+1时刻新息序列向量具体为V_k+1＝z_k+1-H_k+1x_k+1|k。