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

摘要

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

主权项

一种基于双模型切换的二次传递对准方法，应用于由主惯导系统和子惯导系统组成的惯性导航系统，前述主惯导系统和子惯导系统均为捷联式惯性导航系统，其特征在于，前述方法包括以下步骤：步骤1、主惯导系统采用固定频率向子惯导系统发送导航参数，子惯导系统利用主惯导的导航参数完成粗对准，前述导航参数包括速度，姿态和位置信息；步骤2、子惯导系统基于主惯导系统导航参数构造观测量；步骤3、在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算，待失准角缩小到一预设角度α时，进行滤波切换，然后在欧拉角误差模型下使用标准卡尔曼滤波进行迭代解算，直到失准角估计值收敛且稳定；在四元数误差模型下使用扩展卡尔曼滤波进行迭代解算包括以下步骤：11)四元数误差与速度误差方程：式中：δQ为四元数误差，δv为子惯导系统与主惯导系统在导航坐标系下的速度差；矩阵M，U和Y分别为：M=<ωibb>-[ωinn]=0ωE-ωxωN-ωyωU-ωzωx-ωE0ωz+ωU-ωy-ωNωy-ωN-ωz-ωU0ωx+ωEωz-ωUωy+ωN-ωx-ωE0]]>U=-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0]]>Y=-q1-q2-q3q0q3-q2-q3q0q1q2-q1q0]]>式中，为地理坐标系相对于惯性坐标系的角速度在地理坐标系中的投影，为陀螺仪的输出在载体坐标系中的投影；εb为陀螺的随机常值漂移，表示陀螺测量白噪声；为利用速度和位置信息得到的的误差；为子惯导捷联矩阵；为子惯导加速度计输出在载体坐标系的投影；▽b为加速度计在载体坐标系下的随机常值漂移；为地球坐标系相对于惯性坐标系的角速度在导航坐标系的投影；为导航坐标系相对于地球坐标系的角速度在导航坐标系的投影；vn＝[ve vn vu]为子惯导在导航坐标系下的东北天速度；为速度误差方程中的非线性项；12)惯性器件误差方程为：ϵ·ib=0(i=x,y,z)▿·ib=0(i=x,y,z)]]>13)系统方程为：X·=f(x)+GW]]>式中x＝[δq δv ε ▽]T13×1为状态变量；δq表示四元数误差；δv表示子惯导系统与主惯导系统在导航坐标系下的速度差；▽表示为加速度计的随机常值漂移；ε表示系统轴陀螺常值漂移；f(x)由线性和非线性两部分组成，即f(x)＝F·x+p(x)，其中F=F11F1212U03×3F21F2203×3Csn06×403×303×303×3]]>F11=120ωE-ωxωN-ωyωU-ωzωx-ωE0ωz+ωU-ωy-ωNωy-ωN-ωz-ωU0ωx+ωEωz-ωUωy+ωN-ωx-ωE0]]>F12=-12-q1-q2-q3q0q3-q2-q3q0q1q2-q1q00-1/(RM+h)01/(RN+h)00tan(l)/(RN+h)00]]>F22=(vntanl-Vu)/(RN+h)2·ωie·sinl+ve/(RN+h)-(2·ωie·cosl+ve/(RN+h))-2·(ωie·sinl+ve·tanl/(RN+h))-vu/(RM+h)-vn/(RM+h)2·(ωie·cosl+ve/(RN+h))2·vn/(RM+h)0]]>非线性部分RM是地球子午圈主曲率半径，RN是地球卯酉圈主曲率半径，h是高度，Vu为主惯导系统的天向速度，l为主惯导系统的纬度，ωie是地球自转角速度14)建立系统观测方程选取主惯导系统和子惯导系统的速度差作为观测量，得到系统观测量为：Zobs＝[δvE δvN δvU]T＝H·x式中，H＝[04×3 I3×3 06×3]13×3，δv由子惯导系统与主惯导系统在导航坐标系下的速度做差得到；15)根据前述建立的系统状态方程、系统观测方程以及系统的观测量，采用扩展卡尔曼滤波器进行迭代解算，完成第一次对准，估测并补偿失准角；欧拉角误差模型下的标准卡尔曼滤波迭代解算包括以下步骤：21)姿态误差与速度误差方程为：φ·E=φNwiesinL+φNVEtanL/(RN+h)-φUwiecosL-φUVE/(RN+h)-δVN/(RM+h)+ϵxnφ·N=-φEwiesinL-φEVEtanL/(RN+h)-φUVN/(RM+h)+δVE/(RN+h)+ϵynφ·U=φEwiecosL+φEVE/(RN+h)+φNVN/(RM+h)+δVEtanL/(RN+h)+ϵznϵxnϵynϵznT=CbnϵxbϵybϵzbT]]>速度误差方程为：δV·E=-φNfU+φUfN+2δVNwiesinL+δVNVEtanL/(RN+h)-2δVUwiecosL-δVUVE/(RN+h)+▿xnδV·N=φEfU-φUfE-2δVEwiesinL-2δVEVEtanL/(RN+h)-δVUVN/(RM+h)+▿ynδV·U=-φEfN+φNfE+2δVEwiecosL+2δVEVE/(RN+h)+2δVNVN/(RM+h)+▿zn▿xn▿yn▿znT=Cbn▿xb▿yb▿zbT]]>式中：φE是子惯导系统的东向失准角，φN是子惯导系统的北向失准角，φU是子惯导系统的天向失准角；δVE是子惯导系统的东向速度误差，δVN是子惯导系统的北向速度误差，δVU是子惯导系统的天向速度误差；VE为主惯导系统的东向速度,VN为主惯导系统的北向速度，VU为主惯导系统的天向速度；fE为主惯导系统的东向比力,fN为主惯导系统的北向比力，fU为主惯导系统的天向比力；L为主惯导系统的纬度，RM是地球子午圈主曲率半径，RN是地球卯酉圈主曲率半径，h是高度，ωie是地球自转角速度；为子惯导系统x轴陀螺常值漂移，为子惯导系统y轴陀螺常值漂移，为子惯导系统z轴陀螺常值漂移；为子惯导系统x轴加速度计常值偏置,为子惯导系统y轴加速度计常值偏置，为子惯导系统z轴加速度计常值偏置；22)惯性器件误差方程：ϵ·ib=0(i=x,y,z)▿·ib=0(i=x,y,z)λ·i=0(i=x,y,z)]]>式中，λx为子惯导系统x轴安装误差角，λy为子惯导系统y轴安装误差角，λz为子惯导系统z轴安装误差角；可得系统方程为：X·=FX+GW]]>式中X＝[φ δV ε ▽ λ]T15×1为状态变量；φ表示子惯导系统失准角；δV表示子惯导系统的速度误差；ε表示系统轴陀螺常值漂移；▽表示系统加速度计常值偏置；λ表示系统安装误差角；23)F为系统状态转移矩阵：F=F1F2Cbn03×303×303×3Cbn03×309×15]]>其中，矩阵块F1与F2如下：F1=0ωiesinL+VERN+htanL-(ωiecosL+VERN+h)-(ωiesinL+VERN+htanL)0-VNRM+hωiecosL+VERN+hVNRM+h00-fUfNfU0-fE-fNfE0]]>F2=0-1RM+h01RN+h00tanLRN+h0002ωiesinL+VERN+htanL-(2ωiecosL+VERN+h)-(2ωiesinL+VERN+htanL)0-VNRM+h2ωiecosL+VERN+hVNRM+h0;]]>24)建立系统量测方程，选取经过补偿后的主惯导系统与子惯导系统的姿态差和速度差为观测变量，系统量测方程表达为：Z＝HX+V式中：为量测变量，V是量测噪声并且假定其为均值为零的高斯白噪声，其协方差为E[VVT]＝R，δv由主子惯导在导航坐标系下的速度相减得到，为子惯导与经过补偿后的主惯导姿态做差得到，由前述步骤1)得到的失准角可得到矩阵将主惯导系统的捷联矩阵与此矩阵相乘：由可得到经过补偿后的主惯导姿态；H为量测矩阵：H=H1103×303×303×3H1503×3I3×303×303×303×3]]>其中：H11=-T221-T322T121-T3220T21T33-T31T23T332+T312T31T13-T11T33T332+T3120-T12T32T122+T222-T22T32T122+T2221]]>H15=T331-T3220-T311-T322T31T32T332+T3121-T32T33T332+T312T12T23+T13T22T122+T2220-T11T22+T122T122+T222]]>式中：Tij为矩阵的元素；25)根据建立的系统状态方程、系统观测方程以及系统的观测量，采用标准卡尔曼滤波进行滤波迭代解算，直到失准角估计值收敛且稳定；以及步骤4、利用步骤3估计出的失准角来修正子惯导系统的姿态矩阵，得到捷联初始姿态矩阵，以完成二次传递对准。