改进的自适应滤波算法在BDS/INS组合导航中的应用

强明辉,蒋文

(1.兰州理工大学 电气工程与信息工程学院,甘肃 兰州 730050; 2.甘肃省工业过程先进控制重点实验室,甘肃 兰州 730050; 3.兰州理工大学 电气与控制工程国家级实验教学示范中心,甘肃 兰州 730050)

0 引 言

组合导航系统有利于充分利用各导航系统的优点进行信息互补与信息融合,成为导航系统发展的方向。组合导航系统是利用两种或两种以上的导航设备提供多重信息,构成一个多功能、高精度的冗余系统。惯性导航系统(INS)具有自主性、隐蔽性、强干扰性、高机动和完整的导航信息等优点,但是导航误差随时间积累而增长,这使其应用受到了一定的限制。全球定位系统(GPS)是一种全球、全天候、高精度的定位系统,误差不随时间积累,但易受干扰、数据更新率低[1],且GPS受美国国防部直接控制,使用受制于人。北斗导航系统是我国正在建设和运行且拥有绝对自主权的全球卫星导航系统的重要组成部分。因此,将北斗系统(BDS)与INS结合起来进行组合导航,已成为导航系统发展的必然趋势和研究热点,对我国国防建设具有极大的现实意义。

为了提高组合导航的精度,研究人员开始探索不同的算法进行线性化导航的数学模型。基于组合导航的估计算法已经被广泛研究。近几年,陈美华等利用UKF算法进行了MEMS/BD组合导航数据融合研究[2],提高了组合导航系统的精度,使系统具有更好的鲁棒性。为了将EKF应用于非平滑系统,Chatzis提出了不连续性扩展卡尔曼滤波器[3]。

虽然上述方法在一定程度上取得了较好的效果,但仍有一些问题有待解决,如1) BDS的噪声成分复杂,不仅包括白噪声,还包括周围建筑物引起的其他随机环境噪声和电磁干扰;2) INS模型和噪声不确定性;上述问题给信息融合估计带来了一些困难,目前还没有得到很好地解决。本文采用松散组合方式进行研究,以位置、速度作为观测量进行仿真分析。在卡尔曼滤波中引入系统的量测噪声协方差(R)的计算,并对其指数加权,有效地抑制滤波发散。引入衰减记忆因子可以达到较小的当前信号偏差值,增加新观测数据的影响是克服滤波偏差的有效方法,减少了过去收集的数据对滤波的影响。此算法不仅可以避免滤波散度,而且可以有效地减弱环境噪声对组合导航系统的影响,提高导航精度。

1 BDS/INS组合导航系统的误差状态方程

1.1 误差方程

由于位置、速度组合反馈校正模式的组合算法在较长时间内工作时,滤波精度高,组合效果良好,故而采用该组合方案[4]。组合导航系统采用卡尔曼滤波进行设计,首先需要对组合导航系统建模,确定需要估计的状态,建立状态方程与量测方程。本文采用北斗系统的位置和速度误差作为误差状态,其中位置误差δLBE，δLBN，δLBU,速度误差δVBE，δVBN，δVBU.其状态方程为

(1)

式中:XB为n维状态向量;FB为北斗系统的(n×n)阶动态矩阵;GB=I为单位矩阵;WB为n维系统噪声矩阵。采用位置,速度组合方式,选择位置与速度作为观测量,则BDS/INS组合导航系统误差状态方程为

(2)

式中:X(t)为系统状态向量;W(t)为系统噪声向量;F(t)为系统矩阵;G(t)为系统噪声矩。其中[5]:

X(t)= [δφEδφNδφUδVEδVNδVUδLδλδh

εbxεbyεbzεrxεryεrzxyz]T,

(3)

W(t)= [ωgxωgyωgz,ωbxωbyωbz,ωaxωayωaz]T,

(4)

(5)

(6)

式中: E、N、U分别表示东、北、天方向;δφEδφNδφU为姿态角误差;δVE、δVN、δVU为速度误差;δL、δλ、δh为纬度、经度、高度误差;εbx、εby、εbz为陀螺仪的随机常数漂移;εrx、εry、εrz为陀螺仪一阶马尔科夫漂移;x、y、z为加速度一阶马尔科夫漂移;为姿态矩阵;矩阵的选取参见文献[6]。

1.2 BDS/INS组合导航系统量测方程

BDS与INS以位置、速度的组合方式进行组合导航必然出现两组量测值:位置量测差值,即北斗接收机给出的位置信息与INS系统输出的经度、纬度与高度信息的差值;另外是速度量测的差值,即这两个系统给出的相应速度信息的差值[7]。

在东北天坐标系下INS的输出位置信息用其真值与相应误差之和表示为:

(7)

在东北天坐标系下北斗接收机的输出位置信息用其真值与相应误差之差表示为

(8)

式中：Lt,λt,ht为真实位置;NE,NN,NU分别为北斗接收机沿地理坐标系各个方向的位置误差;RM为量测点沿北方向的曲率半径;RN为量测点沿东西方向的曲率半径;L为量测点的纬度。则位置量测误差矢量为:

=HP(t)X(t)+VP(t) ，

(9)

其中:Hp=[03×6diag[RMRNcosL1] 03×9];VP=[NNNENU]T为滤波器噪声; 其中RM=Re(1-2f+3fsin 2L),RN=Re(1+2fsin2L),f=1/289.257,Re=6378137m.

在东北天坐标系下INS的速度量测信息用其真值与相应速度误差之和表示为

(10)

在东北天坐标系下北斗导航系统速度量测信息用其真值与相应速度误差之差表示为

(11)

其中:VN,VE,VU是飞机沿东北天地理坐标系各个坐标轴的真实速度;MN,ME,MU为北斗接收机在东北天坐标系中的各个坐标轴上的测速误差分量。则速度测量矢量定义为:

=HV(t)X(t)+VV(t),

(12)

式中,HV=[03×6diag[1 1 1] 03×9].

由以上推导可知,将位置量测方程与速度量测方程合并,即可得到BDS/INS组合导航系统的量测方程,如下式所示:

=H(t)X(t)+V(t) .

(13)

2 组合导航算法

2.1 衰减自适应记忆滤波

考虑如下离散非线性系统:

Xk=Φk|(k-1)Xk-1+Γk|((k-1)ωk-1,

(14)

Zk=HkXk+νk,

(15)

式中:Xk∈Rm和uk-1∈Rc分别为系统的状态向量和控制向量;Zk∈Rn为量测向量;Hk∈Rp×n为量测矩阵;ω(k)～N(0Qk)为高斯过程噪声;νk为量测噪声,对其统计特性不做假设。传统常规卡尔曼非线性滤波方法可以总结为[8]

(16)

(17)

(18)

(19)

Pk=(I-KkHk)Pk|(k-1),

(20)

(21)

式中,λk≥1为时变衰减因子。λk的更新公式为[8]

(22)

2.2 噪声加权自适应卡尔曼滤波算法

定义量测信息:

rk

(23)

最佳滤波在不同的时刻余差互不相关,可以得到误差协方差矩阵:

Crk

(24)

根据历史误差的记录,可以得到Crk的估计为

(25)

上式用到全部的信息,会增加算法的计算量,会使算法的实时性有所下降,故引入滤波窗口。

(26)

这样能较好地抑制历史数据对状态的影响。将上式(24)带入卡尔曼增益可得新的卡尔曼增益:

(27)

引入新息后根据传统卡尔曼滤波算法可以得到噪声矩阵的更新方程[9]:

(28)

(29)

其中,Δxk=Kkrk.将式(24)带入式(27)可以得到自适应卡尔曼滤波增益的计算公式为

(30)

要想进一步得到当前时刻最优的估计,需得出卡尔曼增益(K)与统计噪声(R)之间的关系,那么必须知道k时刻的后验估计协方差与k时刻的先验估计协方差。

k时刻的后验估计协方差表达式为

(31)

k时刻的先验估计协方差表达式为

(32)

把k时刻的后验估计协方差矩阵的迹记为T[Pk]:

(33)

确定卡尔曼增益的表达式:

(34)

(35)

(36)

其中,Ci(i=1,2,…)是一个适当的整数。这种形式的加权可以减弱以前的测量值的影响。

2.3 自适应组合滤波算法

针对衰减自适应记忆滤波和噪声加权自适应滤波的特点,可对这2种算法进行组合,实现优势互补,从而满足实际需要。首先对系统噪声方差Qk和滤波的量测噪声方差Rk进行在线估计,然后应用衰减自适应记忆滤波;推导出自适应组合滤波算法如下:

(37)

(38)

(39)

(40)

(41)

(42)

(43)

3 仿真分析

3.1 自适应滤波组合算法仿真分析

针对衰减自适应记忆滤波和噪声加权自适应滤波的特点,可对这2种算法进行组合,实现优势互补,从而满足对动态模型噪声干扰不确定的复杂BDS/INS系统。首先对不准确的集成系统进行了滤波,由于环境噪声的不确定性,本文前期验证了此算法的合理性。设目标的运动状态的x=4t+1,其状态方程和测量方程为

x(k+1)=x(k)+ω(k)，

z(k)=x(k)+υ(k) ，

(44)

式中:x(k)为目标的运动状态;z(k)为目标运动状态的测量值;ω(k)为过程噪声；υ(k)为运动目标的过程噪声。采样周期为1。传统卡尔曼滤波与衰减自适应卡尔曼算法对上述系统状态估计的轨迹和误差如图1、2所示。

从上图可以看出采用衰减自适应记忆算法在一定程度上可以提高了滤波器的精度。当环境噪声变化时,衰减自适应记忆算法可以更好地调节滤波噪声方差Rk,从而卡尔曼增益K调节更加灵敏。

3.2 BDS/INS位置组合导航仿真分析

INS和BDS各有优缺点,但在误差传播性能上正好是互补的。INS/BDS组合导航系统可以优势互补,取长补短。对BDS/INS组合导航进行仿真研究时,用BDS的高精度定位信息通过衰减记忆自适应卡尔曼滤波来补偿INS系统的积累误差,提高导航精度。同时,用INS系统的速度和加速度信息对BDS接收机进行速度辅助,以提高BDS接收机的抗干扰能力和动态性能。将此算法用于BDS/INS组合导航。

仿真选取一段无人机飞行的数据,INS输出相应的位置、速度、四元数等,通过其输出的四元数得到姿态矩阵;计算出组和导航系统的转移矩阵F、G以及得出组合导航位置量测方程H。把卫星接收机和INS的输出的位置差值作为量测值,输入到卡尔曼滤波器中,经过滤波后估计INS的误差,进而对INS进行校正。得到导航系统的位置输出。在计算机上进行仿真,得到不同滤波算法定位误差图,如图4、5、6所示。

图3示出了INS系统和卫星导航的原始定位误差。从图3可以看出惯导系统在前100 s定位误差相对较低;但是随时间的变化,定位误差逐渐增加,故单一导航系统难以为无人机提供精确的导航。对比改进的自适应算法(图4)与衰减因子卡尔曼算法(图5)和量测噪声加权算法(图6)的定位误差,可以验证改进的自适应算法可以在一定程度上提高导航的性能和系统的鲁棒性。

4 结束语

本文首先对比了卡尔曼滤波算法和衰减自适应记忆卡尔曼滤波算法对匀速运动的预测;本文提出的衰减自适应记忆卡尔曼算法可以使滤波保持最优。该算法不断更新量测噪声方差,在线自适应估计衰减因子。使预测值更加精确,有效地抑制了卡尔曼滤波估计的发散。其次采用一组飞行数据作为仿真的原始数据,验证了衰减自适应记忆滤波和噪声加权自适应滤波在难以确定噪声的BDS/INS组合导航的适用性。