CN113514064B - 一种鲁棒因子图多源容错导航方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

对于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分别表示地球卯酉圈曲率半径与子午圈曲率半径。

优选地，所述导航传感器残差获取步骤中，所述第二载体导航信息包括：第二载体姿态信息、第二载体速度信息、第二载***置信息；在采样时刻为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时刻所述导航传感器的速度残差项、位置残差项与姿态残差项。

优选地，所述速度残差项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)^TQ_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表示所述导航传感器姿态量测协方差矩阵。

优选地，所述导航传感器故障检测步骤中，对所述导航传感器进行故障检测的方法包括：计算各所述导航传感器的残差均值基于各所述导航传感器的残差与所述残差均值/>的关系，进行故障诊断，如下式所示：

其中，r_k+1为所述导航传感器的残差，η为故障检测阈值系数，S为故障检测结果，若检测结果为0，则表明当前导航传感器导航信息存在故障；若检测结果为1，则表明当前导航传感器导航信息不存在故障。

优选地，所述导航优化步骤具体包括：

若所述导航传感器存在故障，导航优化目标函数按如下式所示：

若所述导航传感器不存在故障，导航优化目标函数按如下式所示：

其中，||r_p-H_pX||²为边缘化的先验约束，为惯性残差，B是所有IMU测量的集合，/>为惯性量测协方差矩阵，/>为所述导航传感器测量残差，C为导航传感器的集合，/>表示导航传感器量测协方差矩阵。

优选地，使用高斯牛顿非线性优化方法对所述目标函数进行求解，并重复执行IMU导航信息获取步骤，直至达到预设条件，得到载体的导航信息。

本发明公开了以下技术效果：

本发明提供了一种鲁棒因子图多源容错导航方法，通过IMU采集加速度计数据、陀螺仪数据，计算得到第一载体导航信息，通过各导航传感器采集第二载体导航信息，通过第一载体导航信息、第二载体导航信息计算各导航传感器的残差，通过残差数据对各导航传感器进行故障检测，并将故障导航传感器进行隔离以及***优化，从而能够有效解决由于导航传感器故障导致基于因子图算法的组合导航***定位精度下降的问题，可靠性高、适用性强。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明鲁棒因子图多源容错导航方法流程图；

图2为本发明实施例中故障注入仿真示意图；

图3为本发明实施例中故障检测示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

参照图1所示，本实施例提供一种鲁棒因子图多源容错导航方法，包括如下步骤：

步骤S1、***初始化：对各导航传感器的导航状态量、导航传感器残差进行初始化；对***导航状态量、导航传感器残差进行初始化；具体包括：

1)初始化载***置信息、载体速度信息、载体姿态信息，其中，所述载***置信息包括：所述载体所在位置的经度λ₀、纬度L₀、高度h₀；所述载体速度信息初始化为0；所述载体姿态信息/>为以四元数形式表示的机体系与地理系的夹角，其中，e为地球坐标系，k表示所述导航传感器的当前采样时刻，n_k表示所述导航传感器当前采样时刻的导航系，b_k表示所述导航传感器当前采样时刻的机体系。

2)初始化导航传感器残差r_k为0，利用所述导航传感器噪声初始化量测协方差Q_k。

步骤S2、惯性测量单元(IMU，Inertial Measurement Unit)导航信息获取：通过IMU采集加速度计数据陀螺仪数据/>并基于所述加速度计数据/>陀螺仪数据/>计算第一载体导航信息；具体包括：

1)分别构建所述IMU中的加速度计、陀螺仪的输出模型，如下式所示：

其中，为机体系相对于导航系在机体系下的分量，即实际导航解算所需要的载体运动角速率，/>为陀螺仪的输出值，b_ω为陀螺仪零偏，n_ω为陀螺仪白噪声；/>为载体实际运动的加速度在导航系下的分量，/>为机体系到导航系的坐标转换矩阵，/>为导航系到机体系的坐标转换矩阵；f^b为加速度计的输出值，g为地球重力加速度，n_a为加速度计白噪声，b_a为加速度计零偏，n表示所述导航传感器采样个数，/>表示地球自转角速率；/>表示受载体运动影响导航系相对于地球坐标系的牵连角速率；/>表示载体运动速率；i表示惯性坐标系；b表示机体坐标系，也称机体系。

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

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

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

其中，表示载体在t_k+1时刻的速度信息，/>表示载体在t_k时刻的速度信息，表示t_k时刻地球自转角速率，/>表示t_k时刻导航系相对于地球坐标系的转动角速度在导航系下的分量，ΔT_IMU表示IMU的采样时间间隔。/>表示载体在t_k+1时刻机体系到导航系的坐标转换矩阵，由t_k+1时刻以四元数形式表示载体姿态信息按如下形式进行计算：

其中，表示t_k+1载体姿态四元数的实部，/>分别表示载体姿态四元数的三个虚部。

4)对于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分别表示地球卯酉圈曲率半径与子午圈曲率半径。

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

所述第二载体导航信息包括：第二载体姿态信息、第二载体速度信息、第二载***置信息。记所述导航传感器采样时刻为t_k+1时刻，当采集到所述导航传感器导航信息Z_k时，按如下方式计算所述导航传感器的残差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时刻所述导航传感器的速度残差项、位置残差项与姿态残差项，具体地有：

速度残差项r_vk+1的计算方法包括：当所述导航传感器所采集到的第二载体速度信息为Z_vk+1时，通过步骤S2中IMU解算的第一载体速度信息计算t_k+1时刻速度残差项r_vk+1为：

其中，表示IMU根据步骤S2递推解算至t_k+1时刻的第一载体速度信息，Q_V表示所述导航传感器速度量测协方差矩阵，具体地有：

其中，n_v表示所述导航传感器速度量测噪声。

位置残差项r_pk+1的计算方法包括：当所述导航传感器所采集到的第二载***置信息为Z_pk+1时，通过步骤S2中IMU解算的所述第一载***置信息计算t_k+1时刻位置残差项r_pk+1为：

r_pk+1＝(Z_pk+1-p_k+1)^TQ_P ^-1(Z_pk+1-p_k+1)

其中，p_k+1表示IMU根据步骤S2递推解算至t_k+1时刻的第一载***置信息，Q_P表示所述导航传感器位置量测协方差矩阵，具体地有：

其中，n_v表示所述导航传感器速度量测噪声。

姿态残差项r_qk+1的计算方法包括：当所述导航传感器所采集到的以四元数表示的第二载体姿态信息Z_qk+1时，通过步骤S2中IMU解算的第一载体姿态信息计算t_k+1时刻姿态残差项r_qk+1为：

其中，表示IMU根据步骤S2递推解算至t_k+1时刻的第一载体姿态信息，表示取姿态误差四元数的虚部，Q_q表示所述导航传感器姿态量测协方差矩阵，具体地有：

其中，n_q表示所述导航传感器姿态量测噪声。

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

对所述导航传感器进行故障检测的方法包括：

1)计算各所述导航传感器的残差均值如下式所示：

其中，Z_j表示所述导航传感器提供的导航信息，h(X)_j表示由IMU递推解算的第一载体导航信息，ΔT_j表示所述导航传感器采样时间间隔，n表示所述导航传感器的采样个数。

2)故障诊断函数按如下方式进行计算：

其中，η为故障检测阈值系数，S为故障检测结果，若检测结果为0，则表明当前导航传感器导航信息存在故障；若检测结果为1，则表明当前导航传感器导航信息不存在故障，为所述导航传感器的残差均值。

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

本实施例构建了包含故障检测结果的***整体待优化目标函数，如下式所示：

其中，||r_p-H_pX||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其它测量和状态则被边缘化并转换为先验；为惯性残差，B是所有IMU测量的集合，/>为惯性量测协方差矩阵；/>为所述导航传感器测量残差，C为导航传感器的集合，表示导航传感器量测协方差矩阵。

若检测出导航传感器故障，S为0，即待优化目标函数该部分为0，不需要被优化，***整体待优化目标函数按如下形式进行计算：

若传感器不存在故障时，***整体待优化目标函数按如下形式进行计算：

使用高斯牛顿非线性优化方法对上述目标函数进行求解，并重复步骤S2-S5，当达到误差收敛状态时或迭代次数达到阈值时则停止优化，输出载体的导航信息，完成载体的导航。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (7)

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

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

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

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

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

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

所述导航传感器残差获取步骤中，所述第二载体导航信息包括：第二载体姿态信息、第二载体速度信息、第二载***置信息；在采样时刻为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时刻所述导航传感器的速度残差项、位置残差项与姿态残差项；

所述速度残差项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)^TQ_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表示所述导航传感器姿态量测协方差矩阵；

所述导航优化步骤具体包括：

若所述导航传感器存在故障，导航优化目标函数按如下式所示：

若所述导航传感器不存在故障，导航优化目标函数按如下式所示：

其中，||r_p-H_pX||²为边缘化的先验约束，为惯性残差，B是所有IMU测量的集合，/>为惯性量测协方差矩阵，/>为所述导航传感器测量残差，C为导航传感器的集合，/>表示导航传感器量测协方差矩阵。

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所述的鲁棒因子图多源容错导航方法，其特征在于，所述导航传感器故障检测步骤中，对所述导航传感器进行故障检测的方法包括：计算各所述导航传感器的残差均值r，基于各所述导航传感器的残差与所述残差均值r的关系，进行故障诊断，如下式所示：

其中，r_k+1为所述导航传感器的残差，η为故障检测阈值系数，S为故障检测结果，若检测结果为0，则表明当前导航传感器导航信息存在故障；若检测结果为1，则表明当前导航传感器导航信息不存在故障。

7.根据权利要求1所述的鲁棒因子图多源容错导航方法，其特征在于，使用高斯牛顿非线性优化方法对所述目标函数进行求解，并重复执行IMU导航信息获取步骤，直至达到预设条件，得到载体的导航信息。