后向平滑容积滤波算法处理与分析组合导航数据

2015-05-06 02:41MarcinURADZINSKIJacekRAPISKI洪海斌
导航定位学报 2015年1期
关键词:全球定位系统卡尔曼滤波容积

燕 欢,郭 杭,Marcin URADZINSKI,熊 剑,Jacek RAPISKI,洪海斌

(1.南昌大学,南昌 330031;2.University of Warmia and Mazury in Olsztyn,Poland)

后向平滑容积滤波算法处理与分析组合导航数据

燕 欢1,郭 杭1,Marcin URADZINSKI2,熊 剑1,Jacek RAPISKI2,洪海斌1

(1.南昌大学,南昌 330031;2.University of Warmia and Mazury in Olsztyn,Poland)

通过研究采用经过改进的容积卡尔曼滤波算法,结合后向平滑滤波的思想联合解算全球定位系统/惯性导航系统数据。两次车载实验在波兰进行,分别是波兰研发的惯性传感器/单频全球定位系统和国产双频双模全球定位系统/惯导组合导航系统与差分全球定位系统数据比较的车载实验。并将实验得到的数据进行如下处理:其一是将单频全球定位系统/惯导联合解算后的结果,与差分全球定位系统数据进行对比;其二是用国产组合导航系统解算结果与差分数据结果进行对比。经处理后发现,低精度的单频全球定位系统/惯导解算的结果,用后向平滑容积卡尔曼滤波算法处理后精度得以提高,达到了中等精度的国产组合导航系统同样的精度。这个结论证明后向平滑滤波结合容积卡尔曼滤波是解算车载组合导航数据的有效方法。

后向平滑滤波;容积卡尔曼滤波;惯性导航系统;差分定位

1 引言

全球定位系统(global positioning system,GPS)与惯性测量单元(inertial measurement unit,IMU)组合导航[1]是指将GPS接收机接收到的数据和惯性导航传感器得到的信息通过一定算法结合起来的组合导航方法,利用其性能上的互补特性,获得比使用其中任意一种系统时更高的导航性能。由于GPS有全天候,全时间的特点,且定位精度比较高,所以用它对IMU进行补偿,并且在依赖一定算法的条件下,可以提高导航精度。目前这类组合导航的方法已经广泛运用于航空、航天、汽车、船舶、军事等多个领域。对于近几年组合导航取得的快速发展,高精度,高稳定性的组合导航算法也越来越受到人们的关注。

容积卡尔曼滤波(cubature Kalman filter,CKF)是一种经过改进的新型的非线性滤波方法[2]。其利用数值积分原则对目标状态的后验概率进行近似。此算法相对于扩展卡尔曼滤波有较好的收敛效果,且实现较为简单,因此成为联合解算的重要算法。此次试验处理中,在容积卡尔曼滤波的基础上加入了平滑的后向递归运算[3],有效减少了观测误差和观测噪声的影响,在提高了精度的基础上加快了收敛速度。

2 容积卡尔曼滤波

首先考虑如下的非线性系统的状态方程和观测方程为

Xk+1=f(x)Xk+GWk

(1)

Zk=HkXk+Vk

(2)

式(1)及式(2)中:Xk为系统的状态向量,Zk为系统的观测向量,假定过程噪声Wk和观测噪声Vk相互独立,且分别服从Wk~N(0,Qk);Vk~N(0,Rk)。

有积分

(3)

式(3)中,I(f)为待积分量,f(x)为系统矩阵,容积卡尔曼滤波首先计算加权函数为标准正态分布密度的积分的基本容积点和对应的权值,采用2n个等权值的容积点{ξj,ωj},即有

(4)

容积点及其对应的权值分别为

(5)

式(5)中,m表示容积点总数,采用三阶容积原则,容积点总数是状态维数的2倍,m=2n;[1]j表示容积点集中地第j个元素。容积卡尔曼算法流程如下5.1和5.2所述。

2.1 时间更新

1)计算容积点

(6)

(7)

式(7)中,Sk-1|k-1=chol(Pk-1|k-1),chol()函数表示对矩阵进行Cholesky分解,应用于对状态方程容积点的计算。

2)计算通过状态方程传播容积点

(8)

3)估计k时刻的状态预测值为

(9)

4)估计k时刻的状态误差协方差预测值为

(10)

式(10)中 ,Qk-1阵为Wk阵的协方差矩阵。

2.2 量测更新

通过cholesky分解可得

(11)

式(11)中,Sk|k-1=chol(Pk|k-1),chol()函数表示对矩阵进行Cholesky分解,应用于对量测方程容积点的计算。

(12)

式(12)中,ξi表示加权后容积点的总数。

可以通过观测方程传播容积点,即

(13)

估计k时刻观测预测值

(14)

估计自相关协方差阵为

(15)

式(15)中,Rk阵为Vk阵的协方差矩阵

估计互相关协方差阵

(16)

估计卡尔曼增益

(17)

综合式(6)~式(17)可得K时刻状态估计

(18)

k时刻协方差阵估计值为

(19)

3 后向平滑卡尔曼滤波

由于后向平滑滤波[4]是根据之前的滤波进行的递归运算,且经过前向滤波可以消除大部分的误差,所以将平滑后的值重新作为起始值就可以进一步地提高精度。所以结合后向平滑的思想与容积卡尔曼滤波的算法进行融合,算法如下:由k-1时刻的状态经过一次 CKF滤波得到k时刻的状态估计,再经过后向平滑过程更新k-1时刻的估计值,最后将更新后的k-1时刻的平滑值进行第二次CKF滤波得到k时刻的状态估计。

状态后向平滑表达式为

(20)

式(20)中,A(k-1)是后向平滑增益。协方差后向平滑表达式为

Pk-1|k=Pk-1|k-1+Ak-1[Pk-k-Pk|k-1]Ak-1

(21)

经过一次前向滤波和一次后向平滑,就可以解算出当前时刻状态估计的实时量。

4 GPS/IMU联合解算

参照文献[2]GPS/IMU联合解算过程,利用波兰研发的单频GPS定位接收机LEI输出的载体的位置信息与ADISIMU联合解算的位置之差作为观测值[5],以ADIS的速度误差、位置误差、姿态误差、陀螺漂移以及加速度零偏等作为状态量,通过后向平滑容积卡尔曼滤波估计出状态误差值,最后通过闭环反馈或者开环反馈对ADIS进行校正[6],最终获得载体的准确位置、速度和姿态信息。其原理图如图1所示。

图1 GPS/IMU联合解算框图

对于状态量

X=[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh,
ζx,ζy,ζz,▽x,▽y,▽z]

(22)

式(22)中,F(t)为状态转移矩阵,G(t)为误差系数矩阵[7],W(t)为白噪声。离散化为

(23)

对于量测方程

(24)

(25)

(26)

式(23)~式(26)中,Q为功率谱密度矩阵,σa和σω分别表示加速度计和陀螺仪的标准偏差[8];Δt为采样间隔;Rk为观测噪声矩阵,σφ、σλ和σh分别表示经度、纬度和高度标准偏差。

5 实测数据处理与分析

数据处理与分析是基于上述的两次车载实验进行的,实验设备分别为ADIS惯性导航仪和LEIGPS接收机组合系统以及国产组合导航系统XW-ADU5660(IMU/GPS组合导航装置)[9]。其中,ADIS惯性导航仪为低精度的惯性导航仪器,其初始对准时间为30s(粗对准时间为10s,准确的对准时间是20s)。陀螺零偏稳定性为0.015°/s。加速度零偏是1.7m/s2,随机噪声是0.135。LEIGPS接收机为单频接收机,其定位误差为2.5m,频率为2Hz。两种传感器均为低精度的惯性导航仪器。XW-ADU5660是中等精度的姿态方位组合导航系统,可以提供载体在静止和运动过程中的方位、姿态、位置和时间等信息,由双天线、GPS、格洛纳斯卫星导航系统[10](globalnavigationsatellitesystem,GLONASS)双频卫星信号接收机、IMU和导航计算机组成。其卫星定位精度较高,单点位置精度(GPS有效):水平位置误差2.4m,速度精度为0.2m/s。方位角误差小于0.12°,姿态角误差小于0.1°。陀螺零偏为10°/h,加速度计零偏小于0.1mm/s2。

实验在波兰UniversityofWarmiaandMazuryinOlsztyn的Kortowo校区内进行,两次车载实验分别利用ADIS惯导/单频LEIGPS组合结果和XW-ADU5660(IMU/双频双模GPS)的输出结果与差分GPS结果进行对比,实验过程详见图4:

图2 车内XW-ADU5660安置图

图3 车顶天线安置

图4 ADIS惯导和LEi GPS接收机安置

图5 车载实验轨迹

从谷歌地图截取整个实验区域的影像图片,并绘制整个车载实验过程的行迹路线图,如图5。

5.1 单频全球定位系统/惯导联合解算的对比分析

实验结束后,将LEI GPS数据和ADIS惯性导航仪器进行IMU/GPS联合解算[8],其算法即为本文建议的后向平滑容积卡尔曼滤波,将处理后的位置、速度和差分GPS位置、速度进行对比。当地的差分GPS位置作为此次实验的标准值。结果如下:

图6 ADIS/GPS处理后的轨迹与差分GPS比较

图7 处理后的速度

图8 处理后的位置与差分GPS位置误差

表1 经处理后的位置与差分GPS位置误差统计/m

北东天误差最大最小平均标准差N方向8.170.011.752.37位置误差E方向10.160.011.552.23U方向8.881.903.712.05

5.2 双频双模导航系统的对比分析

为了与中等精度的车载组合导航实验结果比较,进行了双频双模XW-ADU5660与差分GPS的位置比较实验。由于XW-ADU5660测得的数据GPS时间起始点与差分数据不一样,且XW-ADU5660数据的采样频率较高与差分GPS频率不一样,所以剔除了XW-ADU5660一部分数据,使二者从相同的时间点开始比较,时间周期也处理成同步的。比较结果如图9所示。

图9 差分GPS与XW-ADU5660的轨迹比较

图10 差分GPS与XW-ADU5660的位置误差比较

表2 XW-ADU5660与差分GPS结果相比较后的误差统计/m

通过将两次实验得出的误差进行对比(参见表1和表2):经后向CKF卡尔曼滤波处理的ADIS/LEI GPS数据与差分GPS的位置比较,其标准误差,N方向为2.37 m,E方向为2.23 m,U方向为2.05 m;双频双模XW-ADU5660与差分GPS位置相比较,它们的标准误差,N方向为2.89 m,E方向为1.61 m,U方向为1.95 m。由此可见:后向平滑的CKF算法对误差经行了两次滤波,很大程度上弥补了低精度组合导航设备的不足,使得其精度接达到了中等精度的组合导航仪器XW-ADU5660类似的精度。这说明用后向平滑CKF滤波算法处理低精度的IMU/GPS组合系统(ADIS和LEI)数据,解算结果其位置的精度有了提高。这个结论证明后向平滑滤波结合容积卡尔曼滤波是解算车载组合导航数据的有效方法。

6 结束语

通过实验结果可以看出,经后向CKF卡尔曼滤波处理的ADIS/LEI IMU/GPS数据与差分GPS位置的标准偏差,N方向为2.37 m,E方向为2.23 m,U方向为2.05 m;双频双模XW-ADU5660与差分GPS结果相比较的位置标准误差,N方向为2.89 m,E方向为1.61 m,U方向为1.95 m。后向平滑CKF卡尔曼滤波算法由于其两次平滑滤波以及容积卡尔曼本身的优越性,使得在处理低精度ADIS惯导和LEI GPS接收机的组合导航数据时,基本接近了中等精度的XW-ADU5660导航设备的精度,是一种有效的方法。

[1] 秦永元,汪淑华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社,2010.

[2] 霍光,李冬梅.基于后向平滑容积卡尔曼滤波的单站无源定位算法[D].郑州:解放军信息工程大学,2013.

[3] 董旭荣,张守信,华仲春.GPS/INS组合导航定位及其应用[M].长沙:国防科技大学出版社,1998:12-13.

[4] 高为广,杨元喜,崔先强,等.IMU/GPS组合导航系统自适应Kalman滤波算法[J].武汉大学学报:信息科学版,2006,31(5):3-8.

[5] 刘胜,杨丹,苏旭,等.随机系统高权值便捷UKF滤波方法:中国,CN201310241427.6[P].2013-10-02.

[6] 常志海.导航定位产业的现状和发展趋势[EB/OL].[2014-05-12].http://www.inav.cn/Article/Details/261.

[7] 王睿.基于多传感器融合的定位系统设计与实现[D].北京:北京邮电大学,2012

[8] SAVAGE P G.Strapdown Inertial Navigation Integration Algorithm Design Part 2:Velocity and Position Algorithms[J].Journal of Guidance,Control and Dynamics,1998,21(2):208-221.

[9] BRITTING K R.Inertial Navigation Systems Analysis[M].Hoboken,NJ:John Wiley & Sons Inc,1971.

[10]宁津生,姚宜斌,张小红.全球导航卫星系统发展综述[J].导航定位学报,2013,1(1):3-8.

收稿日期:2014-05-26

第一作者简介:杨安洪(1983),男,四川彭山人,工程师,研究方向为航天测控。

GPS/IMU Data Processing and Analysis with Back Smoothing and CKF Filtering Algorithm

YAN Huan1,GUO Hang1,Marcin URADZINSKI2,XIONG Jian1,Jacek RAPISKI2,HONG Hai-bin1

(1.Nanchang University,Nanchang 330031,China;2.University of Warmia and Mazury in Olsztyn,Poland)

Using the combined volume backword smoothing Kalman filter (CKF) algorithm for processing vehicle navigation data,the result accuracy is higher than that of using the Kalman filter forward only.In this paper,GPS/IMU data come from two vehicle experiments conducted in Poland,namely LEI and ADIS integrated system (single frequency GPS/IMU),XW-ADU5660 (double frequency GPS,GLONASS/IMU) compared with the differential GPS car experiments.And experimental data obtained are processed as follows:one is the result of LEI GPS and ADIS IMU compared with the results of differential GPS positioning data;the other is XW-ADU5660 results compared with differential GPS.The results show that low accurate LEI and ADIS integrated system with the backword CKF smoothing filter algorithm reached the same level of medium accurate navigation instrument XW-ADU5660.

backward smoothing filter;volume Kalman filter;inertial navigation system;GPS differential positioning

2014-06-30

国家自然科学基金(41164001、41374039)。

燕欢(1991),男,山西运城人,硕士生,主要从事惯性导航、组合导航的研究。

P228

A

2095-4999(2015)-01-0083-05

猜你喜欢
全球定位系统卡尔曼滤波容积
全球定位系统助力黑鹳保护
基于深度强化学习与扩展卡尔曼滤波相结合的交通信号灯配时方法
320排CT低剂量容积体部灌注成像强化峰值时间对孤立性周围肺病变诊断价值
怎样求酱油瓶的容积
格拉迪丝·韦斯特:协助开发全球定位系统的隐藏人物
脉冲星方位误差估计的两步卡尔曼滤波算法
卡尔曼滤波在信号跟踪系统伺服控制中的应用设计
全球定位系统
蘑菇点点
基于递推更新卡尔曼滤波的磁偶极子目标跟踪