组合GPS_测距测向仪车载导航和卡尔曼滤波

组合GPS_测距测向仪车载导航和卡尔曼滤波 组合 GPS/ 测距测向仪车载导航和卡尔曼滤波 周宁徐进 ( 南京大学天文系 , 南京 , 210093) 摘要 讨论了 GPS 单点伪距定位的特点 ,然后利用测距测向仪来弥补 GPS 导航的 不足以及提高导航精度 ,并给出了确定初值的一种迭代算法 ,最后提出了八状态的 卡尔曼滤波器 。 GPS 定位 测距测向仪 卡尔曼滤波关键词 分类号 TN965 1 引言 在车载 GPS 导航中 ,应用伪距差分能获得优于 100 m 的定位精度 ,在条件许可的地 方 ,这当然 是 首 选 的 定 位 方 法 。但 仅 就 区 域 差 分 网 而 言 , 需 要 建 立 覆 盖 面 积 达 50 , 1 100 km的差分台 ,并要求运动载体与差分台之间建立起实时数据传输 。鉴于中国现有的 2 状况 ,并不易推广 ,因而如何利用好车载单点定位技术 ,并结合其它导航方法,是很有实 用价值的课 。 2 GPS , 测距测向仪的集成 我们将只讨论 GPS 伪距定位方法 。载波相位动态实时测量存在模糊度求解 ,以及周跳等问题 ,不易实现 ,因此 ,现在普遍仍使用伪距测量来实现实时导航 。 历元 T 时伪距观测量为 : j j j j j Δ ε(δ)( )δδ+ + `p = p+ C 〃t - C 〃t + E+ p 1 io n + t rop i i i i j j i 为观测站 、j 为卫星号 、p为观测站 i 到卫星 j 的几何距离 、`e为观测站 i 到卫星 j 的伪距观 i i j j j δ(δ)δε测量 、t ,t 为接收机钟差 ; 星钟差 、E为星历差 、p 为大气折射改正 、为白噪声 i i io n + t rop Δ 包括相对论效应 ,潮汐改正 ,多路径误差等 ,在精度要求不高的测量中 ,不对它们建 3 ε模 ,而与 一起作噪声处理 。在 SA 政策实施以后 , GPS 单点定位精度降为 100 m。 由于单 独的 GPS 已无法满足导航定位的需要 ,人们纷纷寻求组合的导航方式 ,以达到 精度要求 。对于车载导航来说 ,测距测向仪应用较为广泛 ,主要是因为其低廉的价格 ,几百元的设备即可满足要求 ,而其他导航系统 ,如惯性导航系统 IN S 、Lo ran2C 、Omigar 等 ,虽然 其精度和稳定性都较好 ,但设备相对复杂 ,价格也较贵 ,用于一般的车载导航显得成本过高 , 2000 - 03 - 20 收到原稿 ,2000 - 05 - 26 收到修改稿 这些导航系统一般用于航空及航天 。严格说来 ,测距测向仪并不属于一种自主的导航系统 , 但作为一种辅助设备 ,它能够满足车载导航的要求 。测距测向仪主要包括测向装臵和测距 () 装臵 ,测向装臵 类似于电子罗盘,其价格不高于几百元 ,测距装臵则类似于车辆上的计程 4() 器 ,构造及为简单 ,甚至可以自制 。测距测向仪主要采用航迹推算法 D R来辅助导航 ,可 通过简单的迭代实现 : _ n _ Σγ( )γ= 2 ni = 0 图 1 测距测向仪测量结果示意图 ()注 :图 、文中的矢量用上标小箭头示 ( 作为导航系统 , GPS 能实时给出目标点位的三维地心坐标 ,且其定位结果 在一定程度 ) 上独立 ,定位误差不随时间累积 ,但易受障碍物的影响 ,导致接收机对卫星失锁 ,从而使定 位结果不连续 。而测距测向仪进行航迹推算时则连续性好 ,定位结果前后相关 。其误差将随时间不断增大 ,但测得的坐标增量较准 ,由此可得 , GPS 能以相对独立且稳定的精度改正 测距测向仪结果 ,并给定其初始值 ;相反 ,测距测向仪能在 GPS 卫星失锁时 ,提高定位精度 , 对于我们 10 秒的采样间隔 ,测距测向仪和 GPS 采用了非耦合模式 ,即当 GPS 工作正常时 , 定位结果仅由 GPS 确定 ;测距测向仪基本处于待机状态 ,其定位结果不被采用 ,只根据 GPS 的结果来不断矫正测距测向仪自身的累计偏差 ; 而当 GPS 失锁时或信号不正常时 ,测距测 向仪进入工作状态 。 初值迭代3 在车载导航中 ,初始值的精确确定尤为重要 ,它将影响后续卡尔曼滤波的稳定性 。若初值不好 ,将导致滤波发散 。下面给出的迭代算法 ,利用矢量数字地图的数据进行地图匹配 , 得出精确的初值 ,从而能进行下一步滤波 ,文献 5 中给出了零状态卡尔曼滤波结果与确定 初值后卡尔曼滤波结果的对比 ,可看出后者能较快收敛 ,且稳定性较好 。 6 而初值点位的计算由下式可得: T - 1 T ( )ρ3 X = G 〃G ] 〃G 〃[ A 〃S - `] u u u u _ 0 ρ0 1- 1 l m n 111 l i_ 0 _ 〃 ρ0 2 (ρ称为几何阵 其中 G=, A = m 为 = u iiu 〃〃 n i 〃0 〃 x y ) 用户到卫星 i 的单位矢量, 只与卫星和用户之间的空间构造有关 ; X= 称为 z δC 〃t _ γ 1 X i _ 〃Y (γ用户状态阵 ,为所求未知量 ; S =为卫星状态阵 为卫星 i 的地心坐i = i 〃Z i 〃 ) 标 . ; ρ` i 〃ρ( ) = `为测量阵 伪距测量量; 对上式求解得到未知量 X 。 〃 〃 然后即可进行迭代 ,将式中 x , y , z 用 ΔΔΔ代替x + x , y + y , z + z 0 0 0 ( ) (ΔΔΔ) x , y , z 是一坐标初始估值 , 求出新的状态变量 x ,y ,z , 如此迭代下去 , 直至 0 0 0 εε X - X 〈为止 ,为一预设值 。然后可进行地图匹配 , 设 X 为 GPS 所测初值 ,由于数字n n - 1 0 0 0 矢量地图的精度优于 10 m ,而且在车载导航这个特定应用中 ,流动载体的航迹始终限制在道 (路以内 ,由此我们可以推想 ,可通过一定算法 ,将其坐标作为所测点真值的近似 详细的地图 ) 匹配算法并非本文重点,若迭代后所求点位坐标不在道路上 ,可经过地图匹配算法得出未 知点位的最佳估值 。整个过程属于静态初始化过程 ,基于数字矢量地图的精度 ,我们认为此 初值算法的精度比普通 GPS 应用中的初值算法更高 。 4 卡尔曼滤波 卡尔曼滤波器是线性 、无偏 、最小方差估计器 ,它在处理动态观测数据时以其计算量少及实时性在很多方面得以应用 。在 GPS 导航定位的数据处理中 ,采用卡尔曼滤波 ,能有效 地消除系统的动态噪声和测量噪声 ,对比逐点解算法或多项式拟合法 ,它的动态性更好 ,能 根据新的观测资料不断计算更新系统的统计特性 ,取得较高的导航定位精度 ,所在 GPS 动 7 ,8 态应用中 ,已经成为一种标准的滤波器,我们下面讨论的是扩展的卡尔曼滤波器() 经过了线形化。 根据汽车动态模型 ,假定加速度为零均值的白噪声过程 ,本文提出了八状态卡尔曼滤波 器 ,即状态矩阵为 : Tδδ) ( )( X = x , y , z , ?x , ?y , ?z ,t ,?t 4 由于在 GPS 动态测量中 ,数据是离散采集的 ,故其时间离散动态模型为 : ^ ^ ( )X =A X + W 5 k k , k - 1 k - 1 k - 1 ^ ( X 为 k 时刻的状态估值 、A 为系统转移矩阵 、W 为系统噪声 这里主要是加速 k k , k - 1 k - 1 ) 度扰动。 在此模型下 ,转移矩阵可表示如下 : Δ1 0 0 t 0 0 0 0 Δ0 0 1 0 0 0 0 t 0 0 1 0 0 Δ 0 0 t 0 0 0 0 1 0 0 0 Δt 为采样间隔 。( ) 6 A = k , k - 1 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 Δ 0 0 0 0 0 0 1 t 0 0 0 0 0 0 0 1 8 ×8 在这里我们假设了接收机钟差对时间的二次微商为零均值的随机扰动 ,而测量方程 : ( )Y = C〃X + V 7 k k k k Y 为观测量矩阵 、C为观测矩阵 、V 为测量噪声 。k k k 在 GPS/ 测距测向仪导航中 ,分别就伪距及 D R 算法建立相应测量方程 ,用泰勒级数展 开 ,并线性化 ,可得到标准式 。 () 首先来看伪距的测量方程 ,在前一节初值迭代中 3式已给出了其解的方程 : T - 1 T ρX = G 〃G ] 〃G 〃[ A 〃S - `] u u u u 但是上式适用于初值计算 ,而对于后续观测值解算 ,下面给出一常用解法 : 伪距方程为 : ρρδ ( )`= + c 〃t8 i i 1 2 2 2 2 ρ( ) ( ) ) ( ) δ( )] = X - x + Y - y + Z- z ] + c 〃t 9 `i i i i ( ) 在 x , y , z 附近展开 0 0 0 ) Δ) Δ) Δ( ( ( - X - x x + Y - y y + Z- z z ] i 0 i 0 i 0 ρ( )ρδ] =+ c 〃t + 10 ` i ioρ io 1 2 2 2 2ρ( ) ( ) ( ) 其中= [ X - x + Y - y+ Z- z ] io i 0 i 0 i 0 整理推出观测方程 : ( )ρ11 Y = C〃X + 1 1 1 0 x - X y - Y z - Z 0 10 10 1c ρρρ 101010 x - X y - Y z - Z 0 20 20 2c ρρρ 202020T (ρρρρ) Y = `,`,`,`, C= 1 1 2 3 4 1 x - X y - Y z - Z 0 30 30 3c ρρρ 303030 x - X y - Y z - Z 0 40 40 4c ρρρ 404040 T TΔΔΔδ() ρρρρ) (X = x ,y ,z ,t , ρ= ,,, 1 10 20 30 40 0〃 ? ρ再来推导伪距变化率的观测方程 : 〃? ρδρ= + c 〃t?? i i ) ( ) ( ) ( ) ( ) ( ) ( [ X - x X?- ?x + Y - y ?Y - ?y + Z- z ?Z- ?z ] i i i i i i ( )δ= + c 〃t 12 ?ρ i 1 2 2 2 2 ρ( ) ( ( ) ) 此处= [ X - x + Y - y+ Z- z ] 是一已知量 。解之 , i i i i ] ( )Y = C〃X - U 13 2 2 2 0 x - X y - Y z - Z 111c ρρρ 111 x - X y - Y z - Z 222〃 〃 〃 〃 c ? ? ? ?ρρρ 222T (ρρρρ) = Y = ,,,, C 2 1 2 3 4 2 x - X y - Y z - Z 333c ρρρ 333 x - X y - Y z - Z 444c ρρρ 444 〃 〃 〃 z - Z x - X y - Y 111X + + ZY 111ρ ρρ 111 〃 〃 〃 z - Z x - X y - Y 222+ X Y + Z 222 〃 〃 〃 〃ρ ρρ 222T ( δ) X = x , y , z ,t , U = 2 0 〃 〃 〃 z - Z x - X y - Y 333X Y + Z + 333ρ ρρ 333 〃 〃 〃 x - X y - Y z - Z444X Y + Z + 444 ρρρ 444最后来推导测距测向仪观测量方程 ,由 : n _ _ _ _ _ γγΔγγγ( )= , ] = + 14 i n n - 1 n? i = 1 可知 ,测距测向仪的航迹推算是逐点推算 。由此 ,为了方便计算 ,我们建立一个站心坐 标系 o - x y z , 对于 i 时刻 , 其原点即 i - 1 时刻的位臵 , x 轴指向当地的子午线方向 , z轴则指向天顶 , 这是一个左旋直角坐标系 , y 轴指向东点方向 。也即是就每一点位 , 都分别 建立一个站心坐标系 。 ( θ) θ 测距测向仪观测量为 d ,, d 为测距仪所测得的行走距离 ,为测向仪所测得的方位( ) ( 角 由 x 轴起算 , 顺时方向为正, 对于在 o - x y z 坐标系内 , z 方向的量对一般的城市车 ) 载导航应用影响不大 , 故略去 。有 : θx = d 〃co s ( )] 15 θ y = d 〃sin 然后我们来考虑上述坐标量改变对 W GS - 84 坐标值的影响 。其实只需作一下坐标变 ( ) (ΔΔΔ) 化即可 。由 o - x y z 的 x , y 变化到 W GS - 84 的 x ,y ,z , 由于 o - x y z 是一 , z 轴进行坐标旋转即可 。对于坐标原点个左手系 , 需先乘一个反矩阵 P, 然后分别沿 y x ( ) X 的经纬度 B , L , 有 :i - 1 i - 1 i - 1 Δ - 1 0 xx Δ) ( ) ( ( )y 90?〃= R - L 〃R B -1 〃 y16 z i - 1 y i - 1 Δ0 1 0 z 展开 , 有 : θ - d 〃co s co s L sin B sin L - L co s B co s i - 1 i - 1i - 1i - 1i - 1 Δx θd 〃sin sin L sin B co s L Δsin L co s B y ( )= 17 i - 1 i - 1i - 1i - 1i - 1 Δz - co s B 0 sin B i - 1i - 10 可写为 : Δθ θx = f = P〃d 〃co s+ P〃d 〃sin x x 1 x 2 Δθ θ()y = f =P〃d 〃co s+ P〃d 〃sin18 y y1 y2 Δθz = f = P〃d 〃co s z z 1 ( ) 式中各系数由 17式可得 。 ( θ) ( ) 然后于 d ,= 0 , 0处展开 , 取一次项 , ] 0 9f 9f xxΔx δθδ= d 〃+ 〃 θ9d 9 0 0 9f 9f yyΔδθδy = d 〃+ 〃 ( )19 θ9d 9 0 0 9f 9f zzΔδθδz = d 〃+ 〃 θ9d 9 0 0 ( ) 而 B , L 可由 W GS - 84 坐标转换得出 。 i - 1 i - 1 整理得方程 : X = G〃 Y 1 3 3( )20 T TΔΔΔδθδ) (δ) (X = x ,y ,z , C 〃t , Y = d , 1 3 9f 9f xx θ9d 9 0 0 9f 9f yy θ9d 9 0 0 C= 3 9f 9f zz θ9d 9 0 0 0 0 上式可写为观测方程 : - 1 ( )21 Y = C 〃X 3 3 3 ( ) ( ) ( ) 到此 , 将方程 11, 13, 21合并 , 得总的测量方程 : P〃C0 ρP〃Y P〃G 1 G 1G 0 X 10 C Y 〃( )2= + - U 22 20X 28 ×1 - 1 P〃 Y 0 0 P〃C1 3 1 3×1 10 10 ×1 10 ×8 其中 P, P分别代表了伪距定位和测距测向仪推算的权 , 可以在程序中进行实时控 G 1 制 。 [ 9 ] 然后在确定初值的情况下 , 即可进行滤波了。 )滤波估计a ^ ^ ^ ()23 X = K[ Y - k A X +k k CA X ]k , k - 1 k - 1 k k , k - 1 k - 1 ^ 其中 K为滤波增益阵 、X 为 X 的估计 。 k k k ) b滤波增益计算 : T ( )24 P =A PA + Q k k k - 1 k k - 1 Q 为系统噪声协方差阵 。 k - 1 滤波增益 T T ( )K= P C [ C P C + R ] 25 k k k k k k k R 为测量噪声协方差阵 。 k ) c计算滤波协方差阵 ( )P= P - K〃C P 26 k k k k 由上看出 , 卡尔曼滤波是一个不断预测 , 修正的迭代过程 , 由状态方程 , 测量方程 、三步 ( ) ( ) 计算 a , b , c 及方程 23, 26组成 , 下面给出其简图 : 图 2 卡尔曼滤波测量简图 结束语5 卡尔曼滤波用于 GPS 伪距导航定位和测距测向导航定位时 ,能提高定位精度 ,达到车载导航的要求 ,而且容易实现 。在我国目前还无法普遍建立 GPS 差分站的情况下 ,有很大 的应用价值 。在南京大学 GPS 中心的 GPS 车载导航及矢量地图课题中 ,我们现在用的是 Micro Tracker L P GPS 接收机和测向仪进行车载导航实验 ,在以后的工作中 ,将装上测距 仪 ,进一步改进导航效果 。对于卡尔曼滤波 ,我们已经用 801C51 单片机进行了初步实验 , 实际滤波效果将有待实测数据的检验 。 参考文献 Kee C ,et al . Wide2area Differential GPS as a Fut ure Navigatio n Systen in t he U S. PL AN’94 Co nference .1 Benjamin Peterso n , Richard Hart net t . Integrated GPS/ L ORAN St ruct ures and Issues. Jo urnal of The Instit ute of Naviga2 2 tio n Fall 1998 . 3 Feigl , K , King , R , et al . A Scheme fo r Reducing t he Effect of Selective Availabilit y o n Precise Geo detic Measurement s ( ) f ro m t he Glo bal Po sitio n System. Geop hysical Research L et ters ,1991 ,18 7 :,1289,1292 ( ) 邓幼俊. 一种提高车载单频 GPS 实时定位精度的方法. 陕西天文台台刊 , 1999 ,22 2 :,1284 K. Peerghz Rao ,J . L akshmi/ varatana . An App roach fo r a Faster GPS Tracking Extended Kalman Fit ter . Navigatio n Win2 5 ter 1995 刘基余等. 全球定位系统原理及其应用. 测绘出版社 ,1993 6 ( ) 董绪荣 ,陶大欣. 一个快速 Kalman 滤波方法及其在 GPS 动态数据处理中的应用. 测绘学报 ,1997 ,26 3 :陈小7 ( ) 明 ,刘基余 ,李德仁. O TF 方法及其在 GPS 辅助航空摄影测量数据处理中的应用. 测绘学报 ,1997 ,26 2 :蒋志8 凯. 数学滤波与卡尔曼滤波. 中国科技出版社 , 9 Integrated GPS/ Direction and Distance Surver Set Navigation and Kal man Filter ZHO U Ni ng XU J i n ()Depart ment of Ast ro no my ,Nanjing U niversit y ,Nanjing ,210093 Abstract The navigatio n via integrated GPS/ directio n and distance surver set is mainly discussed in t his paper . An initial solutio n algo rit hm to reduce t he po ssibilit y of diff usio n of o riginal Kalman filter and a 82state extended Kalman filter have been p ropo sed by t he aut ho rs ,and t he linearized measurement and dynamic equatio n have been derived in t his paper . Key words GPS ; D irect ion and D istance Surver Set ; Kal man Filter