CN101187567A - 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法 - Google Patents

Abstract

本发明提供的是一种基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法。预热后连续采集光纤陀螺仪和石英挠性加速度计输出的数据；对采集到的陀螺仪和加速度计的数据进行处理；完成捷联惯导系统的粗对准；粗对准完毕后进入精对准阶段；建立船用捷联惯性导航系统的动基座误差方程；应用最优控制滤波理论设计滤波器，并进行滤波估计；提取船体姿态失准角信息，在组合精对准结束时用它来修正船体姿态，完成精确初始对准；同时，获得陀螺漂移的估计值，实现初始对准阶段的测漂过程，并对陀螺漂移进行补偿，进一步抑制器件误差对船体导航信息的影响。采用本发明的方法可以在保证对准精度和快速性的要求下，实现对光纤陀螺零位漂移的准确估计。

Description

基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法

(一)技术领域

本发明涉及一种捷联惯性导航系统的初始对准技术，尤其涉及一种以船用光纤陀螺捷联惯导系统和另外一种能独立准确提供船体速度信息的导航系统为基础的组合初始精对准方法。

(二)背景技术

初始对准误差是惯性导航系统主要的误差源之一，初始对准的误差对系统误差的影响不仅表现在姿态指标上，而且表现在速度和位置信息的获取上。对准的精度直接影响着导航的精度。因此，在正常导航之前，必须首先完成初始对准过程。

捷联惯导是现在导航技术中比较热门的技术，由于它的低成本受到了越来越多的导航界人士的青睐，而且通过捷联惯导系统与其它导航系统的组合，可以提高导航系统初始对准的精度。常用的组合有捷联惯导SINS/多普勒DVL、捷联惯导SINS/GPS、捷联惯导SINS/天文导航CNS等。针对船用光纤陀螺捷联惯性导航系统，以速度为外观测量的初始对准多采用捷联惯导/多普勒的组合方式。

对于捷联惯性导航系统(Strapdown Navigation System)来说，初始对准的目的是精确的估计姿态失准角并予以补偿，这个目的由于在实际中不可能理想的补偿仪表误差(尤其是陀螺误差)而不可能完全精确地实现。实际中已广泛应用的经典控制理论对准方法常用于实现静态下的初始对准，并能获得较高的精度，但由于其固有缺陷不适用于动态对准；在动基座条件下应采用最优控制方法，通常采用卡尔曼滤波方法。采用这种方法有很多优点：可以克服经典控制理论中船体加速度对对准性能的不利影响，适合于多种运动情况下的初始对准；在估计船体姿态信息的同时可以估计惯性器件(光纤陀螺和加速度计)的误差，实现初始对准过程中的测漂；初始对准过程中可以实现惯性器件误差的测量和补偿，进而实现对准精度的提高。但是，这种普通卡尔曼滤波方法也存在着缺陷：当系统状态变量较多的情况下，滤波估计算法的计算量很大，尤其是在增广向量的情况下，每增加一个状态变量都会使计算量大幅度增加，这种情况不利于初始对准快速性的要求；在估计惯性器件误差(主要是光纤陀螺测漂)过程中，由于方位陀螺漂移的估计时间较长，要想准确测得必须大幅增加估计时间，此时的卡尔曼滤波器变得不稳定甚至开始发散，导致水平测漂失败，姿态角也开始发散。如何解决初始对准的快速性和如何准确估计陀螺漂移以提高对准精度的问题成为当前的首要任务。

(三)发明内容

本发明的目的在于提供一种能够有效提高船用光纤陀螺捷联惯导/多普勒组合初始对准快速性和精度的基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法。

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

(1)首先对光纤陀螺捷联惯性导航系统进行预热，然后连续采集光纤陀螺仪和石英挠性加速度计输出的数据；

(2)对采集到的陀螺仪和加速度计的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，确定此时的纵摇角θ、横摇角γ和航向角ψ姿态信息；

(3)粗对准完毕后进入精对准阶段，首先，继续采集光纤陀螺和加速度计输出的数据，并通过标准的四元数法进行导航解算，获得船体的计算速度、姿态、位置等相关信息；同时，通过多普勒计程仪或者GPS测得船体的速度信息，并把这个速度近似看作船体的真实速度；

(4)建立船用捷联惯性导航系统的动基座误差方程；

(5)应用最优控制滤波理论设计滤波器，并进行滤波估计；

(6)从状态估值

中提取船体姿态失准角信息α、β、γ，在组合精对准结束时用它来修正船体姿态，即纵摇角θ、横摇角γ和航向角ψ，完成精确初始对准；同时，从偏差估计值中获得陀螺漂移的估计值，实现初始对准阶段的测漂过程，并对陀螺漂移进行补偿，进一步抑制器件误差对船体导航信息的影响。

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

1、所述的建立船用捷联惯性导航系统的动基座误差方程的误差方程为：

α、β、γ——计算地理坐标系与真实水平坐标系之间的姿态误差角；

δ——计算地理坐标系与真实地理坐标系之间的纬度误差；

δλ——计算地理坐标系与真实地理坐标系之间的经度误差；

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差；

V_x、V_y——船体的东北向速度；

w_ie——地球自转角速率；

R_E，R_N——船体所在位置的地球曲率半径；

——船体所在位置的纬度；

g——当地重力加速度；

ε_x、ε_y、ε_z——陀螺误差在载体坐标系的投影，  这里简记为陀螺零位漂移；

_x、_y——加速度计在载体坐标系的投影，这里简记为加速度计偏差。

2、所述的滤波器的设计与滤波估计过程包括：

1)离散滤波器模型的建立；

系统的状态方程和量测方程描述如下：

x_k＝A_k-1x_k-1+B_k-1b_k-1+ξ_k-1    (2)

b_k+1＝b_k    (3)

y_k＝H_kx_k+C_kb_k+η_k    (4)

其中，

x_k——第k个观测时刻的n维状态变量

y_k——量测向量

b_k——偏差向量

ξ_k——过程噪声向量，且满足 $E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_l^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kl}}$

η_k——观测噪声向量，且满足 $E\left[{\mathrm{\η}}_k{\mathrm{\η}}_l^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kl}}$

A_k-1、B_k-1、H_k、C_k——时变的系数矩阵，B_k-1表明偏差向量b_k进入动态方程的方式；

2)系统状态的选择，对于船用捷联系统来说，当把偏差分离出来以后，状态向量可以写成以下形式：

x＝[αβγ δδλδV_x δV_y]^T    (5)

偏差状态写成以下形式：

b＝[ε_x ε_yε_z_x_y]^T    (6)

观测状态变量选择如下：

y＝[δV_x δV_y]^T    (7)

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差，在组合对准中可看成捷联惯性导航系统解算速度与外测真实速度之差；

根据式(1)捷联惯导系统误差状态方程，滤波模型参数设置如下：

$A=\left[\begin{array}{ccc}{A}_{11}& A_{12}& A_{13}\\ A_{21}& A_{22}& A_{23}\\ A_{31}& A_{32}& A_{33}\end{array}\right]---\left(8\right)$

A₂₁＝0_2×3    (12)

$B=\left[\begin{array}{cc}-T_{\mathrm{bn}}& 0_{3\×2}\\ 0_{2\×3}& 0_{2\×2}\\ 0_{2\×3}& B_{32}\end{array}\right]---\left(18\right)$

$T_{\mathrm{bn}}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(19\right)$

$B_{32}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(20\right)$

$H=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(21\right)$

C＝0_2×5    (22)

T_bn——从载体坐标系到导航坐标系的转换矩阵；

3)滤波器的估计；

状态估计值

由无偏状态

和偏差b组成，即

${\hat{x}}_k={\tilde{x}}_k+V_k{\hat{b}}_k---\left(23\right)$

整个滤波器的算法可以用以下方程表示：

${\tilde{x}}_k=A_{k-1}{\tilde{x}}_{k-1}+{\tilde{K}}_x\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}]---\left(24\right)$

${\tilde{P}}_x(k/k-1)=A_{k-1}{\tilde{P}}_x\left(k\right)A_{k-1}^T+Q_k---\left(25\right)$

${\tilde{K}}_x\left(k\right)={\tilde{P}}_x(k/k-1)H_k^T{[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k]}^{-1}---\left(26\right)$

${\tilde{P}}_x\left(k\right)=[I-{\tilde{K}}_x\left(k\right)H_k]{\tilde{P}}_x(k/k-1)---\left(27\right)$

其中，

——x的增益矩阵；

——估计值

的协方差阵；

——

的预测值；

偏差估计表示如下：

${\hat{b}}_k={\hat{b}}_{k-1}+K_b\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}-S\left(k\right){\hat{b}}_{k-1}]---\left(28\right)$

$K_b\left(k\right)=M(k+1)[V^T\left(k\right)H_k^T+C_k^T]R_k^{-1}---\left(29\right)$

$M(k+1)=M\left(k\right)-M\left(k\right)S^T\left(k\right){[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k+S\left(k\right)M\left(k\right)S^T\left(k\right)]}^{-1}S\left(k\right)M\left(k\right)---\left(30\right)$

U(k)＝A_k-1V(k-1)+B_k    (31)

$V\left(k\right)=U\left(k\right)-{\tilde{K}}_x\left(k\right)S\left(k\right)---\left(32\right)$

S(k)＝H_kU(k)+C_k        (33)

其中，

K_b(k)——偏差的增益矩阵；

M(k)，U(k)，V(k)，S(k)——为计算需要而引入的变量；

初始状态设置为：

${\tilde{x}}_0=E\left[\tilde{x}\left(0\right)\right],$ $\tilde{P}\left(0\right)=E\left[(\tilde{x}\left(0\right)-{\tilde{x}}_0){(\tilde{x}\left(0\right)-{\tilde{x}}_0)}^T\right],$ ${\hat{b}}_0=0,$ U(0)＝0，M(0)＝P_b(0)；

解算时，首先根据式(23)～式(27)计算无偏状态估计值

根据式(28)～式(33)计算偏差

的估计值，然后通过

来修正状态估值得到

本发明的方法具有以下优点：(1)当系统状态变量较多的情况下，由于偏差状态从系统状态向量中分离出来，降低了系统维数，从而大大减小了计算量，算短了估计时间，提高了初始对准的速度；(2)在估计惯性器件误差(主要是光纤陀螺测漂)过程中，估计时间的增长不会导致滤波估计值的发散和滤波性能的下降，从而使得方位陀螺漂移也能通过这种方法进行估计，同时保证了初始对准的精度。

对本发明的有益效果说明如下：

对载体在多种运动状态进行了Matlab仿真，其中包括：三轴摇摆状态；匀速加摇摆运动；加速运动。

仿真条件如下：

(1)设船体处于三轴摇摆运动状态。模型为

yaw＝yaw_m sin(ω_ht+_h)+K

pitch＝pitch_m sin(ω_pt+_p)    (34)

roll＝roll_m sin(ω_rt+_r)

其中：

yaw，pitch，roll分别表示航向角、纵摇角和横摇角的摇摆角度变量；三轴运动幅度为yaw_m＝10°，pitch_m＝8°，roll_m＝6°；角频率为w_i＝2π/T_i，(i＝h，p，r)，周期T_h＝6s，T_p＝10s，T_r＝5s。初始相位_r，_h，_p都是30°，初始航向K设为90°。

初始真实姿态位0°、0°、90°；

初始失准角α＝1°，β＝1°，γ＝3°；

初始纬度＝45.7796°，初始经度λ＝126.6705°；

陀螺零位漂移ε_x＝ε_y＝ε_z＝0.1°/h；

陀螺刻度因数误差10^-4；

陀螺仪白噪声误差：0.005度/小时；

加速度计零位误差_x＝_y＝1×10^-4g；

加速度计刻度因数误差为10^-4；

加速度计白噪声误差：5×10^-5g：

地球自转角速率w_ie＝7.27220417rad/s；

载体初始位置：北纬45.7796°，东经126.6705°；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g＝9.78049m/s²；

地球自转角速度(弧度/秒)：7.2921158e-5；

常数：π＝3.1415926；

仿真分为两个阶段进行，先进行200秒的粗对准，然后进入精对准阶段。

此时应用本发明方法分别进行3分钟和15分钟的仿真分析，并在精对准进行

600秒以后对船体姿态进行修正。3分钟和15分钟滤波估计的陀螺漂移曲线分别如图1、图2所示。整个对准阶段船体姿态误差输出信息如图3所示。

(2)设船体处于匀速加三轴摇摆运动状态。船体的速度为10m/s，其余仿真条件与(1)相同。仿真3分钟得到陀螺漂移的估计值曲线如图4所示。

(3)设船体处于加速运动状态。船体东向运动加速度1m/s，加速10s，其余仿真条件与(1)相同。精对准估计10分钟所得陀螺漂移估计曲线如图5所示，整个对准阶段船体姿态误差输出信息如图6所示。

仿真结果表明舰船在动基座下进行组合精对准时，采用本发明的方法可以在保证对准精度和快速性的要求下，实现对光纤陀螺零位漂移的准确估计。

(四)附图说明

图1为三轴摇摆状态下利用Matlab进行3分钟精对准仿真得到的陀螺漂移的估计曲线图；

图2为三轴摇摆状态下利用Matlab进行15分钟精对准仿真得到的陀螺漂移的估计曲线图；

图3为三轴摇摆状态下利用Matlab仿真得到整个初始对准中船体姿态误差角的曲线图；

图4为匀速加三轴摇摆状态下利用Matlab进行3分钟精对准仿真得到的陀螺漂移的估计曲线图；

图5为加速状态下利用Matlab进行10分钟精对准仿真得到的陀螺漂移的估计曲线图；

图6为加速状态下利用Matlab仿真得到的整个初始对准中船体姿态误差角的曲线图。

(五)具体实施方式

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

实施实例一

(1)光纤陀螺捷联惯性导航系统预热后连续采集光纤陀螺仪和石英挠性加速度计输出的数据，并对采集到的陀螺仪和加速度计的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，确定此时船体的姿态信息(纵摇角θ，横摇角γ和航向角ψ)。

(2)粗对准完毕，进入精对准阶段。首先，继续采集光纤陀螺和加速度计输出的数据，并通过标准的四元数法进行导航解算，获得船体的计算速度、姿态、位置等相关信息；同时，通过多普勒计程仪或者GPS测得船体的速度信息，并把这个速度近似看作船体的真实速度。

(3)建立船用捷联惯性导航系统的动基座误差方程。不考虑垂直通道，通常的增广状态为7维误差向量。误差方程如下：

α、β、γ——计算地理坐标系与真实水平坐标系之间的姿态误差角；

δ——计算地理坐标系与真实地理坐标系之间的纬度误差；

δλ——计算地理坐标系与真实地理坐标系之间的经度误差；

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差；

V_x、V_y——船体的东北向速度；

w_ie——地球自转角速率；

R_E，R_N——船体所在位置的地球曲率半径；

——船体所在位置的纬度；

g——当地重力加速度；

ε_x、ε_y、ε_z——陀螺误差在载体坐标系的投影，  这里简记为陀螺零位漂移；

_x、_y——加速度计在载体坐标系的投影，这里简记为加速度计偏差；

(4)应用最优控制滤波理论设计滤波器，并进行滤波估计。这里采用把偏差从系统状态向量中分离出来的滤波方法，通过降低系统维数来提高计算的速度和精度。

1)离散滤波器模型的建立。

系统的状态方程和量测方程描述如下：

x_k＝A_k-1x_k-1+B_k-1b_k-1+ξ_k-1    (2)

b_k+1＝b_k                       (3)

y_k＝H_kx_k+C_kb_k+η_k              (4)

其中，

x_k——n维状态变量(第k个观测时刻).

y_k——量测向量

b_k——偏差向量

ξ_k——过程噪声向量，且满足 $E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_l^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kl}}$

η_k——观测噪声向量，且满足 $E\left[{\mathrm{\η}}_k{\mathrm{\η}}_l^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kl}}$

A_k-1、B_k-1、H_k、C_k——时变的系数矩阵，B_k-1表明偏差向量b_k进入动态方程的方式。

2)系统状态的选择。对于船用捷联系统来说，当把偏差(这里为惯性器件误差)分离出来以后，状态向量可以写成以下形式：

x＝[αβγδδλδV_xδV_y]^T    (5)

偏差状态写成以下形式：

b＝[ε_xε_yε_z_x_y]^T    (6)

观测状态变量选择如下：

y＝[δV_xδV_y]^T    (7)

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差，在组合对准中可看成捷联惯性导航系统解算速度与外测真实速度之差；

根据式(1)捷联惯导系统误差状态方程，滤波模型参数设置如下：

$A=\left[\begin{array}{ccc}{A}_{11}& A_{12}& A_{13}\\ A_{21}& A_{22}& A_{23}\\ A_{31}& A_{32}& A_{33}\end{array}\right]---\left(8\right)$

A₂₁＝0_2×3    (12)

$B=\left[\begin{array}{cc}-T_{\mathrm{bn}}& 0_{3\×2}\\ 0_{2\×3}& 0_{2\×2}\\ 0_{2\×3}& B_{32}\end{array}\right]---\left(18\right)$

$T_{\mathrm{bn}}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(19\right)$

$B_{32}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(20\right)$

$H=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(21\right)$

C＝0_2×5    (22)

T_bn——从载体坐标系到导航坐标系的转换矩阵；

3)滤波器的估计算法。

状态估计值

由无偏状态和偏差b组成，即

${\hat{x}}_k={\tilde{x}}_k+V_k{\hat{b}}_k---\left(23\right)$

整个滤波器的算法可以用以下方程表示：

${\tilde{x}}_k=A_{k-1}{\tilde{x}}_{k-1}+{\tilde{K}}_x\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}]---\left(24\right)$

${\tilde{P}}_x(k/k-1)=A_{k-1}{\tilde{P}}_x\left(k\right)A_{k-1}^T+Q_k---\left(25\right)$

${\tilde{K}}_x\left(k\right)={\tilde{P}}_x(k/k-1)H_k^T{[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k]}^{-1}---\left(26\right)$

${\tilde{P}}_x\left(k\right)=[I-{\tilde{K}}_x\left(k\right)H_k]{\tilde{P}}_x(k/k-1)---\left(27\right)$

其中，

——x的增益矩阵；

——估计值的协方差阵；

——的预测值；

偏差估计表示如下：

${\hat{b}}_k={\hat{b}}_{k-1}+K_b\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}-S\left(k\right){\hat{b}}_{k-1}]---\left(28\right)$

$K_b\left(k\right)=M(k+1)[V^T\left(k\right)H_k^T+C_k^T]R_k^{-1}---\left(29\right)$

$M(k+1)=M\left(k\right)-M\left(k\right)S^T\left(k\right){[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k+S\left(k\right)M\left(k\right)S^T\left(k\right)]}^{-1}S\left(k\right)M\left(k\right)---\left(30\right)$

U(k)＝A_k-1V(k-1)+B_k            (31)

$V\left(k\right)=U\left(k\right)-{\tilde{K}}_x\left(k\right)S\left(k\right)---\left(32\right)$

S(k)＝H_kU(k)+C_k                (33)

其中，

K_b(k)——偏差的增益矩阵；

M(k)，U(k)，V(k)，S(k)——为计算需要而引入的变量；

初始状态设置为：

${\tilde{x}}_0=E\left[\tilde{x}\left(0\right)\right],$ $\tilde{P}\left(0\right)=E\left[(\tilde{x}\left(0\right)-{\tilde{x}}_0){(\tilde{x}\left(0\right)-{\tilde{x}}_0)}^T\right],$ ${\hat{b}}_0=0,$ U(0)＝0，M(0)＝P_b(0)；

具体解算时，首先根据式(23)～式(27)计算无偏状态估计值

根据式(28)～式(33)计算偏差

的估计值，然后通过

来修正状态估值得到

(5)从状态估值中提取船体姿态失准角信息α、β、γ，在组合精对准结束时用它来修正船体姿态(纵摇角θ，横摇角γ和航向角ψ)，实现精确初始对准。同时，从偏差估计值

中获得陀螺漂移的估计值，实现初始对准阶段的测漂过程，并对陀螺漂移进行补偿，进一步抑制了器件误差对船体导航信息的影响。

Claims (3)

1.一种基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法，其特征是：

(1)首先对光纤陀螺捷联惯性导航系统进行预热，然后连续采集光纤陀螺仪和石英挠性加速度计输出的数据；

(2)对采集到的陀螺仪和加速度计的数据进行处理，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，确定此时的纵摇角θ、横摇角γ和航向角ψ姿态信息；

(3)粗对准完毕后进入精对准阶段，首先，继续采集光纤陀螺和加速度计输出的数据，并通过标准的四元数法进行导航解算，获得船体的计算速度、姿态、位置等相关信息；同时，通过多普勒计程仪或者GPS测得船体的速度信息，并把这个速度近似看作船体的真实速度；

(4)建立船用捷联惯性导航系统的动基座误差方程；

(5)应用最优控制滤波理论设计滤波器，并进行滤波估计；

(6)从状态估值

中提取船体姿态失准角信息α、β、γ，在组合精对准结束时用它来修正船体姿态，即纵摇角θ、横摇角γ和航向角ψ，完成精确初始对准；同时，从偏差估计值

中获得陀螺漂移的估计值，实现初始对准阶段的测漂过程，并对陀螺漂移进行补偿，进一步抑制器件误差对船体导航信息的影响。

2.根据权利要求1所述的基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法，其特征是：所述的建立船用捷联惯性导航系统的动基座误差方程的误差方程为：

α、β、γ——计算地理坐标系与真实水平坐标系之间的姿态误差角；

δ——计算地理坐标系与真实地理坐标系之间的纬度误差；

δλ——计算地理坐标系与真实地理坐标系之间的经度误差；

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差；

V_x、V_y——船体的东北向速度；

w_ie——地球自转角速率；

R_E，R_N——船体所在位置的地球曲率半径；

——船体所在位置的纬度；

g——当地重力加速度；

ε_x、ε_y、ε_z——陀螺误差在载体坐标系的投影，这里简记为陀螺零位漂移；

_x、_y——加速度计在载体坐标系的投影，这里简记为加速度计偏差。

3.根据权利要求1或2所述的基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法，其特征是：所述的滤波器的设计与滤波估计过程包括：

1)离散滤波器模型的建立；

系统的状态方程和量测方程描述如下：

x_k＝A_k-1x_k-1+B_k-1b_k-1+ξ_k-1    (2)

b_k+1＝b_k    (3)

y_k＝H_kx_k+C_kb_k+η_k    (4)

其中，

x_k——第k个观测时刻的n维状态变量

y_k——量测向量

b_k——偏差向量

ξ_k——过程噪声向量，且满足 $E\left[{\mathrm{\ξ}}_k{\mathrm{\ξ}}_l^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kl}}$

η_k——观测噪声向量，且满足 $E\left[{\mathrm{\η}}_k{\mathrm{\η}}_l^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kl}}$

A_k-1、B_k-1、H_k、C_k——时变的系数矩阵，B_k-1表明偏差向量b_k进入动态方程的方式；

2)系统状态的选择，对于船用捷联系统来说，当把偏差分离出来以后，状态向量可以写成以下形式：

x＝[α β γ δ δλ δV_x δV_y]^T    (5)

偏差状态写成以下形式：

b＝[ε_x  ε_y  ε_z  _x  _y]^T    (6)

观测状态变量选择如下：

y＝[δV_x δV_y]^T    (7)

δV_x、δV_y——当地地理坐标系的轴向速度计算值与真实值之间的误差，在组合对准中可看成捷联惯性导航系统解算速度与外测真实速度之差；

根据式(1)捷联惯导系统误差状态方程，滤波模型参数设置如下：

$A=\left[\begin{array}{ccc}{A}_{11}& A_{12}& A_{13}\\ A_{21}& A_{22}& A_{23}\\ A_{31}& A_{32}& A_{33}\end{array}\right]---\left(8\right)$

A₂₁＝0_2×3    (12)

$B=\left[\begin{array}{cc}-T_{\mathrm{bn}}& 0_{3\×2}\\ 0_{2\×3}& 0_{2\×2}\\ 0_{2\×3}& B_{32}\end{array}\right]---\left(18\right)$

$T_{\mathrm{bn}}=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(19\right)$

$B_{32}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(20\right)$

$H=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(21\right)$

C＝0_2×5    (22)

T_bn——从载体坐标系到导航坐标系的转换矩阵；

3)滤波器的估计：

状态估计值

由无偏状态

和偏差b组成，即

${\hat{x}}_k={\tilde{x}}_k+V_k{\hat{b}}_k--\left(23\right)$

整个滤波器的算法可以用以下方程表示：

${\tilde{x}}_k=A_{k-1}{\tilde{x}}_{k-1}+{\tilde{K}}_x\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}]---\left(24\right)$

${\tilde{P}}_x(k/k-1)=A_{k-1}{\tilde{P}}_x\left(k\right)A_{k-1}^T+Q_k---\left(25\right)$

${\tilde{K}}_x\left(k\right)={\tilde{P}}_x(k/k-1)H_k^T{[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k]}^{-1}---\left(26\right)$

${\tilde{P}}_x\left(k\right)=[I-{\tilde{K}}_x\left(k\right)H_k]{\tilde{P}}_x(k/k-1)---\left(27\right)$

其中，

——x的增益矩阵；

——估计值

的协方差阵；

——

的预测值；

偏差估计表示如下：

${\hat{b}}_k={\hat{b}}_{k-1}+K_b\left(k\right)[y_k-H_k{A}_{k-1}{\tilde{x}}_{k-1}-S\left(k\right){\hat{b}}_{k-1}]---\left(28\right)$

$K_b\left(k\right)=M(k+1)[V^T\left(k\right)H_k^T+C_k^T]R_k^{-1}---\left(29\right)$

$M(k+1)=M\left(k\right)-M\left(k\right)S^T\left(k\right){[H_k{\tilde{P}}_x(k/k-1)H_k^T+R_k+S\left(k\right)M\left(k\right)S^T\left(k\right)]}^{-1}S\left(k\right)M\left(k\right)---\left(30\right)$

U(k)＝A_k-1V(k-1)+B_k          (31)

$V\left(k\right)=U\left(k\right)-{\tilde{K}}_x\left(k\right)S\left(k\right)---\left(32\right)$

S(k)＝H_kU(k)+C_k              (33)

其中，

K_b(k)——偏差的增益矩阵；

M(k)，U(k)，V(k)，S(k)——为计算需要而引入的变量；

初始状态设置为：

${\tilde{x}}_0=E\left[\tilde{x}\left(0\right)\right],$ $\tilde{P}\left(0\right)=E\left[(\tilde{x}\left(0\right)-{\tilde{x}}_0){(\tilde{x}\left(0\right)-{\tilde{x}}_0)}^T\right],$ ${\hat{b}}_0=0,$ U(0)＝0，M(0)＝P_b(0)；

解算时，首先根据式(23)～式(27)计算无偏状态估计值

根据式(28)～式(33)计算偏差

的估计值，然后通过

来修正状态估值得到

Images