CN102486377A - 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法 - Google Patents

Abstract

一种光纤陀螺捷联惯导系统初始航向的姿态获取方法，其特征是通过滤波处理和补偿从加速度计中提取出重力加速度信息，将地球重力矢量投影到惯性坐标系下，其投影分量包含了地球自转角速度的信息，以惯性坐标系中的地球重力矢量为参考信息，利用陀螺和加速度计的输出，估计出初始姿态角，本发明的优点在于在确保初始水平角满足精度要求的基础上，大大提高了光纤陀螺捷联惯导系统初始航向角的精度。

Description

一种光纤陀螺捷联惯导系统初始航向的姿态获取方法

(一)技术领域

本发明涉及的是一种测量技术，特别是涉及一种光纤陀螺捷联惯性系统获取初始航向角的方法。 

(二)背景技术

光纤陀螺捷联惯导系统是一种自主式导航系统，采用牛顿第二定律在初始信息的基础上进行积分运算，可以连续、实时提供姿态、速度和位置信息，具有隐蔽性好，不受干扰等优点，广泛应用于军事武器系统中，而初始姿态，尤其是初始航向角的取得是整个系统的关键环节。 

光纤陀螺捷联惯导系统初始姿态测量的过程包括粗对准和精对准两个阶段。传统的初始姿态测量的方法能实现快速获取高精度的水平姿态角，但是初始航向角精度不够理想，在某些军事应用领域上无法满足要求，因此如何获得高精度的初始航向角成为了某些武器导航系统发展的技术瓶颈。 

(三)发明内容

本发明的目的在于提供一种确保初始水平角满足精度要求的基础上，大大提高光纤陀螺捷联惯导系统初始航向角精度的方法。 

本发明的目的是这样实现的，它包括以下步骤： 

步骤1、光纤陀螺捷联惯性导航系统进行预热； 

步骤2、利用GPS信息将舰船的初始速度参数和初始位置参数装订至舰船光纤陀螺捷联惯性导航系统导航计算机中，所述的舰船初始位置参数包括舰船的初始经度、纬度； 

步骤3、采集光纤陀螺仪和石英加速度计输出的数据，粗略测量出舰船的初始姿态，建立初始方向余弦矩阵C_b ⁱ′； 

步骤4、光纤陀螺捷联惯性导航系统利用步骤2和步骤3得到的初始值，利用光纤陀螺仪输出角速度和石英加速度计输出比例进行导航递推解算，测量舰船在惯性坐标系下的速度； 

步骤5、以光纤陀螺捷联惯导系统测量的速度与载体真实的速度做差，并将此差值作为观测向量，采用卡尔曼滤波技术估测出计算惯性坐标系与真实惯性坐标系之间的三个姿态误差角 

和 

步骤6、利用步骤5估计出的三个姿态误差角，修正粗对准时的方向余弦矩阵C_b ⁱ′，得到修正后的方向余弦矩阵C_b ⁱ； 

步骤7、根据方向余弦矩阵的变换关系，得到载体坐标系b到导航坐标系n的捷联矩阵C_b ⁿ，实现对准。 

本发明还可以包括这样一些特征： 

1.所述的步骤5根据分析光纤陀螺捷联惯性系统处于摇摆基座下的姿态误差方程和速度误差方程，建立系统的状态方程和量测方程，设系统的状态变量为 

由捷联惯导解算的速度和载体真实的速度之差构造的观测量为 

$Z\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{V}_x\\ \mathrm{\δ}{V}_y\\ \mathrm{\δ}{V}_z\end{array}\right]=\left[\begin{array}{c}{\tilde{V}}_x-V_x^i\\ {\tilde{V}}_y-V_y^i\\ {\tilde{V}}_z-V_z^i\end{array}\right]$

式中，δV_x、δV_y、δV_z为惯导解算的速度和载体真实的在三个坐标轴速度之差， 

和 

分别是惯性坐标系与真实惯性坐标系之间的三个姿态误差角，ε_x、ε_y和ε_z为陀螺常值漂移， 

和 

为加速度计零偏， 

和 

为捷联惯导解算的速度，V_x ⁱ、V_y ⁱ和V_z ⁱ为载体真实的速度，其中，载体的真实速度Vⁱ可以表示为 

$V^i=\underset{t_1}{\overset{t_2}{\∫}}{g}^i\mathrm{dt}=\underset{t_1}{\overset{t_2}{\∫}}\left[\begin{array}{c}-g\cos L\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)\\ -g\cos L\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)\\ -g\sin L\end{array}\right]\mathrm{dt}=\left[\begin{array}{c}\frac{-g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{-g\cos L[\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})-\cos \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ -g\sin \mathrm{L\Δt}\end{array}\right]$

Δt＝t₂-t₁，L和λ为当地纬度和经度，ω_ie为地球自转角速度，g为重力加速度，则捷联系统初始姿态测量的状态方程和量测方程可写成： 

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+v\left(t\right)\end{array}\right.$

其中， 

$A\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],$ $G\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]$

$H\left(t\right)=\left[\begin{array}{cccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

W(t)为系统的噪声矢量，v(t)为观测噪声矢量，式中， 

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

$C_b^i=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$ $0_{3\×3}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

2.所述的步骤6修正后的方向余弦矩阵C_b ⁱ为 

$C_b^i=C_{i^{\′}}^i{C}_b^{i^{\′}}$

式中C_i′ ⁱ为由估计出的三个姿态误差角表示的反对称矩阵 

3.所述的步骤7载体坐标系b到导航坐标系n的捷联矩阵C_b ⁿ为 

$C_b^n=C_i^n{C}_b^i$

式中， $C_i^n=\left[\begin{array}{ccc}-\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ -\sin L\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos L\\ \cos L\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \sin L\end{array}\right],$ 由舰船载体坐标系与导 航坐标系之间的方向余弦矩阵C_b ⁿ可测量出舰船准确的初始姿态角，初始姿态角的主值为： 

其中：航向角H定义域为(0°，360°)； 

纵摇角ψ定义域为(-90°，90°)； 

横摇角θ的定义域为(-90°，90°)； 

则初始姿态角测量值为： 

ψ＝ψ_主

本发明的优点在于： 

传统的初始姿态测量的方法需要两个参考信息，一个是地球自转角速度信息 一个是地球重力矢量信息 

对于舰船摇摆基座下陀螺仪敏感的角速度会大大超过地球自转角速度，且包含有许多的噪声，在这样的情况下不能直接采用地球自转角速度 

作为参考信息，但是通过滤波处理和补偿可以从加速度计中提取出重力加速度信息。因此地球重力加速度仍可以作为初始姿态测量的参考信息。如果将地球重力矢量投影到惯性坐标系下，它的投影分量包含了地球自转角速度的信 息，因此，如果以惯性坐标系中的地球重力矢量为参考信息，利用陀螺和加速度计的输出，可以精确估计出初始姿态角。 

本发明的优点可通过如下试验加以验证： 

假设有如下环境： 

(1)光纤陀螺和加速度计的性能如下： 

1)陀螺常值漂移为：ε_x＝ε_y＝ε_z＝0.01°/h，陀螺随机漂移为0.005°/h

2)加速度计零位偏移： ${\▿}_x={\▿}_y={\▿}_z={10}^{-4}g,$ 加速度计随机干扰为10^-5g 

3)粗对准结束后的姿态误差角：(0.1°0.1° 0.5°) 

4)采样时间和滤波周期T＝0.01秒 

5)卡尔曼滤波器的初始值(状态的初值X₀及其协方差P₀，以及系统噪声和量测噪声的协方差矩阵Q、R)设为： 

X₀＝[0 0 0 0 0 0 0 0 0 0 0 0]^T

P₀＝diag{(0.1m/s)²(0.1m/s)²(0.1m/s)²(1°)²(1°)²(2°)²(9.78×10^-4m/s²)²

(9.78×10^-4m/s²)²(9.78×10^-4m/s²)²(0.01°/h)²(0.01°/h)²(0.01°/h)²} 

Q＝diag{(4.89×10^-4m/s²)²(4.89×10^-4m/s²)²(4.89×10^-4m/s²)²

(0.005°/h)²(0.005°/h)²(0.005°/h)² 0 0 0 0 0 0} 

R＝diag{(0.01m/s)²(0.01m/s)²(0.01m/s)²} 

(2)假设舰船在海浪激励下分别绕纵摇轴、横摇轴和方位轴做摇摆运动，其运动模型为： 

舰船的初始位置：北纬45.7796°，东经126.6705°。 

通过计算机仿真比较本发明与传统初始姿态角测量方法的估计效果，摇摆基座下初始精对准误差角曲线如图2和图3所示。其中system1表示用传统的方法估计出的姿态误差角曲线，system2表示本文采用的新方法估计出的姿态误差角曲线。 

仿真结果表明：虽然水平姿态角的估计精度没有传统方法的精度高，但是在某些武器系统中对水平姿态的要求不是很高的条件下，本文的新方法能满足精度要求，从图3可以看出初始航向角的对准精度0.042°，比传统方法的估计高出0.013°。 

(四)附图说明

图1是光纤陀螺捷联惯导系统初始姿态测量的具体实施方式。 

图2是摇摆基座下与传统初始精对准比较的误差角曲线。 

图3是航向误差角比较曲线的局部放大图。 

(五)具体实施方式

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

步骤1、光纤陀螺捷联惯性导航系统进行预热； 

步骤2、利用GPS信息将舰船的初始速度参数和初始位置参数装订至舰船光纤陀螺捷联惯性导航系统导航计算机中，所述的舰船初始位置参数包括舰船的初始经度、纬度； 

步骤3、采集光纤陀螺仪和石英加速度计输出的数据，粗略测量出舰船的初始姿态，建立初始方向余弦矩阵C_b ⁱ′； 

步骤4、光纤陀螺捷联惯性导航系统利用步骤2(装订初始速度参数和初始位置参数)和步骤3得到的初始值，利用光纤陀螺仪输出角速度和石英加速度计输出比例进行导航递推解算，测量舰船在惯性坐标系下的速度； 

步骤5、以光纤陀螺捷联惯导系统测量的速度与载体真实的速度做差，并将此差值作为观测向量，采用卡尔曼滤波技术估测出计算惯性坐标系与真实惯性坐标系之间的三个姿态误差角 和 

步骤6、利用步骤5估计出的三个姿态误差角，可以修正粗对准时的方向余弦矩阵C_b ⁱ′，修正后的方向余弦矩阵为 

$C_b^i=C_{i^{\′}}^i{C}_b^{i^{\′}}---\left(1\right)$

式中C_i′ ⁱ由估计出的三个姿态误差角表示的反对称矩阵 

步骤7、根据方向余弦矩阵的变换关系，可以得到载体坐标系b到导航坐标系n的捷联矩阵C_b ⁿ，实现对准， 

$C_b^n=C_i^n{C}_b^i---\left(2\right)$

式中 

$C_i^n=\left[\begin{array}{ccc}-\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& 0\\ -\sin L\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& -\sin L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos L\\ \cos L\cos \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \cos L\sin \left({\mathrm{\ω}}_{\mathrm{ie}}t\right)& \sin L\end{array}\right],$ L为当地地理纬度由舰船载体坐标系与导航坐标系之间的方向余弦矩阵C_b ⁿ可测量出舰船准确的初始姿态角。 

初始姿态角的主值为： 

其中：航向角H定义域为(0°，360°)； 

纵摇角ψ定义域为(-90°，90°)； 

横摇角θ的定义域为(-90°，90°)； 

则初始姿态角测量值为： 

ψ＝ψ_主

步骤5的有效说明如下： 

根据分析光纤陀螺捷联惯性系统处于摇摆基座下的姿态误差方程和速度误差方程，建立系统的状态方程和量测方程： 

设系统的状态变量为 

由捷联惯导解算的速度和载体真实的速度之差构造的观测量为 

$Z\left(t\right)=\left[\begin{array}{c}\mathrm{\δ}{V}_x\\ \mathrm{\δ}{V}_y\\ \mathrm{\δ}{V}_z\end{array}\right]=\left[\begin{array}{c}{\tilde{V}}_x-V_x^i\\ {\tilde{V}}_y-V_y^i\\ {\tilde{V}}_z-V_z^i\end{array}\right]---\left(4\right)$

式中，δV_x、δV_y、δV_z为惯导解算的速度和载体真实的在三个坐标轴速度之差， 

和 

分别是惯性坐标系与真实惯性坐标系之间的三个姿态误差角，ε_x、ε_y和ε_z为陀螺常值漂移， 和 

为加速度计零偏， 

和 

为捷联惯导解算的速度，V_x ⁱ、V_y ⁱ和V_z ⁱ为载体真实的速度，其中，载体的真实速度Vⁱ可以表示为 

$V^i=\underset{t_1}{\overset{t_2}{\∫}}{g}^i\mathrm{dt}=\underset{t_1}{\overset{t_2}{\∫}}\left[\begin{array}{c}-g\cos L\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)\\ -g\cos L\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}t)\\ -g\sin L\end{array}\right]\mathrm{dt}=\left[\begin{array}{c}\frac{-g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{-g\cos L[\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})-\cos \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ -g\sin \mathrm{L\Δt}\end{array}\right]---\left(5\right)$

Δt＝t₂-t₁，L和λ为当地纬度和经度，ω_ie为地球自转角速度，g为重力加速度，则捷联系统初始姿态测量的状态方程和量测方程可写成： 

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+v\left(t\right)\end{array}\right.---\left(6\right)$

其中， 

$A\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],$ $G\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]$

$H\left(t\right)=\left[\begin{array}{cccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

W(t)为系统的噪声矢量，v(t)为观测噪声矢量 

式中， 

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

$C_b^i=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$ $0_{3\×3}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$ 。

Claims (5)

1.一种光纤陀螺捷联惯导系统初始航向的姿态获取方法，其特征是它包括以下步骤：

步骤1、光纤陀螺捷联惯性导航系统进行预热；

步骤2、利用GPS信息将舰船的初始速度参数和初始位置参数装订至舰船光纤陀螺捷联惯性导航系统导航计算机中，所述的舰船初始位置参数包括舰船的初始经度、纬度；

步骤3、采集光纤陀螺仪和石英加速度计输出的数据，粗略测量出舰船的初始姿态，建立初始方向余弦矩阵C_b ⁱ′；

步骤4、光纤陀螺捷联惯性导航系统利用步骤2和步骤3得到的初始值，利用光纤陀螺仪输出角速度和石英加速度计输出比例进行导航递推解算，测量舰船在惯性坐标系下的速度；

步骤5、以光纤陀螺捷联惯导系统测量的速度与载体真实的速度做差，并将此差值作为观测向量，采用卡尔曼滤波技术估测出计算惯性坐标系与真实惯性坐标系之间的三个姿态误差角 

和 

步骤6、利用步骤5估计出的三个姿态误差角，修正粗对准时的方向余弦矩阵C_b ⁱ′，得到修正后的方向余弦矩阵C_b ⁱ；

步骤7、根据方向余弦矩阵的变换关系，得到载体坐标系b到导航坐标系n的捷联矩阵C_b ⁿ，实现对准。

2.根据权利要求1所述的一种光纤陀螺捷联惯导系统初始航向的姿态获取方法步骤，其特征是：所述的步骤5根据分析光纤陀螺捷联惯性系统处于摇摆基座下的姿态误差方程和速度误差方程，建立系统的状态方程和量测方程，设系统的状态变量为

由捷联惯导解算的速度和载体真实的速度之差构造的观测量为 

式中，δV_x、δV_y、δV_z为惯导解算的速度和载体真实的在三个坐标轴速度之差， 

和 

分别是惯性坐标系与真实惯性坐标系之间的三个姿态误差角，ε_x、ε_y和ε_z为陀螺常值漂移， 

和 为加速度计零偏， 和 

为捷联惯导解算的速度，V_x ⁱ、V_y ⁱ和V_z ⁱ为载体真实的速度，其中，载体的真实速度Vⁱ可以表示为

Δt＝t₂-t₁，L和λ为当地纬度和经度，ω_ie为地球自转角速度，g为重力加速度，则捷联系统初始姿态测量的状态方程和量测方程可写成：

其中，

W(t)为系统的噪声矢量，v(t)为观测噪声矢量，式中， 

。

3.根据权利要求1或2所述的一种光纤陀螺捷联惯导系统初始航向的姿态获取方法步骤，其特征是：所述的步骤6修正后的方向余弦矩阵C_b ⁱ为

式中C_i′ ⁱ为由估计出的三个姿态误差角表示的反对称矩阵

。

4.根据权利要求1或2所述的一种光纤陀螺捷联惯导系统初始航向的姿态获取方法步骤，其特征是：所述的步骤7载体坐标系b到导航坐标系n的捷联矩阵C_b ⁿ为

式中，由舰船载体坐标系与导航坐标系之间的方向余弦矩阵C_b ⁿ可测量出舰船准确的初始姿态角，初始姿态角的主值为：

其中：航向角H定义域为(0°，360°)； 

纵摇角ψ定义域为(-90°，90°)；

横摇角θ的定义域为(-90°，90°)；

则初始姿态角测量值为：

ψ＝ψ_主

5.根据权利要求4所述的一种光纤陀螺捷联惯导系统初始航向的姿态获取方法步骤，其特征是：所述的步骤7中修正后的方向余弦矩阵C_b ⁱ为

式中C_i′ ⁱ为由估计出的三个姿态误差角表示的反对称矩阵

Images