文章编号 :100623242(2004)0420052205基于广义卡尔曼滤波的伪距组合 GPS/ INS导航 3刘海颖 王惠南南京航空航天大学 ,南京 210016摘 要 提出了一种利用广义卡尔曼滤波进行 GPS/ INS组合导航的技术 ,同时给出了一种最优选星算法.采用间接的反馈校正设置 ,直接利用伪距数据和不受噪声污染的星历数据.利用该方法 ,组合导航精度高 ,在导航过程中若丢失 GPS 信息 ,短时间内单纯 INS的导航精度仍能保持.恢复 GPS 信号后组合系统继续正常工作.主题词 组合导航 广义卡尔曼滤波 INS GPS 选星算法中图分类号 :TN96712 文献标识码 :AThe GPS/ INS Pseudorange Integrated NavigationBased on Extend Kalman FilteringLiu Haiying Wang HuinanNanjing University of Aeronautics and Astronautics , Nanjing 210016Abstract Thispaperpresentsan extendKalmanfiltering(EKF) techniqueforGPS/ INSintegratednaviga2tion.Atthesametimeitgivesan efficiencymethodtofindtheoptimalcombinationofthesatellitesthatexam2ined. Itusesindirectfeedbackconfigurationwhichbasedon pseudorangedataandephemeriswithoutbeingaf2fectedby noise. Theintegratednavigationhashigheraccuracyusingthisalgorithm, andifGPS messageisin2terruptedabruptlyintheprocessofintegratednavigation, thepureINS canmaintaina relativehighnavigationaccuracyfora certaintime.When GPS messageisreturnedtonormalreceiving, theintegratedGPS/ INSnav2igationwillresumenormalworking.Subject terms Integratednavigation ExtendKalmanfiltering INS GPS Selectsatellitesalgorithm3 基金项目 教育部博士点基金项目(20030287005)收稿日期 20042032081 引 言全球定位系统 (GPS)和惯性导航系统 (INS)具有优势互补的特点.以适当的方法将两者结合起来成为一个组合导航系统 ,可以提高系统的整体导航精度及导航性能 ,使 INS具有空中再对准能力 ,同时GPS 在 INS信息的辅助下 ,也将大大改善捕获,跟踪和再捕获的能力 ,而且在可见星较少的情况下不致严重影响导航精度.由于 GPS/ INS组合导航系统总体性能远远优于各自独立的系统 ,所以是目前和今后海陆空导航和定位的理想系统.实现 GPS 和 INS的组合方案很多 ,不同组合方案可以满足使用者不同性能要求.卡尔曼滤波器在组合系统中有广泛的 2004年 8 月第 22卷 第 4 期航 天 控 制Aerospace ControlAug12004Vol122 ,No. 4? 1994-2006 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net应用 ,但要求系统噪声和量测噪声必须是零均值的白噪声 ,这在实践中很难满足.通常使用非线性滤波的"线性化"处理 ,然而精度低且不能保证计算稳定.本文采取广义卡尔曼滤波 ,直接利用 GPS 伪距原始数据和 INS预测伪距之差作为观测量 ,利用不受污染的星历数据进行选星和观测阵 H 的计算 ,给出了高效的算法计算 GDOP 值来进行选星.利用本方法大大提高了组合精度且保证计算的稳定性.2 GPS/ INS组合方案本文利用伪距进行 GPS/ INS组合 ,采用间接法闭环校正 ,组合导航精度高 ,且当观测信号不足时INS仍能提供精确的导航参数.其中 INS测量值所得的导航参数经过误差校正后 ,提供给卡尔曼滤波器以计算最优递推估值 Xk+ 1/k + 1和观测矩阵 H ,同时来预测与卫星 Sj之间的伪距ρIj.GPS 测量值ρGj与ρIj之差作为观测量.具体设计如图 1 所示.图 1 GPS/ INS组合导航设计3 组合导航设计本文构造了一个 8 维的卡尔曼滤波器 ,采用了递推选星算法 ,具体设计如下.311 组合系统的状态方程INS的误差状态方程形式如(1)式X.I(t) = FI(t) XI(t) + W I(t) (1)GPS 的误差状态在伪距组合中取两个与时间有关的误差 :钟差δt和钟差漂移率δtr.其微分方程如下δt.= δtr + ωt 和δt.= ωtr (2)将上式写成矩阵形式 ,即X.G (t) = FG (t) XG (t) + W G (t) (3)将 INS误差方程(1)式和 GPS 误差方程(3)式合并 ,则得组合系统状态方程.取状态变量X = [ x1 x2 x3 x4 x5 x6 x7 x8 ]其中 x1 x2 x3 分别为东北天方向的位置误差 ;x4 x5x6 分别为东北天方向的速度误差 ;x7 x8 为钟差和钟差漂移率.得x.1x.2x.3x.4x.5x.6x.7x.8=0 1 0 0 0 0 0 00 0 0 0 0 0 0 00 0 0 1 0 0 0 00 0 0 0 0 0 0 00 0 0 0 0 1 0 00 0 0 0 0 0 0 00 0 0 0 0 0 0 10 0 0 0 0 0 0 0x1x2x3x4x5x6x7x8+ω1ω2ω3ω4ω5ω6ω7ω8(4)(4)式右边最后一项是 INS和 GPS 接受机的噪声 ,可看做零均值的白噪声 ,协方差阵为 Q .对 (4)式连续形式离散化 ,得到测量方程 ,即Xk+1 = φkXk + W k (5)φk =1 T 0 0 0 0 0 00 1 0 0 0 0 0 00 0 1 T 0 0 0 00 0 0 1 0 0 0 00 0 0 0 1 T 0 00 0 0 0 0 1 0 00 0 0 0 0 0 1 T0 0 0 0 0 0 0 1W k =Tω1 + 0.5T2ω2Tω2Tω3 + 0.5T2ω4Tω4Tω5 + 0.5T2ω6Tω6Tω7 + 0.5T2ω8Tω8离散形式噪声协方差阵 Q d 为?35?第 22卷 第 4期 刘海颖等 :基于广义卡尔曼滤波的伪距组合 GPS/ INS导航? 1994-2006 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.netQ d =SpT33SpT220 0 0 0 0 0SpT22SpT 0 0 0 0 0 00 0SpT33SpT220 0 0 00 0SpT22SpT 0 0 0 00 0 0 0SvT33SvT220 00 0 0 0SvT22SvT 0 00 0 0 0 0 0 SfT +SgT33SgT220 0 0 0 0 0SgT22SgT 其中 Sp 为水平方向白噪声谱幅值 ;Sv 为竖直方向白噪声谱幅值 ;Sf 和 Sg 为钟差模型白噪声幅值.312 组合系统观测方程组合导航中 ,设 INS的位置为 (xI yI zI) ,由卫星星历确定的卫星 Sj位置为(xsi ysjzsj) ,则可以得到相应于 INS所在位置处的伪距ρIjρIj = [(xI - xsj)2+ (yI - ysj)2+ (zI - zsj)2]12(6)同时 ,载体上 GPS 接收机相对与卫星 Sj测得的伪距为ρGj.则伪距观测方程可以写成δρ = ρIj - ρGj (7)矩阵形式为δρ1δρ2δρ3δρ4=e11 e12 e13 1e21 e22 e23 1e31 e32 e33 1e41 e42 e43 1δxδyδzcδt(8)上式中(δxδyδz)是在空间坐标系的值 ,应通过下式转换到当地坐标系xyz=- sinλ cosλ 0- cosλsinρ - sinλsinρ cosρcosλcosρ sinλcosρ sinρδxδyδz(9)从而 ,所得观测方程为z1z2z3z4=e11 0 e12 0 e13 0 1 0e21 0 e22 0 e23 0 1 0e31 0 e32 0 e33 0 1 0e41 0 e42 0 e43 0 1 0x1x2x3x4x5x6x7x8+v1v2v3v4(10)上式右端最后一项为测量噪声 ,协方差为 R .将上式写为矩阵形式 ,得到观测方程Zk = HkXk + Vk (11)313 选星算法观测方程中只用到四颗星 ,所以在所有可见星中选取几何精度因子 GDOP 最小者为最佳.当有 n颗可见星时共有 C4n 种星座选择 ,当 n 较大时需要很大的计算量和存储量.本文应用递推选星算法大大加快了计算速度.几何精度因子定义为GDO Pn = [ Trace(HTnHn)-1]1/2(12)利用广义逆 ,上式可改写为GDO Pn = { Trace[H+n (H+n)T]}1/2(13)为了得到几何精度因子的递推关系 ,首先给出G+n 的递推关系.将矩阵 Gn + 1写为分块形式Gn+1 =GneTn+1式中 :en + 1 = [en + 1,1 en + 1,2 en + 1,3 ]T,由矩阵广义逆?45? 航 天 控 制 2004年? 1994-2006 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net性质可知G+n+1 = [ G+n - bn+1 dTn+1 bn+1] (14)式 中 : bn+1 =(cTn+1)+dTn+1 = eTn+1 G+n ,cn+1 ≠0(1 + dTn+1 dn+1)-1G+ndn+1 cn+1 = 0cTn+1 = eTn+1 - dTn+1 Gn从而有如下几何精度因子的递推关系 :当 n ≥4 时 GDO Pn+1 = [(GDO Pn)2- (1 +dTn+1 dn+1)bTn+1 bn+1]1/2(15)当 n < 4 时 Trace[G+n+1(G+n+1)T] =Trace[G+n (G+n)T] + (1 + dTn+1 dn+1)bTn+1 bn+1 (16)对于某时刻可见星 ,每一步的选星方法为 :(1)从最靠近天顶的一颗星开始 ;(2)转到高度角最小的一颗 ;(3)将第二步中卫星方位角分别增加 120°和240°,在这两方位角附近的高度角与第二步中高度角最接近的两星作为第三和第四步递推中的卫星 ;(4)按照剩余可见星高度角从大到小递推.按此方法第四步所得 GDOP 4 即为所求.314 组合导航计算流程图图 2 GPS/ INS组合导航计算流程4 实验结果及分析为了验证本文方法的有效性 ,我们做了静态仿真实验.其中 , GPS接收机采 用 Jupiter OEM板 ,在南航智能楼五楼进行了两个多小时的伪距等数据采集 ;与 GPS 采集所对应时刻的星历数据 ,采用国家大地测量网站(www. ngs. noaa. gov)以 EF18格式提供的数据 ,经转换为 SP3格式 ; INS数据由惯性测量模拟器产生 ;采用 Matlab语言进行仿真计算.仿真时间 2 h.由于篇幅所限 ,只给出了东向位置的仿真数据(图 3)及其结果 (图 4).其中东向位置初始误差值 X0/ 0取 0m ,即假设 INS初始对准 ,协方差 P0 取200m2.图 3 东向位置误差用本文的广义卡尔曼滤波进行组合导航的方法 ,其结果用来修正 INS的误差.误差越小其精度越高 ,而协方差表明了估值结果的正确性.从图中可看出 ,用此方法大大提高了导航精度.另外 ,从误差图中可以看到 ,在 3600s到 5400s之间内有两条脉冲 ,这是因为根据几何精度因子 GDOP 进行选星 ,其值过大或者某颗星不再可见 ,从而改变了观测星座.所以变换瞬间误差很大 , 这从协方差图中也可图 4 组合导航东向位置误差及协方差?55?第 22卷 第 4期 刘海颖等 :基于广义卡尔曼滤波的伪距组合 GPS/ INS导航? 1994-2006 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net看出 ,表明此刻结果不可信.但突变只是瞬间发生 ,很快就恢复 ,对 INS修正不影响.5 结 论本文提出了一种采用广义卡尔曼滤波 ,利用伪距进行 GPS/ INS组合导航的方法 ,并给出一种高效的选星算法.在计算几何精度因子 GDOP 和观测阵 H 时 ,直接利用星历数据 ,而不受各种噪声的影响 ,实际上许多型号的接收机都能得到广播星历数据 ;GPS观测量采用伪距原始数据 ,而不是接收机处理过的位置速度等数据 ,所以从根本上大大提高了导航精度.同时采用间接的,反馈校正卡尔曼滤波方法 ,对下一次进行滤波和预测伪距的数据校正 ,进一步改善滤波结果.而当 GPS 信号不足时 INS能继续提供精确的导航参数.虽然仿真实验是在静态下进行的 ,但利用的是误差数据 ,与实际动态中的情形是一致的.当然 ,本仿真模型中的最优估值只是反馈给了 INS测量数据以便滤波和预测伪距时使用 ,但在实际系统中如果反馈到 INS ,从根本上修正惯性系统内部设置 ,会有更佳的效果.参 考 文 献[1] 王惠南. GPS导航原理与应用[M].北京 :科学出版社 ,2003[2] 张其善 ,吴今培 ,杨东凯.智能车辆定位导航系统及应用[M].北京 :科学出版社 ,2002[作者简介]刘海颖(1980～) ,男 ,河南洛阳人 ,硕士研究生 ,研究方向 :导航定位,组合导航.王惠南(1946～) ,男 ,江苏南通人 ,博士生导师 ,教授 ,主要研究方向 :惯性技术,GPS 组合导航与光学纤维.(上接第 47页)计划期间 ,国家用信息化带动工业化的工作重点有三个方面 :一是以电子信息技术应用为重点 ,提高传统产业生产过程自动化,控制智能化和管理信息化水平 ;二是以先进制造技术应用为重点 ,推进制造业领域的优质高效生产 ,振兴装备制造业 ;三是改造提升重点产业的关键技术,共性技术及其相关配套技术水平,工艺和装备水平.国家实施高技术产业化的主要目标有两个 :一是发展高技术 ,形成新兴产业 ,培育新的增长点 ;二是利用先进技术改造和优化传统产业 ,提高经济增长的质量.由于大力发展工业自动化是加快传统产业改造升级,提高企业整体素质,提高国家整体国力,调整工业结构,迅速搞活大中型企业的有效途径和手段 ,国家将继续通过实施一系列工业过程自动化高技术的产业化专项 ,用信息化带动工业化 ,推动工业自动化技术的进一步发展 ,加强技术创新 ,实现产业化 ,解决国民经济发展面临的深层问题 ,进一步提高国民经济整体素质和综合国力 ,实现跨越式发展.参 考 文 献[1] 章昌南.中国工业自动化市场现状分析报告[ R].上海电气自动化设计研究院 ,2002[2] 刘鑫.工业 PC 及其应用.石油化工控制系统及仪表自动化[M] ,中国石化出版社 ,2003. 8[3] 王立奉. DCS系统的发展及应用.石油化工控制系统及仪表自动化[M] ,中国石化出版社 ,2003. 8[4] 阳宪惠.现场总线的发展动态[ R].清华大学自动化系 ,2003[5] 彭可等.现场总线技术及控制系统 [ R ].中南大学 ,2003. 8[6] 刘向杰等.现场总线控制系统的现状与发展[ R].电力研究院 ,2003[7] 王立奉.现场总线控制系统.石油化工控制系统及仪表自动化[M].中国石化出版社 ,2003. 8[8] 赵伟.网络化 —测量技术与仪器发展的新趋势[ R].清华大学 ,2003. 6[9] 数控系统与数控机应技术发展趋势 [ R ].清华大学 ,2003. 6[10] 冯冬芹等.无线局域网应用于工业控制系统研究[ R].浙江大学工业控制技术国家重点实验室 ,2003[11] 涂博文.过程建模,在线优化及先进控制.石油化工控制系统及仪表自动化[M] ,中国石化出版社 ,2003. 8[作者简介]刘 鑫 (1963～) ,男 ,吉林长春人 ,研究员 (中国计算机行业协会副会长) ,主要研究方向为工业控制计算机及其系统.?65? 航 天 控 制 2004年? 1994-2006 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net