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

权利要求书

1.一种鲁棒因子图多源容错导航方法，其特征在于，包括如下步骤：

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

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

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

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

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

2.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述系统初始化步骤中，各导航传感器的导航状态量包括：载体位置信息、载体速度信息、载体姿态信息，其中，所述载体位置信息包括：所述载体所在位置的经度、纬度、高度；所述载体速度信息初始化为0；所述载体姿态信息为以四元数形式表示的机体系与地理系的夹角。

3.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述系统初始化步骤中，所述导航传感器残差的初始化结果为0，并利用所述导航传感器噪声初始化量测协方差矩阵。

4.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述IMU导航信息获取步骤具体包括：

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

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

5.根据权利要求4所述的鲁棒因子图多源容错导航方法，其特征在于，所述第一载体姿态信息的计算方法包括：

对于IMU的相邻两个采样时刻t_k与t_k+1，根据所述陀螺仪的输出模型的输出结果，计算所述第一载体姿态信息，如下式所示：

其中，为以四元数形式表示的载体在t_k+1时刻的姿态信息，为在积分时间t内以四元数形式表示的载体的姿态信息，为积分时间t内实际导航解算所需要的载体运动角速率；

所述第一载体速度信息的计算方法包括：

对于IMU的相邻两个采样时刻t_k与t_k+1，根据所述加速度计的输出模型的输出结果，计算所述第一载体速度信息，如下式所示：

其中，表示载体在t_k+1时刻的速度信息，表示载体在t_k时刻的速度信息，表示t_k时刻地球自转角速率，表示t_k时刻导航系相对于地球坐标系的转动角速度在导航系下的分量，ΔT_IMU表示IMU的采样时间间隔，表示载体在t_k+1时刻机体系到导航系的坐标转换矩阵，表示t_k时刻加速度计的输出值，n_a为加速度计白噪声，b_a为加速度计零偏，g为地球重力加速度，n为所述导航传感器的采样个数；

所述第一载体位置信息的计算方法包括：

对于IMU的相邻两个采样时刻t_k与t_k+1，根据所述加速度计的输出模型的输出结果，计算所述第一载体位置信息，如下式所示：

其中，λ_k、λ_k+1分别表示载体t_k与t_k+1时刻的经度信息，L_k、L_k+1分别表示载体t_k与t_k+1时刻的纬度信息，h_k、h_k+1分别表示载体t_k与t_k+1时刻的高度信息，分别表示载体速度在x、y、z三个轴向的分量，R_N、R_M分别表示地球卯酉圈曲率半径与子午圈曲率半径。