CN105737828B - 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 - Google Patents

Abstract

本发明公开了一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，步骤如下：建立飞行器导航系统的状态方程；建立SINS/GNSS组合导航系统的动力学方程；建立SINS/GNSS组合导航系统的量测方程；离散化SINS/GNSS组合导航的动力学方程和量测方程；对非线性离散后的动力学方程进行线性化；对线性化的动力学方程采用基于强跟踪的相关熵扩展卡尔曼滤波算法，输出SINS/GNSS组合导航的信息。本发明将强跟踪理论中的渐消因子引入到相关熵扩展卡尔曼滤波算法中，有效地解决了非线性SINS/GNSS组合导航系统的模型不确定性和噪声统计特性的非高斯问题，从而提高了导航精度，增强了导航过程的稳定性。

Description

一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法

技术领域

本发明属于组合导航的技术领域，具体涉及一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，可用于提高飞行器、导弹、舰船、地面车辆的SINS/GNSS组合导航系统的精度。

背景技术

高精度的组合导航一直以来是导航领域关注的关键问题。为了获得更高的导航精度，学者们和工业界人士致力于寻找具有鲁棒性和高精度的滤波方法，用于对载体的位置、速度、姿态等状态进行估计。

捷联惯性导航系统(Serial inertial navigation system，SINS)主要基于加速度二次积分的推算过程，自主性好、导航信息量大，但长时间工作导航精度变差。全球导航卫星系统(Global Navigation Satellite System，GNSS)是以导航卫星为基础的无线电导航系统，具有较高的定位精度和测速精度，但是机载GNSS接收机接收信号时受载体飞行姿态的影响，地面GNSS接收机受所在地域的影响。这两种导航系统单独使用时，均存在不同的限制，难以达到理想的导航精度。而SINS/GNSS组合导航是组合上述两种导航设备的特点，利用导航滤波算法和计算机技术对多种导航信息进行综合处理，显著地提高了导航系统性能，特别是导航精度。

针对SINS/GNSS组合导航系统而言，其组合滤波的非线性状态方程包括捷联惯导力学编排方程和姿态误差方程，故采用的导航滤波方法宜选取为非线性滤波方法。经典的扩展卡尔曼滤波方法通过时间更新和量测更新得到对状态进行估计，但当模型不精确或者噪声统计信息是非高斯时，滤波效果会很差，甚至会导致滤波过程发散。基于最大相关熵准则，有学者提出了相应的相关熵卡尔曼滤波算法，用于解决滤波过程中噪声的非高斯统计特性问题，取得了不错的效果。但是其未考虑模型的不确定性，针对这种模型的不确定性问题，基于强跟踪理论将渐消因子引入到相关熵卡尔曼滤波算法中，提出了一种基于强跟踪的相关熵扩展卡尔曼滤波的SINS/GNSS组合导航方法，能有效地解决SINS/GNSS组合导航系统的非线性问题，以及噪声统计特性的非高斯问题，提高了SINS/GNSS组合导航的精度。

发明内容

针对SINS/GNSS组合导航系统模型的不确定性和噪声的非高斯特性，本发明提供了一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，基于最大相关熵准则建立了相关熵卡尔曼滤波方法，将渐消因子引入到相关熵扩展卡尔曼滤波方法，提高滤波精度。该方法利用强跟踪理论的特点，最大限度的消除模型的不确定性对SINS/GNSS组合导航的精度的影响，提高导航滤波精度，增强导航滤波稳定性。

为了达到上述目的，本发明提出了一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，包括以下五个步骤：

步骤一：建立SINS/GNSS组合导航系统的动力学方程

在东北天地理坐标系下，SINS/GNSS组合导航系统的动力学方程为：

其中，φ_E,φ_N,φ_U分别是东、北、天的高度，v_E,v_N,v_U分别是东、北、天的速度，L,λ,h分别为地理纬度、经度和高度；f_E,f_N,f_U分别为东北天三个轴向的加速度计输出的比力测量值，δv_E、δv_N分别是惯导系统的速度v_E、v_N输出减去GNSS对应输出的速度的近似值，δL、δh是惯导系统的地理纬度L、地理高度v_E输出减去GNSS对应输出的位置的近似值；R_M为R_M＝R_e(1-2e+3esin²L)，R_N为R_N＝R_e(1-esin²L)，其中R_e＝6378137.0m为地球长半径，e＝1/298.257是地球扁率，ω_ie＝7.292×10^-5rad/s是地球自转角速度，g＝9.78m/s²是重力加速度；地球陀螺东北天常值漂移ε_bE,ε_bN,ε_bU分别为：

其中，w_gE,w_gN,w_gU分别是陀螺东、北、天的噪声；

状态向量为x＝[φ_E,φ_N,φ_U,v_E,v_N,v_U,L,λ,h,ε_bE,ε_bN,ε_bU]^T，则其动力学方程为

式中，w为系统的噪声；

步骤二：建立SINS/GNSS组合导航系统的量测方程

选取GNSS输出的载体速度信息v_EG,v_NG,v_UG和位置信息L_G,λ_G,h_G作为量测值组成量测方程：

y＝[v_EG,v_NG,v_UG,L_G,λ_G,h_G]^T (11)

则相应的量测方程为

y＝Hx+v (12)

其中，量测方程系数矩阵H为H＝[I_6×6,O_6×6]，I为单位矩阵，O为零矩阵，v为量测噪声。

步骤三：离散化SINS/GNSS组合导航的动力学方程和量测方程

将式(10)的动力学方程和式(12)的量测方程进行离散化，得到非线性的状态方程和线性的量测方程：

x_k＝f(x_k-1)+w_k-1 (13)

y_k＝Hx_k+v_k (14)

其中，x_k为状态向量，f(·)为非线性离散函数，w_k为系统噪声向量；y_k为量测向量；v_k为量测噪声向量，k是指第k步，代表t_k时刻；系统噪声向量w_k和量测噪声向量v_k满足：

步骤四：对非线性离散后的动力学方程进行线性化

首先，将式(13)中的非线性离散函数f(x_k-1)围绕估计值展开成泰勒级数，并略去二阶以上的项，得线性化之后的状态方程：

x_k＝Φ_k/k-1x_k-1+u_k-1+w_k-1 (16)

其中，

其中，表示状态x的估计值；

步骤五：对线性化的动力学方程采用基于强跟踪的相关熵扩展卡尔曼滤波算法，输出SINS/GNSS组合导航的信息

(1).对由式(16)和式(14)组成的非线性离散系统的状态和误差方差阵P_k进行初始化和时间更新；对状态和误差方差阵分别赋予初值

其中，k取值为k＝1,2,3,…,N，N由滤波时间和采样周期决定；

(2).第k-1步t_k-1时刻的状态估计值为和误差方差阵为P_k-1，对第k步t_k时刻的状态向量x_k和误差方差阵P_k进行时间更新分别得到状态的一步预测

和相应的一步预测的误差方差阵：

其中，“－”表示先验估计，Q_k-1表示系统的噪声方差阵，λ_k为渐消因子，其计算过程如下：

其中，

N_k＝S_k(V_k-HQ_k-1H^T)-R_k(25)

其中，tr(·)为求矩阵迹的算子，R_k表示量测噪声的方差阵；V_k为实际输出残差序列的协方差阵，由下式估算：

其中，为输出残差序列，ρ为遗忘因子，且满足0＜ρ≤1；

(3).计算相关信息熵系数S_k：

其中，G_σ(·)表示带宽为σ的高斯核函数；

(4).计算滤波的增益矩阵K_k：

(5).计算状态估计

(6).计算状态估计的误差方差阵P_k：

通过以上6步循环可得到载体三维的位置(L,λ,h)和速度(v_E,v_N,v_U)的估计值。

本发明的有益效果是：本发明将强跟踪理论中的渐消因子引入到相关熵扩展卡尔曼滤波算法中，有效地解决了非线性SINS/GNSS组合导航系统的模型不确定性和噪声统计特性的非高斯问题，从而提高了导航精度，增强了导航过程的稳定性。

附图说明

图1为本发明的流程图。

图2为本发明中的基于强跟踪的相关熵扩展卡尔曼滤波算法的原理图。

具体实施方式

下面结合附图和实施例对本发明作详细说明。

一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，其流程图如图1所示，包括以下五个步骤:

步骤一、建立SINS/GNSS组合导航系统的动力学方程

在东北天地理坐标系下，SINS/GNSS组合导航系统的动力学方程为

式中，φ_E,φ_N,φ_U分别是东、北、天的高度，v_E,v_N,v_U分别是东、北、天的速度，L,λ,h分别为地理纬度、经度和高度。f_E,f_N,f_U分别为三个轴向的加速度计输出的比力测量值，δv_E、δv_N分别是惯导系统的速度v_E、v_N输出减去GNSS对应输出的速度的近似值，δL、δh是惯导系统的地理纬度L、地理高度v_E输出减去GNSS对应输出的位置的近似值。R_M为R_M＝R_e(1-2e+3esin²L)，R_N为R_N＝R_e(1-esin²L)，R_e＝6378137.0m为地球长半径，e＝1/298.257是地球扁率，ω_ie＝7.292×10^-5rad/s是地球自转角速度，g＝9.78m/s²是重力加速度。地球陀螺常值漂移ε_bE,ε_bN,ε_bU定义为

式中，w_gE,w_gN,w_gU分别是陀螺东、北、天的噪声。

取状态向量为x＝[φ_E,φ_N,φ_U,v_E,v_N,v_U,L,λ,h,ε_bE,ε_bN,ε_bU]^T，则其动力学方程为

式中，w为系统噪声。

步骤二、建立SINS/GNSS组合导航系统的量测方程

选取GNSS输出的载体速度信息v_EG,v_NG,v_UG和位置信息L_G,λ_G,h_G作为量测值组成量测方程：

y＝[v_EG,v_NG,v_UG,L_G,λ_G,h_G]^T (12)

则相应的量测方程为

y＝Hx+v (13)

式中，量测方程系数矩阵H为H＝[I_6×6,O_6×6]，I为单位矩阵,O为零矩阵，v为量测噪声。

步骤三、离散化SINS/GNSS组合导航的动力学方程和量测方程

离散化SINS/GNSS组合导航的动力学方程和量测方程，建立SINS/GNSS组合导航的非线性离散系统。

将上述式(10)的动力学方程和式(12)的量测方程进行离散化，得到非线性的状态方程和线性的量测方程：

x_k＝f(x_k-1)+w_k-1 (14)

y_k＝Hx_k+v_k (15)

式中，x_k为状态向量，f(·)为非线性离散函数，w_k为系统噪声向量；y_k为量测向量，v_k为量测噪声向量，k是指第k步，代表t_k时刻。系统噪声向量w_k和量测噪声向量v_k满足

步骤四、对非线性离散后的动力学方程进行线性化

首先，将式(13)中的非线性离散函数f(x_k-1)围绕估计值展开成泰勒级数，并略去二阶以上项，得线性化之后的状态方程：

x_k＝Φ_k/k-1x_k-1+u_k-1+w_k-1 (17)

式中，

其中，表示状态x的估计值。利用泰勒级数对非线性离散函数进行泰勒级数展开时，只取前两项。以上四个步骤为前期准备过程，下面一个步骤是基于强跟踪的相关熵扩展卡尔曼滤波方法的滤波过程。

步骤五、对线性化的动力学方程采用基于强跟踪的相关熵扩展卡尔曼滤波算法，输出SINS/GNSS组合导航的信息

本发明的基于强跟踪的相关熵扩展卡尔曼滤波算法的原理图参见图2。

(1).对由式(17)和式(15)组成的非线性离散系统的状态和误差方差阵P_k进行初始化和时间更新：首先，对状态和误差方差阵分别赋予初值：

其中，k取值为k＝1,2,3,…,N，N由滤波时间和采样周期决定。比如当滤波时间为100秒，采样周期为1赫兹时，N＝100/1＝100。

(2).设第k-1步t_k-1时刻的状态估计值为和误差方差阵为P_k-1，在此基础之上对第k步即t_k时刻的状态向量x_k和误差方差阵P_k进行时间更新分别得到状态的一步预测

和相应的一步预测的误差方差阵

式中，上标“－”表示先验估计，Q_k-1表示系统的噪声方差阵；λ_k为渐消因子，其计算过程如下：

式中，

N_k＝S_k(V_k-HQ_k-1H^T)-R_k (26)

其中，tr(·)为求矩阵迹的算子，R_k表示量测噪声的方差阵。V_k为实际输出残差序列的协方差阵，一般由下式估算：

其中，为输出残差序列，ρ为遗忘因子，满足0＜ρ≤1，一般来说ρ取为0.95。

(3).计算相关信息熵系数S_k：

其中，范数表示 表示带宽为σ的高斯核函数，即

同样，范数表示

表示

(4).计算滤波的增益矩阵K_k：

(5).计算状态估计

(6).计算状态估计的误差方差阵P_k：

通过以上6步循环进行即可得到载体三维的位置(L,λ,h)和速度(v_E,v_N,v_U)估计值。

本发明由步骤一、步骤二、步骤三、步骤四和步骤五共五个步骤组成，通过建立SINS/GNSS组合导航系统的非线性状态方程和量测方程，然后利用基于强跟踪的相关熵扩展卡尔曼滤波算法将渐消因子引入到相关熵扩展卡尔曼滤波中,提高导航系统的精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (1)

1.一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法，其特征在于，包括以下步骤：

步骤一：建立SINS/GNSS组合导航系统的动力学方程

在东北天地理坐标系下，SINS/GNSS组合导航系统的动力学方程为：

其中，φ_E,φ_N,φ_U分别是东、北、天的高度，v_E,v_N,v_U分别是东、北、天的速度，L,λ,h分别为地理纬度、经度和高度；f_E,f_N,f_U分别为东北天三个轴向的加速度计输出的比力测量值，δv_E、δv_N分别是惯导系统的速度v_E、v_N输出减去GNSS对应输出的速度的近似值，δL、δh是惯导系统的地理纬度L、地理高度v_E输出减去GNSS对应输出的位置的近似值；R_M为R_M＝R_e(1-2e+3esin²L)，R_N为R_N＝R_e(1-esin²L)，其中R_e＝6378137.0m为地球长半径，e＝1/298.257是地球扁率，ω_ie＝7.292×10^-5rad/s是地球自转角速度，g＝9.78m/s²是重力加速度；地球陀螺东北天常值漂移ε_bE,ε_bN,ε_bU分别为：

其中，w_gE,w_gN,w_gU分别是陀螺东、北、天的噪声；

状态向量为x＝[φ_E,φ_N,φ_U,v_E,v_N,v_U,L,λ,h,ε_bE,ε_bN,ε_bU]^T，则其动力学方程为

式中，w为系统的噪声；

步骤二：建立SINS/GNSS组合导航系统的量测方程

选取GNSS输出的载体速度信息v_EG,v_NG,v_UG和位置信息L_G,λ_G,h_G作为量测值组成量测方程：

y＝[v_EG,v_NG,v_UG,L_G,λ_G,h_G]^T················(12)

则相应的量测方程为

y＝Hx+v·····················(13)

其中，量测方程系数矩阵H为H＝[I_6×6,O_6×6]，I为单位矩阵，O为零矩阵，v为量测噪声；

步骤三：离散化SINS/GNSS组合导航的动力学方程和量测方程

将式(11)的动力学方程和式(13)的量测方程进行离散化，得到非线性的状态方程和线性的量测方程：

x_k＝f(x_k-1)+w_k-1························(14)

y_k＝Hx_k+v_k·····························(15)

其中，x_k为状态向量，f(·)为非线性离散函数，w_k为系统噪声向量；y_k为量测向量；v_k为量测噪声向量，k是指第k步，代表t_k时刻；系统噪声向量w_k和量测噪声向量v_k满足：

步骤四：对非线性离散后的动力学方程进行线性化

首先，将式(14)中的非线性离散函数f(x_k-1)围绕估计值展开成泰勒级数，并略去二阶以上的项，得线性化之后的状态方程：

x_k＝Φ_k/k-1x_k-1+u_k-1+w_k-1···················(17)

其中，

其中，表示状态x的估计值；

步骤五：对线性化的动力学方程采用基于强跟踪的相关熵扩展卡尔曼滤波算法，输出SINS/GNSS组合导航的信息

(1).对由式(17)和式(15)组成的非线性离散系统的状态和误差方差阵P_k进行初始化和时间更新；对状态和误差方差阵分别赋予初值

其中，k取值为k＝1,2,3,…,N，N由滤波时间和采样周期决定；

(2).第k-1步t_k-1时刻的状态估计值为和误差方差阵为P_k-1，对第k步t_k时刻的状态向量x_k和误差方差阵P_k进行时间更新分别得到状态的一步预测

和相应的一步预测的误差方差阵：

其中，“－”表示先验估计，Q_k-1表示系统的噪声方差阵，λ_k为渐消因子，其计算过程如下：

其中，

N_k＝S_k(V_k-HQ_k-1H^T)-R_k····················(26)

其中，tr(·)为求矩阵迹的算子，R_k表示量测噪声的方差阵；V_k为实际输出残差序列的协方差阵，由下式估算：

其中，为输出残差序列，ρ为遗忘因子，且满足0＜ρ≤1；

(3).计算相关信息熵系数S_k：

其中，G_σ(·)表示带宽为σ的高斯核函数；

(4).计算滤波的增益矩阵K_k：

(5).计算状态估计

(6).计算状态估计的误差方差阵P_k：

通过以上6步循环可得到载体三维的位置(L,λ,h)和速度(v_E,v_N,v_U)的估计值。