[发明专利]极区双航海惯性导航系统定位信息融合方法

权利要求书

1.极区双航海惯性导航系统定位信息融合方法，其特征在于包括以下步骤：

步骤1：极区内确定格网坐标系G为导航坐标系，其定义为：以舰船所在地平行于格林威治子午面的平面为格网面，其与当地水平面的交线定义为格网北向，格网北向与当地地理系n的北向的夹角为格网角σ，格网天向与当地地理系n的天向重合，格网东向与格网北向、格网天向构成右手正交坐标系，当地地理系n为“东向-北向-天向”定义；

步骤2：在格网坐标系G下建立单轴旋转调制激光陀螺航海惯性导航系统的姿态误差微分方程、速度误差微分方程、位置误差角微分方程，单轴旋转调制激光陀螺航海惯性导航系统编号为1，通过以下步骤实现：

2.1)建立格网坐标系G下单轴旋转调制激光陀螺航海惯性导航系统1的姿态误差微分方程为

其中，为单轴旋转调制激光陀螺航海惯性导航系统1的姿态误差，分别为姿态误差在格网东向、格网北向、格网天向的分量，为格网系相对惯性坐标系的角速度，为格网系下表示的地球自转角速度，分别为在格网东向、格网北向、格网天向的分量，为格网系相对地球的转移角速度，分别为在格网东向、格网北向、格网天向的分量，为与单轴旋转调制激光陀螺航海惯性导航系统1的速度误差、位置误差角相关的格网系相对惯性坐标系的角速度误差，为与单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角相关的地球自转角速度误差，为与单轴旋转调制激光陀螺航海惯性导航系统1的速度误差、位置误差角相关的格网系相对地球的转移角速度误差，为单轴旋转调制激光陀螺航海惯性导航系统1的载体系b₁到格网系G的方向余弦矩阵，为单轴旋转调制激光陀螺航海惯性导航系统1的陀螺输出角速度误差，ε₁＝[ε_x1 ε_y1 ε_z1]^T为该系统中的陀螺漂移，ε_x1、ε_y1、ε_z1为陀螺漂移在相应坐标轴的分量，为该系统中的陀螺噪声，为陀螺噪声在相应坐标轴的分量；

2.2)建立格网坐标系G下单轴旋转调制激光陀螺航海惯性导航系统1的速度误差微分方程为

其中，为单轴旋转调制激光陀螺航海惯性导航系统1的速度误差，分别为速度误差在格网东向、格网北向、格网天向的分量，为比力，分别为比力在格网东向、格网北向、格网天向的分量，为格网坐标系G下的舰船速度，分别为舰船速度格网东向、格网北向、格网天向的分量，为单轴旋转调制激光陀螺航海惯性导航系统1的比力误差，▽₁＝[▽_x1 ▽_y1 ▽_z1]^T为该系统中的加速度计零偏，▽_x1、▽_y1、▽_z1为加速度计零偏在相应坐标轴的分量，为加速度计噪声，为加速度计噪声在相应坐标轴的分量；

2.3)确定格网坐标系G下单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角，同时建立其微分方程，通过以下步骤实现：

2.3.1)将格网系下单轴旋转调制激光陀螺航海惯性导航系统1输出的舰船位置矩阵用真实的舰船位置矩阵表示为

其中，I₃为三阶单位矩阵，为格网系G下的单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角，为当地地理系n与格网系G间的方向余弦矩阵，为地心地固坐标系e与当地地理系n间的方向余弦矩阵，c_ij表示舰船位置矩阵的第i行j列元素，其中，i＝1，2，3，j＝1，2，3；L、λ分别为舰船真实的纬度、经度，δσ₁为单轴旋转调制激光陀螺航海惯性导航系统1的格网角误差，为当地地理系n下的位置误差角，δL₁、δλ₁分别为单轴旋转调制激光陀螺航海惯性导航系统1的纬度输出误差、经度输出误差；

2.3.2)根据(3)式确定单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角为

其中，为位置误差角在格网东向、格网北向、格网天向的分量；

2.3.3)根据(4)式确定单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角各分量间的关系

即单轴旋转调制激光陀螺航海惯性导航系统1位置误差角的格网天向分量与其格网东向分量线性相关，其格网东向、格网北向分量即可完全确定相应的位置误差角

2.3.4)根据(3)式确定对其两侧微分获得位置误差角的微分方程为

步骤3：在格网坐标系G下建立双轴旋转调制激光陀螺航海惯性导航系统的姿态误差微分方程、速度误差微分方程、位置误差角微分方程，双轴旋转调制激光陀螺航海惯性导航系统编号为2，通过以下步骤实现：

3.1)建立格网坐标系G下双轴旋转调制激光陀螺航海惯性导航系统2的姿态误差微分方程为

其中，为双轴旋转调制激光陀螺航海惯性导航系统2的姿态误差，分别为姿态误差在格网东向、格网北向、格网天向的分量，为与双轴旋转调制激光陀螺航海惯性导航系统2的速度误差、位置误差角相关的格网系相对惯性坐标系的角速度误差，为与双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角相关的地球自转角速度误差，为与双轴旋转调制激光陀螺航海惯性导航系统2的速度误差、位置误差角相关的格网系相对地球的转移角速度误差，为双轴旋转调制激光陀螺航海惯性导航系统2的载体系b₂到格网系G的方向余弦矩阵，为双轴旋转调制激光陀螺航海惯性导航系统2的陀螺输出角速度误差，ε₂＝[ε_x2 ε_y2 ε_z2]^T为该系统中的陀螺漂移，ε_x2、ε_y2、ε_z2为陀螺漂移在相应坐标轴的分量，为该系统中的陀螺噪声，为陀螺噪声在相应坐标轴的分量；

3.2)建立格网坐标系G下双轴旋转调制激光陀螺航海惯性导航系统2的速度误差微分方程为

其中，为双轴旋转调制激光陀螺航海惯性导航系统2的速度误差，分别为速度误差在格网东向、格网北向、格网天向的分量，为双轴旋转调制激光陀螺航海惯性导航系统2的比力误差，▽₂＝[▽_x2 ▽_y2▽_z2]^T为该系统中的加速度计零偏，▽_x2、▽_y2、▽_z2为加速度计零偏在相应坐标轴的分量，为加速度计噪声，为加速度计噪声在相应坐标轴的分量；

3.3)确定格网坐标系G下双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角，同时建立其微分方程，通过以下步骤实现：

3.3.1)将格网系下双轴旋转调制激光陀螺航海惯性导航系统2输出的舰船位置矩阵用真实的舰船位置矩阵表示为

其中，为格网系G下的双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角，δσ₂为双轴旋转调制激光陀螺航海惯性导航系统2的格网角误差，为当地地理系n下的位置误差角，δL₂、δλ₂分别为双轴旋转调制激光陀螺航海惯性导航系统2的纬度输出误差、经度输出误差；

3.3.2)根据(9)式确定双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角为

其中，为位置误差角在格网东向、格网北向、格网天向的分量；

3.3.3)根据(10)式确定双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角各分量间的关系

即双轴旋转调制激光陀螺航海惯性导航系统2位置误差角的格网天向分量与其格网东向分量线性相关，其格网东向、格网北向分量即可完全确定相应的位置误差角

3.3.4)根据(9)式确定对其两侧微分获得位置误差角的微分方程为

步骤4：在格网坐标系G下建立单轴旋转调制激光陀螺航海惯性导航系统1与双轴旋转调制激光陀螺航海惯性导航系统2的联合误差状态方程，通过以下步骤实现：

4.1)构建格网坐标系下的联合误差状态，联合误差状态为：单轴旋转调制激光陀螺航海惯性导航系统1姿态误差φ₁^G与双轴旋转调制激光陀螺航海惯性导航系统2姿态误差的差值单轴旋转调制激光陀螺航海惯性导航系统1速度误差与双轴旋转调制激光陀螺航海惯性导航系统2速度误差的差值单轴旋转调制激光陀螺航海惯性导航系统1位置误差角与双轴旋转调制激光陀螺航海惯性导航系统2位置误差角的差值的格网东向分量和格网北向分量，增广单轴旋转调制激光陀螺航海惯性导航系统1的陀螺漂移ε₁、加速度计零偏▽₁以及双轴旋转调制激光陀螺航海惯性导航系统2的陀螺漂移ε₂、加速度计零偏▽₂作为联合误差状态，不考虑高度通道相关的误差状态；联合误差状态矢量x^G(t)为

4.2)分别将步骤2.1)中单轴旋转调制激光陀螺航海惯性导航系统1的姿态误差微分方程式(1)减去步骤3.1)中双轴旋转调制激光陀螺航海惯性导航系统2的姿态误差微分方程式(7)、步骤2.2)中单轴旋转调制激光陀螺航海惯性导航系统1的速度误差微分方程式(2)减去步骤3.2)中双轴旋转调制激光陀螺航海惯性导航系统2的速度误差微分方程式(8)、步骤2.3)中单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角微分方程式(6)减去步骤3.3)中双轴旋转调制激光陀螺航海惯性导航系统2的位置误差角微分方程式(12)，构建格网坐标系下的联合误差状态方程为

其中，为地球自转角速度误差的差值，为格网系相对地球的转移角速度误差的差值，为格网系相对惯性坐标系的角速度误差的差值；

4.3)将步骤4.2)中的联合误差状态方程表示为矩阵形式

其中，系统状态矩阵F(t)、系统噪声矩阵G(t)及系统噪声w(t)分别为：

κ_G是符号的替换简写，h为舰船的高度，R_e为地球的长半轴，f为地球的椭球扁率，1/R_xG、1/R_yG为地球在格网系下相应坐标轴方向的曲率，1/τ_fG为舰船所在位置的扭曲率，分别为的前两行前两列构成的子矩阵，0_i×j代表i行j列的零矩阵；

步骤5：在格网坐标系G下建立单轴旋转调制激光陀螺航海惯性导航系统1与双轴旋转调制激光陀螺航海惯性导航系统2间的观测方程，通过以下步骤实现

5.1)确定观测量z(t)为扣除两套系统间的杆臂效应之后的格网东向速度误差差值、格网北向速度误差差值，以及位置误差角差值，如下：

其中，分别为单轴旋转调制激光陀螺航海惯性导航系统1、双轴旋转调制激光陀螺航海惯性导航系统2各自输出的格网东向速度，分别为单轴旋转调制激光陀螺航海惯性导航系统1、双轴旋转调制激光陀螺航海惯性导航系统2各自输出的格网北向速度；分别为中相应的矩阵元素；

5.2)确定观测方程为

z(t)＝Hx^G(t)+υ(t) (27)

其中，观测矩阵为I₂为二阶单位矩阵，υ(t)为观测噪声；

步骤6：在格网坐标系G下分别对单轴旋转调制激光陀螺航海惯性导航系统1与双轴旋转调制激光陀螺航海惯性导航系统2的姿态、速度、位置矩阵进行更新，按照步骤4、步骤5中所述联合误差状态方程、观测方程进行卡尔曼滤波，对单轴旋转调制激光陀螺航海惯性导航系统1的陀螺漂移ε₁、加速度计零偏▽₁进行估计；

步骤7：根据步骤6中估计得到的单轴旋转调制激光陀螺航海惯性导航系统1的陀螺漂移ε₁、加速度计零偏▽₁，对它们造成的确定性的长期定位误差进行预测补偿，补偿方式为输出校正，预测补偿步骤为：

7.1)根据步骤2.1)、2.2)、2.3)所建立的单轴旋转调制激光陀螺航海惯性导航系统1的姿态误差、速度误差、位置误差角微分方程，确定其误差状态为

误差状态方程同步骤2.1)、2.2)、2.3)所述，同时将步骤6中估计得到的单轴旋转调制激光陀螺航海惯性导航系统1的陀螺漂移ε₁、加速度计零偏▽₁作为外界输入矢量u(t)＝[ε_x1 ε_y1 ε_z1 ▽_x1 ▽_y1]^T；

7.2)离散化步骤7.1)中的误差状态方程，得到单轴旋转调制激光陀螺航海惯性导航系统1的定位误差预测模型

其中，为误差状态的离散化估计量，Φ₁(k+1,k)为系统状态矩阵A₁的离散矩阵，Γ₁(k+1,k)为外界输入矩阵B₁的离散矩阵，Δt为离散间隔，为外界输入量u(t)的离散量，k、k+1为离散化时刻，初始时刻

7.3)根据定位误差预测模型对单轴旋转调制激光陀螺航海惯性导航系统1的确定性的长期定位误差通过卡尔曼滤波进行预测补偿，补偿方式为输出校正，补偿过确定性定位误差后，单轴旋转调制激光陀螺航海惯性导航系统1的位置输出矩阵为

其中，为通过定位误差预测模型估计得到的单轴旋转调制激光陀螺航海惯性导航系统1的位置误差角，分别是的格网东向分量和格网北向分量，为位置矩阵修正值；

步骤8：舰船一旦进入极区，导航坐标系由当地地理坐标系n切换为格网坐标系G，相应的联合误差状态及其协方差也从当地地理坐标系n转换到格网坐标系G下，通过以下步骤实现

8.1)格网坐标系G下表示的联合误差状态x^G(t)通过转换矩阵T从当地地理系n下表示的联合误差状态xⁿ(t)得到，转换方式为x^G(t)＝Txⁿ(t)，其中，

式中，当地地理系n下表示的单轴旋转调制激光陀螺航海惯性导航系统1姿态误差与双轴旋转调制激光陀螺航海惯性导航系统2姿态误差的差值的地理东向、地理北向、地理天向分量分别为单轴旋转调制激光陀螺航海惯性导航系统1速度误差与双轴旋转调制激光陀螺航海惯性导航系统2速度误差的差值的地理东向、地理北向分量分别为δL₁₂、δλ₁₂分别为单轴旋转调制激光陀螺航海惯性导航系统1与双轴旋转调制激光陀螺航海惯性导航系统2的纬度输出误差差值和经度输出误差差值；

8.2)格网坐标系G下表示的联合误差状态的协方差P^G(t)通过转换矩阵T从当地地理系n下表示的联合误差状态的协方差Pⁿ(t)得到，转换方式为

式中，为格网坐标系G下联合误差状态x^G(t)的估计值，为当地地理系n下联合误差状态xⁿ(t)的估计值，E{}表示期望运算符，Pⁿ(t)由当地地理系下的联合误差状态卡尔曼滤波器计算得到；

8.3)转换完成后，按步骤6所述通过卡尔曼滤波器对单轴旋转调制激光陀螺航海惯性导航系统1的陀螺漂移ε₁、加速度计零偏▽₁进行估计，并按步骤7所述的定位误差预测模型对单轴旋转调制激光陀螺航海惯性导航系统1的确定性的长期定位误差进行预测补偿；一旦作为主惯导的双轴旋转调制激光陀螺航海惯性导航系统2故障，可靠性更高且经过确定性定位误差补偿的单轴旋转调制激光陀螺航海惯性导航系统1继续输出定位信息。