第29卷 第2期2008年4月大连交通大学学报J O U R N A L O F D A L I A N J I A O T O N G U N I V E R S I T YV o l.29 N o.2 A p r.2008 文章编号:1673-9590(2008)02-0042-04基于卡尔曼滤波的车辆动态导航定位滤波算法曾洁,尤国红,贾士杰,魏梅,陈少华(大连交通大学电气信息学院,辽宁大连116028)＊摘 要:提出一种G P S动态定位系统模型,即是将G P S的误差等效为马尔柯夫过程,基于标准的卡尔曼滤波算法建立了一种利用G P S对车辆进行动态导航定位的滤波模型及自适应卡尔曼滤波算法.在实际研究过程中将其应用于车辆的导航定位系统,获得了显著的效果.结果验证了自适应卡尔曼滤波方法的可行性.关键词:G P S;导航定位;卡尔曼滤波;自适应滤波中图分类号:T N911.72文献标识码:AV e h i c l e D y n a m i c N a v i g a t i o n P o s i t i o nF i l t e r i n gA l g o r i t h m b a s e do n K a l m a n F i l t e r i n gZ E N GJ i e,Y O UG u o-h o n g,J I AS h i-j i e,W E I M e i,C H E NS h a o-h u a(S c h o o l o f E l e c t r i c a l＆I n f o r m a t i o n E n g i n e e r i n g,D a l i a n J i a o t o n g U n i v e r s i t y,D a l i a n116028,C h i n a)A b s t r a c t:Am o d e l f o r G P S d y n a m i c p o s i t i o n i n g s y s t e mi s p r o p o s e d,a n d a f i l t e r i n g m o d e l f o r v e-h i c l e d y n a m i c n a v i g a t i o n p o s i t i o n s y s t e ma n d a n e wa d a p t i v e K a l m a n f i l t e r a l g o r i t h mi s d e s i g n e db a s e d o n t h e a n a l y s i s o f t h e s t a n d a r d K a l m a n f i l t e r i n g a l g o r i t h m.T h e a p p l ic a t i o n o f t h e s y s t e mt o v e h i c l e n a v i g a t i o n p o s i t i o n s y s t e mp r o v e s t h a t t h e n e wa d a p t i v e K a l m a n f i l t e r i n g a l g o r i t h mi sf e a s i b l e.K e y w o r d s:G P S;n a v i g a t i o n p o s i t i o n;K a l m a n f i l t e r;a d a p t i v e f i l t e r造成G P S定位误差的主要来源[1]有以下几个方面:卫星轨道几何位置的误差;卫星钟偏差及接收机测量误差;大气误差(包括对流层和电离层传播延迟);多径效应误差和用户计算误差以及其它随机干扰误差等.为了提高G P S定位的精度,减小各类随机误差的影响,通常使用卡尔曼滤波法部分地滤掉随机误差,并将其真实的状态最优地估算出来.通常标准的卡尔曼滤波方法需建立准确的系统模型和观测模型,且要求对各类随机误差准确建模.一般将G P S误差因素等效为一阶马尔科夫过程的随机干扰,但容易出现以下两种极端情况:为实现最佳滤波和给出更精确的状态估计,系统方程的状态变量和观测量数目取得很大,造成运算量大,实时性差[2];相反,如果状态方程和观测方程过于简单,又不足以反映系统内在物理规律,或者虽然复杂的系统方程能准确反映具体物理过程,但在化简过程中带来了降维误差或线性化误差,这又易造成滤波发散[2,3],为此本文在标准卡尔曼滤波基础上提出一种自适应扩展卡尔曼滤波方法.＊收稿日期:2007-06-19基金项目:辽宁省教育厅科学研究计划资助项目(2004D118)作者简介:曾洁(1965-),男,副教授,硕士E-ma i l:z e n g y i z e@h o t m a i l.c o m.DOI:10.13291/ ki.djdxac.2008.02.022 第2期曾洁,等:基于卡尔曼滤波的车辆动态导航定位滤波算法43 1 K a l m a n 滤波原理1.1 标准K a l m a n 滤波[4] 设离散系统状态方程和观测方程如下:X (k )=Υ(k ,k -1)X (k -1)+Γ(k -1)W (k -1)(1)Z (k )=H (k )X (k )+V (k )(2)式中,X (k )为k 时刻的n 维状态向量;Z (k )为k 时刻的m 维观测向量;W (k -1)为系统k -1时刻的噪声;V (k )为系统k 时刻的m 维观测噪声;Υ(k ,k -1)为k -1到k 时刻的系统一步转移矩阵;H (k )为k 时刻的观测矩阵;Γ(k -1)为系统噪声矩阵,它表示k -1时刻到k 时刻系统噪声影响状态的程度.假定{W (k )}和{V (k )}为互不相关的零均植的高斯白噪声序列,即有:E {W (K )W T (j )}=Q (k )δk j(3)E {V (K )V T (j )}=R (k )δk j(4)E {W (K )V T (j )}=0(5)式中,Q (k ),R (k )分别为系统噪声和观测噪声的协方差矩阵,则卡尔曼滤波的递推方程为:状态一步预测方程:X (k ,k -1)=Υ(k ,k -1),X (k -1)(6)一步预测估计误差方程:P (k ,k -1)=Γ(k ,k -1)P (k -1)ΥT (k ,k -1)+Γ(k -1)Q (k -1)ΓT (k -1)(7)最优滤波增益方程: K (k )=P (k ,k -1)H T (k )[H (k )P (k ,k -1)H T (k )+R (k )]-1(8)最优滤波估计方程:X (k )=X (k ,k -1)+K (k )[Z (k )-H (k )X (k ,k -1)](9)最优滤波估计误差方程:P (k )=[I -K (k )H (k )]P (k ,k -1)(10)由式(6)～(9)可以看出,在一个滤波周期内,K a l m a n 滤波具有两个明显的信息更新过程:时间更新和观测更新.式(1)和(6)将时间从k -1时刻推进至k 时刻,描述了K a l m a n 滤波的时间更新过程:其余诸式用来修正对时间的更新,其目的是更加准确、合理地利用观测量来描述K a l m a n 滤波的观测更新过程.因此式(8)可以直观理解为:求解新值和合理的利用新值,即如何求取满足不断变化系统参数的增益矩阵K (k ).1.2 基于K a l m a n 滤波的自适应扩展算法消除车辆G P S 定位随机误差的方法有多种,其中比较常用的方法之一是G P S 动态滤波,即利用滤波器消除各种随机误差,从而提高G P S 定位精度.为了改善滤波器的动态性能,使其具有一定的跟踪能力,本文采用一种自适应扩展卡尔曼滤波算法[5].取状态变量为:X={x ,υx ,a x ,εx ,y ,υy ,a y ,εy ,z ,υz ,a z ,εz}(11)式中,3组状态变量分别为x ,y ,z 3个坐标轴方向上的位置、速度、加速度分量εx ,εy ,εz 分别为各种误差源在3个坐标轴方向造成的总位置误差,可用一阶马尔可夫过程等效[4],(11)式中,τx ,τy ,τz 分别为对应马尔可夫过程的相关时间常数;ωx ,ωy ,ωz 分别为[0,σ2x ],[0,σ2y ],[0,σ2z ]高斯白噪声.考虑到降低系统运算量,可将3个轴向的状态变量利用分散卡尔曼滤波技术分别单独进行处理.以x 轴向为例,状态变量为X ={x ,υx ,a x ,εx }T ,系统方程为X =A x X+U x +W ,其中A x =0100001000-1/τa x 0000-1/τx 为系统状态转移矩阵(12).其中,τa x 为加速度相关时间常数,ωa x 为(0,44 大连交通大学学报第29卷a 2a x)的高斯白噪声,实际上是将G P S 定位结果中总的位置误差视为有色噪声扩展的状态变量.系统观测方程为L x =H x X+V x ,其中,观测噪声矢量为V x ={ωL x },ωL x为(0,R 2x )的高斯白噪声,观测矩阵H x ={1,0,0,1}. 根据上述的系统方程和观测方程,建立x 轴向上自适应扩展卡尔曼滤波方程:状态一步预测方程:X (k ,k -1)=Υ1x (k )X (k -1)(13) 一步预测估计误差方程:P (k ,k -1)=λ(k )φx (k ,k -1)P (k -1)φT x (k ,k -1)+Q (k -1)(14) 最优滤波增益方程:K (k )=P (k ,k -1)H T x (k )[H x (k )P (k ,k -1)H T x (k )+R x(k )]-1(15) 最优滤波估计方程:X (k )=X (k ,k -1)+K (k )[L x ＊k )-H x (k )X (k ,k -1)](16)最优滤波估计误差方程:P (k )=[I -K (k )H x (k )](k ,k -1)(17)式中:Υ1x (k )=1T T 22001T 00010000e -T /τx (18)Υx (k ,k -1)=1T (T /τa x -1+e -T /τa x )τa x 001(1-e -T /τa x )τa x 000e -T /τa x 0000e -T /τx (19)其中,式(19)为系统转移矩阵A x 的离散化矩阵,λ(k )是引入的自适应遗忘因子,目的是充分利用现时的测量数据,改善滤波器的动态性能.确定λ(k )的方法见文献[6],同理可得到对应y 轴及z 轴方向上状态变量的滤波算法.建立上述的卡尔曼滤波模型对G P S 输出的位置信息进行处理,可显著提高机动过程中位置及速度的估计精度,有一定动态跟踪能力.但在实际应用中,当G P S 接收机处于静态定位时,滤波精度由最优变为次优,即为了获得较好的动态性能而牺牲了一定的定位精度.自适应扩展卡尔曼滤波方法适用于运动载体动态定位的G P S 动态定位系统,数值滤波稳定性好,克服了标准卡尔曼滤波方法易发散的缺点,故该方法具有较快的收敛性和较强的自适应性,比标准卡尔曼滤波法具有更高的精度.2 自适应扩展K a l m a n 滤波的进一步改进2.1 自适应扩展K a l m a n 滤波尚存问题与对策上述过程是在一种理想条件下进行的,即要求系统的动态噪声和观测噪声为零均值并且统计特性为已知的白噪声,实际上,在动态G P S 定位中,这些条件未必能满足,此时就存在建模误差,如在G P S 观测方程中,经电离层模型改正后残余的电离层延迟作为观测噪声就不是零均值白噪声,而且对于高动态G P S ,其动态噪声很难准确地给出,另外,由于非线性方程线性化时,一定存在线性化误差,这也是一类建模误差.由式(11)～式(19)可以看出,在计算增益方程K (k )时,并不考虑实际的观测值,而只根据前面确定Υ,R ,Q 3个矩阵的数值.如果它们中的任何一个不够准确,都将导致增益K (k )计算的错误,并可能导致滤波过程发散.这也是此种自适应扩展卡尔曼滤波的一个重要缺陷.另一方面,由于受到计算工具等客观条件的限制,使得滤波算法在计算机上实施时,易产生舍入误差积累,使误差协方差阵失去正定性或对称性,从而出现数值计算不稳定现象.一般情况下,当状态向 第2期曾洁,等:基于卡尔曼滤波的车辆动态导航定位滤波算法45量维数超过10时,滤波过程中就可能出现滤波不稳定现象.针对动态G P S导航定位滤波不易确定动态噪声和观测噪声的特点,可采用一种适用于动态G P S定位的改进的自适应卡尔曼滤波方法———S a g e自适应滤波方法(又称极大后验估计器).该方法具有数值稳定性好,存储量小的优点,克服了发散的缺点,具有较强的自适应性.G P S动态导航定位中,经常遇见奇异协方差阵,要解决这些问题,一般采用平方根算法或U D分解算法.为了解决滤波数值计算不稳定问题,人们在实践中提出了许多方法,固定增益滤波,平方根滤波等.2.2 精度的提高虽然自适应扩展卡尔曼滤波在定位和精度方面有了很大的改善,但基于卡尔曼滤波的G P S导航定位滤波方法也存在问题,当使用G P S接收器静态定位时,滤波得到的估计位置精度就会下降.造成这种现象的原因可以总结如下:无论G P S接收器是静止的还是运动的,其滤波模型的一步状态预测方程都相同,均采用方程(13).根据这个方程,k时刻接收器的位置和速度由上一时刻的滤波输出预测得到,即由k-1时刻的位置和速度预测得到.当接收器运动时,方程(13)可提供精确的位置预测,但当接收器静止时,速度初始值为零,而由于速度误差的原因,预测的接收器位置与实际接收器位置产生偏离,导致定位精度的下降.为了克服这样的缺点,当接收器静止时,滤波器考虑采用传统卡尔曼滤波器,并且,状态一步预测方程为:X(k,k-1)=Υ(k,k-1)X(k-1)改进为X(k,k-1)=X(k-1)(20)采用式(20),滤波器在k时刻的输出预测值等于k-1时刻的值,有效地减少了预测中速度误差引起的位置误差.本文提出的自适应扩展的卡尔曼滤波器提高G P S接收器定位精度的有效性,针对静态定位的特点,对传统卡尔曼滤波方程做出了改进,并进一步提出组合滤波结构,完善了动态卡尔曼滤波器的静态滤波性能,如果能同时运用D G P S和卡尔曼滤波技术并采用组合定位的方法,定位精度将有很大的提高,从而为G P S接收器的定位精度问题提供了一种有效的解决途径.3 结 语上述研究结果表明,与以往[6,7]采用的卡尔曼滤波器相比,模型简单,系统运算量降低,实时性较好.将G P S定位误差视为一阶马尔科夫过程,利用自适应卡尔曼滤波器对车辆位置和速度信息进行估计是可行的,且效果良好.为了改善滤波器的动态性能,自适应卡尔曼滤波算法是一种有效的措施.从而说明了本文的G P S车辆导航动态模型,能够有效地降低G P S定位信号的随机干扰,提高车辆动态定位精度,降低车辆导航定位的成本,提高车辆导航定位系统的实用性.参考文献:[1]王惠南.G P S导航原理与应用[M].北京:科学出版社,2003:106-121.[2]房建成,申功勋,高红霞.民用导航型C/A码G P S接收机动态定位的强跟踪卡尔曼滤波研究[J].电子测量与仪器学报,1998,12(2):126.[3]陈小明.高精度G P S动态定位的理论与实践[D].武汉:武汉测绘科技大学,1995.[4]付梦印,邓志红.K a l m a n滤波理论及其在导航系统中的应用[M].北京:科学出版社,2003.[5]自适应滤波算法与实现[M].刘郁林,景晓军,译.北京:电子工业出版社,2004.[6]万德均,房建成,王庆.G P S动态滤波的理论方法及其应用[M].南京:江苏科学技术出版社,2000:66,92,96,99.[7]M O U S S AR.M a k i n g t h e b e s t w i t hG P S i n c a r a p p l i c a t i o n[C].P r o c o f I O NG P S295.C a l i f o r n i a,1995:1819-1823.。
