[发明专利]动态干扰条件下捷联惯导系统的纬度未知自对准方法

摘要

动态干扰条件下捷联惯导系统的纬度未知自对准方法，首先，利用重力加速度在惯性坐标系投影不变的特点，构建几何解析公式，对重力加速度进行积分得到速度信息，用速度信息计算载体所在位置纬度值，然后，基于双矢量定姿原理，利用惯性系重力加速度包含指北信息的特性，实现惯性系下粗略的初始姿态阵的求解，最后，在完成粗对准的基础上，利用速度误差方程、失准角方程和纬度误差方程构建纬度未知动态干扰条件下的精对准误差模型，利用基于新息的自适应滤波方法，解算出纬度误差角和载体的失准角，用纬度误差角补偿纬度值，用失准角修正捷联姿态矩阵，实现捷联惯性导航系统的高精度快速自对准。

主权项

1.动态干扰条件下捷联惯导系统的纬度未知自对准方法，其特征在于：坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向，Y轴指向北向，Z轴指向天向；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；载体惯性坐标系ib0系，是在对准初始时刻将载体坐标系经惯性凝固后的坐标系；步骤1：系统准备阶段，捷联惯导系统进行预热准备，上位导航计算机开启；步骤2：导航算法初始化，正确设置相关参数；步骤3：上位导航计算机控制导航系统向导航系统发送命令协议，采集惯性测量单元IMU中光纤陀螺和加速度计的输出数据，光纤陀螺仪的零点偏置稳定性是0.5度/h，零点偏置重复性是0.5度/h，加速度计的零点偏置小于1mg，零点偏置稳定性小于1mg；步骤4：得到光纤陀螺仪的三轴测量值和加速度计的三轴测量值f^b后，利用重力加速度积分所得速度信息不同时刻在惯性坐标系下的投影，构建几何解析公式，求解载体所在位置的纬度信息，完成存在动态干扰的环境下的纬度求解过程；然后对纬度误差进行分析求解，证明本纬度计算方法的可行性；(1)动态干扰情况下求解纬度信息过程光纤陀螺仪的三轴测量值和加速度计的三轴测量值f^b采集成功后，进入载体晃动基座下纬度信息解算过程，将重力加速度积分所得速度值投影到惯性坐标系下得到利用两个不同时刻惯性坐标系下的速度投影之间的夹角求取纬度，隔离晃动干扰对纬度求解的影响；载体坐标系b系随地球一起转动，惯性坐标系i系和ib0系没有转动，保持不变；由于地球的转动，重力加速度g在惯性空间ib0系内的方向投影由t₁时刻的g^ib0(t₁)变成了t₂时刻的g^ib0(t₂)，设g^ib0(t₁)与g^ib0(t₂)之间的夹角为θ，地球在这段时间内转过角度为α；纬度计算，夹角θ、夹角α和纬度L三个角度之间存在几何关系，因此通过求取θ和α，间接地求取纬度信息L，下面叙述具体过程；因为A与B在同一纬度，所以有式：|AO'|＝|BO'|＝|AO|cosL  (1)由几何定理知：将公式(1)带入(2)得：同理可得：由公式(3)和公式(4)得纬度的求解公式为：cosL＝sin(θ/2)/sin(α/2)  (5)即确定起始时刻t₁和纬度估计时间t₂，ω_ie为地球坐标系e系相对于地心惯性坐标系i系的地球自转角速度大小，在t₁、t₂时间差内地球转过的角度α求得：α＝ω_ie(t₂‑t₁)  (7)在本方法基于惯性空间算法中，重力加速度g在惯性空间ib0系内的方向投影计算方式如公式(8)所示：式中，g^b(t)为t时刻重力加速度g在b系内的方向投影，f^b(t)为t时刻加速度计b系的三轴测量值；载体坐标系b系与载体惯性坐标系ib0系之间的转动关系利用陀螺输出的角速度信息进行更新解算，如式(9)所示：其中初始时刻为单位阵；为光纤陀螺输出值，表示b系相对于i系的载体运动角速率在b系内的投影；为向量的叉乘反对称矩阵；由此，θ由两个时刻的重力加速度确定：式中，<>为内积符号，||表示求取向量的模值；考虑重力加速度单次量测值存在随机误差性；晃动基座下，加速度计输出不仅受加速度计自身漂移误差影响，还受到受量测噪声、载体角振动、线振动的干扰；由于存在的载体线振动干扰，必然使得加速度计某时刻的输出重力加速度值受到影响存在误差项，以至于导致计算出来的θ角不准确，使得纬度计算值存在误差项；更甚者，使得θ角计算严重不准确且大于α角，此时cosL>1，纬度值无法计算，出现计算的结果为奇异值；因此，本方法针对θ角计算不准确的问题，本方法对该算法进行改进；利用积分对数据误差的平滑作用，通过对两个时刻的重力加速度进行相同时间的积分平滑，平滑一段时间内某些时刻因受载体线振动的比力偏差点，抑制和补偿摇摆基座载体线振动的干扰；对两个时刻的重力加速度进行相同时间的积分，积分时间t₀，得到惯性空间ib0系内的速度值以达到避免偶然的加速度计输出带来的纬度计算误差的效果；则θ由下式求得：综上所述，动态干扰情况下求解纬度信息可总结为如下步骤：①根据公式(9)，更新计算坐标转换矩阵；②在时刻t₁和t₂根据公式(8)，分别将加速度计的输出投影到i_b0坐标系，并利用公式(11)进行积分得到和积分时间为t₀；③根据公式(7)计算在t₂‑t₁时间内地球自转转过的角度α；④根据公式(12)求出g^ib0(t₁)与g^ib0(t₂)之间的夹角θ；⑤由公式求(6)出纬度L；(2)对纬度误差进行分析求解设真实纬度为L，纬度误差为δL，计算求出的纬度为则有由于计算纬度过程中α是精确地，则纬度误差主要来源于θ；设两速度投影间的真实角度为θ，角度误差为δθ，计算求出的角度为有设t时刻的重力加速度在i_b0系下投影的误差为载体坐标系与载体惯性坐标系之间转换矩阵的误差为光纤陀螺三轴b系输出的误差为δf^b(t)，则有：即展开得：忽略二阶小量得由公式(13)(14)(15)(17)，则有：将上式展开，近似得：其中ε(t)×为陀螺误差构成的叉乘反对称矩阵，ε(t)很小，忽略二阶小量得：对纬度求解公式(6)求微分得：结合公式(23)、公式(20)和公式(22)，得纬度误差公式：因为ε(t)很小，所以在公式中所占比例很小，则纬度误差主要是由第二项和第四项决定，也就是误差主要受加速度计输出的影响，陀螺误差带来的影响比较小，而通过积分运算加速度计输出的误差也进一步降低，因此这种方法较好完成动基座下纬度的求解；步骤5：在载体所在位置的纬度信息求解完成的基础上，带入计算求得的纬度值，基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵，完成粗对准过程；在惯性系下捷联惯导系统的自对准粗对准算法中，以惯性空间为参考基准，导航坐标系n系与载体坐标系b系之间的初始姿态矩阵分解成4个矩阵求解，姿态矩阵的分解表达式为：其中，载体坐标系b系与载体惯性坐标系为ib0系之间的转换矩阵由公式(9)求解；导航坐标系n系与地心惯性坐标系i系之间的转换矩阵由所求得的运载体所在对准点的地理位置纬度信息和初始对准时间确定：导航坐标系n系与地球坐标系e系之间的转换矩阵：地球坐标系e系与地心惯性坐标系i系之间的转换矩阵：地心惯性坐标系i系与载体惯性坐标系ib0系之间的转动关系是不随时间变化的常值矩阵，其确定过程是姿态矩阵估算问题的关键，是初始对准的核心；因为地心惯性坐标系i系内的速度投影Vⁱ与载体惯性坐标系ib0系内的速度投影有关系式因此可根据速度在两惯性坐标系中的投影Vⁱ和构造矢量，采用双矢量定姿法求解；具体求解过程如下：在粗对准过程中将加速度计的刻度系数误差和安装误差忽略，则加速度计的测量值包含重力加速度g^b、载机垂荡、纵荡、横荡及振动而产生的干扰加速度杆臂干扰加速度和纬度误差引起的重力加速度误差δg^b，加表常值偏置向量为加速度计的常值偏置误差，为高斯白噪声；上述变量中，上标b表示变量在载体坐标系b系的投影；设积分得i_b0坐标系下的速度为：由于为线振动干扰速度，与重力加速度积分的速度相比较小，粗对准过程中可以将其忽略；同理，由纬度误差引起的重力加速度误差可以忽略，故上式可化简为：设n系下的重力加速度值为g_n，则i系下的重力加速度值gⁱ为：设Δt_k＝t_k‑t₀，积分得i系下的速度投影Vⁱ：根据式(30)可知，在tk1、tk2时刻有，式中，t0<tk1<tk2，由上式得：式中，Vⁱ(t_kj)(j＝1,2)由式(32)计算，按式(29)和式(30)计算；将计算得到的代入式(25)，即可计算出得到惯性系下粗略的初始姿态阵，完成捷联惯导系统的粗对准；步骤6：通过上述粗对准方法计算出姿态阵的粗略值，姿态误差角可视为小角，在此基础上利用基于新息的自适应滤波获取载体的失准角，建立了纬度未知动态干扰条件下的精对准误差模型，包含有垂荡、纵荡、横荡引起的干扰运动和由于纬度计算引起的误差；然后采用基于新息的自适应滤波方法解算纬度误差值和载体的失准角，用纬度误差角补偿纬度值，用失准角修正捷联姿态矩阵，得到三轴姿态信息，实现精对准过程，完成初始对准，进入导航状态；(1)精对准误差模型在精对准阶段，载体坐标系b系与导航坐标系n系之间的姿态矩阵可以由三个矩阵相乘得到：式中，导航坐标系n系与地球坐标系e系之间的转换矩阵和地球坐标系e系与地心惯性坐标系i系之间的转换矩阵按照上述粗对准方案中的计算方法进行求取；因此，的确定关键在于地心惯性坐标系i系与载体坐标系b系之间的转换矩阵的确定；设i'系是由陀螺输出计算得到的存在失准角的惯性坐标系，i系是真实的惯性坐标系，为解算得到的存在失准角的惯性坐标系i'系与真实的惯性坐标系i系之间的转换关系；式中，解算得到的存在失准角的惯性坐标系i'系与载体坐标系b系之间的转换矩阵由下式计算：其中，是通过粗对准过程建立的载体坐标系与计算惯性坐标系之间的转换矩阵，作为精对准开始时的转换矩阵；而转换矩阵改变值可以由加速度计和陀螺仪的输出计算得到；由于惯性器件存在各种误差，导致得到的计算惯性坐标系i'与真实的惯性坐标系i之间存在误差；设i'系与i系之间的失准角为三轴值分别表示为和则从i系到i'系的坐标转换矩阵为：将公式(38)带入公式(36)中即可求出综上所述，精对准的主要目的就是求出失准角，用失准角修正捷联姿态矩阵；在精对准过程中，加速度计的刻度系数误差和安装误差不可忽略；设加表刻度系数误差矩阵为δK_A＝diag(δK_Ax,δK_Ay,δK_Az)，加表安装误差矩阵为则加速度计输出的比力在i'系中的投影为：忽略二阶小项之后，可得：将上式移项并整理，得：式中，δaⁱ为等效干扰加速度，且对上式积分，得：设设惯性系i系下的速度误差为δVⁱ，δVⁱ的三轴值分别为和加速度计常值偏置误差的三轴值分别为和高斯白噪声的三轴值分别为和由式(48)得到惯性系i系下的速度误差方程：量测矢量表示为：由姿态转换阵和的微分方程，得到惯性系下的失准角方程：式中，ε^b和ω^b分别为b系的陀螺的常值漂移和量测高斯白噪声，ε^b的三轴值分别表示为和ω^b的三轴值分别表示为和设地球半径为Re，纬度误差δL可由速度误差求得：δL＝δVx/Re  (52)根据惯性系下的速度误差方程、失准角方程和纬度误差方程，系统的状态方程是如下形式：其中，状态矢量X和系统噪声W表示为：状态转移矩阵A和系统噪声矩阵B表示为：式中F＝[1/R_e 0 0]；由公式(50)，得到系统的量测方程为：其中，量测矩阵为H＝[I_3×3 0_3×10]；为不确定性量测干扰，主要由不同方向的干扰速度以及纬度误差引起的误差构成；V_w为量测高斯白噪声；由于存在计算的纬度误差，导致计算结果存在影响；精对准过程中，通过滤波过程，可以估算出陀螺和加速度计的误差，根据公式(24)计算出纬度误差，实时对纬度进行补偿，以降低纬度误差对精对准的影响；(2)采用基于新息的自适应滤波方法系统离散模型的量测方程和状态方程为：式中，X_k为k时刻的状态矢量；Z_k为k时刻的量测矢量；Ф_k,k‑1为k‑1时刻到k时刻的一步转移矩阵；Γ_k‑1为k‑1时刻的系统噪声矩阵；H_k为k时刻的量测矩阵；W_k‑1和V_k为随机干扰的白噪声，U_k为不确定性量测干扰；经典卡尔曼滤波采用新息的理论协方差计算滤波增益矩阵K_k，但是新息的理论协方差无法直接反映出外部量测噪声的变化；本方法使用新息的实际协方差代替理论协方差来计算滤波增益矩阵K_k；这样量测噪声增加，滤波增益矩阵K_k随之增加，反之亦然；对新息进行加权求和：由极大似然估计知系统噪声协方差矩阵R_k的估计值为：R_k的估计值代替经典卡尔曼公式中的R_k得到不难发现，实际上使用实际协方差代替理论协方差，其原理在于使用极大似然估计值对R_k进行实时估算；由于新息的不稳定性，使用实际新息的实际协方差进行计算时，由于其误差大小的不确定性，可能导致滤波不稳定甚至发散；所以本方法对其采用自适应的方式进行计算，增加自适应因子实时调整新的新息在中所占比例；由此，可由公下式计算：其中α为调节因子，用于调节新的新息在新息加权求和过程中所占的比例，可根据经验选择默认值；注意该算法当不存在调节因子α的时候，随着k的增加，旧的新息比例会随之减小；由滤波收敛判据知，当滤波器发散的时候，误差协方差矩阵是无界的，这个时候实际协方差会比理论协方差大很多倍；以此为依据，当新的新息比新息边界大的时候，认为此次的新息误差过大，为降低其比例降低α，反之α采用默认值，以此抑制滤波器发散；α的计算方法如公式如下：其中为允许的误差边界；从公式可以看出，超过误差允许边界，距离越远，α越小，则新的新息在计算过程中所占比例越小；系统基于新息的自适应滤波方程如下：状态一步预测方程：均方误差一步预测方程：滤波增益方程：状态估计方程：均方误差估计方程：P_k＝(I‑K_kH_k)P_k|k‑1  (64e)的计算方程：α的计算方程：新息的计算方程：利用基于新息的自适应滤波方法可估计出系统状态矢量，系统状态矢量的第四项、第五项、第六项即为载体的失准角步骤7：终止程序，关闭系统。