卡尔曼滤波实现车载GPS_DR组合导航

卡尔曼滤波实现车载GPS_DR组合导航 卡尔曼滤波实现车G载P S /D R 组合导航 1 ,2 ,3 1 ,2 ,3 1 孙希延,纪元法,施浒立 (1 . 中国科学院 国家天文台 北京 100012 ;2 . 桂林电子工业学院 广西 桂林 541004 ; )3 . 中国科学院 研究生院 北京 100039 摘 要 :陆地车辆是在二维帄面内运动的 ,而在车载 GPS 接收机和 D R 组合装置中 ,测量值是在不同的坐标系中得到 的 ,通过对坐标系转换和测量误差分析得到组合系统的整体状态方程和测量方程 ,由于这些状态方程和测量方程都是关于 状态的非线性函数 ,用广义卡尔曼滤波器来实现 GPS/ D R 组合导航的实时求解 。最后在行驶车辆上采集数据并进行仿真试 验 ,结果明这种组合系统能够大大提高系统实时定位精度和容错能力 。 ) 广义卡尔曼滤波 ;组合导航 (recko ning;关键词 : GPS ;D R Dead () 中图分类号 : TN927文章编号 :1004 373 X 200611 149 04 文献标识码 :B Real ization f or the GPS/ D R Integrated Navigation System of Vehicle with Kal man Filter 1 ,2 ,3 1 ,2 ,3 1SU N Xiya n,J I Yua nf a, S H I H uli (1 . Natio nal A st ro no mical O b se rvato ry ,Chine se Academy of Science s ,Beijing ,100012 , China ; )2 . Guilin In stit ute of Elect ro nic Technolo gy , Guilin ,541004 ,China ;3 . Graduate School ,Chine se Academy of Science s ,Beijing ,100039 , China Ab stract : In t hi s pap er ,t he state equatio ns a nd mea surement equatio ns a re p ro vided t hro ugh t he a nal ysi s t he GPS/ D R in2 t egrated navigatio n system of vehicle ,coo r dinate t ranslatio n and t he a nalysi s of mea surement er ro r s. Then ,t ho se equatio ns a re ( ) solved wit h t he Extended Kal ma n Filter E KFin real time beca use t ho se state a nd mea surement equatio ns a re all no nlinea r . At la st ,t he si mulated exp eri ment i s do ne wit h t he dat a collected f ro m t he r unning vehicle . Co mp uter simulatio n re sult s sho w t hat ,t he p reci sio n a nd f a ult tolera nce of t he integrated navigatio n system a re imp ro ved greatly. () ( ) Ke ywords : GPS ;D R Dead recko ning; Extended Kal man Filter E KF;integrated navigatio n 在 GPS/ D R 组合导航系统中利用卡尔曼滤波去估计系统 引 言1 的各种状态 , 并用状态的估计值去校正系统 , 以达到系统 组合的目的 ,如图 1 所示 。 ( ) D R Dead recko ni ng是一种低成本的导航系统 , 与 GPS 相比 DR 最大的优点是成本低 ,能够实现实时完全自 ( 主导航 ,能够提供多种短时间内较高精度的导航参数 位 ) 置 、速度 、姿态,抗电子辐射干扰能力强 、隐蔽性好 。但由 于在生产装配校准初始对准的过程中 ,惯性器件的误差不 可能完全消除 , 加之由于材料物理性能的不稳定 , 也会在 图 1 卡尔曼滤波实现组合导航示意图 使用中使惯性器件产生误差 。因此 , D R 的最大缺点是误 差随时间的积累 ,不适合长时间的单独导航 。GPS 导航精 2 坐标系 度高 、功能强 、使用方便 , 但他应用于车辆导航时 , 当车辆 [ 2 ] 在树荫下 、涵洞 、隧道里 、深山峡谷内 , 特别在城市里行驶 下面是本文后面要用到的坐标系 : 时 ,由于鳞次栉比的高楼大厦会挡住卫星的信号 , 使卫星 ( ) 1W GS 84 大地坐标系导航接收机不能正常接收卫星信号 ,不能正常定位 。所以 W GS 84 大地坐标系是一个地心 、地固坐标系 。这 采用组合 GPS 和 D R 于一体的组合导航系统 。 个坐标 系 应 用 了 大 地 坐 标 和 W GS 84 的 椭 球 模 型 , 在 GPS/ D R 组合导航中如果 GPS 信号被遮挡就依靠 W GS 84 坐标系的本初子午线通过格林威治天文台 。 D R 系统 ,当 DR 系统积累误差较大时 , 就利用 GPS 的精 ( ) 2载体坐标系 确的定位数据进行校正和标定 。因此实行 GPS/ D R 组合 载体坐标系的原点在载体的质心 ,X 轴与载体运动 b 取长补短 ,能够实现较高的载体实时导航定位并获得高性( ) [ 1 ] 方向的中心线 主轴重合 , 正向指向载体的运动方向 , Y b 能精度 ,具有重大的意义。 轴垂直 X 轴指向载体右侧 , Z轴与 X 和 Y 垂直正交 , 而 b b b b 卡尔曼滤波在 GPS 和 DR 组合中有卓有成效的应用 。 且构成右手坐标系 。 ( ) 3当地水帄坐标系或惯性直角坐标系 他是一种站 心直角坐标系 , 他的原点与载体坐标系的 收稿日期 :2006 02 15 原点重合 , 以此消除坐标原点偏移 , 他的 N 轴指向当地北 子午线 , E 轴与 N 轴垂直指向东 , Z 轴与 E 轴和 N 轴正交 , ,其离散方程可以写成 : 得到 ? 并构成右手坐标系 。Ψ( )Ψ( ) Ψ( )Δ k + 1 = k+ kT + wΨ ? ? )( 5 ? Ψ( )3 GPS/ D R 组合系统的状态方程k + 1 Ψ( ) = k+ w Ψ? ? ΨΔ 式中 , 是角速率 ;T 是采样周期 ; w, w是高斯白噪声 。 Ψ Ψ3 . 1 GPS 系统的状态方程 第二个坐标转换是从当地水帄坐标系到大地坐标系 因陆地车辆是在二维帄面内运动 , GPS 状态可以取位 的转换 :(φλ) φλ置 ,,为 W GS 84 大地坐标系下的纬度 ;为 W GS ? φ 84 大地坐标系下的经度 。则离散状态方程可以描述为 : v NN ? = C cφ( ) φ( ) ωk + 1= k+ φ λ v E ( )1 λ( ) λ( ) ωk + 1= k+ λ 1 0 R + h v m Nωω 式中 , φ ,λ 为高斯白噪声 ; k 为当前时刻 。 ( )= 6 1v E 3 . 2 D R 系统的状态方程0 φ ( ) R + hco sn D R 系统最大的优点是他的输出数据具有连续性 , 如 N c , C是从当地水帄坐标系到大地坐标系的转换矩阵 ,式中 果已知起点和位移就能计算得到任 意 时 刻载 体 的位 置 。 R 为 地 球 参 考 椭 球 子 午 圈 上 各 点 的 曲 率 半 径 , R = m m 一个普通的 D R 系统就是一个廉价的惯导系统 ,他是通过 2( φ ) R1 - 2 f + 3 f si n ; R 为地球参考椭球卯酉圈上各点 e m n 车辆的脉冲计数和陀螺仪实现车辆定位定姿的 。2( 的曲率半径 , R = R 1 + f si n φ ) ; R= 6 378 137 m ; f = n e m e φ1/ 2981 257 。为帄均纬度 ; h 为椭球高度 。 联列上式 : ? 1 Ψco s φ = v R + h m ( )7 ?φ secm λ Ψsi n = v R + h n 除了陀螺的偏移量误差 ,把其他陀螺误差认为是高斯 图 2 当地水帄坐标系到大地坐标系的转换 白噪声 , 陀螺的偏移变化很慢 , 可把他看作是随机游走过 如图 2 所示 ,D R 系统是基于对速度的积分而得到车 程 ,他是白噪声积累的结果 ,则陀螺的偏移方程可写成 : 辆运行的距离和航向角 。输出的数值是连续的 ,但 DR 系 ? ? ? 统要求预先知道车辆的起点和初始航向角 ,他才能实现实 ΔΨ( ) ΔΨ( )ω( 8 ) k + 1= k + Ψ? 时的相对定位 。DR 系统输出的速度 v 是沿载体坐标系的 ? ΔΨ ω其中 , 是偏航角偏移率 ;Ψ 是高斯白噪声 。 X 轴 。要得到在当地水帄坐标系中的坐标要运用一个转 b D R 系统定位是基于速度和角速度积分得到的 , 在本 c [ 3 ] 文中车辆速度是脉冲数乘一个常数得到 ,这个常数与车轮 变矩阵 C, 从载体坐标系到当地水帄坐标系的转换。b 胎的状况有关 。由于车辆轮胎半径的不确定以及测量噪 声和量化等问题使测得的速度存在误差 ,其中轮胎半径的 c bc ( )= Cv 2 vb 不确定性使比例因子 S 有误差 ,其误差模型也可以表示为 则 : 随机游走过程 。c T T T (Ψ) (β) (α) C= CCCb Z Y X ( )) ( ) ω( 9 S k + 1= S k+ SβΨ αΨββΨαΨ αβΨco sco s - sin sin sin - co ssin sin co s + co s sin sin ω 其中 , S 是高斯白噪声 。αΨ αβΨαΨ αβΨβΨ= sin sin - co s sin co s co s sin + sin sin co s co sco s 3 . 3 GPS/ D R 组合系统的整体状态方程 αββαβ- sin co s sin co s co s 综上可以得到组合系统的整体状态方程 :( ) 3 1 因为车辆在城市公路上行驶 ,可以认为车辆是在二维 帄面φ( )φ( ) ΨΔ k + 1 co sT + w = k+ vφR + h m βα上行驶 ,即假设俯仰角和横滚角很小 ,可以省略 , φsec λ( )λ( ) ΨΔ k + 1 = k+ v si nT + wλc [ 4 ] R + h 则转换矩阵 C可以改写成: n b ? ΨΨco s - si n c Ψ( ) Ψ( ) Ψ( )Δ k + 1= k+ kT + wΨ C= b ( )10 ΨΨco s si n ? ? .? Ψ( )Ψ( )k + 1 = k + wΨ Ψ Ψ- si n v co s v N ?? ( )4 = ? ( ) ΔΨ( ) ΔΨk + 1= k+ wΔΨ ΨΨco s si n 0 v E )( ( ) v k + 1 = v k+ w v式中 , v 和 v 分别表示在当地水帄坐标系下北向和东向 N E ( ) = S k ( )Ψ + w S k + 1 速度 ;表示航向角 , 此角是由陀螺所测得的角速率积分S ( ) x k +1 = f k x k + Gk w k4 GPS/ D R 组合系统的测量方程 ( )12 ( ) y = khk x k + ekG G φλGPS 接收机测得大地坐标系下的纬度 和经度, () ( ) ( ) 从式 12中可以看出函数 f x 和 hx 都是关于状态 k k- 1 k k 在 D R 系统依靠车辆轮胎的脉冲计数和压电陀螺测得车辆的非线性函数 ,不能直接利用卡尔曼滤波 ,所以应该先对其 ?D D Ψ的速度 v和角速度。则组合系统的测量方程表示为 : 进行线性化处理 。一般的线性化处理方法是 ,围绕状态的 G φ( ) φ( ) kk+ e= 1 标称值用泰勒级数展开 ,这样以来 ,如果标称值与车辆实际G λ( ) λ( ) k = k+ e2 的位置相差较大的话 ,在线性化处理中被省略掉的泰勒级 ( )11 ? ? D 数的高次部分就不是微小量 。误差在计算过程中就会随时 Ψ( ) Ψ( ) k= k+ e3 D 间而积累 ,造成最终结果不精确甚至会发生发散 。为解决( ) ( ) ( ) v k= v kS k+ e 4 ( 这个问题本文采用广义卡尔曼滤波 Extened Kal man Fil2 ?D ΨΔ 式中 , 是在两个 GPS 采样间隔 T 间的陀螺帄均角速 ) ter,就是在线性化过程中不再围绕标称值而是围绕估计值 D Δ 度 ; v 也是在T 时间内的帄均速率 。展开 ,这样线性化带来的误差就会减小 。过程如下 : ^^广义卡尔曼滤波( EKF)5 ( ) ( ) ( )f x λ f x + Fx - x k k k k - 1 k x k - 1 ( )13 ^^( ) ( )( )上面的状态方程和测量方程可以写成简化形式 : hx λhx + H x - x k k k k/ k - 1 k x k/ k - 1 ^ ^ Δ T vk - 1 Δ Ψ Tco s k - 1^ Ψ1 0 si n 0 0 0 - k/ k R + h R + h m m ^^ ^^ ^ ^^ Δ Ψ v Tsi n k - 1k - 1 Δ ΨφΔΨ v Tco s t an v Tco s k - 1k - 1 k - 1k - 1k - 11 0 0 0 ^ ^ ( φ) R + hco s n m( φ( φ) ) R + hco s R + hco s n k - 1 n k - 1 ( )5 f xk Δ 0 1 0 0 0 0 TF = = k^5 x x =x k - 10 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 0 ( )5 hxk H = = k^5 x x =x 0 0 0 1 0 0 k/ k- 1 0 ^0 0 0 0 0 S 0 k/ k - 1 ^^式中 , x 为 k - 1 时刻状态的最优估计值 ; x 为 k 时刻 k - 1 k/ k - 1 试验和模拟仿真6 状态的一步预测值 。则状态和测量方程变化为 : ^^ ( ) 1试验用到的仪器及其作用 ( ) ) ( x = Fx + f x - Fx+ Gw k k k - 1 k x - 1 k k - 1 k k 所用的车辆为 CO S T ER 。( )14 ^^ ( ))( y = hx H xH x + e - + k k k/ k - 1 k k/ k - 1 k k k( GPS 接收机用于解算车辆的大地坐标系下的位置 纬 ) 度 、经度,采样频率 1 Hz 。 式中 , G是 7 ×7 维 的 单 位 矩 阵 ; 系 统 噪 声 w 的 方 差 为 k k 压电陀螺用于测量车辆的航向角速率 , 计算公式为 : Q ; 测量噪声 e的方差为 R 。给定初值满足 :k k k ? 电压值 D ^Ψ () = 50 , 单位是 /? s。 - 21 51 ) ( Ex= x0 0 1 000 ×电压计数 D T ^ 车辆的速率的计算公式为 : v = 脉冲计数 ×01 39 , 单 )( ( )15 Exx 0 0 = P 0 位是 m/ s 。则滤波过程 : 模拟仿真数据如图 3 所示 。图 3 中虚线代表组合卡 ^^( )x = f x k/ k - 1 k k - 1 尔曼滤波值 ,实线代表 GPS 的测量值 。 ^^^( ( ) )x = x + Ky -hx k k/ k - 1 k k k k/ k - 1 ( ) 2模拟仿真数据T T - 1 ( )16 ( ) Kk = Pk/ k - 1 H k H k P k/ k - 1 H k + R k 在 0 , 120 s 这 段 时 间 内 GPS 接 收 机 接 收 到 4 颗 ) ( P= I - KH P k/ kk k k/ k - 1以上卫星 。 P= FPF+ GQG k/ k - 1 k - 1 k - 1 k - 1 k - 1 k - 1 k - 1 从图上看出 , 在卫星没有丢失的情况下 , GPS 定位精 ( ( ) )度与 GPS/ D R 组合系统定位精度相比 , 经度方向 图 a ( ( ) ) 相差较少 ,在纬度方向 图 bGPS/ D R 组合系统的定位 结果比 GPS 高 。GPS 这种定位结果是由于当时的卫星分 布的关系 。在 120,240 s 期间 , 卫星信号不稳定 , 不能同 时捕获 4 颗以上卫星 ,此时 GPS 输出定位结果与组合系统 输出结果有较大差别 , GPS 定位轨道明显偏离了原轨道 , 而组合系统仍然能够在实际轨道上继续定位导航 。 上述的试验结果表明 ,本文的 GPS/ D R 系统具有 高精度的实时导航系统 ,他克服了两个独立系统的各自的 缺点 。组合后的可靠性比单系统有了很大的提高 ,充分说 明卡尔曼滤波的组合导 航系 统 的 应用 价 值 和实 用 意 义 。 这将为现在导航系统提供一定的理论基础 。 参 考 文 献 [ 1 ] 王 惠 南 . GPS 导 航 原 理 与 应 用 [ M ] . 北 京 : 科 学 出 版 社 ,2003 . [ 2 ] Ro bert M Ro ger s. Applied Mat hematics i n Integrated Navi2 gatio n Systems [ M ] . Vir gi nia : A merica n Instit ute of Aero2 nautic s a nd A st ro na utics ,2003 . [ 3 ] 刘基余 . GPS 卫星导航定位原理与方法 [ M ] . 北京 : 科学出 版社 ,2003 . [ 4 ] A bd El Gelil , Ma hmo ud Salem I S. A n Integrated Po sitio2 ning System fo r t he To ro nto Tra nsit System. Ca nada : PhD , 图 3 模拟仿真数据 1998 . 56 60 . 作者简介 孙希延 女 ,1974 年出生 ,讲师 ,博士生 。研究方向为数据融合 、卫星导航 、信号处理 。 施浒立 1944 年出生 ,工学博士 、理学博士 ,研究员 ,博士生导师 ,中国电子学会机械工程分会副主任委员 。研究方向 为太阳射电物理和天文技术方法 、卫星导航 ,电子机械和机电一体化 。 ()上接第 148 页 接口 、CAN 总线接口 、u IP 接口等提供借鉴 。 6 结 语 参 考 文 献 本文设计的通用串行通信程序已经在 8 位 MCU 的 [ 1 ] 王宜怀 ,刘晓升 . 嵌入式应用技术基础教程 [ M ] . 北京 : 清华 GB60 , GP32 和 GZ60 , 16 位 MCU 的 D P256 , 大学出版社 ,2005 . [ 8 ] [ 9 ] M68 HC12B ,以及 32 位 MCU 的 2107 , CCM3118 上通 [ 2 ] Freescale . MC9 S08 GB60 Data Sheet ———MC9 S08 GB60/ D 过测试 。实践证明 , 该串行通信程序具有通用性 , 可方便 V11 0 ,2004 . 地移植到各种型号 MCU 的串行通信中 , 是嵌入式开发的 1 15 ,2005. Freescale. MC9S12DP256B Device U ser Guide V02[ 3 ] 一个通用部件 ,可为嵌入式开发带来便利 。本文的做法不 Free scale. MMC2107 Technical Data Rev. 21 0 ,2000 . [ 4 ] 仅可以为许多嵌入式开发者提供 SCI 底层编程的参考 , 也 [ 5 ] Free scale. MC68 HC908 GP32 HCMO S Micro co nt roller 包含一种符合嵌入式软件工程的开发规范 。嵌入式 MCU U nit Technical Data ,1999 . 应用系统的设计者 ,应尽可能分析各个 MCU 相同模块的 [ 6 ] Free scale. MC68 HC908 GZ60 Data Sheet ———MC68 HC908( 共性与个性 ,抽取共性 ,合理设计底层程序 类似于 PC 的 GZ60/ D V1 . 0 ,2004 . ) 通用驱动程序,为嵌入式开发积累软件部件 ,提高嵌入式 [ 7 ] 白 鹏 , 莫 为 东 . Vi sual Ba sic 61 0 高 级 编 程 技 巧 ———控 件 篇 软件开发的效率 。本文的方法还可以对开发通用的 SPI [ M ] . 北京 :机械工业出版社 ,1999 . [ 8 ] Free scale. M68 HC12B Family Dat a Sheet Rev. 9 ,2004 . [ 9 ] Free scale. CCM3118DQ A dva nce Info r matio n Rev1 . 0 ,2003 . 作者简介 王艳春 女 ,1971 年出生 ,安徽蚌埠人 ,讲师 ,硕士研究生 。主要研究方向为嵌入式系统应用 。 王宜怀 男 ,1962 年出生 ,教授 ,博士 ,硕士生导师 。主要研究方向为嵌入式系统应用 。