CN109945859A - 一种自适应h∞滤波的运动学约束捷联惯性导航方法 - Google Patents

Abstract

本发明公开了一种自适应H_∞滤波的运动学约束捷联惯性导航方法，主要步骤包括：定义解算所需的坐标系，根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量，建立运动学约束H_∞滤波模型，利用滑动窗口内的滤波新息构造自适应H_∞滤波方法，估计出速度误差并对系统进行修正，完成运动学约束惯性导航。本发明能够在不增加传感器且不影响捷联惯性导航系统隐蔽性等其他优点的基础上，有效提高捷联惯性导航系统导航精度。

Description

一种自适应H∞滤波的运动学约束捷联惯性导航方法

技术领域

本发明属于导航技术领域，特别涉及一种自适应H_∞滤波的运动学约束捷联惯性导航方法。

背景技术

捷联式惯性导航系统具有自主性强、隐蔽性好、体积小、结构简单、易于维护等优点，在航天、车载、航海领域都得到了广泛应用。如何在保留捷联式惯性导航系统优点的基础上，提高导航精度是急需解决的关键问题。

运动学约束惯性导航是一种无需增加传感器，利用自身运动学特征的导航方法，目前运动学约束惯性导航方法都采用了卡尔曼滤波技术，卡尔曼滤波通常需要测量噪声的功率谱密度和精确的系统模型来最小化状态估计误差的方差，但实际系统的噪声统计特性不准确，系统模型也有一定的不确定性，因此在实际系统中卡尔曼滤波的很难达到理想的估计精度。随着H_∞控制理论的出现，不对信号频谱特性做任何假设且具有更优鲁棒性的H_∞滤波方法发展起来，H_∞滤波将H_∞范数引入滤波问题，使构建的滤波器从干扰输入到滤波误差输出的H_∞范数最小化，这种滤波方法能使最坏干扰情况下的估计误差最小，但传统的H_∞滤波性能受限于初始设定的滤波参数。

发明内容

发明目的：针对上述缺陷，本发明提供一种能够在不增加传感器且不影响捷联惯性导航系统隐蔽性等其他优点的基础上，有效提高捷联惯性导航系统导航精度的自适应H_∞滤波的运动学约束捷联惯性导航方法。

技术方案：本发明提出一种自适应H_∞滤波的运动学约束捷联惯性导航方法，包括如下步骤：

(1)定义解算所需的坐标系并选取滤波状态量和量测量；

(2)建立运动学约束H_∞滤波模型；

(3)利用滑动窗口内的滤波新息构造自适应H_∞滤波方法。

进一步的，所述步骤(1)中定义解算所需的坐标系并选取滤波状态量和量测量的具体步骤如下：

坐标系选择如下：

n系—导航坐标系，x轴、y轴和z轴分别指向载体所在地理位置的东北天方向；

b系—载体坐标系，x轴、y轴和z轴分别指向载体的右前上方向；

根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量；选取系统状态量为：

式中：δv_E，δv_N分别为捷联惯性导航系统东向、北向速度误差；φ_E，φ_N，φ_U分别为系统东向、北向和天向失准角误差；分别为载体坐标系x轴，y轴方向的加速度计常值偏置；ε_x，ε_y，ε_z分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移；[·]^T表示矩阵转置；X为系统状态量组成的列向量；

选取系统量测量为：

y＝[δv_x,δv_z]^T

式中：δv_x,δv_z分别为捷联惯性导航系统载体系右向、上向的速度误差；y为系统量测量组成的列向量。

进一步的，所述步骤(2)中建立运动学约束H_∞滤波模型的具体步骤如下：

2.1)状态方程为：

式中:为系统状态向量的微分，F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵，Γ为噪声驱动阵，W为由陀螺和加速度计噪声构成的系统噪声阵；

2.2)量测方程为：

y＝HX+V

式中：V为量测噪声向量，H为量测矩阵，形式如下：

式中，C_ij(i,j＝1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素；v_E，v_N，v_U分别为捷联惯性导航系统东向、北向和天向速度。

进一步的，所述步骤(3)中利用滑动窗口内的滤波新息构造自适应H_∞滤波方法的具体步骤如下：

对步骤(2)中的模型进行离散化，k时刻的状态量由k-1时刻的状态量递推得到，形式如下：

H_∞滤波估计状态的任意线性组合：

Z_k＝L_kX_k

式中：Z_k为线性组合后的状态量，取L_k为单位阵；

利用自适应H_∞滤波算法对状态量中的速度误差进行估计，其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下：

式中：γ是调节H_∞滤波鲁棒性和精度的因子；

新息η_k可以反映滤波器的性能：

式中：新息η_k为2维列向量，对η_k进行相关运算得到ω_k：

式中，N为新息序列的采样长度，m为计数序号；

可构造γ为下式：

式中：系数μ>0，μ根据实际情况确定，λ(·)为矩阵的最大特征值；

自适应H_∞滤波不断进行时间更新和量测更新，估计出捷联惯性导航系统的东向和北向速度误差后，对系统输出的东向和北向速度进行补偿，直至导航结束。

本发明提出一种自适应H_∞滤波的运动学约束捷联惯性导航方法，该方法建立了捷联惯性导航系统运动学约束的H_∞滤波模型，给出了一种自适应调节H_∞滤波参数的方法，将该自适应H_∞滤波方法应用于运动学约束导航算法，该方法能够有效提高导航精度。

本发明采用上述技术方案，具有以下有益效果：

本发明采用自适应H_∞滤波，利用滑动窗口内的新息构造滤波参数调节模型，使得滤波参数能够根据外界干扰或系统状态的变化进行自适应调节，兼顾滤波精度与系统的鲁棒性，能够提高运动学约束惯性导航算法的精度。

附图说明

图1为本发明的流程图；

图2为具体实施例中载体行驶轨迹图；

图3为具体实施例中滤波参数γ变化图；

图4为具体实施例中3种方法定位误差和载体系速度误差对比曲线图。

具体实施方式

下面结合具体实施例，进一步阐明本发明，如图1至图4所示，本发明所述的一种自适应H_∞滤波的运动学约束捷联惯性导航方法，具体步骤如下：

步骤1)定义解算所需的坐标系并选取滤波状态量和量测量；

坐标系选择如下：

n系—导航坐标系，x轴、y轴和z轴分别指向载体所在地理位置的东北天方向；

b系—载体坐标系，x轴、y轴和z轴分别指向载体的右前上方向；

根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量。选取系统状态量为：

式中：δv_E，δv_N分别为捷联惯性导航系统东向、北向速度误差；φ_E，φ_N，φ_U分别为系统东向、北向和天向失准角误差；分别为载体坐标系x轴，y轴方向的加速度计常值偏置；ε_x，ε_y，ε_z分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移；[·]^T表示矩阵转置；X为系统状态量组成的列向量；

选取系统量测量为：

y＝[δv_x,δv_z]^T (2)

式中：δv_x,δv_z分别为捷联惯性导航系统载体系右向、上向的速度误差；y为系统量测量组成的列向量；

步骤2)建立运动学约束H_∞滤波模型；

2.1)状态方程为：

式中:为系统状态向量的微分，F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵，Γ为噪声驱动阵，W为由陀螺和加速度计噪声构成的系统噪声阵；

2.2)量测方程为：

y＝HX+V (4)

式中：V为量测噪声向量，H为量测矩阵，形式如下：

式中，C_ij(i,j＝1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素；v_E，v_N，v_U分别为捷联惯性导航系统东向、北向和天向速度；

步骤3)利用滑动窗口内的滤波新息构造自适应H_∞滤波方法：

对上述模型进行离散化，k时刻的状态量由k-1时刻的状态量递推得到，形式如下：

H_∞滤波可以估计状态的任意线性组合：

Z_k＝L_kX_k (7)

式中：Z_k为线性组合后的状态量，取L_k为单位阵；

利用自适应H_∞滤波算法对状态量中的速度误差进行估计，其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下：

式中：γ是调节H_∞滤波鲁棒性和精度的因子；

新息η_k可以反映滤波器的性能：

式中：新息η_k为2维列向量，对η_k进行相关运算得到ω_k：

式中，N为新息序列的采样长度，m为计数序号；

可构造γ为式(11)：

式中：系数μ>0，μ根据实际情况确定，λ(·)为矩阵的最大特征值；

根据式(6)至式(11)，自适应H_∞滤波不断进行时间更新和量测更新，估计出捷联惯性导航系统的东向和北向速度误差后，对系统输出的东向和北向速度进行补偿，直至导航结束。

本发明的可行性通过如下半物理仿真加以验证：

(1)陀螺仪随机常值漂移0.01°/h，加速度计常值偏置0.1mg；

(2)载体以10km/h的速度匀速行驶，行驶时长为5h，行驶轨迹如图2所示；

(3)取得陀螺与加速度实测数据后，用3种方法进行半物理仿真导航结果对比，分别是不采用运动学约束导航、传统H_∞滤波运动学约束导航和自适应H_∞滤波运动学约束导航方法。

通过半物理仿真，验证了滤波参数随系统的自适应调节功能(如图3所示)，本方法的导航精度较另两种方法有显著提高(如图4所示)。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种自适应H_∞滤波的运动学约束捷联惯性导航方法，其特征在于，包括如下步骤：

(1)定义解算所需的坐标系并选取滤波状态量和量测量；

(2)建立运动学约束H_∞滤波模型；

(3)利用滑动窗口内的滤波新息构造自适应H_∞滤波方法。

2.根据权利要求1所述的一种自适应H_∞滤波的运动学约束捷联惯性导航方法，其特征在于，所述步骤(1)中定义解算所需的坐标系并选取滤波状态量和量测量的具体步骤如下：

坐标系选择如下：

n系—导航坐标系，x轴、y轴和z轴分别指向载体所在地理位置的东北天方向；

b系—载体坐标系，x轴、y轴和z轴分别指向载体的右前上方向；

根据捷联惯性导航系统状态和载体运动学约束条件选取状态量和量测量；选取系统状态量为：

式中：δv_E，δv_N分别为捷联惯性导航系统东向、北向速度误差；φ_E，φ_N，φ_U分别为系统东向、北向和天向失准角误差；分别为载体坐标系x轴，y轴方向的加速度计常值偏置；ε_x，ε_y，ε_z分别为载体坐标系x轴、y轴和z轴方向的陀螺随机常值漂移；[·]^T表示矩阵转置；X为系统状态量组成的列向量；

选取系统量测量为：

y＝[δv_x,δv_z]^T

式中：δv_x,δv_z分别为捷联惯性导航系统载体系右向、上向的速度误差；y为系统量测量组成的列向量。

3.根据权利要求1所述的一种自适应H_∞滤波的运动学约束捷联惯性导航方法，其特征在于，所述步骤(2)中建立运动学约束H_∞滤波模型的具体步骤如下：

3.1)状态方程为：

式中:为系统状态向量的微分，F为按捷联惯导系统误差方程建立的10×10维状态转移矩阵，Γ为噪声驱动阵，W为由陀螺和加速度计噪声构成的系统噪声阵；

3.2)量测方程为：

y＝HX+V

式中：V为量测噪声向量，H为量测矩阵，形式如下：

式中，C_ij(i,j＝1,2,3)为导航系至载体系姿态转换矩阵的第i行第j列的元素；v_E，v_N，v_U分别为捷联惯性导航系统东向、北向和天向速度。

4.根据权利要求1所述的一种自适应H_∞滤波的运动学约束捷联惯性导航方法，其特征在于，所述步骤(3)中利用滑动窗口内的滤波新息构造自适应H_∞滤波方法的具体步骤如下：

对步骤(2)中的模型进行离散化，k时刻的状态量由k-1时刻的状态量递推得到，形式如下：

H_∞滤波估计状态的任意线性组合：

Z_k＝L_kX_k

式中：Z_k为线性组合后的状态量，取L_k为单位阵；

利用自适应H_∞滤波算法对状态量中的速度误差进行估计，其中状态估计公式、滤波增益矩阵计算公式、估计误差方差矩阵公式如下：

式中：γ是调节H_∞滤波鲁棒性和精度的因子；

新息η_k可以反映滤波器的性能：

式中：新息η_k为2维列向量，对η_k进行相关运算得到ω_k：

式中，N为新息序列的采样长度，m为计数序号；

可构造γ为下式：

式中：系数μ>0，μ根据实际情况确定，λ(·)为矩阵的最大特征值；

自适应H_∞滤波不断进行时间更新和量测更新，估计出捷联惯性导航系统的东向和北向速度误差后，对系统输出的东向和北向速度进行补偿，直至导航结束。