一种多传感器组合导航系统的改进异步融合算法

刘丽丽，林雪原,*，陈祥光,2

1. 烟台南山学院 电气与电子工程系，烟台 265713 2. 北京理工大学，北京 100081

1 引言

目前，多传感器组合导航系统的研究较多，并且均集中于同步信息的融合算法的研究。然而在实际应用中各子导航传感器由于检测装置及采样速率的不同导致多传感器组合导航系统是一种典型的异步信息融合系统。

秦永元等[1-2]提出了典型的异步融合方法，其核心思想是利用时间更新把非同步信号变换成同步信号。周明进等[3]结合非等间隔采样，研究了基于联邦滤波的异步融合，并采用时间更新与量测更新分离的方法解决了非等间隔问题。Jian等[4]研究了脉冲星/星体跟踪定位器/INS组合导航系统的异步融合方法，其核心思想是根据滤波器的速率把多传感器组合导航系统分为姿态滤波器与速度位置滤波器两部分。总体而言，上述算法本质上是次优的。

为了得到性能最优的多传感器组合导航系统的最优异步滤波方法，林雪原[5]、赫飞等[6]提出了在子导航传感器数据采样速率呈有理数倍情况下的最优异步融合方法，其核心思想是基于简单尺度的思想，把当前系统状态向量向前递推，并得到各尺度上的状态方程及对应的量测方程。基于尺度的思想，林雪原等[7-8]研究了基于状态数据块与小波变换相结合的、基于尺度的多传感器组合导航系统的同步信息融合方法。刘磊等[9]研究了采样速率为2的整数倍这一特殊情况下的基于多尺度思想的多传感器组合导航系统融合算法；朱倚娴等[10]研究了子导航传感器存在输出延迟情况下的基于多尺度的多传感器异步信息融合方法。

考虑到大多数子导航传感器具有输出数据速率稳定、且为有理数倍采样率[11]这一现实，本文先简要介绍了基于多尺度思想的多传感器组合导航模型，然后在此基础上给出了在有理数倍采样率下的多传感器组合导航系统的改进异步最优信息融合算法，最后将该算法应用于天文导航(CNS)/全球导航定位系统(GNSS)/捷联惯性导航系统(SINS)/高度表多传感器组合导航系统，进而验证了算法的有效性。

2 基于多尺度的组合导航系统模型

以SINS的误差模型建立系统的状态方程：

x(k+1)=x[(k+1)T0]=

A(kT0)x(kT0)+G(kT0)w(kT0)=

A(k)x(k)+G(k)w(k)

(1)

式中：T0为系统滤波周期，其他各参数的定义及其约束条件见文献[7]；x为状态向量；A为系统状态转移矩阵；G为系统噪声矩阵；w为系统噪声，且是高斯白噪声序列；k为离散时间指标。

将系统(1)表示为块长为M(通常取M=2i，i为大于0的正整数；如果i=0则对应于常规卡尔曼滤波形式)的状态块的形式：

X(m+1):=

[xT(m+1,1),xT(m+1,2),…,xT(m+1,M)]T:=

[xT(mM+1),xT(mM+2),…,xT(mM+M)]T

(2)

式中：m=0,1,2,…；M=2i。X(m+1)中的第s个元素为：

式中：

s=1,2,…,M

w(mM+j-1)+G(mM+s-1)·

w(mM+s-1)

定义过程噪声块向量为：

并可表示为：

式中：B(m)的第r行第c列元素用Br,c(m)表示为

[wT(mM),wT(mM+1),…,wT(mM+M-1)]T

根据以上定义，系统模型(1)可表示为：

(3)

多尺度分析中的尺度算子与小波算子可对信号在平滑值信号空间和细节信号空间进行投影，建立一个涵盖尺度算子与小波算子的矩阵算子Wx以对数据块进行同样的变换[7-8,12]，且Wx是一个正交矩阵，即

(4)

则式(3)可表达为：

假设有N0个子导航传感器对系统(1)独立地进行测量，则相应的测量方程可表示为：

zi(nTi)=Ci(nTi)x(kTi)+vi(nTi)，

i=1,2,…,N0

(5)

式中：Ti为传感器i的采样周期；zi为第i个子导航传感器的量测值；Ci为第i个子导航传感器的量测矩阵；vi为第i个子导航传感器的量测噪声，且是高斯白噪声序列。并假定

Ti=niT0，i=1,2,…,N0

式中：ni为大于0的正整数。

根据前面的定义式(5)，对某一时刻k，若第i个子导航传感器输出有效信息，则结合式(1)的定义，k必为ni的整数倍。同时根据式(2)，k可表示为：

k=(m-1)M+s，1≤s≤M

如果第i个子导航传感器在时刻k有效，并提供量测值，则有：

zi(k)=Ci(k)x(k)+vi(k)=

Ci[(m-1)M+s]x[(m-1)M+s]+

vi[(m-1)M+s]

(6)

式中：i=1,2,…，N0；x[(m-1)M+s]为第m个状态块中的第s个元素。

结合式(2)的定义，式(6)可进一步表示为状态块的形式：

(7)

利用式(4)，式(7)可表示为

3 改进的异步多传感器组合导航系统

如前所描述，在异步融合中，各子导航传感器的数据采集率不同，为此算法的首要任务是判断在每个滤波时刻点上都有哪些子导航传感器提供了量测信息。

经常情况下系统的滤波周期常取T0=1，针对异步多传感器组合导航系统，结合前面的分析，用mod(k-1,ni)是否为零来断定传感器i在k时刻是否有测量值(mod是取(k-1)/ni余数的函数)[9,12]：

mod(k-1,ni)=

当上述结果为零时，说明第i个子导航传感器在时刻k有效并提供量测信息，否则该子导航传感器在时刻k无效且不能提供量测信息。多传感器组合导航系统的异步信息融合算法分为两个过程：时间序贯和传感器测量序贯。下面以第m块为例，给出系统的更新过程。

假设m≠1，利用第(m-1)块的状态估计值对第m块的状态估计值进行预测，有：

(8)

(9)

完成式(8)(9)的预测后，对于第m块内的时刻s=1,2,…,M，依次利用Z(m,1)，Z(m,2)，…，Z(m,M)对系统进行信息处理，过程如下：

(1)当s=1时

若mod[(m-1)M,ni]=0，i=2,…,N0，则第i个传感器在该时刻有测量值zi(m,1)，则有更新方程为

增益矩阵为

(10)

若mod[(m-1)M,ni]≠0，此时传感器i并无测量值，有

在该时刻，如果最后一个(即第N0个)传感器也有测量值zN0(m,1)，则有更新方程为

该时刻的更新结束。

(2)当s≠1时

增益矩阵为

类似地，若mod((m-1)M+s-1,ni)=0，i=2,…,N0，则第i个传感器在该时刻有测量值zi(m,s)，则有更新方程为

若mod((m-1)M+s-1,ni)≠0，此时传感器i并无测量值，有：

(3)当s=M,i=N0时

应用上述方法可判断该数据块的最后时刻、最后一个传感器是否能够提供测量信息后，进而得到状态块的最优估计值如下：

根据矩阵算子WX的特性公式(4)，并应用于上式进而得到如下的最优估计：

4 仿真及分析

为验证本文算法，将上述异步信息融合算法用于CNS/GNSS/SINS/高度表多传感器组合导航系统。滤波器的状态向量取为[8，13-14]：

x(t)=[φE,φN,φU,δvE,δvN,δvU,δL,

δλ,δh,εrx,εry,εrz]T

(11)

式中：δL,δλ,δh为纬度、经度、高度误差；δvE,δvN,δvU为速度误差；φE,φN,φU为数学平台误差角；εrx,εry,εrz为陀螺一阶马尔可夫漂移误差[8]。仿真中各传感器误差参数设置如表1所示。

表1 导航传感器参数误差

假设GNSS、CNS、高度表的数据采样周期分别设定为1 s、2 s、3 s，而SINS的采样周期设定为0.02 s，系统的滤波周期取为1 s；式(2)中的M取2，这样选取的目的是不会导致系统的滤波时间过长进而影响系统的实时性。

文献[5]中提出了一种基于有理数倍采样率的简易尺度异步多传感器信息融合算法(在下面的仿真验证中定义为算法1)，并且仿真试验也验证了该算法相对于文献[1-2]所提异步融合算法具有较好的优越性。为了便于比较基于本文改进算法(在下面的仿真验证中定义为改进算法)与算法1的性能，对相同的高度表、GNSS、CNS、SINS仿真原始数据，本文同时进行了基于两种算法的仿真分析，其中图1～图6分别为基于两种算法的位置、速度与姿态误差曲线。

图1 基于改进算法的位置误差Fig.1 Position error based on the improved algorithm

图2 基于算法1的位置误差Fig.2 Position error based on algorithm 1

图3 基于改进算法的速度误差Fig.3 Velocity error based on the improved algorithm

图4 基于算法1的速度误差Fig.4 Velocity error based on algorithm 1

图5 基于改进算法的姿态误差Fig.5 Attitude error based on the improved algorithm

图6 基于算法1的姿态误差Fig.6 Attitude error based on algorithm 1

图7为飞行器在3 600 s时间段内的飞行轨迹，该轨迹包含了平飞、俯仰、俯冲、转弯、加速、平飞圆周运动等机动过程以方便对算法进行综合评估。航迹初始位置为(118°, 29°, 50 m)，整个运动过程中机体最大加速度为4m/s2、且机体最大速度达到510 m/s，转弯时航向角变化最大变化速率为5(°)/s，加速拉起时俯仰角变化最大变化速率为3(°)/s，转弯时横滚角变化最大变化速率为2(°)/s。

图7 仿真飞行轨迹Fig.7 Simulated flying trajectory

表达方便起见，假设系统经过相应的滤波处理后，仿真图中用Δλ,ΔL,Δh分别代表组合导航系统输出的经度误差、纬度误差、高度误差；ΔvE,ΔvN,ΔvU分别代表组合导航系统输出的东向速度误差、北向速度误差、天向速度误差；Δγ,Δφ,Δθ分别代表组合导航系统输出的横滚角误差、俯仰角误差、航向角误差；t表示仿真时间。

在Intel Core i5-7200U CPU 2.7 GHz处理器环境下，本文算法的滤波时间变化曲线如图8所示。

为了更直观地表述上述结果，表2给出了与图1～图6对应的导航结果数据统计值。

综合分析图1～图6和表2可得,本文算法与算法1相比，本文算法得到的各导航参数误差不仅具有较小的最大、最小值，同时误差的方差也大幅降低，例如位置精度采用本算法后相对于算法1可提高约20%，速度精度提高约15%，姿态精度提高约10%。

表2 试验结果数据统计对比

从图8可以看出，滤波时间最大值小于0.018 s，且平均滤波时间为0.001 7 s，而SINS的采样周期为0.02 s，因而在本文的算法及仿真实验中，当M=2时可保证算法的实时性。

假设常规卡尔曼滤波器的阶数为n，则其计算量与n3成正比[8]。假设与式(11)相对应的n=12。根据式(3)(6)的特点(即本文算法的特点)，当M=2时本文算法的计算量将与(n38n3)之间的某个数成正比；而当M=4时本文算法的量将与(8n364n3)之中的某个数成正比。而且当M=4及M=2时，两种情况下滤波的计算量之比近似为8；故当M=4时，对比图8，可知系统的平均滤波时间大幅增加，同时最大滤波时间将远大于SINS的采样周期，因而无法保证系统的实时性。

图8 本文算法的滤波时间Fig.8 Filtering time of the improved algorithm

文献[7, 9]对M的取值与滤波精度间的关系进行定性探讨，即适当增加M的大小，即M=4,8,…时，将会进一步提高导航参数的精度，但精度提高的幅度会随着M的增加而降低。为此，对M的取值需综合考虑滤波精度与系统实时性两方面因素。

5 结论

基于系统的原始状态方程与量测方程，经变换得到状态数据块向量与当前状态向量之间的关系并构成新的状态方程，将原始量测方程变换为与状态数据块向量之间的关系并构成新的量测方程。以此为基础，利用矩阵算子的尺度与小波特性，在子导航传感器采样率为有理数倍的条件下建立了多传感器组合导航系统的异步信息融合算法。

对本文算法进行仿真分析后，结论如下：

1)相对于传统异步融合算法，本文所提算法可提高各导航参数的精度。原因是，改进算法采用数据分块处理与小波处理相结合的技术，进而把相邻时刻状态向量间的噪声信息剔除、而保留更多的平滑信息[7]。

2)对M值的选择需综合考虑系统的滤波精度与系统实时性两方面因素，在保证系统实时性的前提下，适当增加M的取值可以提高系统的滤波精度。