CN109883426B - 基于因子图的动态分配与校正多源信息融合方法 - Google Patents

Abstract

本发明属于多传感器融合的导航技术领域和时间配准技术领域，具体涉及基于因子图的动态分配与校正多源信息融合方法。针对现代导航系统应用环境日益复杂，传感器的工作时间不连续，量测值时有时无，甚至出现输出迟滞，传统的联邦滤波算法计算复杂，处理量测迟滞问题时精度降低。本发明使用因子图概率模型对导航系统重新建模，保证了系统出现量测类型动态变化时的导航精度同时减少计算量，提高了系统的即插即用特性，针对量测迟滞问题，使用多种传感器信息对延迟量测进行补偿提高量测信息使用率与导航精度。仿真结果表明，所提出的算法在量测值时断时续与输出迟滞的情况下具有更好的鲁棒性与容错性，同时精度也得到了提高。

Description

基于因子图的动态分配与校正多源信息融合方法

技术领域

本发明属于多传感器融合的导航技术领域和时间配准技术领域，具体涉及基于因子图的动态分配与校正多源信息融合方法。

背景技术

传感器技术的大力发展是当代导航手段日趋丰富的主要原因，各种高分辨率、高可靠性传感器的出现使得导航系统发展尤为迅速。其中卫星导航以高精度与高覆盖率被广泛使用，在军事与民事领域，卫星导航系统提供的定位、导航与授时，Position Navigationand Timing，PNT，服务是至关重要的，它为用户提供了一个时空参考基准。但在实际应用背景下，GPS干扰条件存在诸多因素，会导致信号延迟接收或失锁等情况。除GPS外，其他传感器由于环境因素等也可能出现短时间或长时间失效的问题。为此导航系统需要具有对干扰环境下传感器输出动态处理的能力即即插即用能力。

针对量测迟滞问题，国内外学者都做了大量研究工作，文献《一种新的SAR时延补偿算法及其在组合导航中的应用》使用量测值本身进行插值预测，但当数据本身连续迟滞的情况下无法对自身估计校正；文献《考虑量测滞后的INS/SAR组合导航非等间隔滤波算法研究》提出了使用INS数据对迟滞量测量进行补偿的方法，但实际情况中由于惯导精度不足会导致补偿效果不佳；针对传感器即插即用问题，文献《An Improved Multi-SensorFusion Navigation Algorithm Based on the Factor Graph》使用全局概率密度函数作为计算目标，使用过去所有时刻传感器的输出同时对当前时刻导航状态进行估计，其计算复杂，在低成本的导航系统中难以保证实时性；文献《基于因子图的无人机全源导航关键技术研究》使用较为简单的最小二乘方法对当前时刻的可用量测值进行融合，但该方法要求量测类型冗余度较高，增加了导航成本；而使用联邦滤波的方法处理即插即用问题时需要事先预设好可用量测量对应的子滤波器，且系统运行后各子滤波器无论是否存在量测都要进行运算，计算量较大。

发明内容

本发明的目的在于克服多传感器组合导航系统中量测输出迟滞造成的导航精度下降与传感器可用类型变化时造成的导航运算负担增加的问题。

本发明的技术方案如下：

基于因子图的动态分配与校正多源信息融合方法，该方法包括以下步骤：

步骤1：建立导航系统的数学模型；

步骤2：使用多传感器信息对迟滞量测量进行补偿；

步骤3：使用即插即用因子图模型建立系统状态转移与量测的概率模型；

步骤4：对所有可用量测值进行数据融合得到最优估计。

步骤1所述的建立导航系统的数学模型包括系统方程和量测方程，系统方程根据间接法选取系统15维状态量：

其中，

为姿态角误差；δV＝[δV_e δV_n δV_u]^T为速度误差；δp＝[δL δλδh]^T为位置误差；b_g＝[b_gx b_gy b_gz]^T为陀螺常值漂移；b_a＝[b_ax b_ay b_az]^T为加速度计常值偏置；

系统误差模型使用小失准角条件下的线性误差模型：

设系统的状态方程为：

式中，F(t)为状态转移矩阵。w(t)为系统状态转移的噪声。根据Gauss假设，将其定义为白噪声；则系统的噪声协方差矩阵Q为：

式中，S_rg，S_ra为陀螺与加速度计的噪声功率谱密度构成的对角矩阵；

量测方程包括：GPS量测方程、EC量测方程以及OD量测方程，其中GPS量测方程表示为下式：

式中，v为速度、位置误差噪声；H_GPS为量测矩阵：

EC量测方程表示为下式：

式中，Z_EC为姿态误差，包括航向、俯仰、横滚误差；v₂(t)为高斯白噪声。H_EC为量测矩阵：

H_EC(t)＝[I_3×3 0_3×3 0_3×3 0_3×3 0_3×3]

M为误差角转换矩阵，表示由姿态误差角转换为平台失准角：

OD量测方程表示为下式：

式中，v为速度、位置误差噪声；H_OD为量测矩阵：

步骤2所述的使用多传感器信息对迟滞量测量进行补偿的过程包括：

分别计算出相邻时间t_i～t_j内载体的近似线加速度

与

其中，ΔT为迟滞时间，

与

分别为惯导与里程计输出的速度；

根据各自线加速度分别计算出速度补偿量δV_ΔT与位置补偿量δP_ΔT：

其中，

L为当地纬度，h为载体所在高度，R_M为当地子午圈曲率半径，R_N为当地卯酉圈曲率半径；

最终的补偿量为两者加权融合得到：

其中α+β＝1。

步骤3所述的使用即插即用因子图模型建立系统状态转移与量测的概率模型包括，系统的状态空间描述为：

组合导航系统的估计问题被转化为一阶马尔可夫链，由马尔可夫链的性质得：

式中，f(X_i|X_i-1)服从高斯分布，其均值与方差分别为A_i-1X_i-1，

f(Z_i|X_i)服从高斯分布，其均值与方差分别为C_iX_i与

由系统的概率模型得：

p(X_ik|Z_k)＝p(X_ik|X_i-1)p(Z_k|X_ik)

式中p(X_ik|X_i-1)＝1/n(p(X_i|X_i-1))，认为每个量测与状态量融合的结果的精度是等可能的，于是其对应协方差为：

σ²(X_ik|X_i-1)＝n(σ²(X_i|X_i-1))

由高斯分布得：

式中，均值

方差

步骤4所述的对所有可用量测值进行数据融合得到最优估计包括：根据上式可得各状态量与量测融合的结果

再根据每一个子系统所包含的变量的均值与方差进行融合：

本发明的有益效果在于：

针对以SINS/GPS/OD/EC组合为导航源的现代导航系统应用环境日益复杂，传感器的工作时间不连续，量测值时有时无，甚至出现输出迟滞，传统的联邦滤波算法计算复杂，处理量测迟滞问题时精度降低。为了解决上述问题，本发明提出一种基于因子图的多源信息融合导航算法。该方法使用因子图概率模型对导航系统重新建模，保证了系统出现量测类型动态变化时的导航精度同时减少计算量，提高了系统的即插即用特性，针对量测迟滞问题，使用多种传感器信息对延迟量测进行补偿提高量测信息使用率与导航精度。仿真结果表明，所提出的算法在量测值时断时续与输出迟滞的情况下具有更好的鲁棒性与容错性，同时精度也得到了提高。

附图说明

图1是即插即用因子图模型。

图2是跑车运动轨迹图。

图3是GPS量测迟滞条件下的航向角误差对比。

图4是GPS量测迟滞条件下的俯仰角误差对比。

图5是GPS量测迟滞条件下的横滚角误差对比。

图6是GPS量测迟滞条件下的东向速度误差对比。

图7是GPS量测迟滞条件下的北向速度误差对比。

图8是GPS量测迟滞条件下的天向速度误差对比。

图9是GPS量测迟滞条件下的纬度误差对比。

图10是GPS量测迟滞条件下的经度误差对比。

图11是GPS量测迟滞条件下的高度误差对比。

图12是可用传感器类型动态变化条件下的航向角误差对比。

图13是可用传感器类型动态变化条件下的俯仰角误差对比。

图14是可用传感器类型动态变化条件下的横滚角误差对比。

图15是可用传感器类型动态变化条件下的东向速度误差对比。

图16是可用传感器类型动态变化条件下的北向速度误差对比。

图17是可用传感器类型动态变化条件下的天向速度误差对比。

图18是可用传感器类型动态变化条件下的纬度误差对比。

图19是可用传感器类型动态变化条件下的经度误差对比。

图20是是可用传感器类型动态变化条件下的高度误差对比。

具体实施方式

下面结合附图对本发明做进一步描述。本发明的效果可以通过以下仿真说明：

本发明特别涉及导航系统可用传感器输出动态变化与量测迟滞，传统滤波算法计算复杂与精度不足的问题，具体说是一种可以在线实时校正量测输出与分配可用量测进行融合从而增强运算实时性并得到精确更高的导航参数的方法。

步骤1：建立导航系统的数学模型

1.系统方程

根据间接法选取系统15维状态量：

其中，

为姿态角误差；δV＝[δV_e δV_n δV_u]^T为速度误差；δp＝[δL δλδh]^T为位置误差；b_g＝[b_gx b_gy b_gz]^T为陀螺常值漂移；b_a＝[b_ax b_ay b_az]^T为加速度计常值偏置。

系统误差模型使用小失准角条件下的线性误差模型：

设系统的状态方程为：

式中，F(t)为状态转移矩阵。w(t)为系统状态转移的噪声。根据Gauss假设，将其定义为白噪声。则系统的噪声协方差矩阵Q为：

式中，S_rg，S_ra为陀螺与加速度计的噪声功率谱密度(PSD)构成的对角矩阵。

2.量测方程

1)GPS量测方程：

式中，v为速度、位置误差噪声；H_GPS为量测矩阵：

2)EC量测方程：

式中，Z_EC为姿态误差，包括航向、俯仰、横滚误差；v₂(t)为高斯白噪声。H_EC为量测矩阵：

H_EC(t)＝[I_3×3 0_3×3 0_3×3 0_3×3 0_3×3]

M为误差角转换矩阵，表示由姿态误差角转换为平台失准角：

3)OD量测方程：

式中，v为速度、位置误差噪声；H_OD为量测矩阵：

3.连续系统离散化

首先进行模型离散化。设系统的离散方程为：

由于惯导的采样周期很小，所以在—个采样周期内，F(t)可认为是常数矩阵，即F(t)＝F(t_k)＝F_k。同理可得G_k＝G(t_k)。系统状态转移矩阵如下：

系统噪声驱动阵如下：

观测矩阵如下：

H_k＝H(t_k)

系统噪声方差阵计算如下：

式中，M_i+1＝F_kM_i+(F_kM_i)^T，

步骤2：使用多传感器信息对迟滞量测量进行补偿；

设INS、GPS、OD、EC的采样时间分别为T_INS、T_GPS、T_OD、T_EC。设GPS滞后的时间为ΔT，以INS进行插值时由于GPS滞后时间内INS更新次数为k＝ΔT/T_INS，在滞后时间内系统状态转移的协方差矩阵为Q_ΔT＝kQ。INS的补偿量与OD的补偿量分配系数分别为α_INS(α_v1,α_v2,α_v3,α_p1,α_p2,α_p3)、β_OD(β_v1,β_v2β_v3,β_p1,β_p2,β_p3)，且α+β＝1。其中的α＝0.1，β＝0.9，两参考值也可根据传感器精度进行适当调整。

分别计算出相邻时间t_i～t_j内载体的近似线加速度

与

根据各自线加速度分别计算出速度与位置补偿量：

最终的补偿量为两者加权融合得到：

步骤3：使用即插即用因子图模型建立系统状态转移与量测的概率模型；

根据系统的运行情况可以分为以下几种情况：第一种是某段时间内可用传感器类型不变，即系统正常运行；第二种是由于环境等因素导致某正在工作的传感器出现故障，无法输出量测值，即“移除”现象；第三种是已经失灵的传感器又恢复正常工作或作为机会信号以不定时间输出，即“插入”现象。

组合导航系统的估计问题被转化为一阶马尔可夫链。则系统的状态空间描述如下：

在已知所有量测量Z₁,…,Z_n的条件下估计状态X₁,…,X_n的概率密度函数(probability density function，pdf)可以写作f(X₁,…,X_n|Z₁,…,Z_n)。现对系统的状态转移与量测做Gauss分布假设，即状态转移噪声与量测噪声为高斯白噪声。由系统的隐马尔可夫过程的概率密度函数的定义可知：

式中，f(X_i|X_i-1)服从高斯分布，其均值与方差分别为A_i-1X_i-1，

f(Z_i|X_i)服从高斯分布，其均值与方差分别为C_iX_i与

由于系统为马尔可夫过程，则可对状态量做如下假设：系统的第n时刻的状态量X_n只与系统上一时刻即第n-1时刻的状态量X_n-1有关，且与系统n-1时刻之前的状态量无关。观察函数节点f(X_i|X_i-1)，该节点将i-1时刻的信息传递至第i时刻，假设X_i收到的信息可以表示为

则由高斯分布的形式可得：

由概率论知识可知：

式中，均值

方差

同理可得：

式中，

假设系统第i时刻存在观测量Z₁,…,Z_n-1，则由高斯分布的假设可知，由于所有量测模型可以表示为：

(k＝1,…,n-1)。由系统的概率模型可知：

p(X_ik|Z_k)＝p(X_ik|X_i-1)p(Z_k|X_ik)

式中p(X_ik|X_i-1)＝1/n(p(X_i|X_i-1))，这里为简单考虑，可以认为每个量测与状态量融合的结果的精度是等可能的。于是其对应协方差为：

σ²(X_ik|X_i-1)＝n(σ²(X_i|X_i-1))

由和积算法可知：

式中，

该步骤相比联邦滤波器省去了所有子滤波器在所有时刻都要进行一步预测的过程，每一个时间节点进行一步预测的个数等于当前可用传感器的个数，简化了计算量。

步骤4：对所有可用量测值进行数据融合；

根据上式可得各状态量与量测融合的结果

再根据每一个子系统所包含的变量的均值与方差进行融合：

实验条件与内容：

表1仿真中的参数设置

表2各时间段内的传感器的输出情况

表3两种联邦卡尔曼与本发明算法的均方根误差对比

表4联邦卡尔曼与本发明算法的运行时间对比

实验采用真实跑车数据，其中包含INS、GPS、OD与EC四种传感器，整个载体运行轨迹是围绕地图上的三个直线排列建筑物的绕圈运动。载体初始位置为东经126.6815°，北纬45.7773°，开始上电后先静止约3min，然后沿环形路线反复行驶，最后停在初始位置，整个行驶过程持续约22min。

为了验证对量测滞后的补偿结果，设置GPS的输出在487s～732s出现输出滞后现象，滞后的时间为0.5s。

从图3-11可以看出，在GPS出现量测之后的时间段内，改进的量测滞后补偿方法误差最小。在GPS量测迟滞期间内，由于速度与位置量测误差没有被补偿而被直接利用，纬度最大误差达到4m，经度最大误差达到3.72m，东向速度误差最大达到0.84m/s，北向速度最大误差达到0.92m/s，由此可以看出对于出现量测滞后的观测量必须予以补偿或舍弃。未补偿方法则是不使用出现量测滞后的观测量，其导航精度较高但仍然低于改进方法。而单独利用INS对出现量测之后的GPS进行补偿的方法由于使用的MIMU漂移较大，且量测滞后时间稍长，导致其补偿结果并不理想，其中纬度最大误差达到2.45m，经度最大误差达到2.02m，东向速度最大误差达到0.49m/s，北向速度最大误差达到0.54m/s。

为进一步验证本方法与联邦卡尔曼的计算精度与计算时间，将传感器的故障时间按表2设置。

从图11-20与表3可以看出基于因子图的多源信息融合方法效果与最优估计的反馈重置联邦相近，其估计精度得到验证。

从表4可以看出由于因子图算法省去了子滤波器结构，其计算量相比联邦卡尔曼较小，运行时间相对缩短了23.85％，更适合工程应用。

Claims (1)

1.基于因子图的动态分配与校正多源信息融合方法，其特征在于，该方法包括以下步骤：

步骤1：建立导航系统的数学模型；

所述的建立导航系统的数学模型包括系统方程和量测方程，系统方程根据间接法选取系统15维状态量：

其中，

为姿态角误差；δV＝[δV_e δV_n δV_u]^T为速度误差；δp＝[δL δλ δh]^T为位置误差；b_g＝[b_gx b_gy b_gz]^T为陀螺常值漂移；b_a＝[b_ax b_ay b_az]^T为加速度计常值偏置；

系统误差模型使用小失准角条件下的线性误差模型：

设系统的状态方程为：

式中，F(t)为状态转移矩阵；w(t)为系统状态转移的噪声；根据Gauss假设，将其定义为白噪声；则系统的噪声协方差矩阵Q为：

式中，S_rg、S_ra为陀螺与加速度计的噪声功率谱密度构成的对角矩阵；

量测方程包括：GPS量测方程、EC量测方程以及OD量测方程，其中GPS量测方程表示为下式：

式中，v₁(t)为GPS速度、位置误差噪声；H_GPS为GPS量测矩阵：

EC量测方程表示为下式：

式中，Z_EC为姿态误差，包括航向、俯仰、横滚误差；v₂(t)为高斯白噪声；H_EC为EC量测矩阵，H_EC(t)＝[I_3×3 0_3×3 0_3×3 0_3×3 0_3×3]；M为误差角转换矩阵，表示由姿态误差角转换为平台失准角：

OD量测方程表示为下式：

式中，v₃(t)为OD速度、位置误差噪声；H_OD为OD量测矩阵：

步骤2：使用多传感器信息对迟滞量测量进行补偿；

设INS、GPS、OD、EC的采样时间分别为T_INS、T_GPS、T_OD、T_EC；设GPS滞后的时间为ΔT，以INS进行插值时由于GPS滞后时间内INS更新次数为k＝ΔT/T_INS，在滞后时间内系统状态转移的协方差矩阵为Q_ΔT＝kQ；INS的补偿量与OD的补偿量分配系数分别为α_INS(α_v1,α_v2,α_v3,α_p1,α_p2,α_p3)、β_OD(β_v1,β_v2β_v3,β_p1,β_p2,β_p3)，且α+β＝1；

分别计算出相邻时间t_i～t_j内载体的近似线加速度

与

根据各自线加速度分别计算出速度与位置补偿量：

最终的补偿量为两者加权融合得到：

步骤3：使用即插即用因子图模型建立系统状态转移与量测的概率模型；

系统的状态空间描述为：

组合导航系统的估计问题被转化为一阶马尔可夫链，由马尔可夫链的性质得：

式中，f(X_i|X_i-1)服从高斯分布，f(X_i|X_i-1)的均值与方差分别为A_i-1X_i-1，

f(Z_i|X_i)服从高斯分布，f(Z_i|X_i)的均值与方差分别为C_iX_i与

由系统的概率模型得：

p(X_ik|Z_k)＝p(X_ik|X_i-1)p(Z_k|X_ik)

式中p(X_ik|X_i-1)＝1/n(p(X_i|X_i-1))，认为每个量测与状态量融合的结果的精度是等概率的，于是其对应协方差为：

σ²(X_ik|X_i-1)＝n(σ²(X_i|X_i-1))

由高斯分布得：

式中，均值

方差

步骤4：对所有可用量测值进行数据融合得到最优估计；

根据上式可得各状态量与量测融合的结果

再根据每一个子系统所包含的变量的均值与方差进行融合：

Images