CN103047999B - 一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法 - Google Patents

Abstract

一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法，针对舰船主/子组合中的传递对准，设计了一种适用于系泊/匀速直航条件下的陀螺误差快速估计方法，对子惯导陀螺常值误差进行快速估计，在实时多任务操作***的支持下，子惯导***对同一组数据进行循环导航解算与信息融合，充分利用计算机的高速性能，在传递对准过程中，快速完成对陀螺常值误差的估计。本发明不改变导航解算现有算法，不改变传递对准信息融合算法中的滤波器结构与信息匹配方式，充分利用现有导航计算机富余资源，加快传递对准过程中对陀螺误差的估计速度。

Description

一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法

技术领域

本发明涉及舰载主/子惯导快速传递对准方法，特别适用于舰船系泊或匀速直航条件下，陀螺误差的快速估计，为一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法。

背景技术

舰载环境下，主惯导（舰载平台式惯性导航***）的精度一般高于子惯导（如舰载导弹用捷联式惯性导航***）的精度数个量级。子惯导利用主惯导的导航信息完成初始化、失准角估计、器件误差估计的过程称为传递对准。

传递对准作为惯性技术领域的一种通用的、共性的技术，广泛应用于战车、战机、舰船、航天器等武器平台上所搭载子惯导***的初始对准。基于武器***快速反应的要求，传递对准的快速性一直是初始对准的目标。1989年，Kain和Cloutier首次提出“速度+姿态”匹配方式，并配合以载机的摇翼动作，即可在10s内达到1mrad的对准精度。此外，Spalding、Shortelle和Graham等人也验证了上述方法的有效性。

上述方法在强调了对准过程中失准角估计快速性时，弱化或忽略了对器件误差的估计。但是惯性器件存在以下问题：1）在长时间存放后，会存在一定量级的漂移，器件精度越低，漂移越严重；2）存在逐次启动误差，器件精度越低，误差越大。对于中低精度的子惯导***而言，在其初始对准时，若不对器件误差进行估计，必将导致器件误差在长时间导航解算过程中被积分放大，从而降低***导航精度。

事实上，传递对准Kalman滤波器中各状态量的可观测性依赖于信息匹配方式以及载体机动方式。在摇摆运动时，采用“速度+姿态”匹配方式，仪表误差的估计需要数分钟之久，在10s内无法估计。

发明内容

本发明要解决的问题是：舰载环境下，现有传递对准方法忽略了对惯性器件的误差估计，降低了导航精度，并且现有技术对误差的估计不够快速，影响导航的实时性。

本发明的技术方案为：一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法，在实时多任务操作***的支持下对陀螺常值误差进行快速估计，主/子惯导传递对准过程中，子惯导进行导航解算，子惯导利用主惯导的导航数据进行信息融合，设t₀为陀螺常值误差估计的起始时刻，亦为初始对准的起始时刻，t₀~t₁为数据采集、数据存储、实时导航解算与信息融合以及姿态矩阵更新时间段，t₁~t₂为基于t₀~t₁间存储信息的循环导航解算与信息融合以及姿态矩阵更新时间段，上述各时间的确定如下：

T1）针对具体子惯导***，在主/子惯导传递对准中，通过子惯导陀螺仪误差收敛曲线确定完成整个对准所需要的时间，再根据子惯导测量与导航解算更新周期Δt，确定完成整个对准所需要的导航解算总更新次数，设定为k_sum；根据主惯导导航信息更新周期ΔT，确定完成整个对准所需要的信息融合次数，ΔT即为信息融合周期；在确定的工作环境下，导航解算与信息融合两者时序关系确定，导航解算次数确定后，信息融合次数即可确定；

T2）根据子惯导仪表数据质量，主惯导量测信息质量，设定时刻t₁及时间段t₀~t₁，并根据Δt计算得到该时间段内导航解算更新次数，设定为k_sum1；

T3）从整个对准所需要的导航解算总更新次数k_sum中，扣除步骤T2）中t₀~t₁时间段内导航解算更新次数k_sum1，得到完成整个对准还需要完成的导航解算更新次数，设定为k_sum2，k_sum2=k_sum-k_sum1，从而得到t₁~t₂时间段的循环导航解算循环次数，设定为m，m=k_sum2/k_sum1，在m不为整数时，将m值向上圆整，并根据圆整后的m值调整k_sum2；时刻t₂由循环导航解算循环次数m和导航计算机的性能共同确定，在m确定时，导航计算机主频越高，t₂与t₁间差值越小；

根据所述确定的时间，在传递对准中完成子惯导陀螺误差估计，包括如下步骤：

1）在t₀时刻，利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化，得到子惯导的位置、速度与姿态矩阵t为时间变量；瞬时凝固t₀时刻的载体坐标系为b₀，得到I为(3×3)单位矩阵；

2）在时间段t₀~t₁内，子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算，并利用式（6）更新子惯导载体坐标系b相对于b₀系的姿态矩阵

$C_{{b,k}_1}^{b_0}=C_{{b,k}_1-1}^{b_0}(I+\mathrm{\Δt}{\mathrm{\ω}}_{{\mathrm{ib},k}_1}^b\×)---\left(6\right)$

式中，下标k₁=1~k_sum1表示导航解算更新的次数标号，为子惯导陀螺实时采集的数据， $C_{b,0}^{b_0}=C_b^{b_0}\left(t_0\right)=I;$

同时子惯导按照周期ΔT接收来自主惯导的导航信息并存储，所述导航信息包括速度和航向，并记录主惯导导航信息与子惯导仪表数据间的时序关系；通过信息融合进行主、子惯导传递对准；

3）在步骤2）结束t₁时刻，得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b₁，有

4）在时间段t₁~t₂内，子惯导***进行如下工作：

41）对步骤2）中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合，t₁~t₂内进行m次循环导航解算，各子循环周期为：t₁₀~t₁₁、t₂₀~t₂₁、…、t_j0~t_j1、…t_m0~t_m1，j=1~m表示子循环周期的标号，在各子循环的导航解算与信息融合过程中，所用子惯导仪表数据与主惯导信息分别对应t₀~t₁内子惯导***采集的仪表数据与主惯导的导航信息，在每一个子循环周期内，导航解算更新次数与信息融合次数与t₀~t₁内的次数分别相等；循环解算过程中，舰船处于系泊或匀速直航状态，各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置，但因船舶存在摇摆运动，各子循环解算开始时刻的姿态需要如下处理，在各子循环t_j0~t_j1开始时刻t_j0的初始姿态矩阵：

$C_b^n\left(t_{j0}\right)=C_b^n\left(t_{(j-1)1}\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T---\left(7\right)$

式中，为在t_(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵，即第j-1个子循环解算结束时的姿态矩阵， $C_b^{b_0}\left(t_1\right)=C_{{b,k}_{\mathrm{sum}1}}^{b_0},$ $C_b^n\left(t_{10}\right)=C_b^n\left(t_1\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T;$

在每一个子循环过程中，按步骤2）中记录的时序关系进行导航解算与信息融合；

42）在进行上述循环导航解算与信息融合的同时，子惯导对惯性仪表数据进行实时采集，更新计算当前载体坐标系b系相对于b₁系的姿态矩阵其离散形式为：

$C_{{b,k}_2}^{b_1}=C_{{b,k}_2-1}^{b_1}(I+\mathrm{\Δt}{\mathrm{\ω}}_{\mathrm{ib}{k}_2}^b\×)---\left(9\right)$

其中 $C_{b,0}^{b_1}=C_b^{b_1}\left(t_1\right)=I,$ k₂为更新次数，步骤42）最终得到

5）在完成m个周期的循环导航解算以及对应的信息融合后，子惯导***根据t_m1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t₁时刻姿态矩阵的事后估计值，以及t₂时刻载体坐标系b相对于b₁的实时姿态矩阵得到t₂时刻的实时姿态矩阵用于子惯导的导航；同时子惯导得到t_m1时刻的陀螺误差估计值，即t₁时刻陀螺误差的事后估计值，将t_m1时刻的陀螺误差估计值作为最终的陀螺误差估计值；

T4）反复进行上述步骤T2）和步骤T3），通过离线仿真的形式，选择不同的时间点t₁，得到不同的循环导航解算循环数m，根据步骤1）-5）对陀螺误差进行快速估计，选择最能接近T1）中估计效果的t₁和m作为最佳时间点t₁和循环导航计算循环数m，正式用于舰载主/子惯导的传递对准，所述估计效果指T1）中子惯导陀螺仪误差收敛曲线的收敛效果；在实际舰载主/子惯导传递对准过程中进行步骤1）－5），实现陀螺误差快速估计。

所述实时多任务操作***为多任务实时操作***VxWorks，完成以下四个任务，任务的调度由多任务实时操作***VxWorks按照任务的优先级别自动执行，在子惯导中运行以下四个任务：

1）任务一：子惯导仪表数据采集、存储与导航解算与更新解算，优先级别最高；

2）任务二：子惯导仪表数据采集、更新解算，优先级别第二；

3）任务三：子惯导对主惯导进行数据采集、存储与信息融合，优先级别第三；

4）任务四：子惯导基于存储数据的循环导航解算与信息融合，优先级别最低。传递对准过程中，子惯导的导航解算算法为：

$C_{b,k}^n=C_{b,k-1}^n(I+\mathrm{\Δt}{\mathrm{\ω}}_{\mathrm{nb},k}^b\×)---\left(1\right)$

$V_k^n=V_{k-1}^n+\mathrm{\Δt}[C_{b,k-1}^n{f}_k^b-(2{\mathrm{\ω}}_{\mathrm{ie},k-1}^n+{\mathrm{\ω}}_{\mathrm{en},k-1}^n)\×V_{k-1}^n+g^n]---\left(2\right)$

$L_k=L_{k-1}+\frac{\mathrm{\Δt}{V}_{N,k-1}^n}{R_M+h_{k-1}},{\mathrm{\λ}}_k={\mathrm{\λ}}_{k-1}+\frac{\mathrm{\Δt}{V}_{E,k-1}^n\sec L_{k-1}}{R_N+h_{k-1}},h_k=h_{k-1}+\mathrm{\Δt}{V}_{U,k-1}^n---\left(3\right)$

式中，为子惯导载体坐标系b相对导航坐标系n的姿态矩阵，I为(3×3)单位矩阵，下标k=1~+∞表示导航解算的更新次数标号，n表示导航坐标系，b表示载体坐标系，i表示惯性坐标系，表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度，表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度；式（1）中的 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\left(C_b^n\right)}^T({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n),$ 为子惯导陀螺采集数据， ${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N^n/(R_M+h)& V_E^n/(R_N+h)& \left(V_E^n\tan L\right)/(R_N+h)\end{array}\right]}^T,$ $V^n=\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]$ 分别为子惯导的东、北、天向速度，L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度，f^b为子惯导加速度计采集数据，gⁿ为地球重力加速度在导航坐标系中的投影，R_M和R_N分别为导航所在地地球的子午圈和卯酉圈半径；运算符“×”表示向量反斜运算，舰载环境下，直接取子惯导天向速度与高度为零。

传递对准过程中，主、子惯导信息融合为：

采用Kalman滤波器作为信息融合滤波器，取子惯导东向/北向速度误差、失准角和陀螺误差作为***状态向量，即

X=[δV_E δV_N φ_E φ_N φ_U ε_x ε_y ε_z]^T

其中，δV_E/δV_N为东向/北向速度误差；φ_E、φ_N、φ_U分别为纵摇、横摇、航向失准角；ε_x、ε_y、ε_z分别为三轴陀螺误差；

取***状态方程，

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)---\left(4\right)$

式中，A(t)为状态矩阵，W(t)为***噪声，根据***状态变量中速度、失准角误差方程，以及陀螺从载体坐标系b向导航坐标系n的投影关系，状态矩阵A(t)表示为：

$A\left(t\right)=\left[\begin{array}{cccccccc}\frac{V_N}{R}\tan L& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& 0& {-f}_U& f_N& 0& 0& 0\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& f_U& 0& {-f}_E& 0& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})& -T_{11}& {-T}_{12}& {-T}_{13}\\ \frac{1}{R}& 0& -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& -\frac{V_N}{R}& {-T}_{21}& -T_{22}& {-T}_{23}\\ \frac{1}{R}\tan L& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0& -T_{31}& {-T}_{32}& {-T}_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，V_E、V_N为子惯导东向与北向速度，ω_ie为地球自转角速度，R为地球半径，L为当地地理纬度，f_E、f_N、f_U分别为子惯导加速度测量值f^b在导航坐标系n中的投影，T_pq,p,q=1,2,3为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素；

采用“速度＋航向”信息匹配方式，取子、主惯导的速度、航向差值直接构造量测信息，即子/主惯导导航***量测向量为

Z=[V_E-V_ME V_N-V_MN H-H_M]^T

其中，V_E与V_ME分别为子、主惯导的东向速度；V_N与V_MN分别为子、主惯导的北向速度；H与H_M分别为子、主惯导的航向信息，H从子惯导姿态矩阵中直接提取；

***量测方程为

Z(t)=H(t)X(t)+V(t)         （5）

式中，H(t)为量测矩阵，V(t)为量测噪声，根据量测向量与状态向量的关系，有量测矩阵为

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0_{3\×3}\\ 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{22}{T}_{32}}{T_{12}^2+T_{22}^2}& -1& \end{array}\right]$

信息融合过程中，采用闭环修正方式，将信息融合估计得到的速度误差、失准角与陀螺误差估计值反馈回子惯导***参与导航解算。

针对舰船主/子组合中的传递对准，本发明设计了一种适用于系泊/匀速直航条件下的陀螺误差快速估计方法，对子惯导陀螺常值误差进行快速估计，在实时多任务操作***的支持下，子惯导***对同一组数据进行循环导航解算与信息融合，充分利用计算机的高速性能，在传递对准过程中，快速完成对陀螺常值误差的估计。本发明的方法具有如下优点：（1）不改变现有传递对准的信息匹配模式，不要求运载体做特定机动；（2）直接利用目前比较成熟的Kalman滤波技术进行估计；（3）利用导航计算机在姿态更新周期与信息融合周期内的富余计算机资源进行工作，不对计算机性能提出更高要求，虽然对准速度很大程度上仍依赖于计算机性能，但在与现有导航***同样计算机资源配置的情况下，可以取得更快的传递对准速度。

附图说明

图1为本发明沿时间轴对准过程步骤分解图；

图2为本发明使用的信息融合算法示意图；

图3为本发明循环计算过程图；

图4为本发明系泊条件下失准角估计误差图；

图5为本发明系泊条件下对仪表误差的估计曲线图；

图6为本发明匀速直航条件下失准角估计误差图；

图7为本发明匀速直航条件下对陀螺误差的估计曲线图。

具体实施方式

本发明在分析捷联式惯性导航***以“数学平台”代替平台式惯性导航***中“物理平台”的相关特点后认为，在捷联式惯性导航***中，可以利用同一数据进行导航解算，并不断调整数学平台，充分利用计算机的高速性能，在传递对准过程中，快速完成对仪表误差的估计。

下面结合附图对本发明实施方法做更详细地描述：

图1中，t₀为陀螺常值误差估计的起始时刻，t₀~t₁为数据存储、导航解算与信息融合以及更新时间段，t₁~t₂为基于存储信息的循环导航解算与信息融合以及基于陀螺测量信息的更新时间段。Δt为子惯导的惯性仪表数据与导航解算更新周期；ΔT为主惯导信息更新周期与主子惯导信息融合周期，ΔT为Δt的整数倍，在本发明中，Δt为导航解算周期，ΔT为信息融合周期。对于确定的主/子惯导组合***而言，Δt与ΔT为确定值，且子惯导仪表数据与主惯导参考导航信息两者时序关系确定，也即导航解算与信息融合两者时序关系确定，导航解算次数确定后，信息融合次数即可确定。上述t₁的选择以及循环次数m的确定需要根据具体***通过多次仿真确定。具体如下：

T1）针对具体子惯导***，在主/子惯导传递对准中，通过观察子惯导陀螺误差估计曲线的收敛情况，确定完成整个对准所需要的时间，根据子惯导测量与导航更新周期，确定完成整个对准所需要的导航解算总更新次数k_sum；根据主惯导导航信息更新周期，确定完成整个对准所需要的信息融合次数；

T2）根据子惯导仪表数据质量，主惯导量测信息质量，选择时间t₁，并根据Δt计算得到该时间段内导航解算更新次数，设定为k_sum1，t₀~t₁是本发明中获取基础数据的时间段，后面的t₁~t₂时间段根据t₀~t₁时间段获得的数据进行进一步的误差估计，t₀~t₁的长短根据需要根据子惯导、主惯导的数据质量确定；

T3）从整个对准所需要的导航解算总更新次数k_sum中，扣除步骤T2）中t₀~t₁时间段内导航解算更新次数k_sum1，得到完成整个对准还需要完成的导航解算更新次数，设定为k_sum2=k_sum-k_sum1，从而得到t₁~t₂时间段的循环导航解算循环次数，设定为m=k_sum2/k_sum1，在m不为正数时，将m值向上圆整，并根据圆整后的m值调整k_sum2。时间t₂由循环导航解算循环次数和导航计算机的性能共同确定，在m确定时，计算机主频越高，t₂与t₁间差值越小；

T4）根据上述步骤T2）和步骤T3），通过离线仿真的形式，比较选择最佳时间点t₁和循环导航计算循环数m，实际用于主/子惯导的传递对准，所述最佳是指根据选定的时间点t₁和循环数m得到的陀螺误差估计最贴近实际子惯导陀螺误差估计曲线的收敛情况。

本发明在实时多任务操作***的支持下进行陀螺误差估计：

图1中，（a）在t₀~t₁时间段，由于信息融合计算量相对较大，一般需要若干Δt方可完成，若采用单任务程序模式，即完成信息融合后才能进行仪表数据采集与导航解算，则程序设计复杂，很难满足导航解算实时性要求；（b）在t₁~t₂时间段，基于存储的t₀~t₁时间段的数据进行导航解算与信息融合，需要占用除更新外的所有计算机资源，若采用单任务模式，则程序设计复杂，很难满足导航解算实时性要求。

基于单任务模式不能满足上述要求，引入实时多任务操作***VxWorks，在VxWorks环境下，定义如下任务：

1）任务一：仪表数据采集、存储与导航解算与更新解算，优先级别最高；

2）任务二：仪表数据采集、更新解算，优先级别第二；

3）任务三：主惯导数据采集、存储与信息融合，优先级别第三；

4）任务四：基于存储数据的循环导航解算与信息融合，优先级别最低。

上述任务一、三在初始化后被发起，按照任务的优先级别占用导航计算机资源，在不进行信息融合时，任务一单独占用导航计算机资源；在进行信息融合时，若需要进行仪表数据采集、存储与导航解算，则任务三放弃对导航计算机资源的占用，任务一占用，在任务一完成后，任务三重新占用。

在t₁时刻，当任务一完成后，任务一被删除，同时任务二被发起；在任务三完成后，任务三亦被删除，同时任务四被发起；在t₁~t₂时间段，任务二与任务四按照优先级别占用导航计算机资源，在任务二不需要执行时，任务四占用导航计算机资源进行循环导航解算。

本发明所采用的导航解算具体为：

$C_{b,k}^n=C_{b,k-1}^n(I+\mathrm{\Δt}{\mathrm{\ω}}_{\mathrm{nb},k}^b\×)---\left(1\right)$

$V_k^n=V_{k-1}^n+\mathrm{\Δt}[C_{b,k-1}^n{f}_k^b-({2\mathrm{\ω}}_{\mathrm{ie},k-1}^n+{\mathrm{\ω}}_{\mathrm{en},k-1}^n)\×V_{k-1}^n+g^n]---\left(2\right)$

$L_k=L_{k-1}+\frac{\mathrm{\Δt}{V}_{N,k-1}^n}{R_M+h_{k-1}},$ ${\mathrm{\λ}}_k={\mathrm{\λ}}_{k-1}+\frac{\mathrm{\Δt}{V}_{E,k-1}^n\sec L_{k-1}}{R_N+h_{k-1}},$ $h_k=h_{k-1}+\mathrm{\Δt}{V}_{U,k-1}^n---\left(3\right)$

式中，为子惯导载体坐标系b相对导航坐标系n的姿态矩阵，I为(3×3)单位矩阵，下标k=1~+∞表示导航解算的更新次数标号，n表示导航坐标系，b表示载体坐标系，i表示惯性坐标系，表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度，表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度。式（4）中的 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\left(C_b^n\right)}^T({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n),$ 为子惯导陀螺采集数据， ${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N^n/(R_M+h)& V_E^n/(R_N+h)& \left(V_E^n\tan L\right)/(R_N+h)\end{array}\right]}^T,$ $V^n=\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]$ 分别为子惯导的东、北、天向速度，L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度，f^b为子惯导加速度计采集数据，gⁿ为地球重力加速度在导航坐标系中的投影，R_M和R_N分别为导航所在地地球的子午圈和卯酉圈半径；运算符“×”表示向量反斜运算，舰载环境下，直接取子惯导天向速度与高度为零。本发明信息融合具体采用Kalman滤波器与“速度+航向”的匹配方式，如图2所示。

取东向/北向速度误差、失准角和陀螺误差作为***状态向量，即

X=[δV_E δV_N φ_E φ_N φ_U ε_xε_yε_z]^T

其中，δV_E/δV_N为东向/北向速度误差；φ_E~U为纵摇、横摇、航向失准角；ε_x~z为三轴陀螺误差。

取***状态方程，

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+W\left(t\right)---\left(4\right)$

式中，A(t)为状态矩阵，W(t)为***噪声，根据***状态变量中速度、失准角误差方程，以及陀螺从载体坐标系b向导航坐标系n投影关系，状态矩阵A(t)表示为：

$A\left(t\right)=\left[\begin{array}{cccccccc}\frac{V_N}{R}\tan L& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& 0& {-f}_U& f_N& 0& 0& 0\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& f_U& 0& {-f}_E& 0& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})& -T_{11}& {-T}_{12}& {-T}_{13}\\ \frac{1}{R}& 0& -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)& 0& -\frac{V_N}{R}& {-T}_{21}& -T_{22}& {-T}_{23}\\ \frac{1}{R}\tan L& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0& -T_{31}& {-T}_{32}& {-T}_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，V_E、V_N为东向与北向速度，ω_ie为地球自转角速度，R为地球半径，L为当地地理纬度，f_E、f_N、f_U分别为加速度测量值f^b在导航坐标系n中的投影，T_ij(i,j=1,2,3)为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素；

采用速度+航向信息匹配方式。

取子、主惯导的速度、航向差值直接构造量测信息，即***量测向量为

Z=[V_E-V_ME V_N-V_MN H-H_M]^T

其中，V_E与V_ME分别为子、主惯导的东向速度；V_N与V_MN分别为子、主惯导的北向速度；H与H_M分别为子、主惯导的航向信息，H可以从子惯导姿态矩阵中直接提取。

***量测方程为

Z(t)=H(t)X(t)+V(t)         （5）

式中，H(t)为量测矩阵，V(t)为量测噪声，根据量测向量与状态向量的关系，有量测矩阵为

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0_{3\×3}\\ 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{22}{T}_{32}}{T_{12}^2+T_{22}^2}& -1& \end{array}\right]$

***采用闭环修正方式，如图2所示，信息融合过程中，得到的关于速度误差、失准角与陀螺误差被反馈回子惯导***参与运算。完成循环导航解算与信息融合后，得到的t₁时刻陀螺误差估计值，即作为子惯导***陀螺误差的估计值。

下面具体说明本发明的陀螺误差估计过程：

1）在t₀时刻，利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化，得到子惯导的位置、速度与姿态矩阵t为时间变量；瞬时凝固t₀时刻的载体坐标系为b₀，得到I为(3×3)单位矩阵；

2）在时间段t₀~t₁内，子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算，并利用式（6）更新子惯导载体坐标系b相对于b₀系的姿态矩阵

$C_{{b,k}_1}^{b_0}=c_{{b,k}_1-1}^{b_0}(I+\mathrm{\Δt}{\mathrm{\ω}}_{{\mathrm{ib},k}_1}^b\×)---\left(6\right)$

式中，下标k₁=1~k_sum1表示导航解算更新的次数标号，更新次数k₁与姿态矩阵的变量时间t是对应的，为子惯导陀螺实时采集的数据，

同时子惯导按周期ΔT接收来自主惯导的导航信息并存储，所述导航信息包括速度和航向，并记录主惯导导航信息与子惯导仪表数据间的时序关系；通过信息融合进行主、子惯导传递对准；

3）在步骤2）结束_t1时刻，根据导航解算得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b₁，有

4）在时间段t₁~t₂内，子惯导***进行如下工作：

41）对步骤2）中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合，t₁~t₂内进行m次循环导航解算，如图3所示，各子循环周期为：t₁₀~t₁₁、t₂₀~t₂₁、…、t_j0～t_j1、…t_m0~t_m1，j=1~m表示子循环周期的标号，在各子循环的导航解算与信息融合过程中，所用子惯导仪表数据与主惯导信息分别对应t₀~t₁内子惯导***采集的仪表数据与主惯导的导航信息，在每一个子循环周期内，导航解算更新次数与信息融合次数与t₀~t₁内的次数分别相等，在各个子循环过程t_j0～t_j1中，所用仪表数据与主惯导信息分别对应t₀~t₁中各采集点对应，其物理意义体现t₀~t₁时间段的实时性，但解算过程占用t₁~t₂时间的计算机资源，即每次子循环都是对t₀~t₁时间段的数据进行导航解算，在t₁~t₂内进行m次，循环解算过程中，舰船处于系泊或匀速直航状态，各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置，但因船舶存在摇摆运动，各子循环解算开始时刻的姿态需要如下处理，在各子循环t_j0~t_j1开始时刻t_j0的初始姿态矩阵：

$C_b^n\left(t_{j0}\right)=C_b^n\left(t_{(j-1)1}\right){\left(C_b^{b_0}\left(t_1\right)\right)}^T---\left(7\right)$

式中，为在t_(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵，即第j-1个子循环解算结束时的姿态矩阵，

例如在t₁₀~t₁₁循环周期内，t₁₀为起始时刻，其初始姿态矩阵可计算如下：

$C_b^n\left(t_{10}\right)=C_{b_1}^n{C}_{b_0}^{b_1}=C_b^n\left(t_1\right)C_{b_0}^b\left(t_1\right)---\left(8\right)$

式中，为在t₁时刻解算得到的载体坐标系b相对导航坐标系n的姿态矩阵。根据导航解算后得到再进行下一轮循环解算，直至得到

在每一个子循环过程中，按步骤2）中记录的时序关系进行导航解算与信息融合；循环解算结束时，即在t₂时刻，因为是对t₀~t₁时间段的数据进行导航解算，上述循环导航解算得到姿态矩阵的实时性意义对应于t₁时刻，由根据前述导航解算得到；

42）在进行上述循环导航解算与信息融合的同时，子惯导对惯性仪表数据进行实时采集，更新计算当前载体坐标系b系相对于b₁系的姿态矩阵其离散形式为：

$C_{{b,k}_2}^{b_1}=c_{{b,k}_2-1}^{b_1}(I+\mathrm{\Δt}{\mathrm{\ω}}_{{\mathrm{ib},k}_2}^b\×)---\left(9\right)$

其中 $C_{b,0}^{b_1}=C_b^{b_1}\left(t_1\right)=I,$ k₂为更次次数。步骤42）最终得到k₂的取值上限由循环次数m以及计算机主频共同决定。时间段t₁~t₂内步骤41）与步骤42）同时进行，时间上步骤41）会先于步骤42）完成。

5）在完成m个周期的循环导航解算以及对应的信息融合后，子惯导***得到t_m1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t₁时刻姿态矩阵的事后估计值，以及t₂时刻载体坐标系b相对于b₁的实时姿态矩阵得到t₂时刻的实时姿态矩阵 $C_b^n\left(t_2\right)=C_{b_1}^n{C}_b^{b_1}\left(t_2\right)=C_b^n\left(t_{m1}\right)C_b^{b_1}\left(t_2\right),$ 用于子惯导的导航；同时子惯导由第m次子循环的信息融合得到t_m1时刻的陀螺误差估计值，即t₁时刻陀螺误差的事后估计值，将t_m1时刻的陀螺误差估计值作为最终的陀螺误差估计值；

T4）反复进行上述步骤T2）和步骤T3），通过离线仿真的形式，选择不同的时间点t₁，得到不同的循环导航解算循环数m，根据步骤1）－5）对陀螺误差进行快速估计，选择最能接近T1）中估计效果的t₁和m作为最佳时间点t₁和循环导航计算循环数m，此处估计效果指T1）中子惯导陀螺仪误差收敛曲线的收敛效果，正式用于舰载主/子惯导的传递对准，在实际舰载主/子惯导传递对准过程中进行步骤1）－5），实现陀螺误差快速估计。

本发明的有益效果通过如下仿真得以验证：

1）Matlab模拟惯性仪表数据与主惯导数据

（1）系泊条件下模拟数据的产生

舰船在系泊条件下，以正弦规律绕三轴作摇摆运动，其纵摇、横向与航向摇摆数学模型为：

式中，θ、γ和ψ分别为纵摇、横摇与航向的角度变量；A_P、A_R、A_Y分别为纵摇、横摇与航向的摇摆幅值；ω_P、ω_P与ω_Y分别表示纵摇、横摇与航向的摇摆角频率；与分别纵摇、横摇与航向初始相位；ψ₀表示初始航向；ω_i=2π/T_i，i＝P,R,Y，T_i表示相应的摇摆周期。

以上述仿真数据并叠加白噪声作为主惯导信息，主惯导对所述数据的采样周期为1s。

通过上述仿真数据模拟得到子惯导仪表理论数据，并在其上叠加相应的仪表误差作为仪表实际采集数据，子惯导对所述仪表实际采集数据进行采样，用于导航解算，采样周期为10ms。

仿真的相关参数：

舰船初始位置：东经118°、北纬32°；

舰船航速：0m/s；

舰船摇摆幅值：纵摇9°、横摇14°、航摇12°；

舰船摇摆周期：纵摇8s、横摇10s、航摇6s；

舰船摇摆初始相位：均为0；

舰船初始航向：0°；

赤道半径：6378165m；

地球椭球度：1/298.3；

地球表面重力加速度：9.8m/s²；

地球自转角速度：15.04088°/h；

陀螺常值误差：0.5°/h；

陀螺白噪声误差：0.5°/h；

加速度计零偏：500ug；

加速度计白噪声误差：500ug；

主惯导***量测白噪声参数：东/北向速度误差方差0.2m/s、航向误差方差0.15°/h。

（2）匀速直航条件下模拟数据的产生

匀速直航条件下相关参数与系泊条件相同，不同点为：舰船航速为10m/s。

2）导航计算机上数学验证

在x-86结构的导航计算机上分别采用两种方案进行验证，方案一：经典传递对准，即以时间为顺序进行仪表数据采集、导航解算，并在有主惯导量测信息时进行信息融合；方案二：采用本发明设计的快速对准算法，其中数据存储120s，即t₁=120s。分别比较两种方案的对准精度与对准时间。

图4与图5的曲线表明，在系泊条件下，两种方案的对准精度相当，对陀螺误差的估计亦相当。图6与图7表明，在匀速直航条件下，两种方案的对准精度相当，对陀螺误差的估计亦相当。

但两种方案所用的对准时间不同。

对于方案一而言，图4～7中横坐标表示时间，在系泊与匀速直航条件下，方案一完成对失准角的估计需要200s左右，而对陀螺误差的估计大约需要600s。

对于方案二而言，图4～7中横坐标表示迭代次数，完成对失准角的估计需要迭代次数20000次，完成对陀螺误差的估计大约需要60000次。以完成迭代60000次分析计算机耗时，其中前12000次，需要存储惯性仪表数据与主惯导量测数据，耗时120s，该耗时与方案一相同；但在12001～60000（共计480000）次，所用时间由计算机速度决定。

在本仿真中，所用计算机主频333MHz。该型计算机完成一次导航解算以及或需要时间1ms；单独完成一次完成或需要约0.2ms；完成一次信息融合需要25ms。在48000次导航更新中，需要进行信息融合480次，导航更新耗时38.4s，信息融合耗时12s，共计耗时50.4s。在循环计算过程中，计算机需要更新以及***任务调度开销，计算机实际测量耗时59.2s。整个对准过程耗时120+59.2=179.2s，即t₂=179.2s，对陀螺误差的估计时间远小于方案一的600s。

因此本发明设计的方法在对准精度上与经典对准方案相当，但是对陀螺误差的估计速度得到显著提高。

Claims (4)

1.一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法，其特征是在实时多任务操作***的支持下对陀螺常值误差进行快速估计，主/子惯导传递对准过程中，子惯导进行导航解算，子惯导利用主惯导的导航数据进行信息融合，设t₀为陀螺常值误差估计的起始时刻，亦为初始对准的起始时刻，t₀～t₁为数据采集、数据存储、实时导航解算与信息融合以及姿态矩阵更新时间段，t₁～t₂为基于t₀～t₁间存储信息的循环导航解算与信息融合以及姿态矩阵更新时间段，上述各时间的确定如下：

T1)针对具体子惯导***，在主/子惯导传递对准中，通过子惯导陀螺仪误差收敛曲线确定完成整个对准所需要的时间，再根据子惯导测量与导航解算更新周期Δt，确定完成整个对准所需要的导航解算总更新次数，设定为k_sum；根据主惯导导航信息更新周期ΔT，确定完成整个对准所需要的信息融合次数，ΔT即为信息融合周期；在确定的工作环境下，导航解算与信息融合两者时序关系确定，导航解算次数确定后，信息融合次数即可确定；

T2)根据子惯导仪表数据质量，主惯导量测信息质量，设定时刻t₁及时间段t₀～t₁，并根据Δt计算得到该时间段内导航解算更新次数，设定为k_sum1；

T3)从整个对准所需要的导航解算总更新次数k_sum中，扣除步骤T2)中t₀～t₁时间段内导航解算更新次数k_sum1，得到完成整个对准还需要完成的导航解算更新次数，设定为k_sum2，k_sum2＝k_sum-k_sum1，从而得到t₁～t₂时间段的循环导航解算循环次数，设定为m，m＝k_sum2/k_sum1，在m不为整数时，将m值向上圆整，并根据圆整后的m值调整k_sum2；时刻t₂由循环导航解算循环次数m和导航计算机的性能共同确定，在m确定时，导航计算机主频越高，t₂与t₁间差值越小；

根据所述确定的时间，在传递对准中完成子惯导陀螺误差估计，包括如下步骤：

1)在t₀时刻，利用主惯导的位置、速度与姿态信息对子惯导的相应导航信息进行初始化，得到子惯导的位置、速度与姿态矩阵t为时间变量；瞬时凝固t₀时刻的载体坐标系为b₀，得到I为(3×3)单位矩阵；

2)在时间段t₀～t₁内，子惯导按周期Δt进行惯性仪表数据的采集、存储、导航解算，并利用式(1)更新子惯导载体坐标系b相对于b₀系的姿态矩阵

式中，下标k₁＝1～k_sum1表示导航解算更新的次数标号，为子惯导陀螺实时采集的数 据，运算符“×”表示向量反斜运算；

同时子惯导按照周期ΔT接收来自主惯导的导航信息并存储，所述导航信息包括速度和航向，并记录主惯导导航信息与子惯导仪表数据间的时序关系；通过信息融合进行主、子惯导传递对准；

3)在步骤2)结束t₁时刻，得到该时刻的实时载体矩阵瞬时凝固此时刻的载体坐标系为b₁，有

4)在时间段t₁～t₂内，子惯导***进行如下工作：

41)对步骤2)中采集的子惯导惯性仪表数据进行m次循环导航解算和信息融合，t₁～t₂内进行m次循环导航解算，各子循环周期为：t₁₀～t₁₁、t₂₀～t₂₁、t_j0～t_j1、t_m0～t_m1，j＝1～m表示子循环周期的标号，在各子循环的导航解算与信息融合过程中，所用子惯导仪表数据与主惯导信息分别对应t₀～t₁内子惯导***采集的仪表数据与主惯导的导航信息，在每一个子循环周期内，导航解算更新次数与信息融合次数与t₀～t₁内的次数分别相等；循环解算过程中，舰船处于系泊或匀速直航状态，各子循环解算开始时刻的速度、位置直接取上一子循环周期结束时刻的速度与位置，但因船舶存在摇摆运动，各子循环解算开始时刻的姿态需要如下处理，在各子循环t_j0～t_j1开始时刻t_j0的初始姿态矩阵：

式中，为在t_(j-1)1时刻计算得到的载体坐标系b相对导航坐标系n的姿态矩阵，即第j-1个子循环解算结束时的姿态矩阵，

在每一个子循环过程中，按步骤2)中记录的时序关系进行导航解算与信息融合；

42)在进行上述循环导航解算与信息融合的同时，子惯导对惯性仪表数据进行实时采集，更新计算当前载体坐标系b系相对于b₁系的姿态矩阵其离散形式为：

其中k₂为更新次数，运算符“×”表示向量反斜运算，步骤42)最终得到

5)在完成m个周期的循环导航解算以及对应的信息融合后，子惯导***根据t_m1时刻载体坐标系b相对于导航坐标系n的姿态估计矩阵即t₁时刻姿态矩阵的事后估计值，以及t₂时刻载体坐标系b相对于b₁的实时姿态矩阵得到t₂时刻的实时姿 态矩阵用于子惯导的导航；同时子惯导得到t_m1时刻的陀螺误差估计值，即t₁时刻陀螺误差的事后估计值，将t_m1时刻的陀螺误差估计值作为最终的陀螺误差估计值；

T4)反复进行上述步骤T2)和步骤T3)，通过离线仿真的形式，选择不同的时间点t₁，得到不同的循环导航解算循环数m，根据步骤1)－5)对陀螺误差进行快速估计，选择最能接近T1)中估计效果的t₁和m作为最佳时间点t₁和循环导航计算循环数m，正式用于舰载主/子惯导的传递对准，所述估计效果指T1)中子惯导陀螺仪误差收敛曲线的收敛效果；在实际舰载主/子惯导传递对准过程中进行步骤1)－5)，实现陀螺误差快速估计。

2.根据权利要求1所述的一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法，其特征是所述实时多任务操作***为多任务实时操作***VxWorks，完成以下四个任务，任务的调度由多任务实时操作***VxWorks按照任务的优先级别自动执行，在子惯导中运行以下四个任务：

1)任务一：子惯导仪表数据采集、存储与导航解算与更新解算，优先级别最高；

2)任务二：子惯导仪表数据采集、更新解算，优先级别第二；

3)任务三：子惯导对主惯导进行数据采集、存储与信息融合，优先级别第三；

任务四：子惯导基于存储数据的循环导航解算与信息融合，优先级别最低。

3.根据权利要求1或2所述的一种舰载主/子惯导传递对准过程中的陀螺误差

快速估计方法，其特征是传递对准过程中，子惯导的导航解算算法为：

式中，为子惯导载体坐标系b相对导航坐标系n的姿态矩阵，I为(3×3)单位矩阵，下标k＝1～+∞表示导航解算的更新次数标号，n表示导航坐标系，b表示载体坐标系，i表示惯性坐标系，表示地球坐标系e相对于惯性坐标系i在导航坐标系n中的投影角速度，表示导航坐标系n相对于地球坐标系e在导航坐标系n中的投影角速度；式(4)中的 为子惯导陀螺采集数据，  分别为子惯导的东、北、天向速度，L、λ和h分别表示导航坐标系下的速度、纬度、经度和高度，f^b为子惯导加速度计采集数据，gⁿ为地球重力加速度在导航坐标系中的投影，R_M和R_N分别为导航所在地地球的子午圈和卯酉圈半径，运算符“×”表示向量反斜运算，舰载环境下，直接取子惯导天向速度与高度为零。

4.根据权利要求1或2所述的一种舰载主/子惯导传递对准过程中的陀螺误差快速估计方法，其特征是传递对准过程中，主、子惯导信息融合为：

采用Kalman滤波器作为信息融合滤波器，取子惯导东向/北向速度误差、失准角和陀螺误差作为***状态向量，即

X＝[δV_E δV_N φ_E φ_N φ_U ε_x ε_y ε_z]^T

其中，δV_E/δV_N为东向/北向速度误差；φ_E、φ_N、φ_U分别为纵摇、横摇、航向失准角；ε_x、ε_y、ε_z分别为三轴陀螺误差；

取***状态方程，

式中，A(t)为状态矩阵，W(t)为***噪声，根据***状态变量中速度、失准角误差方程，以及陀螺从载体坐标系b向导航坐标系n的投影关系，状态矩阵A(t)表示为：

其中，V_E、V_N为子惯导东向与北向速度，ω_ie为地球自转角速度，R为地球半径，L为当地地理纬度，f_E、f_N、f_U分别为子惯导加速度测量值f^b在导航坐标系n中的投影，T_pq,p,q＝1,2,3为子惯导载体坐标系b相对导航坐标系n的姿态矩阵的各元素；

采用“速度+航向”信息匹配方式，取子、主惯导的速度、航向差值直接构造量测信息，即子/主惯导导航***量测向量为

Z＝[V_E-V_ME V_N-V_MN H-H_M]^T

其中，V_E与V_ME分别为子、主惯导的东向速度；V_N与V_MN分别为子、主惯导的北向速度；H与H_M分别为子、主惯导的航向信息，H从子惯导姿态矩阵中直接提取；

***量测方程为

Z(t)＝H(t)X(t)+V(t)   (8)

式中，H(t)为量测矩阵，V(t)为量测噪声，根据量测向量与状态向量的关系，有量测矩阵为

信息融合过程中，采用闭环修正方式，将信息融合估计得到的速度误差、失准角与陀螺误差估计值反馈回子惯导***参与导航解算。