CN101576385A - 光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法 - Google Patents

Abstract

本发明提供的是一种船用光纤陀螺捷联惯导系统消除不确定性干扰的系泊精对准方法。对于系泊状态下的船用光纤陀螺捷联惯导系统，纵荡，横荡，垂荡等不确定性干扰造成加速度计的输出信息严重污染，很难得到精确的捷联矩阵。设计两级抽取、三级子滤波的FIR低通滤波器，处理加速度计输出信息在计算惯性坐标系内的投影，滤除高频不确定性干扰，得到低频惯性系重力矢量，并据此求得系泊条件下载体在惯性空间运动的线速度。利用惯性系重力矢量和惯性空间运动的线速度进行卡尔曼滤波估计失准角，对捷联矩阵进行补偿，得到较为精确的捷联矩阵，完成船用光纤陀螺捷联惯导系统系泊精对准，对准精度高。

Description

光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法

(一)技术领域

本发明涉及的是一种测量方法，特别是涉及一种捷联惯导系统的初始对准技术，尤其涉及一种船用光纤陀螺捷联惯导系统消除不确定性干扰的系泊精对准方法。

(二)背景技术

惯性导航系统利用陀螺仪、加速度计测量载体在惯性空间的线运动和角运动，通过合理的机械编排和控制实现导航定位。捷联惯导系统的初始对准是指在载体进行自主导航之前通过一定的方法给出载体的初始姿态矩阵。捷联惯导系统导航精度在很大程度上取决于初始对准的精度，因此在导航前得到精确的初始姿态阵尤为重要。

初始对准按基座的运动情况，可分为静基座对准和动基座对准。大量文献和工程实践证明，对于船用光纤陀螺捷联惯导系统，其静基座对准无论在时间上还是在精度上，都达到了很好的要求。在复杂的海洋环境中，由于阵风、浪涌等不确定性干扰的影响，舰船无法对地静止，加速度计信息受到严重污染，因而对准效果往往不太好。而传统的精对准方法(如二阶调平法)在系泊条件下由于受到垂荡、纵荡、横荡或高频随机干扰时对准性能大大下降，往往不能满足对准的精度要求。罗经回路法和方位估算法在舰船的系泊环境下无法使用，因此如何提高不确定干扰条件下船用光纤陀螺捷联惯导系统系泊精对准的精度有着十分重要的意义。

(三)发明内容

本发明的目的在于提供一种能够有效提高船用光纤陀螺捷联惯导系统系泊精对准精度的光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法。

本发明的目的是这样实现的：

本发明包括下列步骤：

(1)采集光纤陀螺捷联惯导系统光纤陀螺仪组件和石英挠性加速度计组件的输出数据，确定精对准的捷联矩阵初值C_b ^n′(0)。

(2)设计两级抽取、三级子滤波的FIR低通滤波器，处理加速度计输出信息在计算惯性坐标系内的投影，得到重力矢量，并据此求得系泊条件下载体在惯性空间运动的线速度；

(3)建立惯性系下以速度误差和失准角为状态变量的卡尔曼滤波状态方程及惯性系速度为量测量的量测方程，利用卡尔曼滤波估计出失准角。

(4)利用步骤(3)估计得到的失准角对姿态矩阵C_b ^n′进行误差补偿，得到精确的姿态矩阵，根据姿态矩阵得到载体姿态，完成初始对准，进入导航。

本发明还可以包括如下特征：

1、步骤1中所述的精对准捷联矩阵初值：

$C_b^{n^{\′}}\left(0\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中θ、γ、

是粗对准结束时刻的纵摇角、横摇角和航向角。

2、设计FIR低通滤波器。依据采样信号与期望信号之间的关系设计两级抽取、三级子滤波的级联滤波方案。FIR低通滤波器由三个子滤波器和两个抽取模块组成。图2中sub-filter表示子滤波器，子滤波器都是设计为带通滤波器，decimation表示抽取模块，M₁和M₂是抽取模块的系数。含有不确定性扰动的加速度计输出信息经过计算可以得到其在计算惯性坐标系内投影的计算值：

$f^{i^{\′}}=C_b^{i^{\′}}{f}^b$

将信息f^i′经过级联滤波器滤波，滤除高频不确定性干扰，得到计算惯性系内的低频重力矢量：

gⁱ＝F_FIR(f^i′)

对于当前t时刻的输入fⁱ，滤波器的输出结果则对应于t_M(t_M＝t-delay)时刻的值。

将低频重力矢量gⁱ经过积分运算，得到系泊条件下载体在惯性空间运动的线速度：

$V_g^i=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}$

3、建立惯性系下以速度误差和失准角误差为状态变量的卡尔曼滤波状态方程及惯性系速度为量测量的量测方程；估计出失准角并对姿态矩阵C_b ^n′进行误差补偿，得到精确的姿态矩阵，根据姿态矩阵得到载体姿态，完成初始对准，进入导航。

1)建立卡尔曼滤波状态方程：

使用一阶线性随即微分方程来描述捷联航姿系统的状态误差：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

系统的状态向量为：

系统的白噪声向量为：

$W={\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

其中δV_E、δV_N、δV_D分别表示X、Y、Z轴的速度误差；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；分别为X、Y、Z轴加速度计的白噪声误差；

分别为X、Y、Z轴陀螺的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中：

$g^i=\left[\begin{array}{c}-g\cos L\cos [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\cos L\sin [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\sin L\end{array}\right]$

C_b ⁱ由陀螺的输出通过四元数更新得到。t₀为精对准开始时间。

2)建立卡尔曼滤波的量测方程：

使用一阶线性随即微分方程来描述捷联航姿系统的量测方程如下：

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

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)＝[I_3×3  0_3×9]

量测量为：

$Z\left(t\right)=V_f^i\left(t\right)+V_g^i\left(t\right)$

其中

$V_g^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}$

$V_f^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{C}_b^{i^{\′}}{f}^b\mathrm{d\τ}$

则系统的状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X+\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{\▿}_w^b\\ {\mathrm{\ϵ}}_w^b\\ 0_{3\×1}\\ 0_{3\×1}\end{array}\right]$

$Z=\left[\begin{array}{c}{\tilde{V}}_{\mathrm{fx}}^i+V_{\mathrm{gx}}^i\\ {\tilde{V}}_{\mathrm{fy}}^i+V_{\mathrm{gy}}^i\\ {\tilde{V}}_{\mathrm{fz}}^i+V_{\mathrm{gz}}^i\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×9}\end{array}\right]X+V_w$

根据系统的状态方程和量测方程，用卡尔曼滤波估计出失准角

4、利用步骤3估计出的误差失准角

得到计算坐标系与真实坐标系之间的姿态误差矩阵

其中

表示

的反对称矩阵：

根据： $C_b^{n^{\′}}\left(t\right)=C_i^{n^{\′}}\left(t\right)C_{i^{\′}}^i{C}_b^{i^{\′}}\left(t_2\right)$

对捷联矩阵C_b ⁿ进行修正，得到精对准结束后的捷联矩阵C_b ⁿ。

根据C_b ⁿ(t)可以得到载体姿态，即纵摇角、横摇角和航向角的主值如下：

由上述三个主值判断真值的公式如下：

θ＝θ_主

至此，初始对准完成，可以进入导航状态。

本发明的方法具有如下优点：(1)在较为恶劣的对准环境中，比如受阵风、涌浪等影响，加速度计输出通过低通FIR滤波器后，横荡、纵荡、垂荡等高频扰动分量显著衰减，低频重力信号在较好的精度范围内保持不变，采用惯性系kalman滤波精对准算法，计算过程中进一步消除剩余的周期干扰，对准精度大大提高；(2)克服了传统的罗经回路法和方位估算法在系泊环境下无法使用的缺点，并且不需要其它设备提供外信息，在不增加系统成本的条件下，可以达到较高适用精度的要求。

为了验证本发明的实用性，进行了三组系泊实验，选取自行研制的光纤陀螺捷联惯性系统(以下简称光纤陀螺捷联惯导系统)进行实验。光纤陀螺捷联惯导系统的光纤陀螺零偏为0.01°/h，加速度计零偏为5×10^-5g。以PHINS与GPS组合作为姿态基准，使用时先将PHINS与GPS做组合对准，然后使PHINS处于组合导航状态。将它输出的姿态角与姿态基准做比较确定对准的精度。以下结果中的姿态误差曲线都是姿态基准的输出与光纤陀螺捷联系统的输出相减得到的。

(1)无锡太湖系泊实验

2008年8月15日，在无锡太湖做了系泊对准实验，实验结果如图6-图11所示。

(2)松花江系泊实验

2008年10月，为了验证光纤陀螺惯性系统快速系泊对准方法和惯性系下光纤陀螺惯性系统系泊精对准方法的实际对准效果，在松花江上进行了对准实验，实验结果如图12-图17所示。从对准结果可以看出，水平对准精度优于0.02°，航向对准精度优于0.06°。

(3)江西九江系泊实验

2008年12月18日，在江西九江做了系泊对准实验，实验结果如图18-图23所示，其对准结果与松花江系泊实验和无锡系泊实验的基本一致。

以上三组实验结果表明，光纤陀螺惯性系统系泊精对准方法在舰船系泊条件可以提高惯性系统的对准精度，使其达到工程应用的标准。

(四)附图说明

图1系泊精对准原理图；图2 FIR滤波器级联滤波方案；图3第一级FIR滤波器的幅频特性；图4第二级FIR滤波器的幅频特性；图5第三级FIR滤波器的幅频特性；图6无锡太湖系泊对准实验航向误差曲线；图7无锡太湖系泊对准实验纵摇误差曲线；图8无锡太湖系泊对准实验横摇误差曲线；图9松花江系泊对准实验航向误差曲线；图10松花江系泊对准实验纵摇误差曲线；图11松花江系泊对准实验横摇误差曲线；图12九江系泊对准实验航向误差曲线；图13九江系泊对准实验纵摇误差曲线；图14九江系泊对准实验横摇误差曲线。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

(1)采集光纤陀螺捷联惯导系统光纤陀螺仪组件和石英挠性加速度计组件的输出数据，利用粗对准给出的姿态信息，建立载体坐标系b和计算地理坐标系n′之间的转换矩阵C_b ^n′，将粗对准结束时刻得到的矩阵C作为精对准的初始捷联矩阵C_b ^n′(0)：

$C_b^{n^{\′}}\left(0\right)=C=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(1\right)$

i和n之间的转换矩阵C_i ⁿ(t)为：

$C_i^n\left(t\right)=\left[\begin{array}{ccc}-\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& \cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& 0\\ -\sin L\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& -\sin L\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& \cos L\\ \cos L\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& \cos L\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)& \sin L\end{array}\right]---\left(2\right)$

其中λ和L分别为捷联航姿系统所在的经度和纬度，ω_ie为地球自转角速率；

初始时刻载体坐标系b和计算惯性坐标系i′之间的转换矩阵：

$C_b^{i^{\′}}={\left[C_i^n\left(0\right)\right]}^T{C}_b^{n^{\′}}---\left(3\right)$

C_b ^i′由导航计算机对陀螺仪敏感的角速度信号采用四元数算法实时计算，建立与C_b ^i′相对应的四元数为：

$Q_b^{i^{\′}}={\left[\begin{array}{cccc}{\tilde{q}}_0& {\tilde{q}}_1& {\tilde{q}}_2& {\tilde{q}}_3\end{array}\right]}^T---\left(4\right)$

时时更新Q，得到与Q相对应的

(2)设计FIR低通滤波器。依据采样信号与期望信号之间的关系设计两级抽取、三级子滤波的级联滤波方案。FIR低通滤波器由三个子滤波器和两个抽取模块组成。图2中sub-filter表示子滤波器，子滤波器都是带通滤波器，decimation表示抽取模块，M₁和M₂是抽取模块的系数。对于当前t时刻的输入fⁱ，滤波器的输出结果则对应于t_M(t_M＝t-delay)时刻的值。根据Malab/fdatool提供的滤波器设计工具，并根据实际采样频率，设计三个子滤波器，相应的幅频特性曲线如图3-5所示。对于当前t时刻的输入fⁱ，滤波器的输出结果则对应于t_M(t_M＝t-delay)时刻的值。含有不确定性扰动的加速度计输出信息：

$f^b=-g^b+{\▿}^b+a_D^b---\left(5\right)$

含有不确定性扰动的加速度计输出信息经过计算可以得到其在计算惯性坐标系内投影的计算值：

$f^{i^{\′}}=C_b^{i^{\′}}{f}^b---\left(6\right)$

将信息f^i′经过级联滤波器滤波，滤除高频不确定性干扰，得到计算惯性系内的低频重力矢量：

gⁱ＝F_FIR(f^i′)    (7)

对于当前t时刻的输入fⁱ，滤波器的输出结果则对应于t_M(t_M＝t-delay)时刻的值。将低频重力矢量gⁱ经过积分运算，得到系泊条件下载体在惯性空间运动的线速度：

$V_g^i=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}---\left(8\right)$

(3)建立惯性系下以速度误差和失准角误差为状态变量的卡尔曼滤波状态方程及惯性系速度为量测量的量测方程；估计出失准角

对姿态矩阵C_b ^n′进行误差补偿，得到精确的姿态矩阵，根据姿态矩阵得到载体姿态，完成初始对准，进入导航。

1)建立卡尔曼滤波状态方程：

使用一阶线性随即微分方程来描述捷联航姿系统的状态误差：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t))为系统的噪声向量；

系统的状态向量为：

系统的白噪声向量为：

$W={\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T---\left(11\right)$

其中δV_E、δV_N、δV_D分别表示X、Y、Z轴速度误差；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；

分别为X、Y、Z轴加速度计的白噪声误差；

分别为X、Y、Z轴陀螺的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(12\right)$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(13\right)$

其中：

$g^i=\left[\begin{array}{c}-g\cos L\cos [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\cos L\sin [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\sin L\end{array}\right]---\left(14\right)$

t₀为粗对准开始时间。

2)建立卡尔曼滤波的量测方程：

使用一阶线性随即微分方程来描述捷联航姿系统的量测方程如下：

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

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)＝[I_3×3 0_3×9]    (16)

量测量为：

$Z\left(t\right)=V_f^i\left(t\right)+V_g^i\left(t\right)---\left(17\right)$

其中

$V_g^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}---\left(18\right)$

$V_f^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{C}_b^{i^{\′}}{f}^b\mathrm{d\τ}---\left(19\right)$

则系统的状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X+\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{\▿}_w^b\\ {\mathrm{\ϵ}}_w^b\\ 0_{3\×1}\\ 0_{3\×1}\end{array}\right]---\left(20a\right)$

$Z=\left[\begin{array}{c}{\tilde{V}}_{\mathrm{fx}}^i+V_{\mathrm{gx}}^i\\ {\tilde{V}}_{\mathrm{fy}}^i+V_{\mathrm{gy}}^i\\ {\tilde{V}}_{\mathrm{fz}}^i+V_{\mathrm{gz}}^i\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×9}\end{array}\right]X+V_w---\left(20b\right)$

根据系统的状态方程和量测方程，用卡尔曼滤波估计出失准角

(4)利用步骤(3)估计出误差失准角

得到计算坐标系与真实坐标姿态之间的误差矩阵

其中

表示

的反对称矩阵：

根据式 $C_b^{n^{\′}}\left(t\right)=C_i^{n^{\′}}\left(t\right)C_{i^{\′}}^i{C}_b^{i^{\′}}\left(t_2\right)$ 对捷联矩阵C_b ⁿ进行修正，得到精对准结束后的捷联矩阵C_b ⁿ。

根据C_b ⁿ(t)可以得到载体姿态，即纵摇角、横摇角和航向角的主值如下：

由上述三个主值判断真值的公式如下：

θ＝θ_主

至此，初始对准完成，可以进入导航状态。

Claims (5)

1、一种光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法，其特征包括以下步骤：

(1)采集光纤陀螺捷联惯导系统光纤陀螺仪组件和石英挠性加速度计组件的输出数据，确定精对准的捷联矩阵初值C_b ⁿ′(0)；

(2)设计两级抽取、三级子滤波的FIR低通滤波器，处理加速度计输出信息在计算惯性坐标系内的投影，得到重力矢量，并据此求得系泊条件下载体在惯性空间运动的线速度；

(3)建立惯性系下以速度误差和失准角为状态变量的卡尔曼滤波状态方程及惯性系速度为量测量的量测方程，利用卡尔曼滤波估计出失准角；

(4)利用步骤(3)估计得到的失准角对姿态矩阵C_b ⁿ′进行误差补偿，得到精确的姿态矩阵，根据姿态矩阵得到载体姿态，完成初始对准。

2、根据权利要求1所述的光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法，其特征是：

所述的精对准捷联矩阵初值：

$C_b^{n^{\′}}\left(0\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中θ、γ、是粗对准结束时刻的纵摇角、横摇角和航向角。

3、根据权利要求2所述的光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法，其特征是：

所述的设计两级抽取、三级子滤波的FIR低通滤波器的方法为：依据采样信号与期望信号之间的关系设计两级抽取、三级子滤波的级联滤波方案，FIR低通滤波器由三个子滤波器和两个抽取模块组成，其中sub-filter表示子滤波器，子滤波器都是设计为带通滤波器，decimation表示抽取模块，M₁和M₂是抽取模块的系数；含有不确定性扰动的加速度计输出信息经过计算得到其在计算惯性坐标系内投影的计算值：

$f^{i^{\′}}=C_b^{i^{\′}}{f}^b$

将信息fⁱ′经过级联滤波器滤波，滤除高频不确定性干扰，得到计算惯性系内的低频重力矢量：

gⁱ＝F_FIR(fⁱ′)

对于当前t时刻的输入fⁱ，滤波器的输出结果则对应于t_M(t_M＝t-delay)时刻的值；将低频重力矢量gⁱ经过积分运算，得到系泊条件下载体在惯性空间运动的线速度：

$V_g^i=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}.$

4、根据权利要求3所述的光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法，其特征是：

所述的建立惯性系下以速度误差和失准角误差为状态变量的卡尔曼滤波状态方程及惯性系速度为量测量的量测方程；估计出失准角并对姿态矩阵C_b ⁿ′进行误差补偿，得到精确的姿态矩阵，根据姿态矩阵得到载体姿态，完成初始对准，进入导航；

1)建立卡尔曼滤波状态方程：

使用一阶线性随即微分方程来描述捷联航姿系统的状态误差：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

系统的状态向量为：

系统的白噪声向量为：

$W={\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_x}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

其中δV_E、δV_N、δV_D分别表示X、Y、Z轴的速度误差；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；分别为X、Y、Z轴加速度计的白噪声误差；

分别为X、Y、Z轴陀螺的白噪声误差；

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& C_b^i\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中：

$g^i=\left[\begin{array}{c}-g\cos L\cos [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\cos L\sin [\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)]\\ -g\sin L\end{array}\right]$

t₀为精对准开始时间；

2)建立卡尔曼滤波的量测方程：

使用一阶线性随即微分方程来描述捷联航姿系统的量测方程如下：

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

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)＝[I_3×3  0_3×9]

量测量为：

$Z\left(t\right)=V_f^i\left(t\right)+V_g^i\left(t\right)$

其中

$V_g^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{g}^i\mathrm{d\τ}$

$V_f^i\left(t\right)=\underset{t_0}{\overset{t}{\∫}}{C}_b^{i^{\′}}{f}^b\mathrm{d\τ}$

则系统的状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{cccc}{0}_{3\×3}& -(g^i\×)& 0_{3\×3}& C_b^i\\ 0_{3\×3}& 0_{3\×3}& -C_b^i& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X+\left[\begin{array}{cccc}{C}_b^i& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{\▿}_w^b\\ {\mathrm{\ϵ}}_w^b\\ 0_{3\×1}\\ 0_{3\×1}\end{array}\right]$

$Z=\left[\begin{array}{c}{\tilde{V}}_{\mathrm{fx}}^i+V_{\mathrm{gx}}^i\\ {\tilde{V}}_{\mathrm{fy}}^i+V_{\mathrm{gy}}^i\\ {\tilde{V}}_{\mathrm{fz}}^i+V_{\mathrm{gz}}^i\end{array}\right]=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×9}\end{array}\right]X+V_w$

根据系统的状态方程和量测方程，用卡尔曼滤波估计出失准角

5、根据权利要求4所述的光纤陀螺捷联惯导系统消除不确定性干扰的精对准方法，其特征是：

利用步骤3估计出的误差失准角

得到计算坐标系与真实坐标系之间的姿态误差矩阵

其中

表示

的反对称矩阵：

根据： $C_b^{n^{\′}}\left(t\right)=C_i^{n^{\′}}\left(t\right)C_{i^{\′}}^i{C}_b^{i^{\′}}\left(t_2\right)$

对捷联矩阵C_b ⁿ进行修正，得到精对准结束后的捷联矩阵；C_b ⁿ。

根据C_b ⁿ(t)得到载体姿态，即纵摇角、横摇角和航向角的主值如下：

由上述三个主值判断真值的公式如下：

θ＝θ_主

至此，初始对准完成，进入导航状态。

Images