[发明专利]一种导航系统的故障处理方法、电子设备及存储介质

权利要求书

1.一种导航系统的故障处理方法，其特征在于，包括以下步骤：

1）初始化标志位，分别设置GPS故障标志位和陀螺、加速度计修正标志位；

2）读取陀螺、加速度计数据和GPS输出故障标志，当读取的GPS输出故障标志不能定位且陀螺、加速度计可以修正测量数据时，执行步骤3），否则执行步骤4）；

3）进行陀螺、加速度计修正；

4）根据读取的陀螺、加速度计数据进行惯导位姿解算和卡尔曼滤波的一步预测，判断GPS输入的标志位是否有效定位，若是则执行步骤5），否则执行步骤6）；

5）进行卡尔曼滤波，执行位姿的反馈校正，输出校正后的结果作为系统输出结果；

6）进行传感器修正条件判定，输出惯导解算的位姿作为系统输出结果。

2.根据权利要求1所述的导航系统的故障处理方法，其特征在于，所述步骤1）初始化标志位，分别设置GPS故障标志位和陀螺、加速度计修正标志位的步骤，还包括，

设置GPS故障标志位为1表示GPS无定位，为0表示GPS有效定位；

设置陀螺、加速度计修正标志位为1表示可以用陀螺与加速度计测量数据进行修正，为0表示不可修正；

初始化时，将GPS故障标志位赋值为0，陀螺、加速度计修正标志赋值为0。

3.根据权利要求1所述的导航系统的故障处理方法，其特征在于，所述步骤3）进行陀螺、加速度计修正的步骤，还包括，

将记录的陀螺、加速度计零偏值分别对应加入陀螺测量的角加速度和加速度计测量的加速度值，更新测量值完成陀螺、加速度计测量数据修正。

4.根据权利要求1所述的导航系统的故障处理方法，其特征在于，所述步骤4）根据读取的陀螺、加速度计数据进行惯导位姿解算和卡尔曼滤波的一步预测的步骤，还包括，

根据陀螺、加速度计的角速度和加速度进行惯导解算出位置、速度、姿态信息；

根据解算后的位置、速度、姿态信息解算出陀螺的零偏估计值和加速度计的零偏估计值。

5.根据权利要求4所述的导航系统的故障处理方法，其特征在于，所述根据陀螺、加速度计的角速度和加速度进行惯导解算出位置、速度、姿态信息的步骤，还包括，

根据陀螺数据输出周期的角增量解算四元数，根据四元数更新姿态矩阵解算姿态；

根据载体系转换到导航系的旋转矩阵、地球自转角速度、载体运动产生的相对于地球的旋转角速度、当前速度在导航系上的投影以及地球重力加速度求解速度增量，进行速度更新；

根据纬度、经度、高度、地球短轴半径与载体高度之和以及地球长轴半径与载体高度之和解算位置。

6.根据权利要求1所述的导航系统的故障处理方法，其特征在于，所述步骤5）进行卡尔曼滤波，执行位姿的反馈校正，输出校正后的结果作为系统输出结果的步骤，还包括，

读取GPS传感器数据，计算卡尔曼滤波量测值，通过卡尔曼滤波法进行滤波估计；

根据解算的惯导位姿进行位姿的反馈校正，将校正后的结果作为系统输出结果。

7.根据权利要求1所述的导航系统的故障处理方法，其特征在于，所述6）进行传感器修正条件判定，输出惯导解算的位姿作为系统输出结果的步骤，还包括，

设置陀螺数据输出周期阈值向量，将相隔时刻估计零偏对应变量的差值与置陀螺数据输出周期阈值进行比较，若差值大于设定的阈值，则卡尔曼滤波未能稳定估计出零偏，无法进行修正，将陀螺、加速度计修正标志位赋值为0。

8.根据权利要求7所述的导航系统的故障处理方法，其特征在于，还包括，

对记录零偏信息向量的每一维数据进行分别判断；

当有任一维数据满足陀螺数据输出周期阈值时，将陀螺、加速度计修正标志位赋值为1，进行传感器数据修正，并返回执行步骤2）。

9.一种电子设备，其特征在于，包括存储器和处理器，所述存储器上储存有在所述处理器上运行的计算机程序，所述处理器运行所述计算机程序时执行权利要求1至8任一项所述的导航系统的故障处理方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序运行时执行权利要求1至8任一项所述的导航系统的故障处理方法的步骤。