基于粒子濾波的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