CN106123921B - 动态干扰条件下捷联惯导系统的纬度未知自对准方法 - Google Patents

Abstract

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

Description

动态干扰条件下捷联惯导系统的纬度未知自对准方法

技术领域

本发明提供的是一种动态干扰条件下捷联惯导系统的纬度未知自对准方法，涉及晃动基座下载体所在位置纬度信息的计算、惯性系下粗略的初始姿态阵的求解、精对准状态空间模型的建立、基于新息的自适应滤波方法对纬度误差角和载体失准角的解算、纬度误差角对纬度值的补偿、失准角对捷联姿态矩阵的修正。本发明适合应用于受外界干扰较大姿态信息时刻都在发生变化、不会提供精确定位信息的场合下的载体的捷联惯导自对准过程，诸如处于隧道深处、深山密林或者深海海底的载体、发动机处于高频振动的汽车、在停机坪上受大风力作用做较大晃动的飞机、在风浪作用下做较大幅度摇摆的舰船、格斗状态下的战斗机等的自对准过程。

背景技术

导航就是以一定的精度、在规定的时间内将载体沿着预定的路线引导至目的地的过程。惯性导航是一种自主式导航技术，它不依靠外界信息，也不向外界辐射任何能量，仅利用陀螺仪和加速度计等惯性测量器件以及初始的导航信息来确定载体运行期间的各项导航参数，隐蔽性好、抗干扰性强，能够全天时、全天候为载体提供完备的运动信息，以其独有的特点始终有着不可替代的地位。其中，捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，然后通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，具有成本低、体积小、重量轻、可靠性高等优点，系统日趋成熟，精度逐步提高，应用范围逐渐扩大。

惯性导航系统在进入导航工作之前，需要获得初始姿态以及位置信息，即进行初始对准过程。初始对准通常分为两个阶段：粗对准阶段和精对准阶段。粗对准阶段可由外界信息或者根据陀螺和加速度计的输出，求出一个粗略的初始姿态矩阵，将失准角估计到一定的精度范围内。然后在粗对准的基础上，以速度误差作为观测量，建立初始对准误差模型，通过一定的滤波算法估计姿态误差角，对初始姿态矩阵进行校正，完成精对准。

静基座自对准技术已比较成熟，而动基座自对准技术依旧是国内外研究的热点。有些诸如发动机处于高频振动的汽车、浪涌下的舰船等受干扰较大的载体，角振动和线振动会导致基座晃动，捷联惯导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。

无论是静基座自对准方法还是动基座自对准方法，无论粗对准还是精对准，都要求准确的知道当地的地理纬度，如果地理纬度存在误差，将可能影响到初始对准的精度。然而在深山荒野，海上等环境中，想要获得当地的地理位置信息并不是一件很容易的事情；或者某些应用场合，如通信卫星跟踪或者武器平台的稳定，有较高的定向要求，而没有必要提供精确的定位要求；或者在某些应急场合，如军事战争中，受到敌方攻击，需要迅速做出反击，而定位系统来不及工作，却急需定向。在上述纬度未知的条件下，想要完成初始时刻自对准，就必须先完成载体所在位置纬度信息值的估计。

在捷联惯导系统精对准过程中，使用卡尔曼滤波方法解算系统状态变量，补偿误差项，解算精确姿态转换矩阵，实现精对准过程。然而在实际系统中，系统往往由于存在各种随机因素而成为随机系统，运算过程中使用近似计算，使得无法准确得到准确的系统模型，系统特性也很难知道，另外，考虑到测量过程中动态干扰条件和纬度未知导致纬度误差项等因素影响，系统中系统干扰过多，噪声不是严格的白噪声，导致卡尔曼滤波效果变差，甚至可能直接导致卡尔曼滤波失效。针对上述滤波环境，有必要建立包含垂荡、纵荡、横荡引起的干扰运动和由于纬度计算引起的纬度误差的精对准误差模型，提出新的滤波方法，高效抑制偏差，解算系统状态变量并补偿误差项，解算精确姿态转换矩阵，实现精对准过程。

发明内容

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

实现本发明目的技术解决方案为：

本发明提出一种动态干扰条件下捷联惯导系统的纬度未知自对准方法，包括以下方面：

(1)上位导航计算机控制导航系统，采集惯性测量单元IMU中光纤陀螺和加速度计的输出信号；

(2)对采集到的陀螺和加速度计的数据进行处理，利用重力加速度积分所得速度信息不同时刻在惯性坐标系下的投影，构建几何解析公式，求解载体所在位置的纬度信息，完成存在动态干扰的环境下的纬度求解过程。然后对纬度误差进行分析求解，证明利用速度信息计算纬度方法的可行性；

(3)带入计算所得纬度信息，进行惯性系下粗对准过程：基于双矢量定姿原理，利用重力加速度在惯性空间方位的改变包含地球北向信息这一特性，求解惯性系下粗略的初始姿态阵；

(4)在惯性系下粗略的初始姿态阵已求得的基础上，建立了纬度未知动态干扰条件下的精对准误差模型，采用基于新息的自适应滤波方法解算纬度误差角和载体的失准角，用纬度误差角补偿纬度值，用失准角修正捷联姿态矩阵，实现精对准过程，完成初始对准，进入导航状态。

本发明的优点：

1.基于重力加速度在惯性空间不变的原理，通过计算两个时刻的重力加速度计算纬度，由于干扰加速度的问题可能导致计算出现较大误差，甚至可能出现纬度不可计算的问题。针对这个问题，本发明提出对重力加速度进行积分，利用速度计算纬度这一算法。相比较于直接采用重力加速度计算纬度，本发明提出的速度计算纬度这一算法能够降低偶然随机误差，使得纬度误差更小，保证纬度计算的精度和有效性。

2.由于本发明中对纬度信息进行计算这一过程，纬度信息值存在一定的误差量。因此本发明中的算法对纬度误差进行了详细分析，利用速度误差方程、失准角方程和纬度误差方程构建精对准误差模型。系统状态矢量中加入纬度误差这一项，滤波所得纬度误差值用来修正纬度值，提高了解算精度，能够更好地解决纬度的补偿问题。

3.本发明提出基于新息的自适应滤波算法，将基于新息的自适应滤波方法应用于初始对准中，使用实际协方差代替理论协方差，增加自适应因子实时调整新的新息比例，可以防止滤波结果发散，使滤波收敛速度更快，俯仰姿态、横滚姿态和航向姿态均快速完成对准工作，能够较好的解决动基座纬度未知条件下的自对准问题。

附图说明

图1为本发明动态干扰条件下捷联惯导系统的纬度未知自对准方法流程示意图；

图2为本发明动态干扰条件下捷联惯导系统的纬度未知自对准方法整体计算示意图；

图3为上位导航计算机采集惯性测量单元IMU信息页面示意图；

图4为惯性坐标系下纬度估计示意图；

图5为基于新息的自适应滤波方程计算流程示意图；

图6受线振动干扰重力加速度和速度的变化量；

图7为纬度信息仿真结果示意图；

图8为XW-ADU7612姿态方位导航系统示意图；

图9为上位导航计算机采集实际姿态信息页面示意图；

图10为实验期间载体运动情况示意图；

(a)航向角运动情况(b)横滚角运动情况(c)俯仰角运动情况

图11为纬度信息实验结果示意图；

图12为纬度信息实验结果分析示意图；

图13为粗对准实验结果示意图；

图14为粗对准实验结果的统计特性示意图；

图15为精对准实验结果示意图。

(a)东向姿态误差角(b)北向姿态误差角(c)天向姿态误差角

图16为捷联式导航对准导航算法流程。

具体实施方式

下面结合方法流程图1和捷联式惯性导航系统整体计算图2对本发明的具体实施步骤进行详细的描述：

在下文实施步骤的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向(E)，Y轴指向北向(N)，Z轴指向天向(U)；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；载体惯性坐标系ib0系，是在对准初始时刻将载体坐标系经惯性凝固后的坐标系。

步骤1：系统准备阶段，捷联惯导系统进行预热准备，上位导航计算机开启；

步骤2：导航算法初始化，正确设置相关参数；

步骤3：上位导航计算机控制导航系统，通过RS232转USB串口，向导航系统发送命令协议“$GCCMD,OUTPUT,COMA,GTIMU,0.01*FF”，以100HZ的数据更新速率，115200bps的波特率，采集惯性测量单元IMU中光纤陀螺和加速度计的输出数据，如图3所示。其中，光纤陀螺仪的零点偏置稳定性是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₂)之间的夹角为θ，地球在这段时间内转过角度为α。如图纬度计算示意图4所示，夹角θ、夹角α和纬度L三个角度之间存在几何关系，因此通过求取θ和α，可以间接地求取纬度信息L，下面叙述具体过程。

因为A与B在同一纬度，所以有式：

|AO'|＝|BO'|＝|AO|cos L (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₁时间内地球自转转过的角度α。

④根据公式(112)求出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ⁿ，则i系下的重力加速度值gⁱ为：

设Δt_k＝t_k-t₀，积分得i系下的速度投影Vⁱ：

根据式(30)可知，在tk1、tk2时刻(t0<tk1<tk2)有

由上式得：

式中，由式(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的增加，旧的新息比例会随之减小。由滤波收敛判据知，当滤波器发散的时候，误差协方差矩阵是无界的，这个时候实际协方差会比理论协方差大很多倍。以此为依据，当新的新息比新息边界大的时候，认为此次的新息误差过大，为降低其比例降低α，反之α采用默认值，以此抑制滤波器发散。

α的计算方法如公式如下：

其中为允许的误差边界。从公式可以看出，超过误差允许边界，距离越远，α越小，则新的新息在计算过程中所占比例越小。

结合滤波算法流程图5可知，系统基于新息的自适应滤波方程如下：

状态一步预测方程：

均方误差一步预测方程：

滤波增益方程：

状态估计方程：

均方误差估计方程：P_K＝(I-K_KH_K)P_KK-1 (64e)

的计算方程：

α的计算方程：

新息的计算方程：

利用基于新息的自适应滤波方法可估计出系统状态矢量，系统状态矢量的第四项、第五项、第六项即为载体的失准角

步骤7：终止程序，关闭系统。

本发明的有益效果如下：

本发明首先在仿真环境下对提出的速度计算维度值的算法进行仿真实验，仿真验证纬度从10°～80°解算变化，证明使用速度方式计算纬度，能够降低偶然随机误差，使得纬度误差更小。然后通过真实实验对本发明提出的纬度未知条件下晃动基座自对准方法整体进行验证，实验结果与高精度的真实载体姿态信息对比，证明本自对准系统的可行性和有效性。

仿真验证纬度从10°～80°使用速度计算维度值的解算变化。

首先进行仿真环境设置：

①载体受到外界扰动，其航向角ψ、俯仰角θ、横滚角γ作周期性变化，如下：

②对仿真模型增加线振动干扰，横荡、纵荡和垂荡引起的线速度为：

A_Dx＝0.02m，A_Dy＝0.03m，A_Dz＝0.3m；ω_Di＝2π/TDi，且T_Dx＝7s，T_Dy＝6s，T_Dz＝8s；为[0,2π]上服从均匀分布的随机相位；

③假设陀螺三个方向轴上的随机常值漂移为0.01°/h，陀螺的随机游走系数为加速度计三个方向轴上的常值偏差为1×10^-4g，加速度计量测白噪声标准差为

④常数设置

赤道半径为R_e＝6378165.0m，椭球扁率为e＝3.352e-3，由万有引力可得地球表面重力加速度为g₀＝9.78049，地球自转角速度是单位是7.2921158e-5；常数π＝3.1415926

算法仿真结果如下：

同时对重力加速度计算维度值的算法和本发明提出的速度计算维度值的算法进行运行解算，仿真验证纬度从10°～80°变化，每10°计算一次。每组数据计算均为90秒，其中速度计算积分时间为40S。受线振动干扰，重力加速度和速度的变化量如图6所示，纬度仿真结果如图7所示。

从重力加速度和速度的变化量可以看出，增加线振动干扰后，重力加速度受影响较大，北向变化均值高达3％。而积分得到的速度变量很小，均不到0.1％。因此，用积分得到的速度可以有效的抑制线振动带来的干扰。

从纬度仿真结果不难看出，使用速度计算纬度的误差普遍比使用重力加速度计算误差要小一些，依旧可以保持较高的精度。而使用重力加速度计算的误差极不稳定，出现计算误差过大甚至无法计算的现象，10-40度区间甚至无法计算出结果。这是由于采用重力加速度进行计算，加速度计的输出带有偶然的随机误差，导致计算的不准确。而对重力加速度进行积分，降低了偶然随机误差，使得纬度误差更小。

(2)通过真实实验对本发明提出的纬度未知条件下晃动基座自对准方法进行验证。

实验地点为北京工业大学校内小广场，纬度信息为39.8709144°。实验装置采用XW-ADU7612姿态方位导航系统，以高精度的三轴姿态信息作为实际姿态信息进行实验。其中，XW-ADU7612姿态方位导航系统装置实物如图8所示，上位导航计算机通过RS422转USB串口，向导航系统发送命令协议“$GCCMD,OUTPUT,COMC,GPFPD,0.01*FF”，以100HZ的数据更新速率，115200bps的波特率采集航向精度达0.1度、姿态精度达0.05度的真实姿态信息，如图9所示。在实验进行期间，载体运动情况如图10所示，从图中可以看出，航向角晃动幅度已经达到8°，橫滚角和俯仰角也有较大的振幅。

首先进行纬度估计。同时对重力加速度计算维度值的算法和本发明提出的速度计算维度值的算法进行运行解算，维度信息估计进行了40次，每次纬度估计仿真时间为90s，其动基座纬度估计两算法对比结果及误差如图11所示，统计出两算法40次纬度估计实验对应的误差均值、误差标准和误差最大值，如图12所示。可以清楚地看出，本发明提出的速度计算维度值的算法使得纬度误差更小，无论是误差均值，误差最大值，还是误差标准差，使用速度计算纬度，其统计特性都优于使用重力加速度进行计算。该纬度估计结果满足误差要求，可为后续自对准提供初始纬度信息。

粗对准过程采用惯性系粗对准算法，历时140s。为验证算法有效性，防止数据随机性，实验重复10组，10次粗对准实验结果如图13所示。对粗对准结果进行数据分析，统计特性如图14所示。可以清楚地看出，在晃动基座粗对准过程中，使用计算出来的纬度与实际纬度计算出来的均值差很小，完全可以满足粗对准的精度要求，可为后续精对准提供粗略初始姿态。

采用基于新息的自适应滤波算法进行精对准计算，精对准时间600秒，精对准初始矩阵采用粗对准误差最大值。本文中α默认值选取为0.8。当判断出新的新息比新息边界大的时候，认为此次的新息误差过大，舍去α默认值重新进行求取。分别用未增加纬度误差补偿算法和本文改进后的增加纬度补偿算法进行精对准计算，对比两算法求解结果如图15所示。可以看得出，基于新息的自适应滤波精对准过程中，俯仰姿态、横滚姿态和航向姿态均快速完成对准工作，收敛速度和对准精度完全满足精对准要求。从图中对比可以看出，增加纬度补偿后，俯仰角、横滚角和航向角的误差角大幅度降低。结果表明，本文在精对准阶段对纬度误差进行补偿，用纬度误差值修正纬度值，提高自对准解算精度。

实验结果表明：

两纬度估计算法解算结果对比证明，本发明提出的速度计算维度值的算法对重力加速度进行积分，能够降低偶然随机误差，使得纬度误差更小，解算结果符合误差随着纬度上升而降低的规律，取得更好的计算结果。未增加纬度误差补偿算法和本文改进后的增加纬度补偿算法结果对比证明，在精对准阶段对纬度误差进行补偿，用纬度误差值修正纬度值，提高自对准解算精度。整体实验结果表明，本发明提出的纬度未知条件下晃动基座自对准方法，能有效在动态干扰情况下计算纬度信息，有效隔离晃动对载体初始对准的影响，有效使用基于新息的自适应滤波器解算纬度误差值和载体的失准角，用纬度误差角补偿纬度值，提高自对准解算精度，实现捷联惯性导航系统的实时高精度快速自对准，进入导航状态。

Claims (1)

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：终止程序，关闭系统。