CN105300407A - 一种用于单轴调制激光陀螺惯导系统的海上动态启动方法 - Google Patents

Abstract

本发明属于惯性导航系统技术领域，针对单轴调制激光陀螺惯导系统在舰船航行过程中完成初始对准的使用需求，公开了一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，步骤为：系统上电启动后，完成数学平台的快速建立，建立基于最优估计理论的Kalman滤波模型，并完成Kalman滤波器的初始化，控制单轴转位机构完成辅助调制运动，利用所述导航解算结果求解Kalman滤波器的线性滤波方程，对求解的所述线性滤波方程进行离散化处理，进行Kalman滤波器的时间更新，进行Kalman滤波器的量测更新，执行预定时间，利用Kalman滤波器的估计结果校正所述惯导系统的姿态误差和器件误差，完成所述惯导系统的初始对准。

Description

一种用于单轴调制激光陀螺惯导系统的海上动态启动方法

技术领域

本发明属于惯性导航系统技术领域，具体涉及一种用于单轴调制激光陀螺惯导系统的海上动态启动方法。

背景技术

激光陀螺惯导系统因具有可靠性高、环境适应性强、启动时间短、成本低、体积小、重量轻等优点而日趋成为主流的惯导产品。初始对准是惯导系统使用前必须完成的工作之一，其精度直接影响着舰船用户使用的准确性和快速反应能力。海上动态启动是指惯导系统在舰船正常海上航行过程中完成初始对准，因此，海上动态启动极大地缩短了舰船码头准备时间、提高了舰船快速反应能力，具有重要的应用价值。

与码头启动或系泊启动等静基座初始对准相比，海上动态启动具有独特的环境特点：由舰船航行引起的惯导系统大范围线运动；由洋流、海浪、阵风等引起的舰船大幅摇摆，使得惯导系统敏感到的地球自转角速度和重力加速度等信息受到严重干扰。惯导系统的大范围线运动导致传统解析对准技术精度大幅下降，无法满足系统使用要求；惯导系统的大幅角运动导致传统的基于网络回路的水平方位粗对准技术精度严重下降，无法满足系统使用要求。

因此，为保证系统海上动态启动的初始对准精度，需解决大范围线运动、大幅角运动条件下的惯导系统数学平台(数学平台即惯导系统导航解算坐标系)建立的问题。

发明内容

本发明为了克服现有技术的缺点和不足，本发明提供了一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，该方法采用航迹寻北解析调平技术完成数学平台的快速建立，利用基于最优估计理论的Kalman滤波技术完成惯导系统初始姿态误差、陀螺和加速度计零位误差的高精度估计与补偿，实现惯导系统的海上动态启动。

本发明解决其技术问题所采用的技术方案是：一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，包括以下步骤：

a)、系统上电启动后，利用航迹寻北解析调平技术完成数学平台的快速建立；

b)、根据所述惯导系统的误差传播规律，建立基于最优估计理论的Kalman滤波模型，并完成所述Kalman滤波器的初始化；

c)、控制单轴转位机构完成辅助调制运动；

d)、基于步骤a)建立的数学平台进行所述惯导系统的导航解算，利用所述导航解算结果求解所述Kalman滤波器的线性滤波方程；

e)、对步骤d)中求解的所述线性滤波方程进行离散化处理；

f)、进行所述Kalman滤波器的时间更新；

g)、利用高精度的外部参考信息进行所述Kalman滤波器的量测更新，所述高精度的外部参考信息是指由卫星导航系统提供的位置、速度等导航信息；

h)、重复步骤c)至步骤g)，执行预定时间；

i)、利用所述Kalman滤波器的估计结果校正所述惯导系统的姿态误差和器件误差，完成所述惯导系统的初始对准。

所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其步骤a)具体为：

(1)航迹寻北——利用所述高精度的外部参考速度信息完成所述系统航迹向的解算，具体解算公式为

$sin\left(H\right)=\frac{V_e}{V}$

$cos\left(H\right)=\frac{V_n}{V}$

式中，H表示所述惯导系统的航向，V、V_e、V_n分别表示所述外部参考速度、东向分速度和北向分速度；

(2)解析调平——利用地球自转角速度恒定已知，可准确计算出重力加速度在惯性空间一定时间内转过的角度，根据惯性系内重力加速与载体系加速度计输出的关系即可解算出载体姿态信息；在解析调平开始时刻将载体系b凝固为惯性系b₀，则所述惯导系统初始对准姿态矩阵可表述为：

$C_b^n\left(t\right)=C_i^n\left(t\right)C_{b_0}^i{C}_b^{b_0}\left(t\right)$

式中，表示惯性坐标系(i系)与导航坐标系(n系)之间的转移矩阵，可由所述外部参考位置信息、地球自转角速率以及解析调平时间t确定；表示凝固惯性系(b₀系)与i系之间的转移矩阵，为常值矩阵，可由惯性系内重力加速与载体系加速度计输出的关系确定；表示载体坐标系(b系)与b₀系之间的转移矩阵，可由陀螺输出的载体角增量信息确定；根据所述解析调平方法确定所述惯导系统姿态矩阵后，即可解算出所述惯导系统的水平姿态信息。

所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其步骤b)中惯导系统的误差传播规律为：

${\stackrel{\·}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{in}^n\×{\mathrm{\φ}}^n+M_1{\mathrm{\δv}}^n+M_2{\mathrm{\δP}}^n-C_b^n{\mathrm{\ϵ}}^b$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\φ}}^n+M_3{\mathrm{\δv}}^n+M_4{\mathrm{\δP}}^n+C_b^n{\▿}^b$

$\mathrm{\δ}{\stackrel{\·}{P}}^n=M_5{\mathrm{\δv}}^n+M_6{\mathrm{\δP}}^n$

${\stackrel{\·}{\mathrm{\ϵ}}}^b=0$

${\stackrel{\·}{\▿}}^b=0$

式中，φⁿ表示所述惯导系统姿态误差角在n系的投影，表示n系相对i系的角速度在n系的投影，δvⁿ、δPⁿ分别表示所述惯导系统在n系的速度误差和位置误差，ε^b、▽^b分别表示所述惯导系统陀螺零偏和加速度计零偏在b系的投影，M_i(i＝1,2,3…6)为相关矩阵，具体如下：

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$ $M_2=\left[\begin{array}{ccc}0& 0& \frac{V_N}{{(R_m+h)}^2}\\ -{\mathrm{\ω}}_{ie}\·\sin L& 0& -\frac{V_E}{{(R_n+h)}^2}\\ {\mathrm{\ω}}_{ie}\·\cos L+\frac{V_E}{(R_n+h)\·{\cos }^{2}L}& 0& -\frac{V_E\·\tan L}{{(R_n+h)}^2}\end{array}\right]$

$M_3=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_n+h}& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_n+h}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R_n+h}\\ -2{\mathrm{\ω}}_{ie}\sin L-\frac{2V_E\tan L}{R_n+h}& -\frac{V_U}{R_m+h}& -\frac{V_N}{R_m+h}\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{2V_E}{R_n+h}& \frac{2V_N}{R_m+h}& 0\end{array}\right]$

$M_4=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{ie}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{(R_n+h){\cos }^{2}L}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\cos L-\frac{{V_E}^2}{(R_n+h){\cos }^{2}L}& 0& \frac{V_N{V}_U}{{(R_m+h)}^2}+\frac{{V_E}^2\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\sin L& 0& -\frac{{V_N}^2}{{(R_m+h)}^2}-\frac{{V_E}^2}{{(R_n+h)}^2}\end{array}\right]$

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{1}{(R_n+h)\cos L}& 0& 0\\ 0& 0& 1\end{array}\right]$ $M_6=\left[\begin{array}{ccc}0& 0& -\frac{V_N}{{(R_m+h)}^2}\\ \frac{V_E\tan L}{(R_n+h)\·\cos L}& 0& -\frac{V_E}{{(R_n+h)}^2\cos L}\\ 0& 0& 0\end{array}\right]$

式中，R_m表示地球子午圈主曲率半径，R_n表示地球卯酉圈主曲率半径，h表示所述惯导系统的解算高度，L表示所述惯导系统的解算纬度，V_E、V_N、V_U分别表示所述惯导系统的解算东向、北向、天向速度，ω_ie表示地球自转角速度。

所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其步骤b)中Kalman滤波模型的建立具体为：

(1)取所述惯导系统姿态误差角、速度误差、位置误差、陀螺常值零偏以

及加速度计常值零偏作为所述Kalman滤波器状态矢量：

X＝[φ_Eφ_Nφ_UδV_EδV_NδV_UδLδλδhε_xε_yε_z▽_x▽_y▽_z]^T

其中，φ_E、φ_N、φ_U表示所述姿态误差角φⁿ沿坐标轴的分量，δV_E、δV_N、δV_U分别表示东向、北向、天向速度误差，δL、δλ、δh分别表示纬度、经度、高度误差，ε_x、ε_y、ε_z分别表示三个轴向陀螺常值零偏，▽_x、▽_y、▽_z分别表示三个轴向加速度计常值零偏；

(2)取所述Kalman滤波器的系统状态方程为：

$\stackrel{\·}{X}=FX+GW$

式中：F为所述Kalman滤波器的系统状态矩阵，W为系统噪声矢量，G为噪声转换矩阵，根据公式(3)可以确定系统状态矩阵F为：

$F=\left[\begin{array}{ccccc}-\[{\mathrm{\ω}}_{in}^n\]& M_1& M_2& -C_b^n& 0_{3\×3}\\ \[f^n\]& M_3& M_4& 0_{3\×3}& C_b^n\\ 0_{3\×3}& M_5& M_6& 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}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

式中，是n系相对i系的旋转角速度在n系上的投影分量组成的反对称矩阵，[fⁿ]是所述惯导系统的加速度计输出比力在n系上的投影分量组成的反对称矩阵，0_3×3表示三维零矩阵，M_i(i＝1,2,3…6)表示所述相关矩阵；

(3)利用所述高精度的外部参考信息构造系统量测矢量，所构造的系统量测矢量为：

Z＝[V_TE-V_EV_TN-V_NL_T-Lλ_T-λ]^T

式中，V_TE、V_TN分别表示所述外部参考东向、北向速度，L_T、λ_T分别表示所述外部参考纬度、经度；

(4)确定系统量测方程为：

Z＝HX+η

式中，H为量测矩阵，η为量测噪声：

$H=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 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& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right].$

所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其步骤d)中Kalman滤波方程如下：

状态一步预测方程为其中Φ_k/k-1分别表示k时刻系统状态一步预测值、k-1时刻系统状态估值、k-1时刻到k时刻的系统状态转移矩阵；

状态估计方程为其中K_k、Z_k、H_k分别为k时刻系统状态估值、系统增益矩阵、量测向量和量测矩阵；

滤波增量方程为其中P_k/k-1、R_k分别为k时刻系统协方差阵的一步预测、系统量测噪声矩阵；

一步预测均方误差方程为其中P_k-1、Q_k-1、Γ_k-1分别为k-1时刻系统协方差阵、系统噪声矩阵和系统噪声驱动矩阵；

估计均方误差方程为其中P_k为k时刻系统状态协方差阵，I是单位阵。

所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其步骤i)中所用公式如下：

(1)姿态校正—利用估计出的惯导系统姿态误差角构成校正矩阵：

$C_{n^{\′}}^n=I+\[\mathrm{\φ}\×\]$

式中，[φ×]表示由姿态误差角组成的反对称阵，经展开计算后得：

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]$

由姿态校正矩阵完成对惯导系统姿态矩阵的校正：

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

(2)器件常值零偏校正—器件常值零偏校正是直接利用Kalman滤波估计出的陀螺常值零偏以及加速度计常值零偏完成陀螺、加速度计输出的实时补偿：

[G_xG_yG_z]^T＝[G_xG_yG_z]^T-[ε_xε_yε_z]^T

[A_xA_yA_z]^T＝[A_xA_yA_z]^T-[▽_x▽_y▽_z]^T。

本发明的有益效果是：

1、针对惯导系统的大范围线运动导致现有解析对准技术精度大幅下降的问题，本发明采用航迹寻北解析调平技术，充分利用外部参考速度信息确定舰船运动航迹向，完成数学平台的快速建立；

2、本发明结合基于最优估计理论的Kalman滤波技术，解决惯导系统的大幅角运动导致传统的基于网络回路的水平方位粗对准技术精度严重下降的问题，完成惯导系统初始姿态误差、陀螺和加速度计零位误差的高精度估计与补偿，实现惯导系统的海上动态启动。

附图说明

图1为本发明的流程图；

图2为本发明具体实施例中的舰船航行轨迹示意图；

图3为本发明具体实施例中的自主导航定位误差曲线图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本实施例中，利用单轴调制激光陀螺惯导系统随试验舰船出海执行任务期间录取的原始数据进行半物理仿真，其中，所述原始数据包括时戳、陀螺敏感舰船角运动信息、加速度敏感舰船线运动信息、惯性测量组件状态信息、GPS位置信息、GPS速度信息、转台测角信息等。

参照图1、图2所示，本发明的方法包括如下步骤：

a)、系统上电启动后，利用航迹寻北解析调平技术完成数学平台的快速建立:

(1)航迹寻北——利用所述高精度的外部参考速度信息完成所述系统航迹向的解算，具体解算公式为

$sin\left(H\right)=\frac{V_e}{V}$

$cos\left(H\right)=\frac{V_n}{V}$

式中，H表示所述惯导系统的航向，V、V_e、V_n分别表示所述外部参考速度、东向分速度和北向分速度。

(2)解析调平——利用地球自转角速度恒定已知，可准确计算出重力加速度在惯性空间一定时间内转过的角度，根据惯性系内重力加速与载体系加速度计输出的关系即可解算出载体姿态信息；在解析调平开始时刻将载体系b凝固为惯性系b₀，则所述惯导系统初始对准姿态矩阵可表述为：

$C_b^n\left(t\right)=C_i^n\left(t\right)C_{b_0}^i{C}_b^{b_0}\left(t\right)$

式中，表示惯性坐标系(i系)与导航坐标系(n系)之间的转移矩阵，可由所述外部参考位置信息、地球自转角速率以及解析调平时间t确定；表示凝固惯性系(b₀系)与i系之间的转移矩阵，为常值矩阵，可由惯性系内重力加速与载体系加速度计输出的关系确定；表示载体坐标系(b系)与b₀系之间的转移矩阵，可由陀螺输出的载体角增量信息确定；下文有关坐标系的定义，若无特殊说明则与此相同；根据所述解析调平方法确定所述惯导系统姿态矩阵后，即可解算出所述惯导系统的水平姿态信息。利用所述航迹寻北方法所得航向和所述解析调平方法所得水平姿态信息，即可快速建立所述惯导系统的数学平台。

b)、根据单轴调制激光陀螺惯导系统误差传播规律建立海上动态启动误差模型：

其中惯导系统的误差传播规律为：

${\stackrel{\·}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{in}^n\×{\mathrm{\φ}}^n+M_1{\mathrm{\δv}}^n+M_2{\mathrm{\δP}}^n-C_b^n{\mathrm{\ϵ}}^b$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\φ}}^n+M_3{\mathrm{\δv}}^n+M_4{\mathrm{\δP}}^n+C_b^n{\▿}^b$

$\mathrm{\δ}{\stackrel{\·}{P}}^n=M_5{\mathrm{\δv}}^n+M_6{\mathrm{\δP}}^n$

${\stackrel{\·}{\mathrm{\ϵ}}}^b=0$

${\stackrel{\·}{\▿}}^b=0$

式中，φⁿ表示所述惯导系统姿态误差角在n系的投影，表示n系相对i系的角速度在n系的投影，δvⁿ、δPⁿ分别表示所述惯导系统在n系的速度误差和位置误差，ε^b、▽^b分别表示所述惯导系统陀螺零偏和加速度计零偏在b系的投影，M_i(i＝1,2,3…6)为相关矩阵，具体如下：

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$ $M_2=\left[\begin{array}{ccc}0& 0& \frac{V_N}{{(R_m+h)}^2}\\ -{\mathrm{\ω}}_{ie}\·\sin L& 0& -\frac{V_E}{{(R_n+h)}^2}\\ {\mathrm{\ω}}_{ie}\·\cos L+\frac{V_E}{(R_n+h)\·{\cos }^{2}L}& 0& -\frac{V_E\·\tan L}{{(R_n+h)}^2}\end{array}\right]$

$M_3=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_n+h}& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_n+h}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R_n+h}\\ -2{\mathrm{\ω}}_{ie}\sin L-\frac{2V_E\tan L}{R_n+h}& -\frac{V_U}{R_m+h}& -\frac{V_N}{R_m+h}\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{2V_E}{R_n+h}& \frac{2V_N}{R_m+h}& 0\end{array}\right]$

$M_4=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{ie}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{(R_n+h){\cos }^{2}L}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\cos L-\frac{{V_E}^2}{(R_n+h){\cos }^{2}L}& 0& \frac{V_N{V}_U}{{(R_m+h)}^2}+\frac{{V_E}^2\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\sin L& 0& -\frac{{V_N}^2}{{(R_m+h)}^2}-\frac{{V_E}^2}{{(R_n+h)}^2}\end{array}\right]$

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{1}{(R_n+h)\cos L}& 0& 0\\ 0& 0& 1\end{array}\right]$ $M_6=\left[\begin{array}{ccc}0& 0& -\frac{V_N}{{(R_m+h)}^2}\\ \frac{V_E\tan L}{(R_n+h)\·\cos L}& 0& -\frac{V_E}{{(R_n+h)}^2\cos L}\\ 0& 0& 0\end{array}\right]$

式中，R_m表示地球子午圈主曲率半径，R_n表示地球卯酉圈主曲率半径，h表示所述惯导系统的解算高度，L表示所述惯导系统的解算纬度，V_E、V_N、V_U分别表示所述惯导系统的解算东向、北向、天向速度，ω_ie表示地球自转角速度。

基于所述惯导系统海上动态启动误差模型，建立Kalman滤波模型：

(1)取所述惯导系统姿态误差角、速度误差、位置误差、陀螺常值零偏以及加速度计常值零偏作为所述Kalman滤波器状态矢量：

X＝[φ_Eφ_Nφ_UδV_EδV_NδV_UδLδλδhε_xε_yε_z▽_x▽_y▽_z]^T

其中，φ_E、φ_N、φ_U表示所述姿态误差角φⁿ沿坐标轴的分量，δV_E、δV_N、δV_U分别表示东向、北向、天向速度误差，δL、δλ、δh分别表示纬度、经度、高度误差，ε_x、ε_y、ε_z分别表示三个轴向陀螺常值零偏，▽_x、▽_y、▽_z分别表示三个轴向加速度计常值零偏；

(2)取所述Kalman滤波器的系统状态方程为：

$\stackrel{\·}{X}=FX+GW$

式中：F为所述Kalman滤波器的系统状态矩阵，W为系统噪声矢量，G为噪声转换矩阵，根据公式(3)可以确定系统状态矩阵F为：

$F=\left[\begin{array}{ccccc}-\[{\mathrm{\ω}}_{in}^n\]& M_1& M_2& -C_b^n& 0_{3\×3}\\ \[f^n\]& M_3& M_4& 0_{3\×3}& C_b^n\\ 0_{3\×3}& M_5& M_6& 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}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

式中，是n系相对i系的旋转角速度在n系上的投影分量组成的反对称矩阵，[fⁿ]是所述惯导系统的加速度计输出比力在n系上的投影分量组成的反对称矩阵，0_3×3表示三维零矩阵，M_i(i＝1,2,3…6)表示所述相关矩阵；

(3)利用所述高精度的外部参考信息构造系统量测矢量，所构造的系统量测矢量为：

Z＝[V_TE-V_EV_TN-V_NL_T-Lλ_T-λ]^T

式中，V_TE、V_TN分别表示所述外部参考东向、北向速度，L_T、λ_T分别表示所述外部参考纬度、经度；

(4)确定系统量测方程为：

Z＝HX+η

式中，H为量测矩阵，η为量测噪声：

$H=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 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& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right].$

c)、控制单轴转位机构完成辅助调制运动。

d)、基于步骤a)建立的数学平台进行所述惯导系统的导航解算，利用所述导航解算结果求解所述Kalman滤波器的线性滤波方程。

Kalman滤波基本算法编排，根据上述系统状态方程和量测方程，可建立离散Kalman滤波方程如下：

状态一步预测方程：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1}$

其中，Φ_k/k-1分别表示k时刻系统状态一步预测值、k-1时刻系统状态估值、k-1时刻到k时刻的系统状态转移矩阵；

状态估计方程：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

其中，K_k、Z_k、H_k分别为k时刻系统状态估值、系统增益矩阵、量测向量和量测矩阵；

滤波增量方程：

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

其中，P_k/k-1、R_k分别为k时刻系统协方差阵的一步预测、系统量测噪声矩阵；

一步预测均方误差方程：

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

其中，P_k-1、Q_k-1、Γ_k-1分别为k-1时刻系统协方差阵、系统噪声矩阵和系统噪声驱动矩阵；

估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

其中，P_k为k时刻系统状态协方差阵，I是单位阵。

e)、对步骤d)中求解的所述线性滤波方程进行离散化处理。

f)、进行所述Kalman滤波器的时间更新。

g)、利用高精度的外部参考信息进行所述Kalman滤波器的量测更新，所述高精度的外部参考信息是指由卫星导航系统提供的位置、速度等导航信息。

h)、重复步骤c)至步骤g)，执行预定时间。

i)、利用所述Kalman滤波器的估计结果校正所述惯导系统的姿态误差和器件误差，完成所述惯导系统的初始对准。将所述Kalman滤波估计出的姿态误差角、陀螺常值零偏以及加速度计常值零偏的估计值反馈回惯导系统参与导航解算。具体公式如下：

(1)姿态校正

利用估计出的惯导系统姿态误差角构成校正矩阵：

$C_{n^{\′}}^n=I+\[\mathrm{\φ}\×\]$

式中，[φ×]表示由姿态误差角组成的反对称阵，经展开计算后得：

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]$

由姿态校正矩阵完成对惯导系统姿态矩阵的校正：

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

(2)器件常值零偏校正

器件常值零偏校正是直接利用Kalman滤波估计出的陀螺常值零偏以及加速度计常值零偏完成陀螺、加速度计输出的实时补偿：

[G_xG_yG_z]^T＝[G_xG_yG_z]^T-[ε_xε_yε_z]^T

[A_xA_yA_z]^T＝[A_xA_yA_z]^T-[▽_x▽_y▽_z]^T

通过对惯导系统姿态误差角、器件常值零偏的校正，能够得到惯导系统高精度初始姿态信息，完成海上动态启动。

本发明方法利用航迹寻北解析调平技术完成数学平台的快速建立，根据单轴调制系统误差传播规律，建立基于最优估计理论的Kalman滤波模型，以高精度的外部参考信息作为量测信息构造Kalman滤波观测方程，同时控制单轴转位机构作辅助运动，实现对激光陀螺惯导系统初始姿态误差、陀螺和加速度计零位误差的估计与补偿。

图3为本发明的具体实施例中的24小时自主导航定位误差图。从图中可以看出，采用本发明的方法，能够准确地完成惯导系统的海上动态启动，定位精度达到同类装备国内领先水平。

上述实施例仅例示性说明本发明的原理及其功效，以及部分运用的实施例，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (6)

1.一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于：包括以下步骤：

a)、系统上电启动后，利用航迹寻北解析调平技术完成数学平台的快速建立；

b)、根据所述惯导系统的误差传播规律，建立基于最优估计理论的Kalman滤波模型，并完成所述Kalman滤波器的初始化；

c)、控制单轴转位机构完成辅助调制运动；

d)、基于步骤a)建立的数学平台进行所述惯导系统的导航解算，利用所述导航解算结果求解所述Kalman滤波器的线性滤波方程；

e)、对步骤d)中求解的所述线性滤波方程进行离散化处理；

f)、进行所述Kalman滤波器的时间更新；

g)、利用高精度的外部参考信息进行所述Kalman滤波器的量测更新，所述高精度的外部参考信息是指由卫星导航系统提供的位置、速度等导航信息；

h)、重复步骤c)至步骤g)，执行预定时间；

i)、利用所述Kalman滤波器的估计结果校正所述惯导系统的姿态误差和器件误差，完成所述惯导系统的初始对准。

2.根据权利要求1所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于，所述的步骤a)具体为：

(1)航迹寻北——利用所述高精度的外部参考速度信息完成所述系统航迹向的解算，具体解算公式为

$sin\left(H\right)=\frac{V_e}{V}$

$cos\left(H\right)=\frac{V_n}{V}$

式中，H表示所述惯导系统的航向，V、V_e、V_n分别表示所述外部参考速度、东向分速度和北向分速度；

(2)解析调平——利用地球自转角速度恒定已知，可准确计算出重力加速度在惯性空间一定时间内转过的角度，根据惯性系内重力加速与载体系加速度计输出的关系即可解算出载体姿态信息；在解析调平开始时刻将载体系b凝固为惯性系b₀，则所述惯导系统初始对准姿态矩阵可表述为：

$C_b^n\left(t\right)=C_i^n\left(t\right)C_{b_0}^i{C}_b^{b_0}\left(t\right)$

式中，表示惯性坐标系(i系)与导航坐标系(n系)之间的转移矩阵，可由所述外部参考位置信息、地球自转角速率以及解析调平时间t确定；表示凝固惯性系(b₀系)与i系之间的转移矩阵，为常值矩阵，可由惯性系内重力加速与载体系加速度计输出的关系确定；表示载体坐标系(b系)与b₀系之间的转移矩阵，可由陀螺输出的载体角增量信息确定；根据所述解析调平方法确定所述惯导系统姿态矩阵后，即可解算出所述惯导系统的水平姿态信息。

3.根据权利要求1所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于，所述步骤b)中惯导系统的误差传播规律为：

${\stackrel{\·}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{in}^n\×{\mathrm{\φ}}^n+M_1{\mathrm{\δv}}^n+M_2{\mathrm{\δP}}^n-C_b^n{\mathrm{\ϵ}}^b$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\φ}}^n+M_3{\mathrm{\δv}}^n+M_4{\mathrm{\δP}}^n+C_b^n{\▿}^b$

$\mathrm{\δ}{\stackrel{\·}{P}}^n=M_5{\mathrm{\δv}}^n+M_6{\mathrm{\δP}}^n$

${\stackrel{\·}{\mathrm{\ϵ}}}^b=0$

${\stackrel{\·}{\▿}}^b=0$

式中，φⁿ表示所述惯导系统姿态误差角在n系的投影，表示n系相对i系的角速度在n系的投影，δvⁿ、δPⁿ分别表示所述惯导系统在n系的速度误差和位置误差，ε^b、分别表示所述惯导系统陀螺零偏和加速度计零偏在b系的投影，M_i(i＝1,2,3…6)为相关矩阵，具体如下：

$\begin{array}{cc}{M}_1=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]& M_2=\left[\begin{array}{ccc}0& 0& \frac{V_N}{{(R_m+h)}^2}\\ -{\mathrm{\ω}}_{ie}\·\sin L& 0& -\frac{V_E}{{(R_n+h)}^2}\\ {\mathrm{\ω}}_{ie}\·\cos L+\frac{V_E}{\left(R_n+h\right)\·{\cos }^{2}L}& 0& -\frac{V_E\·\tan L}{(R_n+h)}\end{array}\right]\end{array}$

$M_3=\left[\begin{array}{ccc}\frac{V_N\tan L-V_U}{R_n+h}& 2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E\tan L}{R_n+h}& -2{\mathrm{\ω}}_{ie}\cos L-\frac{V_E}{R_n+h}\\ -2{\mathrm{\ω}}_{ie}\sin L-\frac{2V_E\tan L}{R_n+h}& -\frac{V_U}{R_m+h}& -\frac{V_N}{R_m+h}\\ 2{\mathrm{\ω}}_{ie}\cos L+\frac{2V_E}{R_n+h}& \frac{2V_N}{R_m+h}& 0\end{array}\right]$

$M_4=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{ie}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{(R_n+h){\cos }^{2}L}& 0& \frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\cos L-\frac{{V_E}^2}{(R_n+h){\cos }^{2}L}& 0& \frac{V_N{V}_U}{{(R_m+h)}^2}+\frac{{V_E}^2\tan L}{{(R_n+h)}^2}\\ -2V_E{\mathrm{\ω}}_{ie}\sin L& 0& -\frac{{V_N}^2}{{(R_m+h)}^2}-\frac{{V_E}^2}{{(R_n+h)}^2}\end{array}\right]$

$\begin{array}{cc}{M}_5=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{1}{(R_n+h)\cos L}& 0& 0\\ 0& 0& 1\end{array}\right]& M_6=\left[\begin{array}{ccc}0& 0& -\frac{V_N}{{(R_m+h)}^2}\\ \frac{V_E\tan L}{(R_n+h)\·\cos L}& 0& -\frac{V_E}{{(R_n+h)}^2\cos L}\\ 0& 0& 0\end{array}\right]\end{array}$

式中，R_m表示地球子午圈主曲率半径，R_n表示地球卯酉圈主曲率半径，h表示所述惯导系统的解算高度，L表示所述惯导系统的解算纬度，V_E、V_N、V_U分别表示所述惯导系统的解算东向、北向、天向速度，ω_ie表示地球自转角速度。

4.根据权利要求1或2或3所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于，所述步骤b)中Kalman滤波模型的建立具体为：

(1)取所述惯导系统姿态误差角、速度误差、位置误差、陀螺常值零偏以及加速度计常值零偏作为所述Kalman滤波器状态矢量：

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$

其中，φ_E、φ_N、φ_U表示所述姿态误差角φⁿ沿坐标轴的分量，δV_E、δV_N、δV_U分别表示东向、北向、天向速度误差，δL、δλ、δh分别表示纬度、经度、高度误差，ε_x、ε_y、ε_z分别表示三个轴向陀螺常值零偏，分别表示三个轴向加速度计常值零偏；

(2)取所述Kalman滤波器的系统状态方程为：

$\stackrel{\·}{X}=FX+GW$

式中：F为所述Kalman滤波器的系统状态矩阵，W为系统噪声矢量，G为噪声转换矩阵，根据公式(3)可以确定系统状态矩阵F为：

$F=\left[\begin{array}{ccccc}-\[{\mathrm{\ω}}_{in}^n\]& M_1& M_2& -C_b^n& 0_{3\×3}\\ \[f^n\]& M_3& M_4& 0_{3\×3}& C_b^n\\ 0_{3\×3}& M_5& M_6& 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}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

式中，是n系相对i系的旋转角速度在n系上的投影分量组成的反对称矩阵，[fⁿ]是所述惯导系统的加速度计输出比力在n系上的投影分量组成的反对称矩阵，0_3×3表示三维零矩阵，M_i(i＝1,2,3…6)表示所述相关矩阵；

(3)利用所述高精度的外部参考信息构造系统量测矢量，所构造的系统量测矢量为：

Z＝[V_TE-V_EV_TN-V_NL_T-Lλ_T-λ]^T

式中，V_TE、V_TN分别表示所述外部参考东向、北向速度，L_T、λ_T分别表示所述外部参考纬度、经度；

(4)确定系统量测方程为：

Z＝HX+η

式中，H为量测矩阵，η为量测噪声：

$H=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 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& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right].$

5.根据权利要求4所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于，所述步骤d)中Kalman滤波方程如下：

状态一步预测方程为其中Φ_k/k-1分别表示k时刻系统状态一步预测值、k-1时刻系统状态估值、k-1时刻到k时刻的系统状态转移矩阵；

状态估计方程为其中K_k、Z_k、H_k分别为k时刻系统状态估值、系统增益矩阵、量测向量和量测矩阵；

滤波增量方程为其中P_k/k-1、R_k分别为k时刻系统协方差阵的一步预测、系统量测噪声矩阵；

一步预测均方误差方程为其中P_k-1、Q_k-1、Γ_k-1分别为k-1时刻系统协方差阵、系统噪声矩阵和系统噪声驱动矩阵；

估计均方误差方程为 $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$ 其中P_k为k时刻系统状态协方差阵，I是单位阵。

6.根据权利要求5所述的一种用于单轴调制激光陀螺惯导系统的海上动态启动方法，其特征在于，所述步骤i)中所用公式如下：

(1)姿态校正—利用估计出的惯导系统姿态误差角构成校正矩阵：

$C_{n^{\′}}^n=I+\[\mathrm{\φ}\×\]$

式中，[φ×]表示由姿态误差角组成的反对称阵，经展开计算后得：

$C_{n^{\′}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]$

由姿态校正矩阵完成对惯导系统姿态矩阵的校正：

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

(2)器件常值零偏校正—器件常值零偏校正是直接利用Kalman滤波估计出的陀螺常值零偏以及加速度计常值零偏完成陀螺、加速度计输出的实时补偿：

[G_xG_yG_z]^T＝[G_xG_yG_z]^T-[ε_xε_yε_z]^T

${\left[\begin{array}{ccc}{A}_x& A_y& A_z\end{array}\right]}^T={\left[\begin{array}{ccc}{A}_x& A_y& A_z\end{array}\right]}^T-{\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T.$