基于改进的扩展卡尔曼滤波在GPS测速中的应用

冯雪丽 颜伏伍 胡杰

摘 要： 汽车电子控制系统对动态测速提出较高的要求，使用位差法和扩展卡尔曼滤波融合算法用于GPS测速比GPS位置差分测速方法具有更高的精度，但在汽车快速转弯时误差较大。为此通过有限差分运算获取滤波验前、验后误差协方差矩阵，增强滤波过程的收敛性，再以有限差分扩展卡尔曼滤波算法对汽车实时位置进行运算以获取实时车速。实验结果表明，相比扩展卡尔曼滤波GPS测速方法，该文算法具有更高的测速精度。

关键词： 汽车电子控制系统； 全球定位系统； 动态测速； 扩展卡尔曼滤波算法； 有限差分运算

中图分类号： TN61?34 文献标识码： A 文章编号： 1004?373X（2016）08?0030?03

Application of improved extended Kalman filtering algorithm in

velocity determination with GPS

FENG Xueli1， 2， YAN Fuwu1， HU Jie1

（1. Automotive Engineering Institute， Wuhan University of Technology， Wuhan 430070， China；

2. Mechanical and Electrical Engineering College， Hangzhou Polytechnic， Hangzhou 311402， China）

Abstract： Dynamic velocity determination had to meet the higher requirement from the vehicle electronic control system. The algorithm combining finite?difference algorithm with extended Kalman filtering algorithm has higher accuracy in GPS velocity determination than GPS location difference velocity determination method， but there is a larger measuring error during the fast turning of car. The finite?difference operation is adopted to achieve the priori error covariance matrix and the posterior error covariance matrix of filtering to enhance the convergence of the filtering process， and then the finite?difference extended Kalman filtering algorithm is used to calculate cars real?time location， so as to get the real?time speed of the vehicle. The experiment results show that the improved algorithm has higher measuring accuracy than that of the extended Kalman filtering velocity determination algorithm.

Keywords： vehicle electronic control system； global positioning system； dynamic velocity determination； extended Kalman filtering algorithm； finite?difference operation

近年来由于信息技术、计算机技术和电子技术的快速发展和应用普及，汽车电子控制技术得到长足的发展，在智能化、网络化、控制精度、控制范围等领域均取得了一定的突破性成果[1]，进一步提升了现代汽车的发展水平；但同时汽车电子控制技术的智能化、网络化和集成化也对汽车状态的测量提出了更高效更快捷更准确的要求[2]。汽车行驶过程中的动态参数如加速度、车速、质心侧偏角、横摆角速度等不仅是汽车性能评估的重要因素，还是汽车电子控制的重要参数，对上述参数的低成本、高精度的测量是提升汽车电子控制水平的关键。目前应用于车辆动态测速的方法[3]主要以基于GPS动态测速方法为主，包含GPS多普勒频移测速、GPS差分测速和GPS扩展卡尔曼滤波测速，但都存在一定不足，前两种方法电路复杂且成本高，不利于产品的小型化和推广，GPS扩展卡尔曼滤波测速虽然成本相对较低，但在车辆快速转弯时测速准确性有待提高[4]。为此本文提出了一种使用有限差分运算改进扩展卡尔曼滤波算法用于车辆动态测速，以提高算法在车辆快速转弯时的测速性能，并进行实验验证。

1 GPS差分测速方法

使用GPS可以快捷、可靠、经济地测量运动中物体的动态速度，测量准确性较高，精度可至毫米级。

其中较为简单的GPS测速方法是GPS位置差分法[5]，坐标系采用NED（North East Down）坐标系，以汽车质心为O点，有三条坐标轴，分别是N轴、E轴和D轴，分别指向地理坐标系的北向、东向和地心方向，该方法建立的模型是将车辆行驶等效为地表平面上的二维运动，将地心方向上的运动等效为噪声，在车辆上使用GPS设置获取被测车辆的经度、纬度、速度、方向角和GPS时间等数据参数[6]，在[t-Δt]时刻获取的车辆位置为[sk-1]，在[t+Δt]时刻获取的车辆位置为[sk+1]。其中[sk-1]，[sk+1]都是位置向量，可得[t]时刻车辆速度[Vk]为：

[Vk=12?Δtsk+1-sk-1] （1）

式中，[Δt]是GPS设备的采样时间，其值为GPS设备工作频率的倒数，单位为s。

在式（1）的基础上可得汽车在[t]时刻的北向速度[Vk，N]和东向速度[Vk，E]，如下所示：

[Vk，N=Vk?cosαVk，E=Vk?sinα] （2）

式中，[α]是汽车在[t]时刻的行驶方向角。这样可获得车辆在任意采样时刻的速度、北向速度和东向速度，并可根据速度获取车辆对应的加速度、北向加速度和东向加速度。但是GPS差分测速度算法抗观测噪声和系统噪声的能力较差[7]，因此引入扩展卡尔曼滤波算法进行GPS测速，以提高测速算法对噪声的抑制能力。

2 有限差分扩展卡尔曼滤波GPS测速算法

2.1 扩展卡尔曼滤波测速算法

基于扩展卡尔曼滤波的GPS测速算法首先需要建立离散非线性模型[8]，如式（3）所示。

[Xk+1，E=fXk，Uk+WYk+1，E=gXk，Uk+ω] （3）

式中：[Xk+1，E]是系统东向状态变量；[Yk+1，E]是系统东向输出结果；[f?]、[g?]是非线性的传递函数；[ω]、[W]是互不相干的测量噪声和系统噪声，测量噪声是由测量传感器造成的，系统噪声是由汽车自身因素造成的误差，包含器件振动噪声及路面颠簸引起的噪声等，相对应的系统方差阵分别是[R、Q]，传递函数的第二个参数[Uk]表征控制变量，由于GPS没有控制变量，因此[Uk]设为0。式（3）的是模型的状态方程和输出观测方程，其中类似地，系统的北向分量和地心分量也由式（3）计算获取。

对两个非线性传递函数在[Xk]一阶泰勒展开，并设：

[AE=?f?XX=X+kCE=?g?XX=X+k] （4）

式中，[X+k]为GPS设备采样时刻后续的状态值估计结果，设汽车东向运动的加速度为[ak，E]，则有：

[ak，E=1ΔtVk，E-Vk-1，E] （5）

同时设对应的东向加速度变化率为[ik，E]，则有汽车东向运动状态分量[Xk，E=Vk，E，ak，E，ik，E]。设汽车在[t]时刻、[t+Δt]时刻、[t+2Δt]时刻对应的行驶距离分别为[Sk]，[Sk+1]，[Sk+2]，可得[Sk+1]，[Sk+2]的计算公式为：

[Sk+1=Sk+Vkt+0.5?ak?t2Sk+2=Sk+1+Vk+1t+0.5?ak+1?t2] （6）

式中，[t+2Δt]时刻对应的行驶速度[Vk+2]为：

[Vk+2=Vk+1+ak+1t+0.5?ik+1?t2] （7）

综上可得式（3）中汽车东向速度状态方程为：

[Xk+1，E=Vk，Eak，E?t0.5?ik，E?t20ak，Eik，E?t00ik，E+W] （8）

将其代入式（4），可得：

[AE=1t0.5?t201t001] （9）

[W]是汽车的系统噪声，使用噪声方差矩阵[Q]表示，[Q]是常系数矩阵，与GPS设备采样频率相关。

[Q=t520t48t36t48t36t22t36t22t×0.09] （10）

在式（9）、式（10）中，[t]是GPS设备的采样周期，其值为GPS设备工作频率的倒数，单位为s。

同样地，结合式（3）～式（7）有[CE=1，1，1，]观测噪声[ω]由GPS设备决定，随设备变化而变化[9]。

最后使用扩展卡尔曼滤波算法对汽车东向速度的估计方程为：

[Pk，Ek-1=AE?Pk-1，Ek-1?ATE+Q （11a）Xk，Ek=Xk，Ek-1+Kk，g?Yk，E-CE?Xk，Ek-1 （11b）Kk，g=Pk，Ek-1?CTECE?Pk，Ek-1?CTE+R （11c）Pk，Ek=I-Kk，g?CE?Pk，Ek-1 （11d）] （11）

结合式（3）和式（11），可得使用扩展卡尔曼滤波算法获取的t时刻汽车东向状态分量最优估计值[Xk，Ek]，同样地可得汽车北向状态分量最优估计值[Xk，Nk]，从而得到汽车在该时刻的水平速度[Xkk]。

使用扩展卡尔曼滤波算法对GPS位差测速算法进行改进可有效抑制噪声对测速精度的影响并提高测速精度，但是扩展卡尔曼滤波算法在收敛速度方面[10]存在一定的问题，在汽车快速转弯时误差较大，因此使用有限差分算法进行优化以进一步提高算法性能。

2.2 有限差分改进

有限差分的原理是利用一阶中心差分法和多项式近似技术求解非线性函数的偏导数，可实现二阶非线性近似功能[11]。设[y=gx]为非线性函数，该函数在[x=x]处的二阶有限中心差分展开为：

[gx≈gx+gDD′xx-x+fDD″x2！x-x2] （12）

式中：[gx]为函数[gx]在[x=x]处的函数值；[gDD′x]是使用差分方法计算出的展开式一阶项系数，其计算如下：

[gDD′x=gx+l-gx-l2l] （13）

式中，[l]是区间长度参数，[fDD″x2！]是展开式二次项系数，同样使用差分方法计算获取，其计算如下：

[gDD″x=gx+l+gx-l-2gxl2] （14）

对式（12）进行泰勒级数展开变为：

[gx=gx+g′xx-x+f″x2！x-x2+ g3x3！h2+g5x5！h4+…x-x+ g4x4！h2+g6x6！h4+…x-x2] （15）

将式（13）、式（14）与式（15）后两项比较发现后两项与高阶项对应，并由[h]来控制精度，通过验证比较，在非线性函数中使用中心差分代替一阶、二阶导数后的精度比代替前的精度要高，这也是将有限差分算法用于改进扩展卡尔曼滤波算法用于GPS测速的原因所在，可有效处理车辆高速转变过程中的非线性变化得到更高的测速精度。有限差分扩展卡尔曼滤波算法步骤为：

（1） 初始化，获取GPS测速模型的状态方程和输出观测方程、[ω]和[W]的系统方差阵分别是[R，Q]及[AE]，[CE]，并引进矩阵：

[Pk，Ek=SXSTXPk+1，Ek=SXSTXQ=SWSTWR=SVSTV] （16）

（2） 进行状态一步预测，即式（11a）；

（3） 对汽车东向、北向的误差协方差矩阵进行预测，此处使用一阶中心差分法获取误差协方差矩阵，东向分量的误差协方差矩阵为：

[SXi，j=fiXk，Ek-1+hAE，i-fiXk，Ek-1-hAE，i2h] （17）

式中：[AE，i]由步长的调节系数[h]调整；[h]可按噪声分布规律设置。

（4） 进行量测预测，如式（18）所示。

[Zk，Ek-1=CE?Xk，Ek-1] （18）

（5） 计算滤波增益，如式（19）所示。

[K=k，Ek-1CTECEPk，Ek-1CTE+R-1] （19）

（6） 进行滤波估，如式（11b）所示。

（7） 进行误差协方差阵估计，如式（11d）所示。

（8） 求解估计误差的Choleskey分解，并代入到式（11）中进行计算获取t时刻汽车东向状态分量最优估计值[Xk，Ek]，同样地可得汽车北向状态分量最优估计值[Xk，Nk]，从而得到汽车在该时刻的水平速度[Vkk=V2k，Nk+V2k，Ek]。

3 实验及分析

为对本文的算法性能进行分析验证，使用Vkel的GPS模块获取车辆的GPS数据，并分别使用扩展卡尔曼滤波GPS测速算法模块和有限差分扩展卡尔曼滤波GPS测速算法模块接收GPS数据并进行测速；对前文中的部分参数进行设置，对调节系数[h]，因统计噪声属于白噪声[9]，因此设定[h=3]；GPS设备采样频率为1 Hz，对应的采样时间[Δt=1 s]。车辆行驶中设置多次高速转弯，将两种算法计算的车辆行驶水平速度分别与水平速度真值进行比较，得到的速度测量误差如图1所示。

相比扩展卡尔曼滤波GPS测速算法，改进后算法测量误差更小，其均方根误差是0.051 9，而扩展卡尔曼滤波GPS测速算法的均方根误差为0.231 8，在车辆多次高速转变情况下，算法改进后均方根误差得到明显提高。改进后算法平均耗时0.148 1 s，而改进前算法平均耗时0.117 4 s，可见有限差分扩展卡尔曼滤波GPS测速算法使用有限差分运算获取滤波验前、验后误差协方差矩阵改进扩展卡尔曼滤波算法增加了算法复杂度从而需要消耗更长的运算时间，但仍在系统设计需求许可范围内。

4 结 语

本文针对位差法和扩展卡尔曼滤波融合算法用于GPS测速时汽车快速转弯时测速误差较大的问题，使用有限差分运算获取滤波验前、验后误差协方差矩阵改进扩展卡尔曼滤波算法增强了算法的收敛性；从而提高了算法在车辆高速转弯时的测速精度，使速度测量值的均方根误差减小，虽然测速响应时间增大但仍在系统设计需求允许范围内；表明基于有限差分扩展卡尔曼滤波的GPS动态测速算法具有更高的性能和应用价值。

参考文献

[1] 郭可怡.汽车电子控制装置中计算机检测控制技术的应用[J].电子技术与软件工程，2015（6）：258.

[2] 周娟英.计算机检测控制技术在汽车电子控制系统中的应用分析[J].自动化与仪器仪表，2014，7（4）：107?108.

[3] 杜娟，孙中豪，姚飞娟，等.GPS测速方法与精度分析[J].全球定位系统，2012，37（6）：13?16.

[4] 杨龙，刘焱雄，周兴华，等.GPS测速精度分析与应用[J].海洋测绘，2007，27（2）：26?29.

[5] 徐宏宇，王浩，王尔申.基于扩展卡尔曼滤波的GPS定位数据处理方法研究[J].科学技术与工程，2012，12（31）：8137?8142.

[6] 余娅，李军.基于车载GPS导航的非线性动态滤波算法[J].计算机与数字工程，2012，40（11）：25?28.

[7] WU F M， YANG Y X， ZHANG L P. A new fusion scheme for accuracy enhancement and error modification in GPS/INS tight integrated navigation [J]. Survey review， 2012， 44（326）： 208?214.

[8] 杨宏，李亚安，李国辉.一种改进扩展卡尔曼滤波新方法[J].计算机工程与应用，2010，46（19）：18?20.

[9] 潘盛辉，郝东亚，张兴达.基于扩展卡尔曼滤波的GPS动态测速算法[J].信息技术，2014，13（12）：13?16.

[10] 何栋炜，彭侠夫，蒋学程，等.永磁同步电机的改进扩展卡尔曼滤波测速算法[J].西安交通大学学报，2011，45（10）：59?64.

[11] 巫春玲，韩崇昭.用于弹道目标跟踪的有限差分扩展卡尔曼滤波算法[J].西安交通大学学报，2008，42（2）：143?146.