车载GPS_DR_地图匹配组合导航系统的自适应联合卡尔曼滤波模型

车载GPS_DR_地图匹配组合导航系统的自适应联合卡尔曼滤波模型 车载 ƒƒ地图匹配组合导航系统的 GPSD R Ξ 自适应联合卡尔曼滤波模型 房建成 申功勋万德钧( )( )北京航空航天大学第五研究室, 100083 东南大学仪器系 摘 要 首次了实现车载 地图匹配组合导航系统最优综合的联合卡尔曼滤波 ƒƒGP SD R 器, 给出了滤波算法, 并提出一种自适应联合卡尔曼滤波器结构及其算法。理论分析及计算机仿 真结果均明, 应用该自适应联合卡尔曼滤波器可大大提高车载 ƒƒ地图匹配组合导航 GP SD R 系统的定位精度及容错能力。 关键词 航位推算系统, 组合导航, 卡尔曼滤波, 自适应算法, GP S 分类号 713TN 1 引言 在陆地车辆导航中, 定位导航系统能够全天候、连续实时地提供高精度的三维速度 GP S 和位置信息, 但是当车辆行驶在高楼林立的市区时, 由于 卫星信号经常受到遮挡, 某些情 GP S () 况下通过 系统实现连续准确的定位是不可能的。而车辆航位推算导航系统 系统是GP S D R 一种自主式的车辆导航系统, 它在短时间内精度较高, 但导航误差将随时间积累。 利用组合导航技术把上述两种导航系统结合起来, 再用数字地图进行定位结果的匹配, 构成 ƒ地 GP SD R 图匹配组合导航系统, 能够获得优于任何单独一种导航系统的导航精度和可靠性。 车载 ƒƒ地图匹配组合导航系统是由 定位系统和系统两个相互独立的子 GP SD R GP S D R 系统构成的多传感器系统。在多传感器系统中, 联合卡尔曼滤波器利用信息分配原理可实现多 传感器信息的最优综合, 并使整个系统具有一定的容错能力, 从而能够获得整体上最优的性能。要实现车载 地图匹配组合导航系统的定位导航信息最优综合, 关键是建立其滤 ƒƒGP SD R 波模型。本文应用多传感器系统信息融合理论中的联合卡尔曼滤波, 研究车载 ƒƒ GP SD R 地图匹配组合导航系统中最重要的部分——ƒ组合导航系统部分滤波模型的建立。GP SD R 2 系统联合卡尔曼滤波器的总体方程 GP SƒD R 组合导航系统的整体状态变量取为 T X = ]g e, V , a , n , V , a , Ε, 7 e e n n 其中, e, n 分别为车辆东向和北向的位置分量, V e , V n 分别为车辆东向和北向的速度分量, a e , a n分别为车辆东向和北向的加速度分量, Ε为速率陀螺漂移误差, 7 为车辆里程表的标定系数。采 1 用机动载体的“当前”统计模型, 建立组合导航系统总体方程如下 ( ) () Ξ 航空科学基金9851101及江苏省应用基础研究基金95026资助课 E BJ 1997- 12- 31 收稿, 1998- 03- 09 修回 第 14 卷 第 5 期 房建成等: 车载 GP SƒDR ƒ地图匹配组合导航系统的自适应联合卡尔曼滤波模型 449 0 1 0 0 0 0 0 0 0 α 0 0 1 0 0 0 0 0 0 ee 0 α1 0 V Ve e 0 0 - 0 0 0 0 0 1 λ Σa ae e α w aΣ a a a e ee e 0 0 0 0 1 0 0 0 α 0 n 0 n()1 = + + 0 0 0 0 0 1 0 0 α 0 VV n 0 n 1 α 0 0 0 0 0 - 0 0 w a 1 a an n λn Σ a a nn Σ aαn Ε Ε w Ε 1 α0 0 0 0 0 0 - 0 0 77 w 7Σ Ε 0 0 0 0 0 0 0 0 0 () 对于 GP S 定位子系统 子系统 1, 取其系统状态变量X 1 = X g , 子系统方程同总体系统方 ( 程。其外观测量为 GP S 接收机输出的车辆东向和北向位置坐标分量 e和 n单位均化成o b s o b s ) m , 观测方程可表示为 () () () ()() 2 Z 1 k =H 1 k X 1 k + V 1 k () () ek w k o b s e () () Z 1 k =, V 1 k =()()nk w k o b s n () 3 这里 1 0 0 0 0 0 0 0 () H 1 k =0 0 0 1 0 0 0 0 2 2 () () () ( ) 其中, w e k 和 v n k 分别为 0, Ρ和 0, Ρ的高斯白噪声序列。e n () 对于D R 系统 子系统 2, 取其系统状态变量 X = X , 子系统方程同总体系统方程, 其外2 g 观测量取为压电晶体陀螺的输出 Ξ 和车辆里程表输出的车辆在采样周期 T 内行进的距离 s, 即 T 系统的观测矢量为 Z = 2 Ξ s ], 可写成 5 V e- 1 Ε + tg Ε ΞΞ 5t V n()Z = 4 = + 2 s Εs 2 n V + V 7 e s ( ) ( ) ()( ) 5 V t即Z 2 t= h 2 [ t, X t+2 2 ( ) 其中, ΕΞ 为速率陀螺漂移误差中的高斯白噪声分量 0, Ρ, Εs 为里程表输出量的观测噪声, 为Ξ 2 () () 0, Ρs 的高斯白噪声。可以看出, 观测方程 4为一非线性方程, 为了应用卡尔曼滤波方 程, 需要进行线性化。 在进行联合卡尔曼滤波器设计时, 令 Β= 0, 则主系统不占有任何全局信息, 而仅仅完成 m 对各子系统估计的综合运算。这时主系统的计算量很小, 而总体系统前向滤波速度最快, 系统 设计最佳。GP S 定位系统所对应的信息分配系数为 Β1 , D R 系统对应的信息分配系数为 Β2。 3 系统的联合卡尔曼滤波模型 () 首先考虑局部卡尔曼滤波器 1 L F 1, 这是一标准卡尔曼滤波器。L F 1 的模型为 (() () () ()) U k + W k 1= X 1 k +5 k X 1 k +()6 () () () () Z 1 k = H 1 k X 1 k + V 1 k () 然后考虑局部卡尔曼滤波器 2 L F 2, 这是对应D R 系统的扩展卡尔曼滤波器。L F 2 的模型为 450 控制与决策1 9 9 9 年 (() () () ()) U k + W k 1= X 2 k +5 k X 2 k +()7 () () () Z 2 k = h 2 [ k , X 2 k +V k 2 则整体状态的最优综合为 δ δ δ ( ) () ) (ΒXk +ΒXk +1, 1= Xk +1 + Β+ Β= 1 11 22 g 1 2 - 1 - 1 - 1 (() ) (()k + 1= P k + 1+ P k + )8 g 1 P 1 2 - 1 - 1 - 1 ((()) ) Q g k +1= Q 1 k +1+ Q 2 k + 1 4 自适应联合卡尔曼滤波器及其算法 在上述 GP SƒD R 组合导航系统的联合卡尔曼滤波器结构中, 考虑到机动加速度均值和方 差的自适应算法, 分别对应于 GP S 定位系统和D R 系统的两局部卡尔曼滤波器L F 1 和L F 2, 实 际上都是自适应滤波器。在联合卡尔曼滤波器中, 对应于各局部滤波器和主滤波器的信息分配 的取值决定了联合卡尔曼滤波器的性能。就前面所设计的 GP SD R 组合导 ƒ, Β, Β 系数 Β1 ,N M 航系统的联合卡尔曼滤波器而言, Β1 , Β2 的取值影响联合卡尔曼滤波器的性能, 因此可通过选 择不同的信息分配系数来改变滤波器的性能, 以满足不同的需要。 根据信息分配原理, 有 Β+ Β= 1。现来考虑一种极端情况, 即令 Β= 0, Β= 1。对L F 1:1 2 1 2 δ δ ( ) ( 0, 故经推导可知, 此时L F 1 输出的状态估计值 Xk +1 对整体状态输出 Xk +因为 Β1 =1 g δ δ ) ) ( ( 1的影响最小。同理, 当 Β2 =0 时, 局部滤波器L F 2 的输出Xk +1 对整体状态输出Xk +2 g ) 1的影响最小。在实际滤波算法中, 不能取 Β1 = 0 或 Β2 = 0, 否则将会出现发散, 但可根据需要 取 Β或 Β为尽可能小的值。1 2 经过大量的计算机仿真研究发现, 当取 Β= Β= 0. 5 时, 联合卡尔曼滤波器的整体输出1 2 δ Xg 有最好的估计精度。若 Β1 > 0. 5, Β2 < 0. 5, 则滤波器的整体输出在性能上以局部滤波器 δδ L F 1 为主, 且 Β越接近 1 而 Β越接近 0, 滤波器的整体输出 X1 所占的比重1 2 g 中, L F 1 的输出 X 越大, 即整个联合卡尔曼滤波器在性能上越接近L F 1, 这正是我们所希望的。 在市区内, 由于遮挡等原因, GP S 定位系统不能正常定位或定位精度较差时, 可取 Βν 1,1 Β? 1, 使联合卡尔曼滤波器的整体状态输出为L F 2 的输出, 即组合系统的输出在性能上接近2 D R 系统的性能。当 GP S 能正常工作, 定位精度较高时, 取 Β= 0. 5, Β= 0. 5, 使联合卡尔曼滤 1 2 波器的整体输出具有较好的综合性能, 即获得较高的定位精度和容错能力。 若能根据具体情况自动调整 Β和 Β的大小, 这时联合卡尔曼滤波器便成为一种自适应联 1 2 合卡尔曼滤波器。如何自动调整 Β, Β的大小? 可利用 GP S 接收机输出的反映定位精度的参1 2 数, 以其作为依据。如对M agge llan 公司的 GP S—O EM 接收机, 其定位精度完全由 PDO P 反映() 包括卫星测量误差, SA 影响, DO P 影响, 可根据其 PDO P 的大小决定 Β的取值。Β, Β的自 1 1 2 适应算法为 0. 99, PDO P ? 2 2PDO P , 2 < PDO P < 5 ƒ()9 , Β= Β= 1 - Β 1 2 1 1ƒPDO P , 5 ? PDO P < 10 12PDO P , 10 ? PDO Pƒ 对于R o ckw e ll 公司的N avca rd GP S 接收机, 其定位精度由PDO P 和FOM 两个参数反映, 这时 Β, Β的自适应算法为1 2 第 14 卷 第 5 期 房建成等: 车载 GP SƒDR ƒ地图匹配组合导航系统的自适应联合卡尔曼滤波模型 451 () () 54PDO P + 110FOM , ƒƒPDO P < 5 ()Β= Β= Β10 , 1 - 1 2 1 () () PDO P ? 51ƒ2PDO P + 1ƒ20FOM , 5 计算机仿真结果 s 的速度沿 45?航向匀速运动, GP S 定位数据在静态实测数据的基 2 m ƒ 假定车辆以 10 础上仿真得到。采用上述自适应联合卡尔曼滤波算法, 进行 GP SƒD R 组合导航系统动态定位 的最优综合, 计算机仿真结果如图 1 所示。图中虚线为原始 GP S 定位误差, 实线为组合导航系 统经自适应联合卡尔曼滤波器最优综合后的定位误差。可以看出, 采用自适应联合卡尔曼滤波 2 器的 GP SƒD R 组合导航系统定位精度优于采用集中滤波模型的定位精度。 图 1 计算机半物理仿真结果 联合卡尔曼滤波器的实际应用研究时间不长, 仍有不少问题需要进一步探索。将其应用于 车辆 GP SD R 组合导航系统是一种新的尝试, 对于如何提高滤波器的容错性能和精度以及系 ƒ 统鲁棒性等问题, 还需要进一步研究。 参考文献 1 周宏仁, 敬忠良, 王培德, 等. 机动目标追踪. 北京: 国防工业出版社, 1991 , . . ,2 R am ja t tan A N G ro ss P AA Ka lm an f ilte r m o de l fo r an in teg ra ted land veh ic le nav iga t io n sy stemJ o f N av iga t io n ( ) 1995, 48 2: 293- 302 , . . , 1994, 41 3 C a r lso n N A B e ra rducc i M PF ede ra ted Ka lm an f ilte r sim u la t io n re su lt sJ o f th e In st itu te o f N av iga t io n( ) 3: 297- 321 , , . , Gao YK rak iw sk y E J A bo u sa lem M A Com p a r iso n and ana ly sis o f cen t ra lizeddecen t ra lized and fede ra ted f il4 et a l2 ( ) . , 1993, 40 1: 69- 86te r sJ o f th e In st itu te o f N av iga t io n ( ) 房建成. 申功勋, 万德钧, 等. 市区车辆导航监控与交通管理信息系统研究. 中国公路学报, 1997, 10 3: 94- 100 5 ( ) 房建成, 万德钧. 组合导航系统在车辆导航中的应用. 东南大学学报, 1996, 26 3: 96- 102GP S 6 ƒƒAn A da p t ive Federa ted Ka lm an F ilter M ode l f or GPSD RM a p M a tch in g In tegra ted Na v iga t ion Sy stem in L an d Veh ic le F a n g J ia n ch en g , S h en G on g x u n W a n D ej u n ()B e ijing U n ive r sity o f A e ro nau t ic s & A st ro nau t ic s ()So u th ea st U n ive r sity ƒƒA bstrac t A fede ra ted Ka lm an f ilte r m ech an iza t io n to rea lize th e op t im a l syn th e sis o f GP SD R M ap M a tch2 452 控 制与决策1 9 9 9 年 ing in teg ra ted land nav iga t io n sy stem is f ir st ly de signed, and it s a lgo r ithm is p re sen ted a lso. M o reo ve r, an . adap t ive fede ra ted Ka lm an f ilte r m ech an iza t io n and it s a lgo r ithm a re c rea t ive ly p ropo sed fo r th is sy stemT h e , th eo re t ica l ana ly sis and com p u te r sim u la t io n re su lt s bo th show th a tby u sing th is adap t ive fede ra ted Ka lm an f ilte r, th e po sit io n ing accu racy and fau lt- to le rance cap ab ility o f th e GP SD R M ap M a tch ing in teg ra ted nav i2 ƒƒ .ga t io n sy stem can be g rea t ly im p ro ved , , , , Key words GP Sdead recko nn ingin teg ra ted nav iga t io nKa lm an f ilte r ingadap t ive a lgo r ithm 作 者 简 介 房建成 1965 年生。1996 年在东南大学获工学博士学位, 现为北京航空航天大学博士后研究人员。研究方向为定位导 航与测控技术, 最优估计理论与应用, 多传感器信息融合技术。 申功勋 1935 年生。1958 年毕业于北京航空航天大学, 现为北京航空航天大学教授, 博士生导师。研究方向为定位导航 与测控技术, 光电技术与精密测量, 精密机械与计算机辅助工程。 万德钧 1932 年生。1953 年毕业于南京工学院, 现为东南大学教授, 博士生导师。研究方向为定位导航与测控技术, 系 统建模与仿真, 精密机械与计算机辅助工程。