车载GPS_DR组合导航系统数据融合算法研究

1、车载 GPS/ D R 组合导航系统数据融合算法研究常青 ,郑平方 ,柳重堪 ,张其善( 北京航空航天大学电子工程系 ,北京 100083)摘 要 :本文介绍了车载 GPS/ DR 组合导航系统的设计 ,建立了表示车辆加速度及影响组合导航系统定位精度的主要误差的数学模型 。针对迭代扩展组合卡尔曼滤波算法中 ,由于 DR 系统误差的 引入导致滤波效果不好的问题 ,提出了一种新的组合导航系统的数据融合算法 。对实际跑车数据的处理结果表明 ,该算法在提高组合导航系统的定位精度及可靠性和完整性方面是有效的 。关键词 :全球定位系统 ;组合导航 ;航位推算法 ;数据融合中图分类号 :V47412文章编号

2、 :10002436X(2000) 0220042207文献标识码 :AThe Studyof Data Fusion Algorithm of GPS/ D RIntegrated Vehicular Navigation SystemCHANG Qing ,ZHENG Ping2fang ,L IU Zhong2kan ,ZHANG Qishan(Department of Electronic Engineering ,Beijing University of Aeronautics and Astronautics ,Beijing 100083 ,China)Abstract :

3、In this paper ,the design of the GPS/ DR integrated vehicular navigation system is presented ,and themathematical models are founded for the acceleration of vehicle and the main errors which affect the positioning accuracy of the integrated vehicular navigation system1Also ,a new data fusion algorit

4、hm of the integrated vehicu2 lar navigation system is proposed to solve the problem of unsatisfactory filtering effect due to the introduction of the DR system error in the iterated , extended and federated Kalman filter algorithm1The experimental results show that this algorithm is effective for th

5、e improvement of the positioning accuracy , reliability and integrity of the integrated vehicular navigation system1Key words :global positioning system ;integrated navigation ; dead reckoning ; data fusion1引言全球定位系统 ( GPS) 是美国国防部研制的第二代卫星导航定位系统1 ,它为全球用户提 供全天候 、连续 、实时的高精度位置 、速度和时间信息 ,因此在军 、民方面都具有重要的开发

6、和 利用价值 。目前 , GPS 定位技术除了广泛应用于飞机和水面船只的导航定位外 ,在陆地车辆的 导航定位中也获得了广泛的应用 。车辆导航定位系统是在进入九十年代以后发展起来的一项 新技术 ,近年来 ,随着城市车辆数量的日益增多 ,如何在城市里有效地指挥和管理公共服务车 辆及监控一些特种专用车辆已成为交通运输部门 ,城市规划部门及金融 、公安 、保密等特殊部 门面临的一个重要问题 ,这使得车辆导航定位系统更加受到关注2 4 。航位推算5 (DR2Dead收稿日期 :1999203216 ;修订日期 :1999211211基金项目 :国家重点科技攻关计划项目 (9621302205202)学术

7、论文Reckoning) 和 GPS 是两种不同的车辆定位技术 ,二者各有弱点 。DR 系统的弱点是定位误差随时间的推移发散 ,且需要由其它系统提供车辆的初始位置和初始方位角 ,因此 DR 系统不能单 独长时间使用 ,目前的车辆导航定位系统均以 GPS 作为定位手段 。GPS 系统的弱点是当车辆穿行在城市高楼区 、林荫道 、涵洞等处时 ,常出现卫星信号遮挡问题 ,导致 GPS 的功能失效 ,不 能正常定位 ,这一弱点是由 GPS 系统采用的被动定位方式决定的 。如果将两个系统组合起 来 ,就可以在出现卫星信号遮挡问题时 ,利用 DR 系统的完全自主性对 GPS 系统进行补充 ,因此组合系统比某

8、一个单独的系统具有更好的可靠性和完整性 ,于是 GPS/ DR 组合导航系统应 运而生 。数据融合算法是组合导航系统研究中的一个关键问题 ,这个问题解决的好坏 ,决定了 能否进一步提高系统的定位精度及可靠性和完整性 。本文首先介绍了车载 GPS/ DR 组合导航系统的设计 ,建立了该导航系统的数学模型 ,然后针对迭代扩展组合卡尔曼滤波算法中 ,由于 DR 系统误差的引入导致滤波效果不好的问题 ,提出了一种新的组合导航系统的数据融合算 法 ,对实际跑车数据的处理结果证明了算法的有效性 。2车载 GPS/ D R 组合导航系统的组成车载 GPS/ DR 组合导航系统的组成如图 1 所示 ,其中 ,

9、中心控制器完成对 GPS 、陀螺和里 程仪的数据采集 ,然后进行数据融合 ,输出定位结果 。数据融合算法对整个系统的定位精度起 关键作用 。图 1 车载 GPS/ DR 组合导航系统的组成3 基于迭代扩展卡尔曼滤波方法的数据融合算法311离散状态方程的建立与航空导航不同的是陆地车辆导航是在二维平面内进行的 ,因此组合导航系统的卡尔曼 滤波状态变量取为X = X TX T T12X T = e n eX T = ;其中nen ;12e nse 和 n 分别为东向位置和北向位置 , 单位为m ; e 和 n 分别为东向速度和北向速度 , 单位为m/ s ;e 和 n 分别为东向加速度和北向加速度

10、, 单位为m/ s2 ;e 和n 分别为各种误差源在两个坐标轴 上造成的总的位置误差 , 单位为 m ;为陀螺输出的相对转角误差 , 单位为 rad ;s 为里程仪输 出的距离误差 , 单位为 m 。由文献 4 知 ,可采用一阶马尔可夫过程描述 ,s ,e和n , 即 = - + s = - S + e = - e + n = - n + sseenn其中 , , , 分别为对应马尔可夫过程的相关时间常数的倒数 ; , , , 是均值senSen为零 、方差分别为 q2 , q2 , q2 , q2 的高斯白噪声 。由于受到许多外界因素的影响 , 如路线形Sen据文献 6 的计算结果 , 这一

11、过程是连续 、有界 、零均值的平稳随机过程 。考虑到城市环境下车辆行驶过程的复杂性和 Singer 模型的局限性7 , 应采用机动加速度的“当前”统计模型 7 描述 车辆加速度 。所谓机动加速度的“当前”统计模型 , 即令e = ae + aeae = - a ae + an = an + an a n = - a an + aeenn其中 ae , an为零均值有色加速度噪声 ; ae , an为机动加速度均值 , 它在每一采样周期内为常数 ;a ,a 为对应马尔可夫过程的相关时间常数的倒数 ; a , a 是均值为零 、方差分别为 2a ,enene2, 22 的高斯白噪声 。根据以上模型

12、和典型的离散处理方法 , 并假设数据采样周期为 T ,a a ae n n得到系统的离散状态方程为X ( k + 1) = ( k + 1) X ( k) + U ( k) + W ( k)(1)108 202 8X ( k) = X T ( k)X T ( k) T为 a b 阶零矩阵 , 下同)其中 =(012a b2X Te ( k)n ( k)1 ( k) = e ( k)n ( k)e ( k)n ( k) e ( k) n ( k) X T2 ( k) = ( k)S ( k) I2 202 202 202 2TI2 2I2 202 202 2C1C2E102 202 202 20

13、2 2e - T0e - T1 =( I2 2为二阶单位矩阵 , 下同)2 =0SE2ci ( i = 1 , 2 , 3 , 4) 7 C1 = diag c1 , c2C2 = diag c3 , c4E1 = diag e - ae T , e - an T E2 = daig e - e T , e - n T01 4 Tui ( i = 1 , 2 , 3 , 4 , 5 , 6) 7 U ( k) = u1u2u3u4u5u6W ( k) 为离散时间白噪声序列 。系统过程噪声协方差矩阵为08 2Q102 8Q ( k) = E W ( k) W T ( k) =Q2Q11 06 2

14、Q2 = diag2 ( 1 -e - 2 ) ,T 2- 2 T其中Q1 =( 1 - eS ) (1 -S02 6Q12Q11见文献 7 , Q12 = diag2 (1 -e - 2e T) ,2e - 2n T) 。en312观测方程的建立将 GPS 接收机输出的位置 er , nr 、速度 e r , n r及陀螺和里程仪输出的距离和相对转角 S 和作为外部观测量 , 则外部观测量与状态量之间具有如下关系z1 ( k) = er ( k) = e ( k) +e ( k) + v1 ( k)z2 ( k) = nr ( k) = n ( k) +n ( k) + v2 ( k)z3

15、( k) = e r ( k) = e ( k) + v3 ( k)z4 ( k) = n r ( k) = n ( k) + v4 ( k)z5 ( k) =( k) = ( k) - ( k - 1) + ( k) + v5 ( k)z6 = S ( k) = Te2 ( k) + n2 ( k) + S ( k) + v6 ( k)其中 ( k) 为 k T 瞬时车辆运动方向与北向的夹角 , 以顺时针为正 。( k) 和 ( k - 1) 为k - 1( k) = arctan e ( k)( k - 1) = (0) + ( i ) + ( i ) + n ( k)i = 1取 0 ,

16、 - 三个值中之一 ; vi ( k ) ( i = 1 , 2 , 3 , 4 , 5 , 6) 是均值为零 、方差为 r2 的高斯白噪i声 。于是观测方程为Z ( k) = h X ( k) + V ( k)Z ( k) = ZT ( k)ZT ( k) T12ZT ( k) = e ( k)ZT ( k) = ( k)n ( k)e ( k)n ( k) S ( k) 1rrrr2h1 X ( k) h2 X ( k) h X ( k) =10000100001000010000000010000100h1 X ( k) = H1 ( k) X1 ( k) =X1 ( k)arctan

17、e ( k)+ - ( k - 1) + ( k)n ( k)h2 X ( k) =Te2 ( k) + n2 ( k) + S ( k)V ( k) 的均值为零 , 方差为 R ( k) = diag R1 , R2 , R1 = diag r2 , r2 , r2 , r2 , R = diag r2 , r2 。1 2 3 4 25 6由于观测方程为非线性方程 , 为将其线性化 , 采用扩展卡尔曼滤波技术 , 即在一步预测值处将其展成泰勒级数 , 并忽略高次项 , 得Z ( k) h X ( k , k - 1) + H X ( k , k - 1)X ( k) - X ( k , k

18、- 1) + V ( k)02 2I2 2H 2 202 202 202 202 202 2I2 2I2 202 202 2I2 202 202 2h1h3h2h4其中H X ( k , k - 1) =H 2 2 = n - e h1 = e2 + n2h2 = e2 + n2X ( k , k - 1)X ( k , k - 1) Te Tn h4 =h3 =e2 + n2e2 + n2X ( k , k - 1)X ( k , k - 1)令 ( k) = Z ( k) - h X ( k , k - 1) + H X ( k , k - 1) X ( k , k - 1) ( k) H

19、 X ( k , k - 1) X ( k) + V ( k)(2)式 ( 2) 就是线性化的观测方程 。观测方程的线性化必然引入线性化误差 , 为消除此误差 , 可做迭代 , 即在得到的滤波估计处重新将观测方程泰勒展开 , 这种既使用扩展卡尔曼滤波技术 , 又 做迭代的方法称为迭代扩展卡尔曼滤波方法 。313 数据融合算法在式 ( 1) 和式 ( 2) 的基础上 , 利用文献 7 给出的加速度均值自适应的卡尔曼滤波算法 , 可 以得到组合导航系统的迭代扩展组合卡尔曼滤波算法 。对实际跑车数据的处理结果表明 , 该 算法的效果不好 。主要原因在于陀螺和里程仪的误差被引入了滤波器的位置 、速度和

20、加速度 的估计中 。为了提高整个组合系统的估计精度 , 首先必须提高位置 、速度和加速度的估计精度 , 为此应将两个系统分离 。然后利用这些具有较高精度的估计 , 再对陀螺和里程仪的误差进 行估计 , 此时应将两个系统结合 , 见图 2 。只有这样 , 才能在数据融合过程中做到既提高组合 导航系统的定位精度 , 又能将陀螺和里程仪的误差尽可能准确地估计出来 。为此我们提出如下的递推算法 ( 为消除观测方程的线性化误差 , 可做迭代) :1) X ( k , k - 1) = ( k , k - 1) X ( k - 1)2 - 1 T2 I2 2TI2 2I2 202 202 202 202

21、202 2E202 202 202 202 202 2I2 202 202 202 202 2TI2 2I2 202 202 202 2 ( k , k - 1) =其中22) P1 ( k , k - 1) = 1 ( k , k - 1) P1 ( k - 1) T ( k , k - 1) + Q ( k - 1)11P ( k , k - 1) = ( k , k - 1) P ( k - 1) T ( k , k - 1) + Q ( k - 1)3) K1 ( k) = P1 ( k , k - 1) HT ( k) H ( k) P ( k , k - 1) HT ( k) +

22、R ( k) - 111111K( k) = P ( k , k - 1) HT X ( k , k - 1) H X ( k , k - 1) P ( k , k - 1) HT X ( k , k - 1) + R ( k) - 14) X ( k) = X ( k , k - 1) + GTGT T12g2 T其中G1 = K1 ( k) Z1 ( k) - h1 X ( k , k - 1) G2 = g1g1 , g2是列向量 K( k) Z ( k) - h X ( k , k - 1) 的第九和第十个元素 。5) P1 ( k) = I - K1 ( k) H1 ( k) P1

23、( k , k - 1)P ( k) = I - K( k) H X ( k , k - 1) P ( k , k - 1)图 2 数据融合示意图假设 k T 时刻 GPS 接收机发生信号丢失 ,不能给出定位解 ,那么数据融合停止 ,此时由航 位推算系统按下式继续给出车辆的位置e ( k ) = e ( k - 1) + s ( k ) sin( k - 1) +( k ) n ( k ) = n ( k - 1) + s ( k ) cos( k - 1) +( k ) ( 3)(4)4实验验证为了验证数据融合算法的有效性 , 1998 年 4 月 3 日进行了组合导航系统的跑车实验 , 路

24、线 为北京航空航天大学校内逸夫馆 体育馆 3 号楼 学校正东门 1 号楼 主楼广场 , 数据采 样周期为 1 秒 。将 GPS 与航位推算系统按照上面的算法进行数据融合 ,结果示于图 3图 6 , 计算时初始条件选取为 : = = 0101 ,a =a = 1 , = 2000 , = 2000 ,a =a= 018 , = 2e ne nsens = 011 , = = 10 , X (0) = z1 (0) , z2 (0) , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 TenR = diag 102 , 102 , 12 , 12 , 01052 , 22 , P (0

25、) = diag 202 , 202 , 12 , 12 , 0122 , 0122 , 52 , 52 , 0112 , 22 P1 (0) = diag 202 , 202 , 12 , 12 , 0122 , 0122 , 52 , 52由图 3 知 , 滤波曲线与 GPS 观测曲线基本一致 。图 4 所示为图 3 中 AB 段的局部放大 ,从图 3 滤波曲线与 GPS 观测曲线比较图 4 图 3 中 AB 段的局部放大图 4 可以看出 ,滤波曲线对比于原始曲线有很好的平滑效果 ,达到了滤波的目的 。图 5 是用滤波器给出的陀螺和里程仪的误差估计对 DR 系统进行误差补偿前后的曲线 ,从

26、图 5 可以看出 , DR 系统经过误差补偿后 ,误差发散得到有效控制 ,这说明滤波器给出的陀螺和里程仪的误差估计是很准确的 。图 6 是误差补偿前后的 DR 系统和原始 GPS 系统北向位置差曲线 ( 东向位置差曲线与图 6 类似) ,从图 6 可以看出 ,误差补偿后的 DR 系统与 GPS 的北向位置差始终保 持在较小的范围内 ,充分证明了算法的有效性 。图 5 误差补偿前后的 DR 曲线比较图 6 误差补偿前后的 DR 和 GPS 北向位置差曲线5 结束语本文提出的算法在给出车辆的位置估计及陀螺和里程仪的误差估计的同时 ,还可以自动给出车辆的方位角 (见式 (3) 和式 (4) ) ,陀螺和里程仪的误差估计越准确 ,则给出的方位角越准 确 。由于当 GPS 系统丢失信号时 ,DR 系统是以滤波器给出的位置估计和方位角作为车辆的 初始位置和初始方位角的 ,因此滤波估计越准确 ,切换到 DR 系统后 ,DR 系统正常工作的时间位置估计及陀螺和里程仪的误差估计 ,且计算结果还表明 ,当 GPS 系统丢失信号时 ,DR 系统在短时间内 (至少 150s) 完全可以满足定位精度的要求 ,这些充分说明了该算法在提高组合导 航系统的定位精度和可靠性 、完整性方面是有效的 ,它将成为组合导航系统的核心 。参考文