CN106931966B - 一种基于泰勒高阶余项拟合的组合导航方法 - Google Patents

Abstract

本发明公开了一种基于泰勒高阶余项拟合的组合导航方法，首先将组合导航非线性系统模型一阶线性化并将泰勒级数展开后的高阶余项建模为与状态耦合的未知干扰；然后在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器可由多高斯分布拟合辨识出泰勒高阶余项的均值和协方差并实现了同时利用两者联合反馈校正组合导航系统的状态估计及协方差。解决了当组合导航系统模型非线性较强时现有技术导航定位精度较差的问题。

Description

一种基于泰勒高阶余项拟合的组合导航方法

技术领域

本发明属于组合导航技术领域，涉及一种基于泰勒高阶余项拟合的组合导航方法，

背景技术

所谓组合导航就是将两种或两种以上不同类型导航子系统按某种方式相组合，以达到提高系统精度和改善系统可靠性等目的。它具有如下优点：(1)能有效利用各子系统的导航信息，提高组合系统定位精度；(2)允许在各子系统工作模式间进行自动转换，从而进一步提高系统工作可靠性；(3)可实现对各子系统及其元件的校准，从而放宽了对子系统技术指标的要求。因此组合导航系统已成为当前及未来导航技术的重点发展方向之一，也是各国国防技术研究的重点。

现有解决组合导航系统高精度定位的技术均基于估计策略，即面向组合导航非线性模型时采用非线性滤波算法，包括扩展Kalman滤波(EKF)、无迹Kalman滤波(UKF)、粒子滤波(PF)等。EKF的基本思想是将非线性系统模型进行泰勒级数展开，略去二阶以上的高阶项，得到非线性系统的一阶线性化模型，然后基于该一阶线性化模型采用Kalman滤波进行计算。但是当导航系统非线性较强时，略去泰勒展开式的高阶项将会引起较大的线性化误差，从而导致EKF的滤波误差增大甚至发散。UKF是对非线性系统的概率密度函数进行近似，而不是对系统非线性函数进行近似，因此不需要求导雅克比矩阵，而且UKF采用确定型采样，仅需要采样很少的点就可完成UT变换，但是UKF在模型不准确或状态发生突变的情况下鲁棒性不佳、跟踪精度差。PF通过随机采样对概率密度函数进行近似，但是随机采样的特点使得它需要大量的粒子点来近似非线性函数的概率分布，所以PF的计算量很大，实时性差且易出现粒子匮乏问题。

当组合导航系统模型非线性较强时，传统非线性滤波算法导航定位精度较差或者计算量巨大。为提高组合导航定位精度，本发明在辨识策略下提出一种基于泰勒高阶余项拟合的组合导航方法。主要思想是：将非线性系统一阶线性化并将泰勒级数展开后的高阶余项建模为与状态耦合的未知干扰，显然该未知干扰也应具有和状态相同的演化特性，即至少具有前两阶矩。通过拟合辨识出该泰勒高阶余项的均值和协方差并同时利用两者去联合反馈校正基于一阶线性化模型的状态估计及协方差，从而提高导航定位精度。

发明内容

本发明的目的是提供一种基于泰勒高阶余项拟合的组合导航方法，解决了当组合导航系统模型非线性较强时现有技术导航定位精度较差的问题。

本发明所采用的技术方案是，一种基于泰勒高阶余项拟合的组合导航方法，首先将组合导航非线性系统模型一阶线性化并将泰勒级数展开后的高阶余项建模为与状态耦合的未知干扰；然后在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器可由多高斯分布拟合辨识出泰勒高阶余项的均值和协方差并实现了同时利用两者联合反馈校正组合导航系统的状态估计及协方差。

进一步的，具体按照以下步骤实施：

步骤1、构建INS/GPS组合导航系统模型：

该步骤中以INS平台自身解算量作为状态构建状态方程，以GPS输出的载体速度位置信息作为量测构建量测方程，两者便构成了INS/GPS组合导航系统的状态空间模型；

将组合导航系统非线性动态方程的泰勒高阶余项建模为与状态耦合的未知干扰，则可将通过设计联合估计与辨识框架解决组合导航系统高精度定位问题；

步骤2、单传感器系统下设计协方差校正滤波器：

步骤1中泰勒高阶余项与状态耦合，显然其具有均值和协方差特性，本步骤在单传感器系统下基于多区间量测设计协方差校正滤波器，实现联合状态估计与泰勒高阶余项均值和协方差拟合辨识，并同时利用两者信息去联合反馈校正基于一阶线性化模型的导航状态估计及协方差。

进一步的，步骤1中构建状态方程的具体方法为：

取导航坐标系为东、北、天地理坐标系，以载体在东北天方向的速度v_e,v_n,v_u，位置λ,h，惯导平台的姿态误差角φ_e,φ_n,φ_u以及陀螺常值漂移d_e,d_n,d_u为状态量，即则INS/GPS组合导航系统的状态方程包括：

速度方程：

经、纬、高方程：

姿态误差方程：

陀螺常值漂移：

其中，f_e,f_n,f_u为东、北、天方向的比力量测值，其来自加速度计的输出；δv_e,δv_n,δh在实际计算中可以由INS的速度、位置输出减去GPS的对应输出来近似获得；w_ie为地球自转角速度；g为地球重力加速度。陀螺常值漂移d_e,d_n,d_u为随机常数，为子午圈曲率半径,为卯酉圈曲率半径，a表示地球长半径，e表示地球椭率；

联合式(1)(2)(3)(4)可将组合导航系统的连续时间非线性状态方程表示为：

其中，为x(t)的一阶微分，f(x(t))为连续时间非线性动态函数，w(t)为连续时间系统噪声。

进一步的，步骤1中构建组合导航系统的量测方程的具体方法为：

选取GPS输出的载体速度信息v_eg,v_ng,v_ug和位置信息λ_g,h_g作为量测y，

则组合导航系统的量测方程为：

y_k＝Hx_k+v_k ⑹，

其中，

进一步的，步骤1中泰勒高阶余项建模的具体方法为：

将连续时间模型的状态方程和量测方程离散化后再由泰勒级数展开一阶线性化，并保留泰勒高阶余项且将其建模为未知干扰，处理后可得到，

其中，为雅克比矩阵，能计算得到，可看作已知输入，a_k-1是泰勒高阶余项等效的未知干扰，显然它与状态相耦合，w_k-1为系统噪声，v_k为量测噪声。

进一步的，步骤2的具体方法为：

在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器基于双层EM算法框架；多区间量测的选取原则是将单传感器系统的划窗量测分为l个区间，即其中表示k-l时刻至k时刻所得的量测集合，同理表示k-l-1+j时刻至k时刻所得的量测集合，l为窗长，j为正整数其取值范围是1,…,l；以上每个分段对应一个区间量测且独立并行执行第一层EM，即联合状态估计与泰勒高阶余项均值辨识，该层EM框架的输出为泰勒高阶余项的均值集合；第二层EM以第一层EM框架的输出作为输入然后由混合多高斯分布拟合辨识泰勒高阶余项的均值和协方差；将两层EM算法顺序组合并由泰勒高阶余项的均值和协方差联合反馈校正组合导航状态估计及协方差，该过程即构成了协方差校正滤波器。

本发明的有益效果是：

(1)本发明通过将非线性模型由泰勒级数展开并将泰勒高阶余项建模为与状态耦合的未知干扰，从而提供了一种在辨识策略下解决组合导航系统高精度定位问题的全新思路，避免了传统估计策略下的精度损失问题；

(2)本发明在单传感器系统下基于多区间量测设计了协方差校正滤波器，该滤波器可实现拟合辨识泰勒高阶余项的均值和协方差并同时利用两者信息去联合反馈校正导航状态及协方差，提高了导航定位精度。

附图说明

图1是INS/GPS组合导航协方差校正滤波结构图；

图2是单传感器下基于双层EM框架的协方差校正滤波器设计图；

图3是组合导航速度估计RMSE比较图；

图4是组合导航位置估计RMSE比较图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种基于泰勒高阶余项拟合的组合导航方法，首先将组合导航非线性系统模型一阶线性化并将泰勒级数展开后的高阶余项建模为与状态耦合的未知干扰，显然该未知干扰具有与状态相同的演化特性，即至少具有前两阶矩；然后在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器可由多高斯分布拟合辨识出泰勒高阶余项的均值和协方差并实现了同时利用两者联合反馈校正组合导航系统的状态估计及协方差。

本发明一种基于泰勒高阶余项拟合的组合导航方法，具体为：

步骤1、INS/GPS组合导航系统泰勒高阶余项建模：

惯性导航系统(INS)可全天候、全球范围内自主隐蔽地导航定位。但INS存在着定位误差随时间积累的致命缺陷，难以单独完成精度较高的长航时飞行任务。全球定位系统(GPS)能够提供高精度的位置、速度信息，但其抗干扰能力较差，且动态跟踪性能一般。可见，INS和GPS两个导航系统性能互补，前者长期稳定性差、短期稳定性好，而后者刚好相反，因此可以采用组合导航技术将两者组合起来，构成INS/GPS组合导航系统。

取导航坐标系为东、北、天地理坐标系，以载体在东北天方向的速度v_e,v_n,v_u，位置λ,h，惯导平台的姿态误差角φ_e,φ_n,φ_u以及陀螺常值漂移d_e,d_n,d_u为状态量，即则INS/GPS组合导航系统的状态方程包括：

速度方程：

经、纬、高方程：

姿态误差方程：

陀螺常值漂移：

其中，f_e,f_n,f_u为东、北、天方向的比力量测值，其来自加速度计的输出；δv_e,δv_n,δh在实际计算中可以由INS的速度、位置输出减去GPS的对应输出来近似获得；w_ie为地球自转角速度；g为地球重力加速度。陀螺常值漂移d_e,d_n,d_u为随机常数。为子午圈曲率半径,为卯酉圈曲率半径。其中，a表示地球长半径，e表示地球椭率。

联合式(1)(2)(3)(4)可将组合导航系统的连续时间非线性状态方程表示为：

其中，为x(t)的一阶微分，f(x(t))为连续时间非线性动态函数，w(t)为连续时间系统噪声。

若以GPS输出的载体速度信息v_eg,v_ng,v_ug和位置信息λ_g,h_g作为量测y，即则组合导航系统的量测方程为：

y_k＝Hx_k+v_k ⑹，

其中，v_k为离散时间量测噪声，k为离散时间。

方程(5)和方程(6)构成了组合导航系统状态空间模型。将上述模型经离散化处理然后将非线性动态函数f(x_k-1)由泰勒级数展开一阶线性化，并保留泰勒高阶余项且建模为未知干扰a_k-1，该过程为，

其中，x_k-1和分别为k-1时刻状态的真实值和估计值，和分别为f(x_k-1)的一阶导数和二阶导数，它们的值分别为：和运算符号为偏导计算，表示x_k-1的转置。

处理后的组合导航系统模型为，

其中，为雅克比矩阵，能计算得到，可看作已知输入，a_k-1是泰勒高阶余项等效的未知干扰，显然它与状态相耦合，w_k-1为系统噪声，v_k为量测噪声。

通过对组合导航系统模型作上述变换，可将组合导航系统高精度定位看作联合优化问题，从而可在联合估计与辨识框架下解决该问题。考虑到未知干扰与状态耦合，本发明在单传感器系统下基于多区间量测设计了协方差校正滤波器，实现了联合状态估计(基于一阶线性化模型)与未知干扰(即泰勒高阶余项)均值和协方差拟合辨识，并同时利用泰勒高阶余项的两者信息去联合反馈校正基于一阶线性化模型的导航状态估计及协方差，从而提高导航定位精度。本发明解决组合导航系统高精度定位的方案如图1所示：

步骤2、单传感器系统下设计协方差校正滤波器：

由于单个数据无法拟合其分布，所以基于单区间量测也不能实现协方差校正滤波器，一般的协方差校正滤波器设计方法均基于多传感器系统，每一传感器单独并行执行第一层EM算法。为了降低系统复杂度，本发明在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器同样基于双层EM算法框架。每一区间量测均独立并行执行第一层EM，即联合状态估计与泰勒高阶余项均值辨识，该层EM框架的输出为泰勒高阶余项的均值集合。第二层EM以第一层EM框架的输出作为输入，然后由混合多高斯分布拟合辨识泰勒高阶余项的均值和协方差。将两层EM算法顺序组合并由泰勒高阶余项的均值和协方差联合反馈校正组合导航状态估计及协方差，该过程即构成了协方差校正滤波器。单传感器系统下所设计的基于双层EM框架的协方差校正滤波器结构如图2所示。

在图2中，将单传感器系统的划窗量测分为l个区间，即其中表示k-l时刻至k时刻所得的量测集合，同理表示k-l-1+j时刻至k时刻所得的量测集合，l为窗长，j为正整数其取值范围是1,…,l。在每个分段量测区间下并行执行第一层EM算法，即联合状态估计(基于一阶线性化模型)与未知干扰(即泰勒高阶余项)均值特性辨识，并且分别辨识出高阶余项的均值第二层EM以第一层EM框架输出的均值辨识集合作为输入，然后基于多高斯拟合辨识出泰勒高阶余项的均值和协方差由该均值和协方差经Kalman滤波可联合反馈校正基于一阶线性化模型的状态估计及协方差，所得的导航系统状态估计值即为协方差校正滤波结果，该结果可提高导航定位精度。

实施例：INS/GPS组合导航

以式(7)所示的离散时间INS/GPS组合导航系统模型作为本发明仿真实例。在该仿真实例中，本发明仅对导航速度和位置进行校正，此时，组合导航系统模型，即公式(7)中的状态方程应该是：

x_k＝F_k-1x_k-1+A_k-1+Ma_k-1+w_k-1，

其中，矩阵M＝H'。

模型参数设置如下：陀螺常值漂移为0.01(o)/h，随机游走为初始姿态角误差为1′。GPS在东北天方向的位置误差均方根为(5,5,8)m，速度误差均方根均为0.05m/s。INS在三个方向上的位置误差和速度误差分别为15m，0.5m/s。假设目标沿直线匀速运动，INS和GPS的采样周期均为0.1s,滤波周期也为0.1s。

仿真参数设置如下：在第一层EM框架下，将单传感器量测划分为10个量测区间，即第一层EM算法的迭代次数为5次。第二层EM框架下，高斯混合分项为5个，第二层EM算法的迭代次数为5次。蒙特卡洛仿真10次。

本发明比较了基于双层EM算法的协方差校正滤波器与传统估计策略下的非线性滤波算法EKF、CKF以及辨识策略下的标准EM算法对于组合导航系统的定位精度。仿真结果如下：

图3分别给出了四种算法对目标东向速度的估计RMSE，相应的图4为四种算法对目标东向位置的估计RMSE。表1在数值上比较了四种算法对目标速度、位置的估计RMSE。仿真结果说明：一方面，EKF,CKF以及标准EM算法对于组合导航速度和位置定位精度相当；另一方面，协方差校正滤波器的导航定位效果要优于其他三种算法。对所产生现象的解释：(1)EKF是一种一阶近似算法而CKF至少可达到二阶精度，但是EKF和CKF估计RMSE相近说明该组合导航系统泰勒高阶余项很小，而标准EM算法的思想是由泰勒高阶余项的均值去校正状态估计，当泰勒高阶余项较小时，标准EM算法辨识策略的优势自然无法体现，所以它的估计RMSE与EKF，CKF相近；(2)协方差校正滤波器能够拟合辨识出泰勒高阶余项的均值和协方差，并同时利用两者信息去联合反馈校正基于一阶线性化模型的导航状态估计及协方差，所以协方差校正滤波器的估计精度更高。

表1组合导航系统位置和速度估计RMSE比较

传统解决组合导航精确定位的思路是针对非线性系统模型的非线性滤波等估计策略，本发明创新性地提出将非线性模型由泰勒级数展开并将泰勒高阶余项建模为与状态耦合的未知干扰，从而提供了一种在辨识策略下解决组合导航系统高精度定位问题的全新思路，避免了传统估计策略下精度损失问题。另外，注意到泰勒高阶余项与状态耦合，此时它至少应具有均值和协方差特性，因此本发明在单传感器系统下基于多区间量测设计了协方差校正滤波器，该滤波器可实现拟合辨识泰勒高阶余项的均值和协方差并同时利用两者信息去联合反馈校正导航状态及协方差，提高了导航定位精度。

Claims (3)

1.一种基于泰勒高阶余项拟合的组合导航方法，其特征在于，首先将组合导航非线性系统模型一阶线性化并将泰勒级数展开后的高阶余项建模为与状态耦合的未知干扰；然后在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器由多高斯分布拟合辨识出泰勒高阶余项的均值和协方差，并同时利用泰勒高阶余项的均值和协方差去联合反馈校正组合导航系统的状态估计及协方差；

具体按照以下步骤实施：

步骤1、构建INS/GPS组合导航系统模型：

该步骤中以INS平台自身解算量作为状态构建状态方程，以GPS输出的载体速度位置信息作为量测构建量测方程，两者便构成了INS/GPS组合导航系统的状态空间模型；

将组合导航系统非线性状态方程的泰勒高阶余项建模为与状态耦合的未知干扰，通过设计联合估计与辨识框架解决组合导航系统高精度定位问题；

其中，将组合导航系统的连续时间非线性状态方程表示为：

为x(t)的一阶微分，f(x(t))为连续时间非线性动态函数，w(t)为连续时间系统噪声；

泰勒高阶余项建模的具体方法为：

将连续时间模型的状态方程和量测方程离散化后再由泰勒级数展开一阶线性化，并保留泰勒高阶余项且将其建模为未知干扰，处理后可得到，

其中，为雅克比矩阵，能计算得到，可看作已知输入，a_k-1是泰勒高阶余项等效的未知干扰，显然它与状态相耦合，w_k-₁为系统噪声，v_k为量测噪声，x_k为k时刻状态真实值，为k-1时刻的状态估计值；

步骤2、单传感器系统下设计协方差校正滤波器：

步骤1中泰勒高阶余项与状态耦合，显然其具有均值和协方差特性，本步骤在单传感器系统下基于多区间量测设计协方差校正滤波器，实现联合状态估计与泰勒高阶余项均值和协方差拟合辨识，并同时利用泰勒高阶余项的均值和协方差去联合反馈校正基于一阶线性化模型的组合导航系统状态估计及协方差；

其中，在单传感器系统下基于多区间量测设计协方差校正滤波器，该滤波器基于双层EM算法框架；多区间量测的选取原则是将单传感器系统的划窗量测分为l个区间，即其中表示k-l时刻至k时刻所得的量测集合，同理表示k-l-1+j时刻至k时刻所得的量测集合，l为窗长，j为正整数其取值范围是1,…,l；在每个分段量测区间下并行执行第一层EM算法，即联合状态估计与泰勒高阶余项均值辨识，该层EM框架的输出为泰勒高阶余项的均值集合；第二层EM以第一层EM框架的输出作为输入然后由混合多高斯分布拟合辨识泰勒高阶余项的均值和协方差；将两层EM算法顺序组合并由泰勒高阶余项的均值和协方差联合反馈校正组合导航系统的状态估计及协方差，该过程即构成了协方差校正滤波器。

2.如权利要求1所述的一种基于泰勒高阶余项拟合的组合导航方法，其特征在于，所述步骤1中构建状态方程的具体方法为：

取导航坐标系为东、北、天地理坐标系，以载体在东北天方向的速度v_e,v_n,v_u，位置λ,h，INS平台的姿态误差角φ_e,φ_n,φ_u以及陀螺常值漂移d_e,d_n,d_u为状态量，即则INS/GPS组合导航系统的状态方程包括：

速度方程：

经、纬、高方程：

姿态误差方程：

陀螺常值漂移：

其中，f_e,f_n,f_u为东、北、天方向的比力量测值，其来自加速度计的输出；δv_e,δv_n,δh在实际计算中可以由INS的速度、位置输出减去GPS的对应输出来近似获得；w_ie为地球自转角速度；g为地球重力加速度；陀螺常值漂移d_e,d_n,d_u为随机常数，为子午圈曲率半径,为卯酉圈曲率半径，a表示地球长半径，e表示地球椭率；

联合式(1)(2)(3)(4)可将组合导航系统的连续时间非线性状态方程表示为：

其中，为x(t)的一阶微分，f(x(t))为连续时间非线性动态函数，w(t)为连续时间系统噪声。

3.如权利要求2所述的一种基于泰勒高阶余项拟合的组合导航方法，其特征在于，所述步骤1中构建组合导航系统的量测方程的具体方法为：

选取GPS输出的载体速度信息v_eg,v_ng,v_ug和位置信息λ_g,h_g作为量测y，

则组合导航系统的量测方程为：

y_k＝Hx_k+v_k ⑹，

其中，