[发明专利]一种鲁棒因子图多源容错导航方法

说明书

本发明公开一种鲁棒因子图多源容错导航方法，包括：系统初始化：对各导航传感器的导航状态量、导航传感器残差进行初始化；IMU导航信息获取：通过IMU采集加速度计数据、陀螺仪数据，并基于加速度计数据、陀螺仪数据计算第一载体导航信息；导航传感器残差获取：基于各导航传感器实时采集第二载体导航信息，基于第一、第二载体导航信息计算各导航传感器的残差；导航传感器故障检测：基于各导航传感器的残差数据对各导航传感器进行故障检测；导航优化：对故障导航传感器进行隔离，基于故障信息，利用因子图优化求解载体导航信息。本发明能够解决由于导航传感器故障，导致基于因子图算法的组合导航系统定位精度下降的问题，可靠性高、适用性强。

技术领域

本发明涉及组合导航技术领域，特别是涉及一种鲁棒因子图多源容错导航方法。

背景技术

惯性基多源导航信息融合技术是指以惯性导航技术为基础，并融合其它导航信息的多源信息融合技术。构建基于惯性导航技术的鲁棒信息融合方法一直是组合导航领域的研究热点。然而，若其它导航传感器产生故障，将故障信息带入组合导航系统中，将大大降低导航过程的估计精度。目前针对组合导航系统的鲁棒融合方法，主要采用基于模型的故障检测算法与基于信号处理的故障检测方法对故障信息进行检测。在此基础之上结合各类滤波算法，以提供一种鲁棒的融合导航方法。

相较于传统卡尔曼滤波算法，因子图算法可利用历史时刻的多个量测信息同时对历史时刻的状态量进行批量估计，得到系统状态量的全局最优解，提高状态估计精度。为了提高导航方法的鲁棒性，可采用层次化因子图的方式。该方法由外到内逐层进行故障诊断，并判断故障发生的地方。但该方法需要建立完整清晰的因子图层次化模型并明确各层次中变量的概率依赖关系，系统建模计算量大，复杂性高。因此，需要一种可利用现有因子图模型特点的鲁棒因子图多源容错导航方法。

发明内容

本发明的目的是提供一种鲁棒因子图多源容错导航方法，以解决现有技术的问题，能够解决由于导航传感器故障，导致基于因子图算法的组合导航系统定位精度下降的问题，可靠性高、适用性强。

为实现上述目的，本发明提供了如下方案：本发明提供一种鲁棒因子图多源容错导航方法，包括如下步骤：

系统初始化：对各导航传感器的导航状态量、导航传感器残差进行初始化；

惯性测量单元IMU导航信息获取：通过IMU采集加速度计数据、陀螺仪数据，并基于所述加速度计数据、陀螺仪数据计算第一载体导航信息；

导航传感器残差获取：基于各导航传感器实时采集第二载体导航信息，基于所述第一载体导航信息、第二载体导航信息计算各导航传感器的残差；

导航传感器故障检测：基于各导航传感器的残差数据对各导航传感器进行故障检测；

导航优化：对故障导航传感器进行隔离，基于故障信息，利用因子图优化求解载体导航信息。

优选地，所述系统初始化步骤中，各导航传感器的导航状态量包括：载体位置信息、载体速度信息、载体姿态信息，其中，所述载体位置信息包括：所述载体所在位置的经度、纬度、高度；所述载体速度信息初始化为0；所述载体姿态信息为以四元数形式表示的机体系与地理系的夹角。

优选地，所述系统初始化步骤中，所述导航传感器残差的初始化结果为0，并利用所述导航传感器噪声初始化量测协方差矩阵。

优选地，所述IMU导航信息获取步骤具体包括：

分别构建所述IMU中的加速度计、陀螺仪的输出模型；

对于所述IMU的两个相邻采样时刻，基于所述加速度计、陀螺仪的输出模型的输出结果，分别计算载体的第一载体导航信息，所述第一载体导航信息包括第一载体姿态信息、第一载体速度信息、第一载体位置信息。

优选地，所述第一载体姿态信息的计算方法包括：