基于粒子滤波的INS/磁力计融合定位算法

夏景平,胡辉,欧敏辉,颜瑜军

(华东交通大学 信息工程学院,江西 南昌 330013)

基于粒子滤波的INS/磁力计融合定位算法

夏景平,胡辉,欧敏辉,颜瑜军

(华东交通大学 信息工程学院,江西 南昌 330013)

针对城市峡谷场景中GPS信号容易受到建筑物的遮挡、反射,导致智能终端的GPS定位精度降低甚至无法定位的问题,本文在分析城市峡谷场景中GPS定位误差的基础上,在智能终端上实现了基于粒子滤波融合INS输出的水平速度和磁力计方位角的多传感器定位算法。实验结果表明,该算法的平均定位误差是3.19 m,相比于GPS的13.81 m,降低了76.9%,相比于EKF和UKF融合算法的4.84 m和4.82 m,分别降低了34.1%和33.8%.

多传感器融合;粒子滤波;城市峡谷;智能终端

0 引 言

随着智能终端的普及和通信技术的迅猛发展,智能终端的位置信息在基于位置的服务(LBS)应用中发挥着举足轻重的作用[1]。智能终端的全球卫星导航系统(GNSS)能为人们提供了全天候、实时的位置和时间信息,在空旷场景下定位性能良好,然而在高楼密集、高楼间距离短的城市峡谷环境下,卫星分布在与道路平行的“条状”天空,导致多径干扰和几何精度因子过大,卫星定位的精度会大幅降低,甚至无法定位[2]。

针对城市峡谷中GNSS定位精度低的问题,Groves提出了阴影匹配(SM)算法,该算法利用三维建筑模型预测卫星可见性,通过与实际观测卫星可见性对比来实现定位,静态实验结果在过街方向能达到米级的定位精度,但国内匮乏的三维建筑模型限制了该算法的推广[3]。随着磁力计、陀螺仪、加速度计(MARG)等传感器大量集成在车载导航系统和智能终端上,基于传感器定位技术的研究受到了国内外学者的广泛关注。Dai等通过消除传感器测量的异常值来减少惯性导航系统(INS)的累积误差,设计了基于卡尔曼滤波(KF)的INS/DR组合定位方法,但其高精度的传感器设备和模型累积误差限制了大量的应用场景[4]。刘兴川等基于KF和粒子滤波(PF)的自适应加权融合算法,设计适用于智能终端的WLAN/MARG/GPS组合定位系统,相比WLAN/MARG定位子系统和GPS/MARG定位子系统的平均定位误差分别减少了52%和63%,但WLAN/MARG定位子系统需要预先存储参考点位置的信号强度指纹信息数据库,不适用于当前市场[5]。当传感器数量逐渐增多,扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)等虽然能融合多传感器数据在一定程度上提高定位性能,但当模型的非线性化程度严重、噪声复杂非高斯时,上述方法定位性能严重下降[6]。此外,PF利用状态空间中的随机样本来近似的表示概率密度函数,较好地解决了模型近似非线性非高斯所引起的误差。Tehrani等利用PF融合GPS、惯性测量单元、里程计和激光扫描仪数据来提高定位精度,实验表明，该方法比GPS在X和Y方向平均定位误差分别降低了0.83 m和1.44 m[7],但目前这些组合定位方法均需要昂贵的传感器,不适用于市场的广泛性。

针对以上问题,本文在分析城市峡谷场景中GPS定位误差的基础上,在智能终端上实现了基于粒子滤波融合INS输出的水平速度和磁力计方位角的多传感器定位算法。智能终端的磁力计数据较准确地反映运动方向,智能终端加速度计和陀螺仪组成的INS输出的水平速度较符合真实运动状态,本文在城市峡谷中使用PF融合智能终端INS输出的水平速度和磁力计方位角数据来实现定位。通过真实城市峡谷下的实验表明,该算法比使用EKF、UKF进行数据融合定位误差更小,相比于GPS的定位误差有很大程度的降低,具有实际参考价值。

1 粒子滤波融合定位算法设计

PF是一种基于蒙特卡罗方法和递推贝叶斯估计的统计滤波方法,通过对粒子后验概率的求解,得到目标状态的最优估计,广泛适用于非线性非高斯系统中。其基本思想是:首先依据系统状态向量的经验条件分布,在状态空间产生一组随机样本的集合(也称粒子),并以样本均值代替积分运算,然后根据观测量,不断地调整粒子的权重和位置,通过调整后的粒子信息修正最初的条件分布。当粒子数足够多时,修正后的经验条件分布将收敛于系统状态向量真实的条件分布,而状态向量的估计值可以通过粒子的均值近似得到[8]。

本文使用PF融合INS输出的水平速度和磁力计方位角来提高定位精度,并采用PV(Position,Velocity)模型来模拟行人在城市峡谷场景中使用智能终端定位,定义系统状态向量为Xk,观测向量为Zk.

Xk=[xa,k,va,k,xc,k,vc,k]T,

(1)

Zk=[vk,θk]T,

(2)

式中:xa,k和xc,k分别为沿街和过街的位置分量;va,k和vc,k分别为沿街和过街的速度分量;vk为智能终端INS输出的水平方向速度;θk为磁力计输出的方位角。本文设计的PF算法分为以下7个步骤:

1) 初始化

2) 系统状态更新

假设在k-1时刻行人的位置坐标为(xa,k-1,xc,k-1),沿街和过街方向的速度分别为va,k-1和vc,k-1,在考虑系统噪声的情况下,系统状态更新方程为

(3)

式中:T为时间间隔;σx,a、σv,a、σx,c、σv,c为系统噪声。

3) 系统观测更新

联立式(1)、式(2),通过磁力计观测的方位角将INS输出的水平速度分解到沿街和过街方向,则k时刻系统观测量更新方程为

(4)

式中,εv、εθ为运动速度和磁力计方位角的观测噪声。

4) 粒子权值计算及归一化

(5)

(6)

5) 粒子重采样

本文采用随机重采样法来进行重采样[9],即随机产生N个[0 1]区间内的数,当粒子的权重大于随机数时被保留,则权重较大的粒子会被多次复制,权重较小的粒子将逐渐消失。

6) 状态估计输出

通过更新后的粒子状态和权重,加权统计得到系统当前状态Xk:

(7)

7) 判断是否结束。若是,则退出算法;否则,令k=k+1,返回步骤2)。

2 实验验证及结果分析

实验场景如图1所示,起始位置A坐标为28°44′52.7594″ N,115°51′40.0041″ E,44.85 m,终止位置B坐标为28°44′52.7180″ N,115°51′44.0702″ E,45.12 m,实验路径AB距离为110.33 m. 通过4台南方S82RTK及进行静态联测,获取基准点A和B坐标,并在基准点上架设徕卡TS06全站仪测量建筑物顶点坐标,建立了3D建筑物轮廓模型[10],如图2所示。

智能终端选用小米2S手机,其中GPS的数据更新率为1 Hz,加速度计、陀螺仪、磁力计的数据更新率为10 Hz,采集的数据长度为85 s。

图1 实验路径A-B和城市峡谷实景图

图2 3D建筑物轮廓模型

在图1所示实验路径的C点和D点,利用建筑物轮廓模型计算建筑物的边界仰角如图3、图4所示,GPS星空分布图由卫星历书计算得到,其中封闭曲线内的卫星可见。可知手机进入峡谷后由于建筑物的遮挡导致GPS可见卫星数减少到3颗,故手机接收了不可见卫星的非直达信号用于定位,导致多径误差影响定位精度,定位结果如图5所示。

图3 C点建筑物边界仰角与GPS星空分布

图4 D点建筑物边界仰角与GPS星空分布

图5 GPS在沿街和过街方向定位误差

图5示出了手机GPS的定位误差,第20 s手机到达实验路径上的C点,此时即将进入峡谷,由于建筑物的影响GPS定位误差开始变大。第53 s手机到达实验路径上的D点,可知峡谷中GPS的定位误差明显高于峡谷外的C点,其中沿街方向最大误差为7.1 m,过街方向最大误差为21.6 m. 本文从C点即运动20 s后开始使用PF融合定位算法。

由图6所示的方位角对比可知,手机进入峡谷后GPS定位误差的增大使GPS方位角发生剧烈波动,最大方位角达到176.9°,严重偏离真实运动方向,而磁力计输出的方位角在参考方位角的91.2°上下波动,平均值略低于参考方位角,较符合运动情况。

图7示出了速度对比,初始时刻即将运动GPS输出速度为0,第3 s之后GPS速度保持为恒定的1.25 m/s,一直小于参考速度的1.31 m/s.从第20 s开始,手机通过加速度计和陀螺仪采集载体数据来进行惯性导航定位, 可知手机INS输出的水平速度在参考速度1.31 m/s上下波动,其均值为1.30 m/s,较符合运动情况。

图6 GPS和磁力计方位角对比

图7 GPS速度与INS水平速度对比

从第20 s开始,本文采用PF融合INS输出的水平速度和磁力计方位角定位,如图8所示,EKF、UKF、PF三种滤波方法在沿街方向的定位误差上下波动并逐渐收敛到真实值附近,相对GPS定位误差波动幅度较小,定位误差较GPS分别降低了1.90 m,1.71m和2.35 m. 过街方向定位误差如图9所示,可知在手机即将进入峡谷时GPS定位误差迅速大幅增加,而三种滤波方法的定位误差波动幅度较小,远低于GPS的定位误差。

如图10所示,手机进入峡谷后三种滤波方法的绝对定位误差波动幅度较小,均远低于GPS,其中PF融合算法的定位误差最小。由表1所示的定位误差统计可知,本文使用PF进行数据融合的绝对定位误差为3.19 m,低于EKF的4.84 m和UKF的4.82 m,这是由于模型的非线性和噪声的不确定性,PF较EKF、UKF有更好的滤波性能。综上,使用PF融合INS输出的水平速度和磁力计方位角的定位误差明显低于GPS,具有实际意义。

图8 INS水平速度/磁力计融合沿街方向误差对比

图9 INS水平速度/磁力计融合过街方向误差对比

图10 INS水平速度/磁力计融合定位误差对比

表1 定位误差统计

3 结束语

本文基于建筑物模型和GPS星空分布情况,分析城市峡谷场景中GPS定位误差,并在智能终端上实现了基于粒子滤波的INS/磁力计融合定位算法。该算法充分利用智能终端传感器数据,通过PF融合智能终端INS输出的水平速度和磁力计方位角数据来实现定位,在真实的城市峡谷中,基于智能终端平台进行实验验证。实验结果表明本文提出的算法比GPS的定位误差降低了76.9%,相比于EKF、UKF融合算法的定位误差分别降低了34.1%和33.8%,证实了智能终端上多传感器融合定位的有效性。

[1] KIM D, LEE S, BAHN H. An energy-efficient positioning scheme for location-based services in a smartphone[C]//IEEE, International Conference on Embedded and Real-Time Computing Systems and Applications. IEEE Computer Society, 2016:139-148.

[2] HSU L T, GU Y, KAMIJO S. NLOS correction/exclusion for GNSS measurement using RAIM and city building models[J]. Sensors, 2015, 15(7):17329-49.

[3] GROVES P D, WANG L, WALTER D,etal. The four key challenges of advanced multisensor navigation and positioning [C]//Position, Location and Navigation Symposium-PLANS 2014, 2014 IEEE/ION. IEEE, 2014:773-792.

[4] DAI H, LI J X, JIN H M. Application of robust kalman filtering to integrated navigation based on inertial navigation system and dead reckoning[C]//International Conference on Artificial Intelligence and Computational Intelligence， 2010:189-193.

[5] 刘兴川,吴振锋,林孝康. 基于自适应加权算法的WLAN/MARG/GPS组合定位系统[J]. 清华大学学报(自然科学版), 2013(7):955-960.

[6] 刘德春,谭信. 非线性滤波算法性能对比[J]. 电子设计工程, 2011, 19(13):49-51.

[7] TEHRANI H, MITA S, LONG H. Multi-sensor data fusion for autonomous vehicle navigation through adaptive particle filter[C]//Intelligent Vehicles Symposium (IV), 2010 IEEE, 2010: 752-759.

[8] 赵琳. 非线性系统滤波理论[M]. 北京：国防工业出版社, 2012.

[9] 周瑞,李志强,罗磊. 基于粒子滤波的WiFi行人航位推算融合室内定位[J]. 计算机应用, 2016,36(5):1188-1191.

[10]胡辉,颜瑜军,欧敏辉. 一种基于EKF的GPS/SM组合定位算法[J]. 全球定位系统, 2016, 41(2):7-13.

INS/Magnetometer Fused Localization Algorithm Based on Particle Filter

XIA Jingping,HU Hui,OU Minhui,YAN Yujun

(SchoolofInformationEngineering,EastChinaJiaotongUniversity,Nanchang330013,China)

GPS signals are more likely to be blocked or reflected by tall buildings in urban canyons, resulting in poor positioning accuracy even positioning failure to smart terminal. Based on the analysis of the GPS location error in urban canyons, horizontal velocity of INS and Azimuth of magnetometer data are fused by particle filter to positioning based on smart terminal. Experimental results show that the average error of proposed algorithm is 3.19m,which is reduced by 76.9% compared to 13.81 m of GPS, and also reduced 34.1% and 33.8% compared to 4.84 m of EKF and 4.82m of UKF fusion algorithm.

Multi-sensor fusion; particle filter; urban canyons; smart terminal

10.13442/j.gnss.1008-9268.2017.03.001

2016-12-05

江西省自然科学基金(编号：20142BAB207001); 江西省教育厅科学技术研究项目(编号：GJJ14369)

TN961

A

1008-9268(2017)03-0001-06

夏景平 (1990-),男,硕士研究生,主要研究方向为卫星导航定位。

胡辉 (1970-),男,博士,教授,主要研究方向为卫星导航定位、并行算法与并行处理、机器视觉。

欧敏辉 (1990-),男,硕士研究生,主要研究方向为卫星导航定位。

颜瑜军 (1990-),男,硕士研究生,主要研究方向为卫星导航定位。

联系人: 胡辉 E-mail: gnss523@163.com