车辆组合导航系统自适应UKF滤波算法

陆文昌，费龑栋，陈 龙，汪若尘

(江苏大学汽车与交通工程学院，江苏镇江212013)

车辆组合导航中Global Positioning System/Dead Reckoning(GPS/DR)是较常用的组合导航方法。GPS/DR组合导航定位滤波算法多采用扩展卡尔曼滤波(EKF)或改进EFK实现［1-2］。EKF方法忽略了泰勒展开式的高次项，当误差较大，泰勒展开式高次项不可忽略时，滤波精度会降低，不稳定甚至发散。Julier和Uhlmann提出的无迹卡尔曼滤波Unscented Kalman filter(UKF)算法［3-4］，UKF 算法基于 UT 变化，利用非线性模型，避免引入线性化误差，从而提高了滤波精度，并且省去计算Jacobi矩阵的过程，更加容易实现。

但UKF算法是基于精确的数学模型和过程噪声和观测噪声已知为基础的，当噪声统计特性将发生变化，UKF算法的精度和稳定性将会降低。针对此问题，S.Ranji，等［5］提出了利用神经网络理论跟踪噪声统计特性;石勇，等［6］提出了一种改进的Sage-Husa滤波;周卫东，等［7］提出了 PSO-UKF 组合滤波，都取得了一定的成效。

笔者利用一种改进的开窗估计算法，对噪声进行实时跟踪，而后利用在线估计得到的噪声协方差矩阵进行UKF滤波，从而得到状态估计值与预测值，并与UKF方法进行对比。

1 GPS/DR组合导航系统建模

1.1 系统状态方程

在陆地上车辆的运动可以近似认为是在二维平面内的运动，且可分解为东西方向的运动和南北方向的运动。采用“当前”统计模型［8-9］，系统状态向量 X=［xeveaexnvnanε ψ］T，其中：xe，ve，ae表示载体东向位置分量、速度分量、加速度分量;xn，vn，an表示载体北向位置分量、速度分量、加速度分量;ε为陀螺仪的漂移误差;ψ为里程仪标定系数。

系统的状态方程为：

设采样周期为T，将式(1)离散化后得离散化方程：

式中：τae，τan分别为载体东向北向加速度变化率的相关时间常数;τε为陀螺仪的1阶马尔科夫常数;分别为载体“当前”东向、北向加速度分量的均值;ωae，ωan，ωε，ωψ分别为均值为 0、方差为，，，的高斯白噪声。

1.2 系统的观测方程

GPS接收机输出的信号分别为xeo，xno，单位为m。陀螺仪输出角速度ω，里程仪在一个采样周期内输出距离S，令：式中：ωye，ωyn，εyω，εys分别为东向、北向、陀螺仪和里程仪的观测噪声，其近似均值为0、方差为，，，的高斯白噪声。

观测方程离散化得到非线性方程：

2 系统滤波算法设计

滤波算法流程如图1。

图1 滤波算法流程Fig.1 Filter algorithm process

2.1 UKF 滤波算法过程［10-11］

1)状态初始化

初始估计值

初始方差：

2)取 k－1 时刻 Sigma点 χk－1，则：

权值选取：

式中：尺度因素λ=α2(n+Γ)－n，Γ取0，α为一很小正数，β在高斯分布下最优取2。

3)系统时间更新

4)系统测量更新

由于现实车辆运动中噪声协方差矩阵具有随机性和时变性，在传统UKF滤波过程中，通过先验公式确定的噪声统计特性在整个过程中是不变的，故当噪声协方差矩阵变化时，UKF滤波系统的精度和鲁棒性会产生影响。

2.2 系统误差协方差矩阵跟踪

由于“当前”统计模型系统误差在观测周期内为常量或围绕常量变化，可以采用固定窗口内取平均值对误差协方差矩阵进行拟合［12］。通常情况下，开窗大小N的取值决定了估计的精度。N取越大，精度越高［13］，但同时计算量也加大。因此，笔者提出一种自适应方法来动态调整开窗大小，平衡滤波精度与算法计算速度：

式中：o(k)为k时刻系统的更新权值;N0为开始时刻设定开窗大小值;r(k)为分配系数，0≤r(k)≤1。当车辆行驶姿态稳定时，分配系数r(k)取较小值。

2.2.1 观测噪声协方差矩阵跟踪根据开窗估计法以及新息序列的定义：

利用SIGMA点集，得到UKF算法中，新息序列的协方差矩阵：

实时估计的协方差为：

式中：z0=k－Nk+1。观测噪声向量协方差矩阵Rk可以根据式(21)、式(22)在线估计得：

由式(23)可以得到由当前观测新息以及之前的迭代新息对观测噪声进行协方差在线跟踪后得到的估计值。

2.2.2 过程噪声协方差矩阵跟踪

模型系统误差为qk，残差Vk，在tk－i时刻：Vk=

由于“当前”统计模型在短时间内误差在微小范围内浮动，故满足E(V^Xk－i)=0，根据开窗估计原理，并由式(24)能得到模型系统误差qk的无偏估计：

将式系统方程(2)代入式(25)，令 Uk=0，有：

状态向量预报值 ^Xk的预报残差V^Xk协方差矩阵：

取

又根据系统状态方程(2)以及式(24)、式(26)和式(28)得到：

可以利用上式求得状态方程过程噪声协方差矩阵PX^k，并利用所求得的PX^k进行标准UKF滤波。

3 系统仿真与分析

为了验证设计滤波系统的性能，比较与传统UKF方法的滤波效果，设计了如下的仿真试验。滤波时间为200 s，采样周期为1 s，车辆以东向速度10 m/s，北向速度10 m/s，做匀速直线运动，仿真参数：过程噪声观测噪声m/s2)2。时间常数τae=τan=1 s，开窗大小Nk取固定值 10，状态初值取 X0=［0，10，0，0，10，0，0，0］T。其中实际过程噪声Qk和观测噪声Rk按如下规律变化：

分别采用UKF和自适应UKF方法进行MATLAB仿真，得到如图2的仿真曲线。从图2东向位置误差上来看，从50 s开始当噪声统计特性变化以后，传统UKF开始有较大震荡而自适应UKF滤波能保持较好的收敛性能，滤波精度更高，误差基本维持在3 m以内。

图2 东向、北向位置误差(自适应UKF/UKF)Fig.2 Position error in north direction(adaptive UKF/UKF)

通过对表1与图2的观察，可以看出，当噪声统计特性存在变化时，自适应UKF对比UKF有着明显的优势。UKF算法会出现局部发散，整体震荡的现象。自适应UKF方法，加入了误差估计，虽然增加了算法计算时间，但比UKF方法精度更高，鲁棒性更强。

表1 东向、北向误差统计对比Table 1 East lnorth error statistics comparison

4 结语

针对车辆定位过程中的噪声扰动问题，将基于开窗估计观测噪声和过程协方差矩阵跟踪的自适应UKF滤波算法应用于陆地车辆GPS/DR组合导航定位过程。并通过MATLAB软件进行仿真试验，结果表明，自适应UKF算法要优于传统UKF。

［1］房建成，申功勋，万德钧.一种自适应联合卡尔曼滤波器及其在车载GPS/DR组合导航系统中的应用研究［J］.中国惯性技术学报，1998，6(4)：1-6.Fang Jianchen，Shen Gongxun，Wan Dejun.An adaptive combined Kalman filter used for GPS/DR integrated navigation system in vehicle［J］.Journal of Chinese Inertial Technology，1998，6(4)：1-6.

［2］刘国海，施维，李康吉.插值改进EKF算法在组合导航中的应用［J］.仪器仪表学报，2007，28(10)：1897-1901.Liu Guohai，Shi Wei，Li Kangji.The application of Interpolation improved EKF algorithm used in the integrated navigation［J］.Journal of Instrument and Meters，2007，28(10)：1897-1901.

［3］Julier S J，Uhlmann J K.A General Method for Approximating Nonlinear Transformations of Probability Distributions［R］.Oxford：Robotics Research Group，Department of Engineering Science，University of Oxford，1994.

［4］Julier S J，Uhlmann J K.A new method for the nonlinear transformation of means and covariance in filters and estimators［J］.IEEE transportation.A.C.，2000，45(3)：476-481.

［5］Ramji S，Latha G，Ramakrishnan S.Estimation and interpolation of underwater low frequency ambient noise spectrum using artificial neural networks［J］.Applied Acoustics，2009，70(8)：1111-1115.

［6］石勇，韩崇绍.自适应UKF算法在目标跟踪中的应用［J］.自动化学报，2011，37(6)：755-759.Shi Yong，Han Chongshao.Adaptive UKF algorithm used in target tracking［J］.ACTA Automatica Sinica，2001，37(6)：755-759.

［7］周卫东，吉宇人，乔相伟，等.基于修正PSO-UKF的SINS/GPS组合导航滤波算法［J］.中国惯性技术学报，2011，19(5)：565-570.Zhou Weidong，Ji Yuren，Qiao Xiangwei，et al.An SINS/GPS integrated navigation filter algorithm based on modified PSP-UKF［J］.Journal of Chinese Inertial Technology，2011，19(5)：565-570.

［8］张文乾.基于GPS/DR和GSM的车辆定位导航系统设计与实现［D］.哈尔滨：哈尔滨工程大学，2009.Zhang Wenqian.The Design and Implementation of Vehicle Navigation System Based on GPS/DR and GSM［D］.Harbin：Harbin Engineering University，2009.

［9］岳海波.GPS/DR车载组合导航定位系统研究［D］.南京：南京理工大学，2009.Yue Haibo.Research of Vehicle Integrated Navigation Positioning System Based on GPS and DR［D］.Nanjing：Nanjing University of Science and Technology，2009.

［10］陈阳舟，孙章固，马海波.基于平方根UKF的车辆组合导航［J］.系统工程与电子技术，2008，33(5)：926-928.Chen Yangzhou，Sun Zhanggu，Ma Haibo.Vehicle integrated navigation based on square root UKF［J］.Journal of Systems Engineering and Electronics，2008，33(5)：926-928.

［11］刘涛，解永春.UKF稳定性研究及其在相对导航中的应用［J］.宇航学报，2010，31(3)：739-748.Liu Tao，Xie Yongchun.UKF stability study and its application in the relative navigation［J］.Journal Acta Astronautica，2010，31(3)：739-748

［12］杨元喜，张双成.导航解算中的系统误差及其协方差矩阵拟合［J］.测绘学报，2004，33(5)：20-23.Yang Yuanxi，Zhang Shuangchen.The error of navigation system and the calculating and fitting of covariance matrix［J］.Journal of Surveying and Mapping，2004，33(5)：20-23.

［13］许承权，吴文英，吴邵祖.单频GPS精密单点定位的逐次滤波法模型及其精度分析［J］.测绘科学，2010，35(4)：23-25.Xu Chenquan，Wu Wenying，Wu Shaozu.Single frequency GPS precise point positioning of successive filter model and its accuracy analysis［J］.Surveying and Mapping，2010，35(4)：23-25.