一种鲁棒因子图多源容错导航方法与流程

技术特征：
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
分别表示地球卯酉圈曲率半径与子午圈曲率半径。6.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述导航传感器残差获取步骤中，所述第二载体导航信息包括：第二载体姿态信息、第二载体速度信息、第二载体位置信息；在采样时刻为t
k 1
时刻，所述导航传感器的残差r
k 1
如下式所示：r
k 1
＝[r
vk 1 r
pk 1 r
qk 1
]
t
其中，r
vk 1
、r
pk 1
、r
qk 1
分别表示t
k 1
时刻所述导航传感器的速度残差项、位置残差项与姿态残差项。7.根据权利要求6所述的鲁棒因子图多源容错导航方法，其特征在于，所述速度残差项r
vk 1
的计算方法包括：当所述导航传感器所采集到的第二载体速度信息为z
vk 1
时，通过所述第一载体速度信息计算t
k 1
时刻速度残差项r
vk 1
为：其中，表示t
k 1
时刻的第一载体速度信息，q
v
表示所述导航传感器速度量测协方差矩阵；所述位置残差项r
pk 1
的计算方法包括：当所述导航传感器所采集到的第二载体位置信息为z
pk 1
时，通过所述第一载体位置信息计算t
k 1
时刻位置残差项r
pk 1
为：r
pk 1
＝(z
pk 1
‑
p
k 1
)
t
q
p
‑1(z
pk 1
‑
p
k 1
)其中，p
k 1
表示t
k 1
时刻的第一载体位置信息，q
p
表示所述导航传感器位置量测协方差矩阵；所述姿态残差项r
qk 1
的计算方法包括：当所述导航传感器所采集到的以四元数表示的第二载体姿态信息z
qk 1
时，通过所述第一载体姿态信息计算t
k 1
时刻姿态残差项r
qk 1
为：
其中，表示所述第一载体姿态信息，表示取姿态误差四元数的虚部，q
q
表示所述导航传感器姿态量测协方差矩阵。8.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述导航传感器故障检测步骤中，对所述导航传感器进行故障检测的方法包括：计算各所述导航传感器的残差均值基于各所述导航传感器的残差与所述残差均值的关系，进行故障诊断，如下式所示：其中，r
k 1
为所述导航传感器的残差，η为故障检测阈值系数，s为故障检测结果，若检测结果为0，则表明当前导航传感器导航信息存在故障；若检测结果为1，则表明当前导航传感器导航信息不存在故障。9.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，所述导航优化步骤具体包括：若所述导航传感器存在故障，导航优化目标函数按如下式所示：若所述导航传感器不存在故障，导航优化目标函数按如下式所示：其中，||r
p
‑
h
p
x||2为边缘化的先验约束，为惯性残差，b是所有imu测量的集合，为惯性量测协方差矩阵，为所述导航传感器测量残差，c为导航传感器的集合，表示导航传感器量测协方差矩阵。10.根据权利要求9所述的鲁棒因子图多源容错导航方法，其特征在于，使用高斯牛顿非线性优化方法对所述目标函数进行求解，并重复执行imu导航信息获取步骤，直至达到预设条件，得到载体的导航信息。

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


技术研发人员：王炳清 赖际舟 吕品 李志敏
受保护的技术使用者：南京航空航天大学
技术研发日：2021.04.23
技术公布日：2021/10/19