基于无迹卡尔曼滤波传感器信息融合的车辆导航算法

摘要：针对复杂道路条件下车辆的导航问题，将全球定位系统（GPS）与车载终端传感器系统相结合，提出了基于多传感器系统的车辆精确定位模型，并针对扩展类卡尔曼滤波易产生突发性误差而导致的安全问题，采用基于Sigma点的无迹卡尔曼滤波器（UKF）传感器信息融合算法根据实时的道路状况和车辆自身的运动状态给出符合要求的状态估值，实验与基于多项式扩展卡尔曼滤波车辆传感器信息融合算法在精度和效率方面进行了比较，结果表明，基于UKF传感器信息融合的算法在复杂路况下的估计精度和运行效率都有显著提高，能够根据当前的路线情况和车载传感器的反馈信息快速地估计出车辆的运动状态，实时计算出动态的车辆控制输入

关键词：车辆导航； 无迹卡尔曼滤波； 传感器信息融合； Sigma点滤波

中图分类号：U495； TP18 文献标志码：A

0引言

卡尔曼滤波擅长处理多变量非平稳随机过程和时变系统的滤波问题，被广泛应用于自动导航领域导航算法必须达到较高的精度且能适应各种运行状态的变化，才能确保导航的安全性要求应用最广泛的多传感器信息融合算法是扩展卡尔曼滤波（Extened Kalman Filter，EKF）[1]

EKF对非线性模型的Taylor展开式作截断处理，舍弃高阶项在强非线性系统中局部线性化假设失效，可能会使虑波发散；同时繁琐的矩阵计算使得EKF的工程实现困难，计算机效率低下

为了克服EKF的缺点，研究人员提出了基于Sigma点的方法，代表算法是无迹卡尔曼滤波（Unscented Kalman Filter， UKF）UKF在不提高计算复杂度的情况下，较EKF的精度大大提高，而且UKF易于实现，不用像EKF那样进行复杂的分析，不需要计算复杂的雅可比矩阵[2-6]文献[7]对陆上机动车辆的传感器信息融合模型进行了详细分析，GPS定位和速度观测信

息可以为车辆的运动提供额外的信息对机动系统的控制可以通过从传感器得到的观测信息，对运动状态作先验估计实现[8]多项式卡尔曼滤波是基于扩展卡尔曼滤波器的原理，将车辆运动模型进行泰勒分解，取不同阶数达到控制精度的目的[9]

基于非线性滤波控制方案的模型不确定性和外部振动的鲁棒性，在非线性滤波算法的设计中，可以通过调节扰动观测或者未知输入观测的校正因子来解决[10-11]本文针对扩展类卡尔滤波的高计算复杂度以及突发性误差问题，应用基于平面控制的方法，在文献[12]的基础上加入指数形式的稳定反馈因子[10-11]，通过自适应地控制车辆运动，结合车载传感器建立了车辆运动的多传感器状态空间模型，设计了基于无迹卡尔曼滤波的传感器信息融合算法SUKF，对车辆运动状态进行估值实验结果表明，本文的算法在运行效率和计算精度方面都有明显提高

1车辆导航模型

导航的非线性控制方法主要包括Ren等[13]提出的基于Lyapunov的控制方法和Singh等[14]提出的基于模型的控制方法等，而基于平面控制的方法在文献[12]中得到了改进，更适用于车辆导航模型

1.1 车辆平面运动模型

假设车辆在一个常纬度区域内运动，可以得到如下运动学公式：

1.2 多传感器观测模型

车辆的定位需要两个观测集合：第一个观测集合为车辆的初始坐标，来源于IMU或GPS；第二个观测集合由一系列到参考平面pj的距离d（k）组成通过视觉传感器或者雷达获取数据如图1所示，参考平面pj可以通过pjr和pjn描述，pjr表示参考平面到坐标系OXY原点O的距离，pjn表示参考平面与OX轴的夹角由几何关系可知，到参考平面pj的观测距离如下：

至此，车辆运动的状态空间模型已经建立完成通过车辆的方位角构建其运动模型，在运动过程中，结合IMU、GPS、雷达、视觉传感器的反馈信息建立车辆的观测模型，接下来就需要选择动态估计算法对车辆运行中的状态进行估计和预测，以指导车辆平稳运行

2 UKF传感器信息融合算法

无迹卡尔曼滤波（UKF）基于最优高斯逼近的卡尔曼滤波器框架的递归最小均方误差（Minimum MeanSquare Error，MMSE）估值器，能够解决EKF的一些逼近问题因为EKF及其变体是在精度上对非线性模型的泰勒公式作截断处理，在观测模型状态的估值统计中经常引入瞬间的大误差，这在对高度非线性化模型作局部线性化假设时异常明显逼近变量的随机概率分布比逼近任何非线性函数更加容易[15]，UKF并没有逼近预测模型的非线性过程和观测模型，而是逼近状态随机变量的高斯分布，这样对任何非线性模型，模型状态的均值和协方差都能精确到二阶，而误差只到三阶甚至更高

2.1 无迹变换

无迹变换用于计算状态随机变量的统计信息，状态随机变量一般通过非线性的高斯变换获得假设L维随机变量x的任意非线性函数为

令x的均值为，协方差为Px为了使用无迹变换计算y的统计信息，首先通过先验随机变量x的均值和协方差，使用确定性方法选出2L+1个权重样本，称为Sigma点，Si={ωi，χi}选择模式满足如下要求：

图2显示了无迹变换的整个过程对于无迹变换中Sigma点的选择模式，随着状态空间（L）维数的增加，限制所有Sigma点分布的球面边界的半径也相应增加即使能够正确计算出先验分布的均值和协方差，在效率方面也会大大降低如果模型的非线性结构非常重要的话，这会更加困难为了解决这个问题，设置缩放因子κ，使Sigma点的分布可以逼近或者远离先验分布的均值这样，均值到第i个Sigma点的距离|χi-|就可以按比例（L+κ）进行缩放当κ=0时为L，当κ>0时Sigma点远离均值，当κ

2.2 无迹卡尔曼滤波信息融合算法

2.2.1初始化

首先对车辆的状态向量、状态向量误差协方差矩阵设置初始值：

2.2.2计算Sigma点

每一个周期都包含Sigma点计算、状态更新、观测更新三个子过程Sigma点的计算过程就是2.1节的无迹变换过程

2.2.4观测更新

观测更新计算车辆的误差协方差矩阵：

3 仿真实验

实验环境配置如下：

CPU型号为Inter Core Duo CPU，E8400 @3.00GHz 3.00GHz；

操作系统为Windows 7 32位操作系统；

模拟平台为Matlab R2012a（7.14.0.739）

实验中，位移单位为m，时间单位为s，速度单位为m/s，加速度单位为m/s2，角用弧度（rad）表示

图3代表车辆在平面内的运行轨迹，车辆从原点出发，向东南方向行驶；图4是显示车辆运行过程中方向角随时间的变化过程；图5～8是车辆运动轨迹和速度变化过程在X方向和Y方向上的分解图5、图7分别表示车辆运行轨迹在水平方向和竖直方向的随时间的变化情况；图6、图8分别表示车辆运行速度随时间的变化情况，图中X轴表示数据的采集周期

从图3和图4可以看出，在运动的后期，随着误差的不断积累，基于扩展卡尔曼滤波的PEKF在位移和方向角上都出现了突发性大误差这在速度变化引起车辆状态剧烈变化（如拐弯、急刹车）过程中出现的概率较大，容易引发安全事故从分解图上看，图5和图6分别表示车辆的运动在X轴方向上的位移和速度变化过程从图5和图6中可以看出，X方向上经历类似加速、匀速、再加速的过程，车辆状态由匀速向加速过程变换时，PEKF出现了突发性误差，而SUKF则平缓地实现了过渡

同样的现象可以在Y方向的运动中看到，从图7和图8可以看出，车辆整体是加速和匀速过程，但由于最后阶段的匀速状态被破坏，速度的变化幅度大幅增加，PEKF出现了突发性误差

综上所述，PEKF的误差在车辆状态切换时出现的概率较大这是对运动模型进行泰勒分解后，对运动模型作截断处理引起的[3]，这种系统突发性误差也是安全性的重大隐患之一还有一种情况是在急速转弯，或者车辆状态连续转换引起的误差快速累积，如图9所示，SUKF能够更快速地校正车辆的运动状态不断切换中保持更高的稳定性和效率

表2显示了车辆运行过程中各个分量的均方根误差值，从中可以看出，SUKF方法的精度更高，性能更好

表3显示了运行时间的均值情况，虽然不够精确，但是在相同的运行环境下，从一定程度上可反映出运动复杂性对PEKF的性能产生了较大影响通过对各种运行轨迹的统计发现，在一般的简单轨迹上，PEKF在时间上更具优势，而随着运动轨迹的复杂程度不断增加，这种优势也逐渐成为其劣势所在

综合比较，基于多传感器状态空间的UKF信息融合算法SUKF克服了多项式卡尔曼滤波算法因为误差积累和车辆状态因拐弯等发生状态快速变化引起的突发大误差的缺点；而且，SUKF采用高斯概率分布的方式，避免了复杂的矩阵运算，在运行效率和精度上都有明显提高

4结语

本文将基于平面运动的导航模型用于车辆导航中，利用GPS导航系统与车载终端传感器的反馈信息建立车辆运动的状态空间模型通过对车辆传感器信息系统建模，对车辆运动进行更精确的控制通过UKF优化模型算法，采用高斯概率分布估计车辆的运动状态实验表明，基于无迹卡尔曼滤波器传感器信息融合的算法对各种复杂环境的适应性更强，计算效率更高；而且通过调整无迹变换中的样本数量可以在精度和计算效率之间寻求动态平衡；与多项式卡尔曼滤波器相比可以显著提高精度，克服EKF带来的突发性误差大、计算效率低的问题

参考文献：

[1]RIGATOS G， TZAFESTAS S G. Extended Kalman filtering for fuzzy modelling and multisensor fusion [J]. Mathematical and Computer Modelling of Dynamical Systems： Methods， Tools and Applications in Engineering and Related Sciences， 2007，13（3）：251-266.【一个文献错5个单词！！！】

[2]JULIER S， UHLMANN J，DURRANTWHYTE H F. A new method for the nonlinear transformations of means and covariances in filters and estimators [J]. IEEE Transactions on Automatic Control， 2000， 45（3）：477-482.

[3]JULIER S J， UHLMANN J K. Unscented filtering and nonlinear estimation [J]. Proceedings of the IEEE， 2004，92（3）：401-422.

[4]van de MERWE V R， WAN E， JULIER S. Sigmapoint Kalman filters for nonlinear estimation and sensorfusion applications to integrated navigation [C]// Proceedings of the 2004 AIAA Guidance， Navigation and Control Conference and Exhibit. [S.l.]： AIAA， 2004：701-730.

[5]SARRKA S. On unscented Kalman filtering for state estimation of continuoustime nonlinear systems [J]. IEEE Transactions on Automatic Control， 2007，52（9）：1631-1641.

[6]KANDEPU R， FOSS B， IMSLAND L. Applying the unscented Kalman filter for nonlinear state estimation [J]. Journal of Process Control， 2008， 18（7/8）：753-768.

[7]LI W， LEUNG H. Simultaneous registration and fusion of multiple dissimilar sensors for cooperative driving [J]. IEEE Transactions on Intelligent Transportation Systems， 2004，5（2）： 84-98.

[8]KARLSSON R， SCHN T B， TRNQUIST D， et al. Utilizing model structure for efficient simultaneous localization and mapping for a UAV application [C]// Proceedings of the 2008 IEEE Aerospace Conference. Piscataway： IEEE， 2008：301-313.

[9]赵汝祺，赵祚喜，赵汝准.基于多项式卡尔曼滤波器的车辆定位试验[J].农业工程学报，2012，28（19）：49-56.

[10]RIGATOS G G. Modelling and control for intelligent industrial systems： adaptive algorithms in robotics and industrial engineering [M]. Berlin： SpringerVerlag， 2011.

[11]XIA Y Q， ZHU Z， FU M Y， et al. Attitude tracking of rigid spacecraft with bounded disturbances [J]. IEEE Transactions on Industrial Electronics， 2011， 58（2）： 647-659.

[12]LéCHEVIN N， RABBATH C A. Sampleddata control of a class of nonlinear flat systems with application to unicycle trajectory tracking [J]. ASME Journal of Dynamical Systems， Measurement and Control， 2006， 128（3）：722-728.

[13]REN W， BEARD R W. Trajectory tracking for unmanned air vehicles with velocity and heading rate constraints [J]. IEEE Transactions on Control Systems Technology， 2004， 12（5）： 706-716.

[14]SINGH L， FULLER J. Trajectory generation for a UAV in urban terrain， using nonlinear MPC [C]// Proceedings of the 2001 American Control Conference. Piscataway： IEEE， 2001， 3： 2301-2308.

[15]JULIER S J， UHLMANN J K. A general method for approximating nonlinear transformations of probability distributions [R]. Oxford： University of Oxford， Department of Engineering Science， Robotics Research Group， 1996： 202-229.

[16]PRESS W H， TEUKOLSKY S A， VETTERLING W T， et al. Numerical recipes in C： the art of scientific computing [M]. 2nd ed. Cambridge： Cambridge University Press， 1992.

[17]GORDON N， SALMOND D， EWING C. Bayesian state estimation for tracking and guidance using the bootstrap filter [J]. Journal of Guidance， Control and Dynamics， 1995， 18（6）： 1434-1449.