[发明专利]一种基于双模型切换的二次传递对准方法

权利要求书

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°。