第25卷第8期 2015年8月 计算机技术与发展 COMPUTER TECHNOLOGY AND DEVELOPMENT Vo1.25 No.8 Aug. 2015 基于非线性卡尔曼滤波的车辆定位优化算法 卞月根,张伟 (南京邮电大学通信与信息工程学院,江苏南京210003) 摘要:智能交通系统(ITS)是未来交通系统发展的重要趋势,为了实现智能交通所提供的各种功能,必须获知车联网中 车辆的准确位置。因此,如何快速准确地实现车辆定位是现代智能交通系统所要研究的一个重要问题。实际系统中一般 都是非线性系统,所以需要采用非线性的卡尔曼滤波算法。文中采用了非线性无迹卡尔曼滤波算法。无迹卡尔曼滤波在 车辆运动状态发生突变时,车辆定位精度有所下降。为了提高无迹卡尔曼滤波算法在车辆运动状态发生突变时的定位精 度,文中将自适应的交互多模算法和无迹卡尔曼滤波算法相结合,进一步提高了车辆的定位精度,同时也更能适应车辆的 各种机动运动状态。仿真实验结果表明,交互多模无迹卡尔曼滤波算法的定位精度相较于无迹卡尔曼滤波算法有显著 提升。 关键词:车辆定位;卡尔曼滤波;交互多模算法;非线性模型 中图分类号:TP301.6 文献标识码:A 文章编号:1673—629X(2015)08—0080—04 doi:10.3969/j.issn.1673—629X.2015.08.017 An Optimization Algorithm of Vehicle Positioning Based on Nonlinear Kalman Filter BIAN Yue-gen,ZHANG Wei (College of Communication&Information Engineering,Nanjing University of Posts and Telecommunications,Nanjing 210003,China) Abstract:Intelligent Transportation Systems(ITS)is all important trend in the development of future transport systems.In order to pro— vide the various functions,he tsystem should acquire the exact posiiton of he tvehicle.How to achieve accurate and rapid vehicle position is all important issue which modern intelligent transportation sysems tmust go to research.The actual systems are generally nonlinear sys・ em,so a nonltinear unscented Kalman filer atlgorithm is used.When the vehicle is in motion is mutatd,tehe accuracy of unscented Kal- man filer atlgorithm is declined.Due to improving the accuracy of vehicle posiiton while the vehicle is motor-driven,the interacting mul- tiple model algorithm is combined wih tthe unscented Kalman filtering.At he tsame time,the impreved algorithm can adapt o a tvariety of motion state of he tvehicle.Simulation results show hatt he posititoning accuracy of interacting mul卸le model unscented Kalman ifltering lgoraithm is obviously better than unscented Kalman filtering algorihm.t Keywords:vehiclelocation;unscentedKalmanfilter;interactingmultiplemodelalgorithm;non-linearmodel 1概述 阶段,还是根据多组距离数据计算待测节点位置阶段, 随着科学技术的不断发展,交通智能化是未来发 都带有误差,这样最终定位出来的车载位置 必然带 有较大误差。因此,这时就需要根据车载运动特点对 所测数据进行进一步的处理,以期能得到更精确的车 载位置提高定位精度就需要将随机干扰和测量误差从 系统中滤除,或者能够降低它们的影响,从而能够从含 有噪声的采集信息中获取被测量系统的参数和状态。 在这里考虑使用应用较为广泛的卡尔曼滤波算法,最 早提出的卡尔曼滤波算法只能适用于线性系统的滤 网络出版时阀:2015—06—23 展的重要趋势。智能交通…能提供的重要功能之一在 于对车辆的精确定位。目前,对车辆的精确定位方法 主要依靠GPS 全球卫星定位系统。然而GPS信号 在隧道、高架桥、地下室等建筑物的遮挡下非常微弱, 因此惯性自主导航定位方法的研究必不可少,也成为 现今的一个研究热点。 通过网络对车载进行定位 无论是在测量距离 收稿日期:2014—10—14 修回日期:2015—01—14 基金项目:江苏省普通高校研究生科研创新计划项目(CXLX13_456) 作者简介:卞月根(1989一),男,硕士,研究方向为车联网、智能交通及无线通信技术。 网络出版地址:http://www.enki.net/kcms/detail/61.1450.TP.20150721.1453.099.html 第8期 卞月根等:基于非线性卡尔曼滤波的车辆定位优化算法 -81. 波。但是,实际存在的大多数系统模型都是非线性的, 如在车载定位导航系统等,这就导致了研究非线性滤 波算法的必要性。为了使卡尔曼滤波算法能够适用于 非线性系统 ,学者们进行了广泛而深入的研究。首 先是布西等在提出了仅适用于线性系统的卡尔曼滤波 算法之后,对其进行了扩展,提出了扩展卡尔曼滤波 (Extended Kalman Filter)算法 。该算法主要是采用 函数近似的方法来实现非线性系统的滤波,具体做法 就是将非线性函数泰勒展开并取其一阶截断近似的方 法,按照此方法能够得到次优滤波效果。与扩展卡尔 曼滤波算法相似的还有标称状态下的滤波算法,也即 将非线性函数在标称状态处泰勒展开并一阶截断取近 似。不过这类方法共同之处都是将非线性函数泰勒展 开并取其一阶截断近似,滤波精度只能达到一阶近似 的效果。为了能够提高近似估计的精度,可以对泰勒 展开式做二阶、三阶截断处理,这样做确实能够提高估 计精度,但同时也带来了更大的计算复杂度。 文中利用车载行驶路程信息作为观测量的一项, 建立非线性统计的车载运动模型。无迹卡尔曼滤波算 法能够适用于非线性系统,定位精度高,计算复杂度 低,因此文中采用无迹卡尔曼滤波算法作为基本定位 算法,得出经过滤波的车载位置信息。交互多模算法 采用多个模型描述动态系统,当系统状态发生变化时, 也能很好地描述系统状态,这是无迹卡尔曼滤波算法 所不具备的功能。因此,为了提高无迹卡尔曼滤波算 法在车载运动状态突然发生变化时的定位精度,文中 将交互多模算法和无迹卡尔曼滤波算法相结合,进一 步提高了车载定位精度,同时也更能适应车载的各种 运动状态。 2 系统模型及交互多模无迹卡尔曼滤波算法 文中将无迹卡尔曼滤波算法 “ 作为基本的滤波 算法。但是无迹卡尔曼滤波算法无法自动适应车载的 运动状态变化,当车载突然加减速或者转弯时,会出现 较大的定位误差,这一点在文中的实验仿真部分也可 以很明显的看出。而交互多模算法作为一种自适应的 算法能很好地克服这一问题,因此文中采用交互多模 算法 的思想来描述车载的运动模型,即构建多个 车载运动模型,同时,对每一个车载运动模型都利用无 迹卡尔曼滤波算法进行滤波,并且对滤波的输入和输 出进行交互,这样得到的最终滤波结果必然要优于单 独的无迹卡尔曼滤波算法。 在无迹卡尔曼滤波算法中引入交互多模算法,在 保证基本滤波精度的前提下,又能有效克服当车载运 动状态切换时,滤波误差增大并且收敛慢的问题,真正 实现准确、实时的定位效果。 将无迹卡尔曼滤波算法和交互多模算法相结合就 是在交互多模算法的步骤中,利用无迹卡尔曼滤波算 法来计算状态估计和状态的协方差估计。下面将介绍 这两种算法相结合的具体计算步骤。 假定有m个非线性模型: = + 群, , )J=1,2,…,m (1) 其中, 是均值为零,协方差矩阵为Q 的白噪声 序列。 用一个马尔可夫链来控制这些模型之间的转换, 马尔可夫链的转移概率矩阵为: P=I:【r_pP¨ …P : !I m] (2) p …p j 非线性观测模型: Z =h(X , ) (3) 其中, 为m维系统观测噪声。 基于交互多模的无迹卡尔曼滤波算法步骤可归纳 如下: 1)输人交互(以模型_『为例说明),计算滤波输入。 假定模型i到模型.『的转移概率为P ,模型_『在k 一1时刻的概率为p (k一1),则模型 的预测概率(归 一化常数)为: =∑p ( 一1) r (4) pj(k—l/k一1)=∑p (后一l/k一 1)P (k一1/k一1) 模型i到模型 的混合概率: r — ( 一1/k一1)=∑p ii=l (J}一1)/ci (5) 模型.『的混合状态估计: (|j}一l/k一1)=∑X =l (后一l/k一1) (.i}一 1/k一1) (6) 模型 的混合协方差估计: ( 一1/k一1)=∑ (后一1/k一1){尸J(后一 1/k一1)+[X (k一1/k一1)一 ,(k一1/k一1)]・[X ( 一 1/k一1)一 (k一1/k一1)] } (7) 2)无迹卡尔曼滤波(模型_『),计算各模型的滤波 输出。 对应于模型 ,以模型_『在k一1时刻的状态估计 值 ( 一1/k一1)和误差协方差矩阵ei(k一1/k一1) ・82・ 计算机技术与发展 第25卷 及新的观测信息z(_}j)作为输入进行无迹卡尔曼滤波 估计。 (1)利用k一1时刻的状态估计值 (k一1/k一1) 和误差协方差矩阵户 (k一1/k一1)求状态变量的 点 值 :(k一1/k一1)及其相应的一阶权值∞ 和二阶权 值∞:。 (2)将亭点值 :(k一1/k一1)通过模型_『的状态方 程传播得到状态的一步预测 点值 (k/k一1),然后 计算状态的一步预测值 (k/k一1)和一步预测协方 差矩阵 (k/k一1)。 (3)将一步预测sigma点值 ( 一1)通过系统 的观测方程传播得到观测值的sigma点值 :(k/k一 1),然后分别计算观测预测值zl(k/k一1)、观测协方 差矩阵 (k/k一1)及状态观测互协方差矩阵P (k/k 一1)。 (4)量测更新。 =pXZ(,、k/k一1)/P (k/k一1) (8) (k/k)= (k/k一1)+ [z(J})一 (k/k一1)] (9) (k/k)= (k/k一1)一E ̄(k/k一1)^ (10) 3)模型概率更新,计算在获得新的观测信息之后 各模型的概率。 模型_『的似然函数: Aj(k)= (2仃)以l Sj(k)I。 一 1 vTsjj(k)vj} (11) 其中 vj=Z(k)一zf(k/k一1) (12) 2n st(k)=∑∞ [x ̄(k/k一1)一zj(k/k一 1)][x}(k/k一1)一 (k/k一1)] (13) 则模型.『的概率更新: XI』(k)=At(k)c/c (14) m 一 其中,c为归一化常数,且c=∑ (k)cj。 4)输出交互。 总的状态估计: ( )=∑ J;I ) ( ) (15) 总的协方差估计: ( J})=∑ (1=I ){ ( )+[ ( )一 X(k/k)]・[ ,(k/k)一X(k/k)] } (16) 通过以上算法的基本步骤可以明显看出,最终的 滤波输出是多个模型的加权平均值,权值即为该时刻 模型正确描述车载运动状态的概率。 基于交互多模的无迹卡尔曼滤波算法在对车载状 态向量进行最优估计时,采用了多个模型,这样当车载 运动状态发生改变时,总能找到最适合描述车载当前 运动状态的模型。并且通过概率更新也能将最适合描 述车载实际运动状态的模型概率设置为最大,这样最 终滤波输出就能最接近状态向量的实际值。无迹卡尔 曼滤波算法只能采用同一模型对车载运动状态进行描 述,缺乏灵活性。基于交互多模的无迹卡尔曼滤波算 法很好地解决了这一问题,在实际对车载运动状态进 行估计时,具有较好的自适应性。因此在滤波性能上, 基于交互多模的无迹卡尔曼滤波算法相较于无迹卡尔 曼滤波算法肯定会有所提升,这种提升效果在车载运 动状态切换时必然更为明显。 3仿真分析 本节将给出一些仿真结果与分析。由于受实验条 件的限制,文中在仿真实验时,首先对车载的运动轨迹 进行模拟,同时记录车载的真实位置信息,然后将真实 的位置信息加上均值为零、方差为1O的高斯随机噪声 作为观测向量。从观测向量的获得过程中可以看出, 观测数据中的车载位置的误差在10 m左右。仿真实 验采用的仿真工具是MATLAB R2010a,PC环境为 Windows XP。 车辆在 及y方向均做匀变速运动,车载起始位 置位于(200,100)坐标处,速度初始值为(10,5)。沿 着 方向和y方向同时匀速、加速及减速运动。整个 运动过程中匀速、加速及减速三种状态均运行50 s,误 差采样周期为0.1 s。仿真实验图如图1所示。 肪向位#,m (a)车载定位轨迹放大部分 滤波时间『0.h (b)无迹卡尔曼滤波定位误差 第8期 卞月根等:基于非线性卡尔曼滤波的车辆定位优化算法 .83. 滤波时问/0.1 (C)交互多模无迹卡尔曼滤波定位误差 滤波时间,0.1 s (d)无迹卡尔曼滤波 方向速度估计 滤波时间/0.1 s (e)交互多模无迹卡尔曼滤波 方向速度估计 滤波时间,0.1 8 (f)无迹卡尔曼滤波Y方向速度估计 滤渡时同『0.1 s (g)交互多模无迹卡尔曼滤波y方向速度估计 图1仿真实验图 从图(b)和(c)可以看出,车载在状态切换时,无 迹卡尔曼滤波算法定位误差会迅速增大且收敛慢,状 态切换越大,误差就越大,收敛也越慢;而交互多模无 迹卡尔曼滤波算法定位误差在车载状态切换时也会增 大,但增大幅度较无迹卡尔曼滤波算法小,并且收敛迅 速。从表1中的实验数据也可以看出,交互多模无迹 卡尔曼滤波算法定位效果优于无迹卡尔曼滤波算法。 从图(d)和(e)及图(f)和(g)可以很明显看到,车载状 态切换时,无迹卡尔曼滤波算法对速度的估计偏差增 大,且较长时间才收敛;而交互多模无迹卡尔曼滤波算 法对速度的估计偏差则无明显变化。本实验模拟了车 载沿着 方向及y方向均做“匀速一加速一减速”运 动,整个过程车载运动状态变化较大,交互多模无迹卡 尔曼滤波定位效果较无迹卡尔曼滤波定位效果提升 明显。 表1 车载沿着 及y方向匀变速 时仿真实验测量的定位误差 m 算法 无迹卡尔曼滤波 1.834 8 2.017 1 2.018 1 1.793 7 交互多模无迹卡尔曼滤波1.703 9 1.650 1 1.721 8 1.61o 9 4结束语 交互多模无迹卡尔曼滤波算法对车载的定位效果 确实优于无迹卡尔曼滤波算法,提升效果在车载运动 状态转换时表现得尤其明显。事实上,交互多模无迹 卡尔曼滤波和无迹卡尔曼滤波的核心算法都是无迹卡 尔曼滤波算法,前者之所以有定位效果的提升主要是 因为结合了交互多模这种自适应算法,能够在车载运 动状态转换时,找到最适合描述车载运动状态的模型, 而单独的无迹卡尔曼滤波算法则不能做到这点。这 样,在车载运动状态变换时,无迹卡尔曼滤波算法由于 模型不适合从而定位误差必然增大,并且误差是渐近 收敛的,收敛速度较慢,因此导致了整体的定位效果不 如交互多模无迹卡尔曼滤波算法。 参考文献: [1]梁伟,郑印,王沁艳,等.智能交通系统消息交换的复 杂度研究[J].计算机技术与发展,2014,24(1):81-84. [2]李鑫.提高GPS定位精度的改进卡尔曼滤波算法研究 [D].南京:南京理工大学,2004. [3]王建强,李世威,曾俊伟.车联网发展模式探析[J].计算机 技术与发展,201 1,21(12):235-238. [4] 智勇.城市车辆定位系统的设计与实现[D].大连:大连 理工大学,2013. [5]张璇.分层线性模型的最大后验估计[J].统计与信息论 坛,2011,26(1):l0—15. [6] 吴志华,丁杨斌,申功勋.改进的非线性鲁棒EKF算法及 其应用[J].计算机工程与应用,201l,47(3):207—209. (下转第89页) 第7期 王景中等:一种快速有效的QR码定位方法 代情报,2011,31(6):151—154. ・89・ 5 结束语 文中充分考虑到QR码特有的结构特征,采用了 分步定位的方法,先获得QR码可能所在的区域,然后 在该区域内进行精确定位。由于QR整体呈正方形, 文中利用了QR码的这一特征,运用Hough变换去检 [5]王维虎,刘延申.基于二维码的物流管理及应用[J].物联 网技术,2012,2(12):36—38. [6]孙明,傅隆生,杨信廷,等.用于QR码自动识读的图像 分析方法[J].电子科技大学学报,2009,38(6):1017— 1020. 测正方形区域从而获得QR码的大致轮廓,再进行精 确的定位,这样比直接在整个图像中寻找探测图形提 高了成功率。QR码的精确定位过程中,寻找满足1: [7]李人哲,许[8]孙道达,赵刚.模糊QR码图像的恢复[J].计算机应用 健,王瑞,等.基于稀疏表示的QR码识别 与软件,2013,30(6):258—261. 1:3:1:1的线段是整个精确定位过程中最耗时的部 [J].计算机应用,2013,33(1):179-181. 分。文中借鉴了KMP算法的思想,克服了原有方法当 [9]熊用,汪鲁才,艾琼龙.基于相关匹配的QR码识别方法 匹配过程中失配时需要回溯的缺点。经过实验测试, [J].仪器仪表学报,2011,32(3):571-576. 文中方法可以有效地提高QR码的识别率和识别速 [10]张民,郑建立.基于符号特征的QR码识别算法[J].计 度,对于传统方法中在比较复杂背景下识别率低的问 算机工程,2011,37(4):278—280. [11]孙文静,钱华.改进BM算法及其在网络入侵检测中的 题有了较大改善。 应用[J].计算机科学,2013,40(12):174—176. 由于文中方法用到了Hough变换去检测正方形, [12]鲁宏伟,魏凯,孔华锋.一种改进的KMP高效模式匹配 耗时虽然有一定的提高但还可以考虑其他的检测方 算法[J].华中科技大学学报:自然科学版,2006,34(1O): 法,进一步提升时间效率。 41-43. [13]严蔚敏,吴伟民数据结构[M].c语言版.北京:清华大学 参考文献: 出版社,1997:80—82. [1] ISO/IEC18004:2006.QR code 2005 bar code symbology [14]卫晋伟,戴曙光,穆平安.基于形态学和Hough变换的QR speciifcation[S].Swi ̄edand:ISO,2006. 码校正与定位方法[J].电脑与信息技术,2010,18(6):32 [2] 国家质量技术监督局.中华人民共和国家标准一快速响应 —35. 矩阵码(QRCode)[s].北京:中国标准出版社,2000. [15]Farukawa Y,Shinagawa Y.Accurate and robust line segment [3]朱雯晶,夏翠娟.二维码在图书馆移动服务中的应用一以 extraction by analyzing distribution around peaks in hough 上海图书馆为例[J].现代图书情报技术,2012,28(7):115 space[J].Computer Vision and Image Understanding,2003, —120. 92(1):1-25. [4]王世慧,杜伟.手机QR码在图书馆的应用探析[J].现 (上接第83页) 究[D].哈尔滨:哈尔滨工程大学,2013 [7] Julier S J,Uhlmann J K.Unscented ifltering and nonlinear es・ [11]Song Q,Han J D.An adaptive UKF algorihtm ofr hte state and timation[J].Proceedings of the IEEE,2004,92(3):041— parameter estimation of a mobile robot[J].Aeta Automatiea 422. Sinica,2008,34(1):72-79. [8]Julier S J,Uhlmann J K.Reduced sigma point filters for the [12]胡炜.交互多模型目标跟踪算法的研究[D].沈阳:东北 propagation of means and covariances through nonlinear trans- 大学,2010. formations[C]//Proceedings of the American control confer- [13]孔金生,翟弦.车辆导航中的交互多模卡尔曼滤波跟踪 ence.[S.1.]:IEEE,2002:887-892. 方法[J].计算机工程与应用,2009,45(14):198-200. [9]Han Pengxin,Mu Rongjun.Effective fault diagnosis based on [14]韩兴斌,胡卫东,杨世海.应用交互多模(IMM)算法跟踪低 strong tracking UKF[J].Aircraft En ̄neefing nad Aerospace 空目标[J].火力与指挥控制,2003,28(5):48-51. Technology,2011,83(5):275-282. [15]肖 宁,王玫,雷延锋.基于交互多模卡尔曼滤波器的定 [10]唐苗苗.车载组合导航系统自适应无迹卡尔曼滤波算法研 位算法[J].无线电通信技术,2013,39(1):80-83.