一種基于改進UKF濾波的GPS+PDR組合定位方法

牛 歡，廉保旺

(西北工業(yè)大學(xué)電子信息學(xué)院，陜西 西安 710072)

?

一種基于改進UKF濾波的GPS+PDR組合定位方法

牛 歡，廉保旺

(西北工業(yè)大學(xué)電子信息學(xué)院，陜西 西安 710072)

針對接收信號質(zhì)量惡化的環(huán)境,提出了一種適用于信號遮蔽環(huán)境的改進GPS+PDR組合定位算法。該方法用短時間內(nèi)的陀螺儀積分?jǐn)?shù)據(jù)校正數(shù)字羅盤的航向偏差，在一定程度上消除了數(shù)字羅盤受到的偶發(fā)干擾。采用約束殘差的無跡卡爾曼濾波(UKF)算法對GPS和行人航跡推算(PDR)定位信息進行融合處理，有效克服了PDR定位中累積航向誤差產(chǎn)生的位置漂移問題，提高了算法的定位精度和穩(wěn)定性。試驗結(jié)果表明，改進算法能有效抑制數(shù)字羅盤的漂移誤差，航向相對誤差平均降低56%；行人步行時，GPS定位標(biāo)準(zhǔn)誤差為2.67 m，單純PDR定位標(biāo)準(zhǔn)誤差為6.83 m；隨機給予若干點GPS數(shù)據(jù)輔助定位，標(biāo)準(zhǔn)誤差降至3.12 m；全程融合GPS與PDR定位，標(biāo)準(zhǔn)誤差可降至1.94 m。

GPS；行人航跡推算；組合導(dǎo)航；多傳感器數(shù)據(jù)融合；精度；無跡卡爾曼濾波

在密集建筑區(qū)、大型商場等信號遮蔽嚴(yán)重的環(huán)境中，因信號質(zhì)量變差，GPS定位精度迅速下降甚至無法定位。目前針對室內(nèi)等信號遮蔽環(huán)境的主要定位技術(shù)包括紅外定位技術(shù)、射頻識別定位技術(shù)(radio frequency identification,RFID)、ZigBee技術(shù)、行人航跡推算技術(shù)(pedestrian dead reckoning，PDR)等[1-2]。其中，基于MEMS傳感器的行人航跡推算技術(shù)，無需接收外部信號，通過測定步頻、計算步長和航向角，即可完成基本的位置計算，并且受環(huán)境影響較小，因此得到了廣泛研究。

PDR技術(shù)具有短時間內(nèi)定位精度高，但定位誤差隨時間積累逐漸增大的特點。文獻[3—4]指出了航向估計偏差是PDR定位誤差的重要來源。文獻[5—6]提出了WiFi和地標(biāo)識別融合定位算法，并輔助地磁測量組件，通過WiFi指紋識別和建筑標(biāo)志特征匹配，利用MEMS設(shè)備接收肌電信號，能夠有效提升定位精度和穩(wěn)定度。但是，融合WiFi指紋特征或建筑特征等算法均需要布置額外的硬件設(shè)施，提前測繪生成特征數(shù)據(jù)庫以供后續(xù)匹配使用，系統(tǒng)構(gòu)建復(fù)雜，實現(xiàn)代價較高，并且目前大部分室內(nèi)消費場所不具備上述條件，可穿戴設(shè)備的市場應(yīng)用尚不成熟，主流設(shè)備仍然以智能手環(huán)為主，需要與智能手機或其他設(shè)備通信后才能查看結(jié)果，這給普通消費者的使用帶來不便。

本文基于智能手機平臺，通過MEMS傳感器采集數(shù)據(jù)，利用加速度計測量值進行步伐檢測，以陀螺儀短期測量值校正磁羅盤航向，進而獲得行人航跡推算定位結(jié)果，最后采用約束殘差的無跡卡爾曼濾波(uanscented kalman filier，UKF)算法，將PDR定位結(jié)果與GPS定位結(jié)果相融合，能有效克服外界突發(fā)擾動，提高了信號遮蔽環(huán)境下行人定位結(jié)果的精度與穩(wěn)定性。

1 算法原理

UKF方法的具體計算過程可參考文獻[7],由于此法本質(zhì)上仍然屬于卡爾曼濾波體系，其保留了與傳統(tǒng)卡爾曼濾波器類似的缺陷。比較嚴(yán)重的一點就是當(dāng)系統(tǒng)趨于穩(wěn)定狀態(tài)時，所有卡爾曼濾波器均趨于穩(wěn)態(tài)[8]。此時濾波器的預(yù)測協(xié)方差和增益同時趨于極小值。如果系統(tǒng)狀態(tài)發(fā)生突變，此時濾波殘差會迅速增大，而預(yù)測協(xié)方差和增益會存在一定滯后性，進而使UKF濾波器喪失對系統(tǒng)狀態(tài)突變的跟蹤。為了克服UKF濾波算法在系統(tǒng)穩(wěn)態(tài)時針對狀態(tài)突變的響應(yīng)缺失問題，可以通過適當(dāng)改變方差的加權(quán)因子，進而改善濾波器的收斂性[9]。

模型噪聲和觀測噪聲的非線性系統(tǒng)狀態(tài)空間模型如下

xk=f(xk-1,uk-1)+wk-1

(1)

zk=h(xk)+vk

(2)

式中，k表示不同的離散時刻，xk/xk-1和zk分別為濾波系統(tǒng)的狀態(tài)量與觀測量；uk-1為濾波系統(tǒng)的輸入變量；f()和h()表示已知且有界的非線性傳遞函數(shù)；噪聲wk-1和vk均為高斯白噪聲，其方差分別為Qk和Rk。

狀態(tài)量的更新形式同一般的高斯濾波器結(jié)構(gòu)，即

(3)

(4)

LMDk=diag[η1,k,η2,k,…,ηn,k]

(5)

式中,Pk,k-1為一步預(yù)測狀態(tài)的自方差，LMDk為對角漸消陣；ηi,k≥1，i=1,2,…,n，為漸消因子，與濾波器增益呈正相關(guān)性[10]；χi,k,k-1是經(jīng)過UT變換后的預(yù)測狀態(tài)Sigma點集中的元素。計算Sigma點集時，通常采用對稱采樣策略，為了提高算法的實時性，可使用改進的UT變換，減少Sigma點集中的點數(shù)，進一步提高UKF效率[11]。

為了盡可能地抑制由殘差增大所產(chǎn)生的跟蹤誤差，設(shè)法對上述UKF系統(tǒng)進行殘差約束。文獻[9]證明對上述濾波器進行殘差約束的充分條件如下

(6)

(7)

當(dāng)濾波器對系統(tǒng)狀態(tài)的刻畫比較準(zhǔn)確時[12]，可以近似得到

(8)

式中，Pxkzk為狀態(tài)值和觀測值的協(xié)方差；C0,k為殘差估計矩陣，濾波器達到最優(yōu)增益時，其值為0。

由式(8)成立的條件結(jié)合上文可推出殘差陣C0,k的更新公式如下

(9)

式中，ρ為濾波遺忘因子，取值范圍為(0,1]。

2 系統(tǒng)架構(gòu)和流程

2.1 基于智能手機的PDR定位

PDR定位原理如文獻[13]所述，本文采用文獻[14]提出的方法進行步頻檢測和步長測定。航向可通過數(shù)字羅盤感知地磁場強度變化進行計算，利用智能手機內(nèi)置數(shù)字磁羅盤所測得的磁場信息(Mx,My,Mz)，可得到當(dāng)?shù)氐拇艌鏊椒至縓h與Yh。

磁航向角[15]計算公式為

(10)

式中，φM0為磁航向角，查表得到當(dāng)?shù)卮牌?，修正后可以得到測試者位置的航向角φM。

如上文所述，航向偏差是PDR定位誤差的重要來源，磁羅盤除受內(nèi)部誤差干擾外，本身更容易受各類鐵制品和使用環(huán)境的磁場影響，導(dǎo)致航向誤差增大。而陀螺儀傳感器本身不受磁場干擾影響，但智能手機內(nèi)置的陀螺儀普遍精度較低,其零偏普遍分布在40°-80°/h，最大可達100°/h，根據(jù)其測量值得出的相對航向僅在較短時間內(nèi)具有有效性。

依據(jù)上述傳感器特性，采用閉環(huán)控制法進行航向校正，具體處理過程可以按照下述階段進行。

(1) 初始化對準(zhǔn)，用電子羅盤輸出估計陀螺儀零位殘差。

(2) 利用Adams方法求出陀螺儀相對航向，然后與電子羅盤輸出航向比較，結(jié)果進入控制環(huán)路。

(3) 濾波估計下一狀態(tài)，補償羅差，得到修正航向。

航向修正流程如圖1所示。

圖1 航向修正流程

2.2 改進的組合定位方法

采用上述約束殘差的UKF算法(RC-UKF)將GPS定位結(jié)果與PDR結(jié)果進行融合處理。定義濾波器的狀態(tài)向量為

(11)

式中，E、N分別為“東-北-天”空間坐標(biāo)系中的東向坐標(biāo)和北向坐標(biāo)；L為行人步長；θ為航向角。

觀測向量為

(12)

狀態(tài)方程為

(13)

式中，Wk為動態(tài)噪聲擾動，各分量可近似認(rèn)為服從均值為0的高斯分布。

圖2展示了改進的RC-UKF融合算法處理流程，GPS定位信息提供了起始位置和濾波初值，加速度計判定運動狀態(tài)變化，陀螺儀和電子羅盤協(xié)同提供航向角，系統(tǒng)檢測到運動狀態(tài)變動時運行濾波算法更新運動狀態(tài)。

3 試驗設(shè)計和數(shù)據(jù)分析

3.1 試驗環(huán)境選擇和參數(shù)設(shè)定

試驗時使用的測試設(shè)備為Lenovo公司生產(chǎn)的K52e78型號智能手機，為確保GPS信號的可控性，選取西北工業(yè)大學(xué)學(xué)生操場為測試環(huán)境，含有一標(biāo)準(zhǔn)400 m跑道，場地開闊無遮擋，衛(wèi)星信號接收環(huán)境良好，具有高精度經(jīng)緯度信息，起始坐標(biāo)設(shè)定為(34.032 771,108.760 946)，參考點坐標(biāo)設(shè)定為(34.032 994,108.760 791)。測試路徑包含直行、直角轉(zhuǎn)彎、環(huán)形軌道等部分，全長360 m，其中直行段長邊110 m，寬邊70 m，環(huán)形軌道長90 m，半徑37 m。具體路徑如圖3所示，用字母A、B、C、D注明轉(zhuǎn)彎節(jié)點，白色星號標(biāo)注路徑起始位置。為統(tǒng)一標(biāo)準(zhǔn)，便于分析，試驗時將PDR測試結(jié)果轉(zhuǎn)化為絕對坐標(biāo)。

圖2 RC-UKF融合算法系統(tǒng)框

圖3 測試路徑

3.2 算法性能分析

航向測試時，用一個可控電磁鐵作為數(shù)字羅盤的干擾源。所用電磁鐵在常溫下測得表面平均磁場強度約0.5 T，可對20 cm內(nèi)的電子羅盤產(chǎn)生有效干擾。取正北方向為航向零值，逆時針為正方向，試驗采用45°、90°、180°等不同轉(zhuǎn)向角度情形。

試驗結(jié)果見表1和表2，通過比較測量航向與實際航向，可見無論是否有干擾，改進算法均能提升航向精度。無干擾時，轉(zhuǎn)向階段航向角的標(biāo)準(zhǔn)偏差小于10°，且轉(zhuǎn)向幅度越小，偏差越小；有干擾時，羅盤示數(shù)已失去意義，而改進算法把轉(zhuǎn)向階段航向角的標(biāo)準(zhǔn)誤差降低了56%，仍具有一定參考價值。

GPS模塊定位時，除設(shè)備冷啟動搜星階段耗時較長，后續(xù)單次定位耗時大約在3～8 s。為模擬信號遮蔽環(huán)境，對系統(tǒng)融合模塊加入人工阻塞，每5 s進行一次阻塞發(fā)生判定，單次阻塞片段持續(xù)5 s?？梢酝ㄟ^調(diào)整阻塞發(fā)生概率P以表征GPS信號接收環(huán)境的惡劣程度，阻塞率P值越大，信號環(huán)境越差。

表1 轉(zhuǎn)向階段航向角測試數(shù)據(jù) (°)

表2 轉(zhuǎn)向階段航向角標(biāo)準(zhǔn)偏差

調(diào)整P值，沿設(shè)定路線在不同P值下各重復(fù)進行10次試驗，用目標(biāo)轉(zhuǎn)彎節(jié)點與參考點之間的相對坐標(biāo)平均值作為估計結(jié)果。表3—表5展示了各P值下的定位結(jié)果和標(biāo)準(zhǔn)誤差(RMSE)。

由表3—表5可見，不同P值下PDR定位結(jié)果偏差很大，但比較穩(wěn)定，不受阻塞影響。隨著P值的減小，GPS定位精度迅速提高，相應(yīng)的UKF和RC-UKF融合算法定位精度也隨之提高，但比較兩種算法的RMSE可知，RC-UKF算法的穩(wěn)定性更好。

表3 P=0.8時不同算法節(jié)點坐標(biāo)估計分析 m

表4 P=0.2時不同算法節(jié)點坐標(biāo)估計分析 m

表5 算法坐標(biāo)估計的RMSE對比 m

表6描述了P=0.05時各種算法的誤差分析結(jié)果，通過比較發(fā)現(xiàn)，RC-UKF算法的標(biāo)準(zhǔn)誤差降至1.94 m，標(biāo)準(zhǔn)差0.62 m,具有最高的定位精度。

表6 P=0.05時算法誤差分析 m

同時，圖4展示了P=0.05時各算法的定位結(jié)果?？梢钥闯鯣PS定位基本能顯示出完整路徑，定位偏差不受運動軌跡形狀的影響，比較穩(wěn)定；PDR定位在起始直行段誤差很小，隨時間推移逐漸增大，在轉(zhuǎn)彎階段航向會出現(xiàn)明顯偏差，環(huán)形軌道的定位結(jié)果已嚴(yán)重偏離真實位置。比較傳統(tǒng)UKF算法與RC-UKF算法的定位結(jié)果，可以看出兩種算法均能很好地描繪出行人運動軌跡，但改進算法在突發(fā)轉(zhuǎn)彎處的偏離程度更小，在環(huán)形軌跡處更貼合真實路徑。

圖4 算法性能對比

4 結(jié) 語

信號遮蔽環(huán)境中，因GPS信號質(zhì)量變差，其可用性無法得到保障，結(jié)合PDR技術(shù)輔助定位，可以提供更持續(xù)的定位服務(wù)。本文提出的改進濾波方法，能提升行人姿態(tài)突變或受到外部擾動后的跟蹤能力和濾波精度。試驗結(jié)果表明，改進算法的定位精度更高，定位結(jié)果連續(xù)性強，具有更好的穩(wěn)健性。

[1] 白國振, 楊勇明, 李廈, 等. 基于射頻識別的智能室內(nèi)定位系統(tǒng)[J].信息與控制, 2013, 42(3): 364-370.

[2] 唐健,戴廷煜,袁細(xì)保,等.RFID,GPS和GIS技術(shù)集成在物流配送系統(tǒng)中的應(yīng)用研究[J].測繪通報,2007(10):47-49.

[3] FARID Z, NORDIN R, ISMAIL M. Recent Advances in Wireless Indoor Localization Techniques and System[J]. Journal of Computer Networks & Communications, 2013(42): 15-17.

[4] 李艷,管斌,王成賓,等.電子磁羅盤航向角誤差推導(dǎo)及分析[J].儀器儀表學(xué)報,2013,34(7):1566-1572.

[5] CHEN Z, ZOU H, JIANG H, et al. Fusion of WiFi, Smartphone Sensors and Landmarks Using the Kalman Filter for Indoor Localization [J]. Sensors, 2015, 15(1): 715-732.

[6] 錢偉行,朱欣華,蘇巖.基于足部微慣性/地磁測量組件的個人導(dǎo)航方法 [J].中國慣性技術(shù)學(xué)報,2012,20(5): 567-572.

[7] JULIER S T, UHLMANN T K, DURRANT-WHYTE HF. A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators [J].IEEE Transactions on Automatic Control, 2000, 45(3):477-482.

[8] ADAK M, SOARES C G. Effects of Different Restraints on the Weld-induced Residual Deformations and Stresses in a Steel Plate [J]. The International Journal of Advanced Manufacturing Technology, 2014, 71(1): 699-710.

[9] ZHOU W D, QIAO X W, JI Y R, et al. An Innovation and Residual-based Adaptive UKF Algorithm [J]. Journal of Astronautics, 2010, 31(7): 1798-1804.

[10] 陳偉. 基于GPS和自包含傳感器的行人室內(nèi)外無縫定位算法研究 [D].合肥:中國科學(xué)技術(shù)大學(xué),2010.

[11] 馬宏陽,程鵬飛,王潛心.一種改進的 UKF 算法在捷聯(lián)慣導(dǎo)初始對準(zhǔn)中的應(yīng)用[J].測繪通報,2015(7): 18-22.

[12] SIMON D, SIMON D L. Aircraft Turbofan Engine Health Estimation Using Constrained Kalman Filtering [R]. USA:NASA,2003.

[13] 徐偉.基于Android手機的室內(nèi)定位技術(shù)研究與實現(xiàn)[D].武漢:華中師范大學(xué),2014.

[14] 田增山,朝磊,邢培基,等.行人導(dǎo)航系統(tǒng)中航跡推算參數(shù)估計方法的研究[J].電子技術(shù)應(yīng)用,2009, 35(12):84-87.

[15] 黃旭,王常虹,伊國興.利用磁強計及微機械加速度計和陀螺的姿態(tài)估計擴展卡爾曼濾波器 [J].中國慣性技術(shù)學(xué)報, 2005, 13(2): 27-30.

An Integrated Positioning Method for GPS+PDR Based on Improved UKF Filtering

NIU Huan,LIAN Baowang

(School of Electronics and Information， Northwestern Polytechnical University， Xi’an 710072， China)

In view of the deterioration of

signal quality, an improved GPS+PDR fusion positioning system applicable for signal shielding environment has been proposed. This method uses gyro integrating data within short time to correct heading deviation of digital compass, which has eliminated the incidental interference for digital compass to some extent significantly. UKF (unscented Kalman filter) algorithm used for constraining residual has been adopted to make fusion treatment for GPS and PDR positioning information, which has got over the position drift problem produced by cumulative heading error in PDR positioning effectively and improved the positioning accuracy and stability of algorithm. Experimental results show that the improved algorithm can restrain the drift error of digital compass effectively and the relative heading error has decreased by 56% on average. During walking, the standard error of GPS positioning is 2.67 m and the standard error of pure PDR positioning is 6.83 m. GPS data in several points have been offered randomly to assist positioning, the standard error decreased to 3.12 m. With GPS and PDR positioning integrated in the whole process, the standard error can be decreased to 1.94 m.

GPS; PDR; integrated navigation; multi-sensor data fusion; accuracy; UKF

牛歡，廉保旺.一種基于改進UKF濾波的GPS+PDR組合定位方法[J].測繪通報，2017(7)：5-9.

10.13474/j.cnki.11-2246.2017.0213.

2017-03-13

國家自然科學(xué)基金(61301094)；西北工業(yè)大學(xué)研究生創(chuàng)意創(chuàng)新種子基金(Z2017027)

牛 歡(1993—)，男，碩士生，主要研究方向為多傳感器組合導(dǎo)航。E-mail:nih.a@163.com

P228

A

0494-0911(2017)07-0005-05