CN104330092B - 一种基于双模型切换的二次传递对准方法 - Google Patents

Abstract

本发明提供一种基于双模型切换的二次传递对准方法，包括以下步骤：主惯导系统采用固定频率向子惯导系统发送导航参数，子惯导系统利用主惯导的导航参数完成粗对准；子惯导系统基于主惯导系统导航参数构造观测量；在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定；利用估计出的失准角来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，以完成二次传递对准。本发明的方法同时具备了线性模型滤波收敛速度快，精度高，非线性模型使用范围广的优点，同时可满足系统快速性，精确性，健壮性的要求。

Description

一种基于双模型切换的二次传递对准方法

技术领域

本发明属于惯性导航系统技术领域，特别是一种基于双模型切换的二次传递对准方法。

背景技术

传递对准是利用主子惯导系统的速度、姿态等导航参数进行匹配，滤波估计出子惯导系统相对与主惯导系统的失准角，从而建立精确的子惯导数学平台，同时对子惯导的导航信息进行初始化的过程。

目前，在失准角为小角度条件下的快速传递对准技术已经较为成熟，在快速性和精确性方面都能满足需求。然而，在工程应用中经常会出现失准角为大角度的情况。在这种情况下传统的线性误差模型将不能准确的描述惯导系统的误差传播特性，从而会给滤波结果带来较大误差，甚至导致滤波发散。

文献《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》第35卷中，文献《Comparison of SDINS In-Flight Alignment Using Equivalent Error Models》中研究了一种可用于大失准角条件下的基于四元数误差的非线性模型。该模型能应用在大失准角条件下，但存在着滤波收敛慢，滤波收敛精度略差的缺点。

发明内容

本发明旨在针对在惯性导航系统动基座对准中存在大失准角时的快速与精确传递对准问题，提出一种基于双模型切换的二次传递对准方法。

为实现上述目的，本发明所采用的技术方案如下：

一种基于双模型切换的二次传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，前述方法包括以下步骤：

步骤1、主惯导系统采用固定频率向子惯导系统发送导航参数，子惯导系统利用主惯导的导航参数完成粗对准，前述导航参数包括速度，姿态和位置信息；

步骤2、子惯导系统基于主惯导系统导航参数构造观测量；

步骤3、在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定；以及

步骤4、利用步骤3估计出的失准角来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，以完成二次传递对准。

进一步的实施例中，前述预设角度α的取值范围为：|α|≤5°。

由以上本发明的技术方案可知，本发明的有益效果在于：

1、相对于基于四元数的非线性误差模型，本发明的方法具有计算量小，滤波收敛快，对准精度高的优点。

2、相对于基于欧拉角的线性误差模型，本发明的方法具有适用范围广的优点，既可用于大失准角条件下，也可用于小失准角条件下。

3、本发明的方法同时具备了线性模型滤波收敛速度快，精度高，非线性模型使用范围广的优点，同时满足了系统的快速性，精确性，健壮性要求。

附图说明

图1是二次传递对准仿真试验，基于双模型切换的二次传递对准与基于四元数误差的传递对准结果对比图。

图2是传递对准跑车试验中，对主惯导系统补偿不同角度时的姿态失准角估计曲线。

具体实施方式

为了更了解本发明的技术内容，特举具体实施例并配合所附图式说明如下。

根据本发明的较优实施例，一种基于双模型切换的二次传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，前述方法包括以下步骤：

步骤1、主惯导系统采用固定频率向子惯导系统发送导航参数，子惯导系统利用主惯导的导航参数完成粗对准，前述导航参数包括速度，姿态和位置信息；

步骤2、子惯导系统基于主惯导系统导航参数构造观测量；

步骤3、在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定；以及

步骤4、利用步骤3估计出的失准角来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，以完成二次传递对准。

作为一个较佳的实施例，前述预设角度α的取值范围为：|α|≤5°。例如可取值为2°。

下面详细说明上述各步骤的具体实施。

1)在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，其实现包括以下步骤：

11)四元数误差与速度误差方程：

式中：δQ为四元数误差；矩阵M，U和Y分别为：

式中，为地理坐标系相对于惯性坐标系的角速度在地理坐标系中的投影，为陀螺仪的输出在载体坐标系中的投影；ε^b为陀螺的随机常值漂移，表示陀螺测量白噪声；为利用速度和位置信息得到的的误差；为子惯导捷联矩阵；为子惯导加速度计输出在载体坐标系的投影；为加速度计在载体坐标系下的随机常值漂移；为地球坐标系相对于惯性坐标系的角速度在导航坐标系的投影；为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影；vⁿ＝[v_e v_n v_u]为子惯导在导航坐标系下的东北天速度；为速度误差方程中的非线性项；

12)惯性器件误差方程为：

13)系统方程为：

式中为状态变量；

f(x)由线性和非线性两部分组成，即f(x)＝F·x+p(x)，其中

非线性部分

14)建立系统观测方程

选取主惯导系统和子惯导系统的速度差作为观测量，得到系统观测量为：

Z_obs＝[δv_E δv_N δv_U]^T＝H·x

式中，H＝[0_4×3 I_3×3 0_6×3]_13×3，δv由子惯导系统与主惯导系统在导航坐标系下的速度做差得到；

15)根据前述建立的系统状态方程、系统观测方程以及系统的观测量，采用扩展卡尔曼滤波器进行迭代解算，完成第一次对准，估测并补偿失准角。

2)待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定，其实现包括以下步骤：

21)姿态误差与速度误差方程为：

速度误差方程为：

式中：φ_E是子惯导系统的东向(平台)失准角，φ_N是子惯导系统的北向(平台)失准角，φ_U是子惯导系统的天向(平台)失准角；δV_E是子惯导系统的东向速度误差，δV_N是子惯导系统的北向速度误差，δV_U是子惯导系统的天向速度误差；V_E为主惯导系统的东向速度,V_N为主惯导系统的北向速度，V_U为主惯导系统的天向速度；f_E为主惯导系统的东向比力,f_N为主惯导系统的北向比力，f_U为主惯导系统的天向比力；L为主惯导系统的纬度，R_M是地球子午圈主曲率半径，R_N是地球卯酉圈主曲率半径，h是高度，ω_ie是地球自转角速度；为子惯导系统x轴陀螺常值漂移，为子惯导系统y轴陀螺常值漂移，为子惯导系统z轴陀螺常值漂移；为子惯导系统x轴加速度计常值偏置,为子惯导系统y轴加速度计常值偏置，为子惯导系统z轴加速度计常值偏置。

22)惯性器件误差方程：

式中，λ_x为子惯导系统x轴安装误差角，λ_y为子惯导系统y轴安装误差角，λ_z为子惯导系统z轴安装误差角。

可得系统方程为：

式中为状态变量；

23)F为系统状态转移矩阵：

其中，矩阵块F₁与F₂如下：

24)建立系统量测方程，选取经过补偿后的主惯导系统与子惯导系统的姿态差和速度差为观测变量，系统量测方程表达为：

Z＝HX+V

式中：为量测变量，V是量测噪声并且假定其为均值为零的高斯白噪声，其协方差为E[VV^T]＝R，δv由主子惯导在导航坐标系下的速度相减得到，为子惯导与经过补偿后的主惯导姿态做差得到，由步骤1)得到的失准角可得到矩阵将主惯导系统的捷联矩阵与此矩阵相乘：

由可得到经过补偿后的主惯导姿态；

H为量测矩阵：

其中，

式中：T_ij为矩阵的元素。

25)根据建立的系统状态方程、系统观测方程以及系统的观测量，采用标准卡尔曼滤波进行滤波迭代解算，直到失准角估计值收敛且稳定。

作为可选的实施方式，在判断失准角估计值的收敛时，可采用多种现有的收敛判据，例如朱立华等提出的“传递对准中滤波器的收敛判据”(载于中国惯性技术学报第19卷3期，2011年6月)。

作为另选的实施方式，也可以选取安装误差角估计值作为收敛判断的状态，计算其统计特征，并与设定阈值进行对比，判定滤波是否收敛。

当然，以上所提出的两个收敛判据仅仅是示例性的说明，本实施例所提及的迭代解算截止条件的收敛判据并不以此为限制。

最后，再利用步骤3最后估计出的失准角估计值来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，从而完成二次传递对准。

为了进一步对上述方法进行说明，充分展现出该方法的可靠性与精确性，进行了仿真试验与跑车试验。

(1)仿真初始条件及参数设置:

11)载体东、北、天速度分别为0 m/s，100m/s，0 m/s；载体初始位置：纬度为40°，经度为120°高度为1000m；载体初始姿态为：俯仰角为0°，滚转角为0°，偏航角为0°。

12)子惯导加速度计常值偏置设为5mg；子惯导陀螺常值漂移设为10(deg/h)。

13)设子惯导在三个轴向相对于主惯导系统的安装误差角分别为10°，20°，30°。

(2)仿真试验结果及分析

图1分别给出了基于双模型切换和基于四元数误差模型下的子惯导姿态失准角估计误差曲线。其中实线1基于双模型切换下的误差曲线，虚线2为基于四元数误差模型的误差曲线。在四元数误差模型下，经过多次仿真实验发现，子惯导姿态失准角可在10s内快速收敛到1°以内，因此可将切换时间设置在第10s。由图1可以看出在四元数误差模型下，三个轴的姿态失准角收敛速度较慢，估计误差分别为0.07°，0.1°，0.23°。而在使用本方法后可以观察到姿态失准角估计误差在13s后快速的收敛，三个轴的姿态失准角误差分别为0.03°，0.01°，-0.01°。因此可以说明本方法在大失准角条件下可以有效地、准确地估计出姿态失准角，完全满足传递对准算的高精度和快速性的要求。

(3)跑车试验设备安装

为了方便调整主子惯导之间的安装误差角，加工了多个具有一定倾角的小铝板。同时，在用来固定主子惯导的基座上，打有多个螺丝孔用来调整航向的安装角。在跑车试验中，主子惯导之间的安装角约为0°，5°，32°。

(4)跑车试验结果及分析

因为主子惯导之间的安装角已经大概知道，所以在跑车试验时直接将安装角补偿给主惯导进入二次对准。在三个轴向通过补偿不同的角度后，对比对准后的姿态失准角估计值来判定该方法是否可行。如图2所示，在跑车试验中设置了3组补偿角，分别为(0°,-5°,32°)，(0°,-5.5°,32.5°)和(0°,-6°,31.5°)。在三组不同补偿角下，姿态失准角均能在滤波下收敛。且通过多次试验，发现在三组不同补偿角下，姿态失准角估计值收敛精度较高，在0.1°以内波动。因此可以说明本方法在工程应用中具有一定的实用性。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims (2)

1.一种基于双模型切换的二次传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，其特征在于，前述方法包括以下步骤：

步骤1、主惯导系统采用固定频率向子惯导系统发送导航参数，子惯导系统利用主惯导的导航参数完成粗对准，前述导航参数包括速度，姿态和位置信息；

步骤2、子惯导系统基于主惯导系统导航参数构造观测量；

步骤3、在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定；

在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算包括以下步骤：

11)四元数误差与速度误差方程：

式中：δQ为四元数误差，δv为子惯导系统与主惯导系统在导航坐标系下的速度差；矩阵M，U和Y分别为：

式中，为地理坐标系相对于惯性坐标系的角速度在地理坐标系中的投影，为陀螺仪的输出在载体坐标系中的投影；ε^b为陀螺的随机常值漂移，表示陀螺测量白噪声；为利用速度和位置信息得到的的误差；为子惯导捷联矩阵；为子惯导加速度计输出在载体坐标系的投影；▽^b为加速度计在载体坐标系下的随机常值漂移；为地球坐标系相对于惯性坐标系的角速度在导航坐标系的投影；为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影；vⁿ＝[v_e v_n v_u]为子惯导在导航坐标系下的东北天速度；为速度误差方程中的非线性项；

12)惯性器件误差方程为：

13)系统方程为：

式中x＝[δq δv ε ▽]^T _13×1为状态变量；δq表示四元数误差；δv表示子惯导系统与主惯导系统在导航坐标系下的速度差；▽表示为加速度计的随机常值漂移；ε表示系统轴陀螺常值漂移；

f(x)由线性和非线性两部分组成，即f(x)＝F·x+p(x)，其中

非线性部分R_M是地球子午圈主曲率半径，R_N是地球卯酉圈主曲率半径，h是高度，V_u为主惯导系统的天向速度，l为主惯导系统的纬度，ω_ie是地球自转角速度14)建立系统观测方程

选取主惯导系统和子惯导系统的速度差作为观测量，得到系统观测量为：

Z_obs＝[δv_E δv_N δv_U]^T＝H·x

式中，H＝[0_4×3 I_3×3 0_6×3]_13×3，δv由子惯导系统与主惯导系统在导航坐标系下的速度做差得到；

15)根据前述建立的系统状态方程、系统观测方程以及系统的观测量，采用扩展卡尔曼滤波器进行迭代解算，完成第一次对准，估测并补偿失准角；

欧拉角误差模型下的标准卡尔曼滤波迭代解算包括以下步骤：

21)姿态误差与速度误差方程为：

速度误差方程为：

式中：φ_E是子惯导系统的东向失准角，φ_N是子惯导系统的北向失准角，φ_U是子惯导系统的天向失准角；δV_E是子惯导系统的东向速度误差，δV_N是子惯导系统的北向速度误差，δV_U是子惯导系统的天向速度误差；V_E为主惯导系统的东向速度,V_N为主惯导系统的北向速度，V_U为主惯导系统的天向速度；f_E为主惯导系统的东向比力,f_N为主惯导系统的北向比力，f_U为主惯导系统的天向比力；L为主惯导系统的纬度，R_M是地球子午圈主曲率半径，R_N是地球卯酉圈主曲率半径，h是高度，ω_ie是地球自转角速度；为子惯导系统x轴陀螺常值漂移，为子惯导系统y轴陀螺常值漂移，为子惯导系统z轴陀螺常值漂移；为子惯导系统x轴加速度计常值偏置,为子惯导系统y轴加速度计常值偏置，为子惯导系统z轴加速度计常值偏置；

22)惯性器件误差方程：

式中，λ_x为子惯导系统x轴安装误差角，λ_y为子惯导系统y轴安装误差角，λ_z为子惯导系统z轴安装误差角；

可得系统方程为：

式中X＝[φ δV ε ▽ λ]^T15×1为状态变量；φ表示子惯导系统失准角；δV表示子惯导系统的速度误差；ε表示系统轴陀螺常值漂移；▽表示系统加速度计常值偏置；λ表示系统安装误差角；

23)F为系统状态转移矩阵：

其中，矩阵块F₁与F₂如下：

24)建立系统量测方程，选取经过补偿后的主惯导系统与子惯导系统的姿态差和速度差为观测变量，系统量测方程表达为：

Z＝HX+V

式中：为量测变量，V是量测噪声并且假定其为均值为零的高斯白噪声，其协方差为E[VV^T]＝R，δv由主子惯导在导航坐标系下的速度相减得到，为子惯导与经过补偿后的主惯导姿态做差得到，由前述步骤1)得到的失准角可得到矩阵将主惯导系统的捷联矩阵与此矩阵相乘：

由可得到经过补偿后的主惯导姿态；

H为量测矩阵：

其中：

式中：T_ij为矩阵的元素；

25)根据建立的系统状态方程、系统观测方程以及系统的观测量，采用标准卡尔曼滤波进行滤波迭代解算，直到失准角估计值收敛且稳定；

以及

步骤4、利用步骤3估计出的失准角来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，以完成二次传递对准。

2.根据权利要求1所述的基于双模型切换的二次传递对准方法，其特征在于，前述预设角度α的取值范围为：|α|≤5°。