基于粒子濾波的INS/磁力計(jì)融合定位算法

夏景平,胡輝,歐敏輝,顏瑜軍

(華東交通大學(xué) 信息工程學(xué)院,江西 南昌 330013)

基于粒子濾波的INS/磁力計(jì)融合定位算法

夏景平,胡輝,歐敏輝,顏瑜軍

(華東交通大學(xué) 信息工程學(xué)院,江西 南昌 330013)

針對城市峽谷場景中GPS信號(hào)容易受到建筑物的遮擋、反射,導(dǎo)致智能終端的GPS定位精度降低甚至無法定位的問題,本文在分析城市峽谷場景中GPS定位誤差的基礎(chǔ)上,在智能終端上實(shí)現(xiàn)了基于粒子濾波融合INS輸出的水平速度和磁力計(jì)方位角的多傳感器定位算法。實(shí)驗(yàn)結(jié)果表明,該算法的平均定位誤差是3.19 m,相比于GPS的13.81 m,降低了76.9%,相比于EKF和UKF融合算法的4.84 m和4.82 m,分別降低了34.1%和33.8%.

多傳感器融合;粒子濾波;城市峽谷;智能終端

0 引 言

隨著智能終端的普及和通信技術(shù)的迅猛發(fā)展,智能終端的位置信息在基于位置的服務(wù)(LBS)應(yīng)用中發(fā)揮著舉足輕重的作用[1]。智能終端的全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)能為人們提供了全天候、實(shí)時(shí)的位置和時(shí)間信息,在空曠場景下定位性能良好,然而在高樓密集、高樓間距離短的城市峽谷環(huán)境下,衛(wèi)星分布在與道路平行的“條狀”天空,導(dǎo)致多徑干擾和幾何精度因子過大,衛(wèi)星定位的精度會(huì)大幅降低,甚至無法定位[2]。

針對城市峽谷中GNSS定位精度低的問題,Groves提出了陰影匹配(SM)算法,該算法利用三維建筑模型預(yù)測衛(wèi)星可見性,通過與實(shí)際觀測衛(wèi)星可見性對比來實(shí)現(xiàn)定位,靜態(tài)實(shí)驗(yàn)結(jié)果在過街方向能達(dá)到米級(jí)的定位精度,但國內(nèi)匱乏的三維建筑模型限制了該算法的推廣[3]。隨著磁力計(jì)、陀螺儀、加速度計(jì)(MARG)等傳感器大量集成在車載導(dǎo)航系統(tǒng)和智能終端上,基于傳感器定位技術(shù)的研究受到了國內(nèi)外學(xué)者的廣泛關(guān)注。Dai等通過消除傳感器測量的異常值來減少慣性導(dǎo)航系統(tǒng)(INS)的累積誤差,設(shè)計(jì)了基于卡爾曼濾波(KF)的INS/DR組合定位方法,但其高精度的傳感器設(shè)備和模型累積誤差限制了大量的應(yīng)用場景[4]。劉興川等基于KF和粒子濾波(PF)的自適應(yīng)加權(quán)融合算法,設(shè)計(jì)適用于智能終端的WLAN/MARG/GPS組合定位系統(tǒng),相比WLAN/MARG定位子系統(tǒng)和GPS/MARG定位子系統(tǒng)的平均定位誤差分別減少了52%和63%,但WLAN/MARG定位子系統(tǒng)需要預(yù)先存儲(chǔ)參考點(diǎn)位置的信號(hào)強(qiáng)度指紋信息數(shù)據(jù)庫,不適用于當(dāng)前市場[5]。當(dāng)傳感器數(shù)量逐漸增多,擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)等雖然能融合多傳感器數(shù)據(jù)在一定程度上提高定位性能,但當(dāng)模型的非線性化程度嚴(yán)重、噪聲復(fù)雜非高斯時(shí),上述方法定位性能嚴(yán)重下降[6]。此外,PF利用狀態(tài)空間中的隨機(jī)樣本來近似的表示概率密度函數(shù),較好地解決了模型近似非線性非高斯所引起的誤差。Tehrani等利用PF融合GPS、慣性測量單元、里程計(jì)和激光掃描儀數(shù)據(jù)來提高定位精度,實(shí)驗(yàn)表明，該方法比GPS在X和Y方向平均定位誤差分別降低了0.83 m和1.44 m[7],但目前這些組合定位方法均需要昂貴的傳感器,不適用于市場的廣泛性。

針對以上問題,本文在分析城市峽谷場景中GPS定位誤差的基礎(chǔ)上,在智能終端上實(shí)現(xiàn)了基于粒子濾波融合INS輸出的水平速度和磁力計(jì)方位角的多傳感器定位算法。智能終端的磁力計(jì)數(shù)據(jù)較準(zhǔn)確地反映運(yùn)動(dòng)方向,智能終端加速度計(jì)和陀螺儀組成的INS輸出的水平速度較符合真實(shí)運(yùn)動(dòng)狀態(tài),本文在城市峽谷中使用PF融合智能終端INS輸出的水平速度和磁力計(jì)方位角數(shù)據(jù)來實(shí)現(xiàn)定位。通過真實(shí)城市峽谷下的實(shí)驗(yàn)表明,該算法比使用EKF、UKF進(jìn)行數(shù)據(jù)融合定位誤差更小,相比于GPS的定位誤差有很大程度的降低,具有實(shí)際參考價(jià)值。

1 粒子濾波融合定位算法設(shè)計(jì)

PF是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法,通過對粒子后驗(yàn)概率的求解,得到目標(biāo)狀態(tài)的最優(yōu)估計(jì),廣泛適用于非線性非高斯系統(tǒng)中。其基本思想是:首先依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布,在狀態(tài)空間產(chǎn)生一組隨機(jī)樣本的集合(也稱粒子),并以樣本均值代替積分運(yùn)算,然后根據(jù)觀測量,不斷地調(diào)整粒子的權(quán)重和位置,通過調(diào)整后的粒子信息修正最初的條件分布。當(dāng)粒子數(shù)足夠多時(shí),修正后的經(jīng)驗(yàn)條件分布將收斂于系統(tǒng)狀態(tài)向量真實(shí)的條件分布,而狀態(tài)向量的估計(jì)值可以通過粒子的均值近似得到[8]。

本文使用PF融合INS輸出的水平速度和磁力計(jì)方位角來提高定位精度,并采用PV(Position,Velocity)模型來模擬行人在城市峽谷場景中使用智能終端定位,定義系統(tǒng)狀態(tài)向量為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為磁力計(jì)輸出的方位角。本文設(shè)計(jì)的PF算法分為以下7個(gè)步驟:

1) 初始化

2) 系統(tǒng)狀態(tài)更新

假設(shè)在k-1時(shí)刻行人的位置坐標(biāo)為(xa,k-1,xc,k-1),沿街和過街方向的速度分別為va,k-1和vc,k-1,在考慮系統(tǒng)噪聲的情況下,系統(tǒng)狀態(tài)更新方程為

(3)

式中:T為時(shí)間間隔;σx,a、σv,a、σx,c、σv,c為系統(tǒng)噪聲。

3) 系統(tǒng)觀測更新

聯(lián)立式(1)、式(2),通過磁力計(jì)觀測的方位角將INS輸出的水平速度分解到沿街和過街方向,則k時(shí)刻系統(tǒng)觀測量更新方程為

(4)

式中,εv、εθ為運(yùn)動(dòng)速度和磁力計(jì)方位角的觀測噪聲。

4) 粒子權(quán)值計(jì)算及歸一化

(5)

(6)

5) 粒子重采樣

本文采用隨機(jī)重采樣法來進(jìn)行重采樣[9],即隨機(jī)產(chǎn)生N個(gè)[0 1]區(qū)間內(nèi)的數(shù),當(dāng)粒子的權(quán)重大于隨機(jī)數(shù)時(shí)被保留,則權(quán)重較大的粒子會(huì)被多次復(fù)制,權(quán)重較小的粒子將逐漸消失。

6) 狀態(tài)估計(jì)輸出

通過更新后的粒子狀態(tài)和權(quán)重,加權(quán)統(tǒng)計(jì)得到系統(tǒng)當(dāng)前狀態(tài)Xk:

(7)

7) 判斷是否結(jié)束。若是,則退出算法;否則,令k=k+1,返回步驟2)。

2 實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

實(shí)驗(yàn)場景如圖1所示,起始位置A坐標(biāo)為28°44′52.7594″ N,115°51′40.0041″ E,44.85 m,終止位置B坐標(biāo)為28°44′52.7180″ N,115°51′44.0702″ E,45.12 m,實(shí)驗(yàn)路徑AB距離為110.33 m. 通過4臺(tái)南方S82RTK及進(jìn)行靜態(tài)聯(lián)測,獲取基準(zhǔn)點(diǎn)A和B坐標(biāo),并在基準(zhǔn)點(diǎn)上架設(shè)徠卡TS06全站儀測量建筑物頂點(diǎn)坐標(biāo),建立了3D建筑物輪廓模型[10],如圖2所示。

智能終端選用小米2S手機(jī),其中GPS的數(shù)據(jù)更新率為1 Hz,加速度計(jì)、陀螺儀、磁力計(jì)的數(shù)據(jù)更新率為10 Hz,采集的數(shù)據(jù)長度為85 s。

圖1 實(shí)驗(yàn)路徑A-B和城市峽谷實(shí)景圖

圖2 3D建筑物輪廓模型

在圖1所示實(shí)驗(yàn)路徑的C點(diǎn)和D點(diǎn),利用建筑物輪廓模型計(jì)算建筑物的邊界仰角如圖3、圖4所示,GPS星空分布圖由衛(wèi)星歷書計(jì)算得到,其中封閉曲線內(nèi)的衛(wèi)星可見?？芍謾C(jī)進(jìn)入峽谷后由于建筑物的遮擋導(dǎo)致GPS可見衛(wèi)星數(shù)減少到3顆,故手機(jī)接收了不可見衛(wèi)星的非直達(dá)信號(hào)用于定位,導(dǎo)致多徑誤差影響定位精度,定位結(jié)果如圖5所示。

圖3 C點(diǎn)建筑物邊界仰角與GPS星空分布

圖4 D點(diǎn)建筑物邊界仰角與GPS星空分布

圖5 GPS在沿街和過街方向定位誤差

圖5示出了手機(jī)GPS的定位誤差,第20 s手機(jī)到達(dá)實(shí)驗(yàn)路徑上的C點(diǎn),此時(shí)即將進(jìn)入峽谷,由于建筑物的影響GPS定位誤差開始變大。第53 s手機(jī)到達(dá)實(shí)驗(yàn)路徑上的D點(diǎn),可知峽谷中GPS的定位誤差明顯高于峽谷外的C點(diǎn),其中沿街方向最大誤差為7.1 m,過街方向最大誤差為21.6 m. 本文從C點(diǎn)即運(yùn)動(dòng)20 s后開始使用PF融合定位算法。

由圖6所示的方位角對比可知,手機(jī)進(jìn)入峽谷后GPS定位誤差的增大使GPS方位角發(fā)生劇烈波動(dòng),最大方位角達(dá)到176.9°,嚴(yán)重偏離真實(shí)運(yùn)動(dòng)方向,而磁力計(jì)輸出的方位角在參考方位角的91.2°上下波動(dòng),平均值略低于參考方位角,較符合運(yùn)動(dòng)情況。

圖7示出了速度對比,初始時(shí)刻即將運(yùn)動(dòng)GPS輸出速度為0,第3 s之后GPS速度保持為恒定的1.25 m/s,一直小于參考速度的1.31 m/s.從第20 s開始,手機(jī)通過加速度計(jì)和陀螺儀采集載體數(shù)據(jù)來進(jìn)行慣性導(dǎo)航定位, 可知手機(jī)INS輸出的水平速度在參考速度1.31 m/s上下波動(dòng),其均值為1.30 m/s,較符合運(yùn)動(dòng)情況。

圖6 GPS和磁力計(jì)方位角對比

圖7 GPS速度與INS水平速度對比

從第20 s開始,本文采用PF融合INS輸出的水平速度和磁力計(jì)方位角定位,如圖8所示,EKF、UKF、PF三種濾波方法在沿街方向的定位誤差上下波動(dòng)并逐漸收斂到真實(shí)值附近,相對GPS定位誤差波動(dòng)幅度較小,定位誤差較GPS分別降低了1.90 m,1.71m和2.35 m. 過街方向定位誤差如圖9所示,可知在手機(jī)即將進(jìn)入峽谷時(shí)GPS定位誤差迅速大幅增加,而三種濾波方法的定位誤差波動(dòng)幅度較小,遠(yuǎn)低于GPS的定位誤差。

如圖10所示,手機(jī)進(jìn)入峽谷后三種濾波方法的絕對定位誤差波動(dòng)幅度較小,均遠(yuǎn)低于GPS,其中PF融合算法的定位誤差最小。由表1所示的定位誤差統(tǒng)計(jì)可知,本文使用PF進(jìn)行數(shù)據(jù)融合的絕對定位誤差為3.19 m,低于EKF的4.84 m和UKF的4.82 m,這是由于模型的非線性和噪聲的不確定性,PF較EKF、UKF有更好的濾波性能。綜上,使用PF融合INS輸出的水平速度和磁力計(jì)方位角的定位誤差明顯低于GPS,具有實(shí)際意義。

圖8 INS水平速度/磁力計(jì)融合沿街方向誤差對比

圖9 INS水平速度/磁力計(jì)融合過街方向誤差對比

圖10 INS水平速度/磁力計(jì)融合定位誤差對比

表1 定位誤差統(tǒng)計(jì)

3 結(jié)束語

本文基于建筑物模型和GPS星空分布情況,分析城市峽谷場景中GPS定位誤差,并在智能終端上實(shí)現(xiàn)了基于粒子濾波的INS/磁力計(jì)融合定位算法。該算法充分利用智能終端傳感器數(shù)據(jù),通過PF融合智能終端INS輸出的水平速度和磁力計(jì)方位角數(shù)據(jù)來實(shí)現(xiàn)定位,在真實(shí)的城市峽谷中,基于智能終端平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)結(jié)果表明本文提出的算法比GPS的定位誤差降低了76.9%,相比于EKF、UKF融合算法的定位誤差分別降低了34.1%和33.8%,證實(shí)了智能終端上多傳感器融合定位的有效性。

[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] 劉興川,吳振鋒,林孝康. 基于自適應(yīng)加權(quán)算法的WLAN/MARG/GPS組合定位系統(tǒng)[J]. 清華大學(xué)學(xué)報(bào)(自然科學(xué)版), 2013(7):955-960.

[6] 劉德春,譚信. 非線性濾波算法性能對比[J]. 電子設(shè)計(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] 趙琳. 非線性系統(tǒng)濾波理論[M]. 北京：國防工業(yè)出版社, 2012.

[9] 周瑞,李志強(qiáng),羅磊. 基于粒子濾波的WiFi行人航位推算融合室內(nèi)定位[J]. 計(jì)算機(jī)應(yīng)用, 2016,36(5):1188-1191.

[10]胡輝,顏瑜軍,歐敏輝. 一種基于EKF的GPS/SM組合定位算法[J]. 全球定位系統(tǒng), 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

江西省自然科學(xué)基金(編號(hào)：20142BAB207001); 江西省教育廳科學(xué)技術(shù)研究項(xiàng)目(編號(hào)：GJJ14369)

TN961

A

1008-9268(2017)03-0001-06

夏景平 (1990-),男,碩士研究生,主要研究方向?yàn)樾l(wèi)星導(dǎo)航定位。

胡輝 (1970-),男,博士,教授,主要研究方向?yàn)樾l(wèi)星導(dǎo)航定位、并行算法與并行處理、機(jī)器視覺。

歐敏輝 (1990-),男,碩士研究生,主要研究方向?yàn)樾l(wèi)星導(dǎo)航定位。

顏瑜軍 (1990-),男,碩士研究生,主要研究方向?yàn)樾l(wèi)星導(dǎo)航定位。

聯(lián)系人: 胡輝 E-mail: gnss523@163.com