[发明专利]导航系统以及确定导航信息的方法

权利要求书

1.一种确定导航信息的方法，用于重新启动的船舶载体上的惯性导航设备，其特征在于，包括：

第一惯性导航设备从第二惯性导航设备接收第一导航信息，并利用所述第一导航信息针对预先设置的惯性导航计算模型进行速度、位置和姿态的初始化，其中所述第一惯性导航设备与所述第二惯性导航设备为对等的惯性导航设备，位于相同的船舶载体的不同位置，并且所述第一惯性导航设备为重新启动的惯性导航设备；

第一惯性导航设备从第二惯性导航设备接收第二导航信息；以及

所述第一惯性导航设备基于所述第二导航信息以及所述第一惯性导航设备测量的第三导航信息，利用所述惯性导航计算模型以及预先设置的递归运算模型，确定第四导航信息作为所述第一惯性导航设备输出的导航信息，其中

所述惯性导航计算模型为基于所述第一惯性导航设备测量的导航参数的惯性导航计算模型，并且其中

所述第一导航信息和所述第二导航信息包括所述第二惯性导航设备测量的姿态、速度以及位置信息；

所述第三导航信息包括所述第一惯性导航设备测量的姿态、速度以及位置信息；

所述第四导航信息包括所述第一惯性导航设备的载体的速度以及位置信息；

所述惯性导航计算模型是基于第一惯性导航设备，利用捷联惯性导航算法获得的线性模型；并且

所述递归运算模型为卡尔曼滤波器，并且其中

所述卡尔曼滤波器是基于以下方程的卡尔曼滤波器：

状态方程：以及

量测方程：

其中，X为状态变量，并且

式中，φ_E为第一惯性导航设备的横摇角误差、φ_N为第一惯性导航设备的纵摇角误差、φ_U为第一惯性导航设备的航向角误差、δv_E为第一惯性导航设备的东向速度误差、δv_N为第一惯性导航设备的北向速度误差、δv_U为第一惯性导航设备的垂向速度误差、δL为第一惯性导航设备的纬度误差、δλ为第一惯性导航设备的经度误差、δh为第一惯性导航设备的高度误差、为第一惯性导航设备的X陀螺零偏、为第一惯性导航设备的Y陀螺零偏、为第一惯性导航设备的Z陀螺零偏、为第一惯性导航设备的X加速度计零偏、为第一惯性导航设备的Y加速度计零偏以及为第一惯性导航设备的Z加速度计零偏，并且

其中，

式中，L为第二惯性导航设备纬度、λ为第二惯性导航设备经度、h为第二惯性导航设备高度、v_E为第二惯性导航设备东速、v_N为第二惯性导航设备北速、v_U为第二惯性导航设备垂速、f_E为第一惯性导航设备东向加速度、f_N为第一惯性导航设备北向加速度、f_U为第一惯性导航设备垂向加速度、ω_ie为地球自转角速度、R_E为地球子午圈半径以及R_N为地球卯酉圈半径、为第二惯性导航设备的X陀螺零偏过程白噪声、为第二惯性导航设备的Y陀螺零偏过程白噪声、为第二惯性导航设备的Z陀螺零偏过程白噪声、为第二惯性导航设备的X加速度计零偏过程白噪声、为第二惯性导航设备的Y加速度计零偏过程白噪声以及为第二惯性导航设备的Z加速度计零偏过程白噪声，并且

L_s、λ_s、h_s、v_Es、v_Ns和v_Us分别表示第一惯性导航设备系统解算输出的纬度、经度、高度、东速、北速和垂速，L_r、λ_r、h_r、v_Er、v_Nr和v_Ur表示第二惯性导航设备系统解算输出的纬度、经度、高度、东速、北速和垂速，以及

H＝[0_6×3 I_6×6 0_6×6]，V为6维向量，表示纬度的观测白噪声、经度的观测白噪声、高度的观测白噪声、东速的观测白噪声、北速的观测白噪声和垂速的观测白噪声，并且其中

方法还包括：第一惯性导航设备针对所述第一惯性导航设备与所述第二惯性导航设备之间杆臂补偿，并且其中

针对所述第一惯性导航设备与所述第二惯性导航设备进行杆臂补偿的操作，包括按照以下公式进行杆臂补偿：

以及

式中，L_b、λ_b、h_b、v_Eb、v_Nb和v_Ub分别表示第一惯性导航设备载体坐标系下的纬度、经度、高度、东速、北速和垂速，L_B、λ_B、h_B、v_EB、v_NB和v_UB分别表示第二惯性导航设备载体坐标系下的纬度、经度、高度、东速、北速和垂速，以及

以及为第二惯性导航设备的姿态矩阵，计算如下

其中R、P和H为第二惯性导航设备的横摇角、纵摇角和航向角，以及

为第二惯性导航设备在地理坐标系上的三个姿态角速度；

r_x、r_y、r_z为沿横轴、纵轴和方位轴方向的杆臂，并且

方法还包括：确定所述第二惯性导航设备与所述第一惯性导航设备之间的延时；以及根据所确定的延时，对所述第一导航信息进行补偿，并且其中

根据所确定的延时，对所述第一导航信息进行补偿的操作，包括：

若Td＝△T

info_pps＝info[k]*(△T-Td)/T+info[k+1]*(T-(△T-Td))/T

若Td△T

设△T1＝Td-△T，n＝1，

若△T1T，则进行以下循环计算：△T1＝△T1-T，n++，直至△T1T为止，此时info_pps＝info[k+n]*(T-△T1)/T+info[k+n+1]*△T1/T，其中

Td为第二惯性导航设备的激光陀螺采集数据进行滤波预处理的滤波延时，并且根据以下公式计算：Td＝滤波阶数/(2*采样频率)；

info_pps是所述第二惯性导航设备在k+1中断时刻计算完后输出至被牵引惯导的导航信息；

info[k]为所述第二惯性导航设备在第k个内部时钟所测量的导航信息，info[k+1]为所述第二惯性导航设备在第k+1个内部时钟所测量的导航信息；

△T是第一惯性导航设备的第n个内部时钟与第二惯性导航设备第k+1个内部时钟之间的延时；以及

T是第一惯性导航设备与第二惯性导航设备的内部时钟的周期。

2.一种用于船舶载体的导航系统，包括通信连接的第一惯性导航设备和第二惯性导航设备，其中所述第一惯性导航设备与所述第二惯性导航设备为对等的惯性导航设备，位于所述船舶载体的不同位置，并且所述第一惯性导航设备为重新启动的惯性导航设备，其特征在于，所述第一惯性导航设备被配置为执行以下操作：

所述第一惯性导航设备从所述第二惯性导航设备接收第一导航信息，并利用所述第一导航信息针对预先设置的惯性导航计算模型进行速度、位置和姿态的初始化；

第一惯性导航设备从第二惯性导航设备接收第二导航信息；

以及

所述第一惯性导航设备基于所述第二导航信息以及所述第一惯性导航设备测量的第三导航信息，利用预先设置的惯性导航计算模型以及递归运算模型，确定第四导航信息作为所述第一惯性导航设备输出的导航信息，其中

所述惯性导航计算模型为基于所述第一惯性导航设备测量的导航参数的惯性导航计算模型；并且

所述递归运算模型为用于修正所述第一惯性导航设备的递归运算模型，并且其中

所述第一惯性导航设备和所述第二惯性导航设备能够互换，并且其中

所述第一导航信息和所述第二导航信息包括所述第二惯性导航设备测量的姿态、速度以及位置信息；

所述第三导航信息包括所述第一惯性导航设备测量的姿态、速度以及位置信息；

所述第四导航信息包括所述第一惯性导航设备的载体的速度以及位置信息；

所述惯性导航计算模型是基于第一惯性导航设备，利用捷联惯性导航算法获得的线性模型；并且

所述递归运算模型为卡尔曼滤波器，并且其中

所述卡尔曼滤波器是基于以下方程的卡尔曼滤波器：

状态方程：以及

量测方程：

其中，X为状态变量，并且

式中，φ_E为第一惯性导航设备的横摇角误差、φ_N为第一惯性导航设备的纵摇角误差、φ_U为第一惯性导航设备的航向角误差、δv_E为第一惯性导航设备的东向速度误差、δv_N为第一惯性导航设备的北向速度误差、δv_U为第一惯性导航设备的垂向速度误差、δL为第一惯性导航设备的纬度误差、δλ为第一惯性导航设备的经度误差、δh为第一惯性导航设备的高度误差、为第一惯性导航设备的X陀螺零偏、为第一惯性导航设备的Y陀螺零偏、为第一惯性导航设备的Z陀螺零偏、为第一惯性导航设备的X加速度计零偏、为第一惯性导航设备的Y加速度计零偏以及为第一惯性导航设备的Z加速度计零偏，并且

其中，

式中，L为第二惯性导航设备纬度、λ为第二惯性导航设备经度、h为第二惯性导航设备高度、v_E为第二惯性导航设备东速、v_N为第二惯性导航设备北速、v_U为第二惯性导航设备垂速、f_E为第一惯性导航设备东向加速度、f_N为第一惯性导航设备北向加速度、f_U为第一惯性导航设备垂向加速度、ω_ie为地球自转角速度、R_E为地球子午圈半径以及R_N为地球卯酉圈半径、为第二惯性导航设备的X陀螺零偏过程白噪声、为第二惯性导航设备的Y陀螺零偏过程白噪声、为第二惯性导航设备的Z陀螺零偏过程白噪声、为第二惯性导航设备的X加速度计零偏过程白噪声、为第二惯性导航设备的Y加速度计零偏过程白噪声以及为第二惯性导航设备的Z加速度计零偏过程白噪声，并且

L_s、λ_s、h_s、v_Es、v_Ns和v_Us分别表示第一惯性导航设备系统解算输出的纬度、经度、高度、东速、北速和垂速，L_r、λ_r、h_r、v_Er、v_Nr和v_Ur表示第二惯性导航设备系统解算输出的纬度、经度、高度、东速、北速和垂速，以及

H＝[0_6×3 I_6×6 0_6×6]，V为6维向量，表示纬度的观测白噪声、经度的观测白噪声、高度的观测白噪声、东速的观测白噪声、北速的观测白噪声和垂速的观测白噪声，并且其中

并且，所述第一惯性导航设备被配置为执行以下操作：第一惯性导航设备针对所述第一惯性导航设备与所述第二惯性导航设备之间杆臂补偿，并且其中

针对所述第一惯性导航设备与所述第二惯性导航设备进行杆臂补偿的操作，包括按照以下公式进行杆臂补偿：

以及

式中，L_b、λ_b、h_b、v_Eb、v_Nb和v_Ub分别表示第一惯性导航设备载体坐标系下的纬度、经度、高度、东速、北速和垂速，L_B、λ_B、h_B、v_EB、v_NB和v_UB分别表示第二惯性导航设备载体坐标系下的纬度、经度、高度、东速、北速和垂速，以及

以及为第二惯性导航设备的姿态矩阵，计算如下

其中R、P和H为第二惯性导航设备的横摇角、纵摇角和航向角，以及

为第二惯性导航设备在地理坐标系上的三个姿态角速度；

r_x、r_y、r_z为沿横轴、纵轴和方位轴方向的杆臂，并且

第一惯性导航设备还被配置为进一步执行以下操作：确定所述第二惯性导航设备与所述第一惯性导航设备之间的延时；以及根据所确定的延时，对所述第一导航信息进行补偿，并且其中

根据所确定的延时，对所述第一导航信息进行补偿的操作，包括：

若Td＝△T

info_pps＝info[k]*(△T-Td)/T+info[k+1]*(T-(△T-Td))/T

若Td△T

设△T1＝Td-△T，n＝1，

若△T1T，则进行以下循环计算：△T1＝△T1-T，n++，直至△T1T为止，此时info_pps＝info[k+n]*(T-△T1)/T+info[k+n+1]*△T1/T，其中

Td为第二惯性导航设备的激光陀螺采集数据进行滤波预处理的滤波延时，并且根据以下公式计算：Td＝滤波阶数/(2*采样频率)；

info_pps是所述第二惯性导航设备在k+1中断时刻计算完后输出至被牵引惯导的导航信息；

info[k]为所述第二惯性导航设备在第k个内部时钟所测量的导航信息，info[k+1]为所述第二惯性导航设备在第k+1个内部时钟所测量的导航信息；

△T是第一惯性导航设备的第n个内部时钟与第二惯性导航设备第k+1个内部时钟之间的延时；以及

T是第一惯性导航设备与第二惯性导航设备的内部时钟的周期。