基于蜂窩網(wǎng)格粒子濾波的行人導(dǎo)航航向估計方法

曾慶化，萬駿煒，劉建業(yè)，黃 凱，顧姍姍

（南京航空航天大學(xué) 自動化學(xué)院，南京 210016；2. 中航工業(yè)陜西寶成航空儀表有限責(zé)任公司，寶雞 721006）

基于蜂窩網(wǎng)格粒子濾波的行人導(dǎo)航航向估計方法

曾慶化1，萬駿煒1，劉建業(yè)1，黃 凱2，顧姍姍1

（南京航空航天大學(xué) 自動化學(xué)院，南京 210016；2. 中航工業(yè)陜西寶成航空儀表有限責(zé)任公司，寶雞 721006）

為了提高低成本行人慣性導(dǎo)航系統(tǒng)的實時導(dǎo)航精度，針對行人慣性導(dǎo)航系統(tǒng)航向角發(fā)散問題，提出一種適用于實時行人導(dǎo)航系統(tǒng)的蜂窩網(wǎng)格粒子濾波算法（CSPF，cellular structure particle filter）。該方法將二維平面劃分為緊密相連的蜂窩六邊形區(qū)域，記錄行人運動的歷史軌跡信息，采用粒子濾波方法對航向誤差補償量進(jìn)行估計。蜂窩網(wǎng)格粒子濾波算法在行人慣性導(dǎo)航系統(tǒng)的實驗結(jié)果表明，引入了蜂窩網(wǎng)格粒子濾波的行人慣性導(dǎo)航方法與常規(guī)方法相比能有效改善實時行人慣性導(dǎo)航精度。在二維行走實驗中，水平位置誤差百分比由5.3%改進(jìn)到1.7%；三維行走實驗中，水平誤差百分比由4.2%改進(jìn)到2.9%。

行人導(dǎo)航；慣性導(dǎo)航；蜂窩結(jié)構(gòu)；粒子濾波

行人導(dǎo)航的概念提出于20世紀(jì)80~90年代，隨著衛(wèi)星導(dǎo)航系統(tǒng)的發(fā)展，基于衛(wèi)星導(dǎo)航的行人導(dǎo)航系統(tǒng)正在被廣泛的應(yīng)用[1]。當(dāng)行人處于信號遮蔽的環(huán)境下，利用衛(wèi)星導(dǎo)航技術(shù)的行人導(dǎo)航定位方式無法有效定位跟蹤[2]，因此僅依賴衛(wèi)星導(dǎo)航技術(shù)無法實現(xiàn)行人實時無縫導(dǎo)航。為了解決無衛(wèi)星導(dǎo)航情況下的行人導(dǎo)航問題，美國Draper實驗室在上世紀(jì)末首先提出了將MEMS慣性測量組件安置于人體足部進(jìn)行行人移動導(dǎo)航的方法。此方法使用零速修正技術(shù)，很好地解決了MEMS導(dǎo)航系統(tǒng)誤差發(fā)散問題，近些年正廣泛被國內(nèi)外學(xué)者研究[3-5]。

然而，行人慣性導(dǎo)航系統(tǒng)中存在航向角發(fā)散問題，由于航向角的不可觀測性，這個關(guān)鍵問題一直未能得到非常有效地解決，航向角的誤差成為整個導(dǎo)航系統(tǒng)誤差的主要來源[6]。為解決航向角發(fā)散的問題，國內(nèi)外學(xué)者進(jìn)行了相關(guān)的研究，現(xiàn)有方法大多需要借助附加信息，如：建筑信息融合算法[7]、磁航向輔助算法[8]等，這些附加信息不僅增加了系統(tǒng)的體積、功耗，而且更易受到客觀條件情況或者外界惡劣環(huán)境影響，從而大大限制了行人導(dǎo)航系統(tǒng)的實際應(yīng)用。

為了提高基于純慣性技術(shù)的行人導(dǎo)航系統(tǒng)精度，德國宇航局的學(xué)者創(chuàng)新的提出將區(qū)域劃分的算法，采用粒子濾波和后期地圖匹配處理的方法得到最優(yōu)導(dǎo)航結(jié)果[9]。該方法數(shù)據(jù)計算量較大，適用于后期處理分析，且對處理器的運算能力要求高，不適合低成本處理器的應(yīng)用。本文在對上述各種行人導(dǎo)航方法進(jìn)行深入研究的基礎(chǔ)上，取長補短，將區(qū)域劃分的概念和粒子濾波的思想引入行人導(dǎo)航系統(tǒng)的航向角估計和修正中，提出并實現(xiàn)CSPF算法，最終提高了行人慣性實時導(dǎo)航精度。實驗表明，新方法能夠有效地在線修正航向角并取得良好的實時導(dǎo)航結(jié)果，可以滿足實時行人導(dǎo)航需要。

1 基于CSPF方法的行人導(dǎo)航系統(tǒng)原理

粒子濾波（PF-particle filter）是利用蒙特卡洛方法遞推貝葉斯估計的一種濾波方法[10]，其核心思想是采用一組具有相應(yīng)權(quán)值的隨機(jī)樣本集合（通常稱為粒子）逼近所需的后驗概率密度。CSPF算法采用區(qū)域劃分思想，將行人行走區(qū)域劃分為緊密連接的蜂窩六邊形。捷聯(lián)慣導(dǎo)解算模塊向濾波器提供行人步長和方位信息，這些量將作為行人行走歷史信息被保存在系統(tǒng)中，作為濾波器參數(shù)調(diào)整的依據(jù)。

1.1 行人導(dǎo)航算法總體框架

傳統(tǒng)行人導(dǎo)航算法包含捷聯(lián)慣導(dǎo)算法和零速修正算法（SINS+ZUPT）。傳統(tǒng)捷聯(lián)導(dǎo)航算法在足部運動過程中，利用慣性器件信息解算，當(dāng)檢測到零速時刻，系統(tǒng)速度被置零，系統(tǒng)的橫滾俯仰角使用加速度計信息進(jìn)行重新計算，以抑制水平姿態(tài)角度誤差的發(fā)散。但是對于系統(tǒng)航向角度，傳統(tǒng)算法無法進(jìn)行有效抑制。

本文研究的包含CSPF航向修正的行人導(dǎo)航算法在傳統(tǒng)算法的基礎(chǔ)上引入基于蜂窩網(wǎng)格形狀劃分的粒子濾波算法。基于蜂窩網(wǎng)格形狀劃分的粒子濾波算法可以綜合考慮行人行走的歷史信息，進(jìn)行航向角的修正計算。引入CSPF算法后，行人導(dǎo)航算法總體框架如圖1所示。

相鄰零速修正時刻之間的捷聯(lián)慣導(dǎo)輸出位置之差可以計算出行人位移矢量，行人的位移矢量作為航位推算算法（DR，Dead-Reckoning）的輸入量。航位推算算法過程中，系統(tǒng)記錄行人行走信息，這些歷史信息將在粒子濾波解算中被使用，以得到航向角的補償量。航向角補償量被實時反饋至捷聯(lián)解算部分，用于航向角的修正。

圖1 基于CSPF算法的行人導(dǎo)航算法結(jié)構(gòu)Fig.1 Pedestrian navigation algorithm structure based on CSPF algorithm

1.2 歷史行走信息的記錄方法

蜂窩六邊形有效地將行人行走區(qū)域進(jìn)行無縫劃分后，可通過圖2形式對六邊形及進(jìn)行編碼。圖中表示了經(jīng)過一步行走之后，行人的位置從區(qū)域（1,2）穿過第 3邊進(jìn)入?yún)^(qū)域（2,3）的情況。計數(shù)值進(jìn)行表示這種穿越信息，如圖 2所示的情況發(fā)生后，的值累加一次，其余的情況類似。通過計數(shù)值可以有效實現(xiàn)行人行走信息的記錄。

圖2 區(qū)域劃分示意圖Fig2 Schematic zoning

1.3 DR+CSPF算法融合研究

捷聯(lián)慣導(dǎo)解算可以實時量測提取步長信息，這些信息可以換算出行人二維位置（E, N）狀態(tài)遞推方程：

式中：S為一步步長，φ為人體的方位角。此外圖中還表示了置于足部的系統(tǒng)航向角Ψ。φ應(yīng)用在航位推算算法中，表示行人步長在二維空間中的方向，而Ψ則表示載體/器件的航向角，示意圖如圖3所示，其中E、N表示地理坐標(biāo)系的東向和北向，xb、yb表示載體（腳）坐標(biāo)系。

SINS+ZUPT算法量測到一步信息{ Sk-1,φk-1}后，可以認(rèn)為方位角φ中存在誤差，誤差范圍與φ的變化情況相關(guān)。采樣得到各個粒子的方位角：

圖3 行人步進(jìn)示意圖Fig.3 Stepping schematic of pedestrian

得到步長信息和方位角信息后，各粒子二維位置狀態(tài)一步遞推方程如下：

將歷史信息引入更新粒子權(quán)值：

更新完成粒子的權(quán)值之后，即可計算出方位角的濾波估計值：

式中，k為補償系數(shù)。求得系統(tǒng)航向角的補償量之后實時反饋回系統(tǒng)，進(jìn)行航向角誤差的在線補償以提高實時導(dǎo)航系統(tǒng)的精度。

2 CSPF方法實現(xiàn)研究

粒子穿越六邊形蜂窩網(wǎng)格邊界的過程存在多種形式[11]，考慮到研究效率和行走實際，本文實現(xiàn)的CSPF算法以粒子穿越網(wǎng)格區(qū)域邊界的可能性為6種（6邊）為例，且假設(shè)粒子每次最多只能穿越一個網(wǎng)格區(qū)域邊界。

CSPF算法在實現(xiàn)過程中的流程示意圖如圖4所示。

根據(jù)捷聯(lián)系統(tǒng)的輸出進(jìn)行粒子位置的更新操作，接著判斷粒子的移動穿越情況并進(jìn)行濾波計算得出航向角濾波估計，最后得出航向角修正量并反饋回捷聯(lián)系統(tǒng)等待下一個粒子位置更新周期的到來。

圖4 CSPF算法實現(xiàn)過程Fig.4 Implementation process of CSPF algorithm

3 實驗結(jié)果分析

為了驗證本文介紹的CSPF算法的有效性，本文進(jìn)行了行人行走實驗驗證。實驗過程中，低成本MEMS慣性器件MPU-6050置于足部測量行人運動過程中的足部運動信息，器件輸出頻率選擇為100 Hz。實驗場地為南京航空航天大學(xué)明故宮校區(qū)四號樓內(nèi)部，起始和終點位置選為同一點以便分析。取其中兩組實驗數(shù)據(jù)進(jìn)行分析說明。試驗 1：水平二維行走的導(dǎo)航結(jié)果對比圖（見圖5）；試驗2：三維行走（含上下樓梯）的水平二維位置導(dǎo)航結(jié)果對比圖（見圖6）。

從導(dǎo)航結(jié)果對比分析可知，采用本文介紹的CSPF算法之后，航向角的漂移均得到有效的抑制，導(dǎo)航結(jié)果明顯改進(jìn)，二維行走實驗的二維導(dǎo)航結(jié)果誤差由5.3%改善為1.7%，三維行走實驗的二維導(dǎo)航結(jié)果誤差從4.2%改善為2.9%。實驗結(jié)果驗證了CSPF算法的可用性。

表1 導(dǎo)航參數(shù)對比Tab.1 Navigation parameter comparison

圖5 二維行走實驗導(dǎo)航結(jié)果對比Fig.5 Comparison for 2D-walking navigation results

圖6 三維行走實驗導(dǎo)航結(jié)果對比Fig.6 Comparison for 3D-walking navigation results

4 結(jié) 論

針對行人慣性導(dǎo)航系統(tǒng)中航向角誤差漂移問題，本文提出并實現(xiàn)一種基于CSPF方法的行人導(dǎo)航系統(tǒng)航向角修正算法。該算法具有計算量小，可實現(xiàn)性強(qiáng)、實時性能好等特點。將該方法應(yīng)用行人行走實驗結(jié)果表明，該算法可以有效改善行人導(dǎo)航系統(tǒng)中的航向角，導(dǎo)航結(jié)果明顯更接近實際行走路線曲線。本方法可以實現(xiàn)不依賴于外部信息源進(jìn)行航向角的修正，具有較好的自主性和工程應(yīng)用參考價值。

（

）：

[1] 許睿. 行人導(dǎo)航系統(tǒng)算法研究與應(yīng)用實現(xiàn)[D]. 南京：南京航空航天大學(xué)，2008.

XU Rui. Research and application on navigation algorithm of pedestrian navigation system[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2008.

[2] Pinchin J, Hide C, Abdulrahim K, et al. Integration of heading-aided MEMS IMU with GPS for pedestrian navigation[C]//Proceedings of the 24th International Technical Meeting of The Satellite Division of the Institute of Navigation. 2011: 1346-1356.

[3] Bancroft J B, Lachapelle G. Data fusion algorithms for multiple inertial measurement units[J]. Sensors, 2011, 11(7): 6771-6798.

[4] 張金亮，秦永元，梅春波. 基于 MEMS 慣性技術(shù)的鞋式個人導(dǎo)航系統(tǒng)[J]. 中國慣性技術(shù)學(xué)報，2011，19(3): 253-256. ZHANG Jin-liang, QIN Yong-yuan, MEI Chun-bo. Shoemounted personal navigation system based on MEMS inertial technology[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 253-256.

[5] Ali A, El-Sheimy N. Low-cost MEMS-based pedestrian navigation technique for GPS-denied areas[EB/OL]. Journal of Sensors, 2013. http://dx.doi.org/10.1155/2013/197090.

[6] Afzal M H, Renaudin V, Lachapelle G. Magnetic field based heading estimation for pedestrian navigation environments[C]//2011 IEEE International Conference on Indoor Positioning and Indoor Navigation(IPIN). 2011: 1-10.

[7] 謝波，江一夫，嚴(yán)恭敏，等. 個人導(dǎo)航融合建筑平面信息的粒子濾波方法[J]. 中國慣性技術(shù)學(xué)報，2013，21(1)：1-6.

XIE Bo, JIANG Yi-fu, YAN Gong-min. Novel particle filter approach for fusing building plane into pedestrian navigation[J]. Journal of Chinese Inertial Technology, 2013, 21(1): 1-6.

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

QIAN Wei-xing, ZHU Xin-hua, SU Yan. Personal navigation method based on foot-mounted MEMS inertial/magnetic measurement unit[J]. Journal of Chinese Inertial Technology, 2012, 20(5): 567-572.

[9] Robertson P, Angermann M, Krach B, et al. Inertial systems based joint mapping and positioning for pedestrian navigation[C]//Proc. ION GNSS. 2009: 3057-3068.

[10] 張共愿，趙忠. 粒子濾波及其在導(dǎo)航系統(tǒng)中的應(yīng)用綜述[J]. 中國慣性技術(shù)學(xué)報，2006，14(6)：91-94.

ZHANG Gong-yuan, ZHAO Zhong. Overview of particle filter and its applications in integrated navigation system[J]. Journal of Chinese Inertial Technology, 2006, 14(6): 91-94.

[11] Robertson P, Angermann M, Krach B. Simultaneous localization and mapping for pedestrians using only foot-mounted inertial sensors[C]//Proceedings of the 11th International Conference on Ubiquitous Computing. 2009: 93-96.

Heading compensation algorithm based on cellular structure particle filter for pedestrian navigation

ZENG Qing-hua1, WAN Jun-wei1, LIU Jian-ye1, HUANG Kai2, GU Shan-shan1
(Navigation Research Center, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China; 2. AVIC Shaanxi Baocheng Aviation Instrument Co., Ltd., Baoji 721006, China)

To improve the heading accuracy of inertial pedestrian navigation system, a compensation method for the heading error in inertial pedestrian navigation system is proposed which is based on hexagon particle filter (CSPF，cellular structure particle filter). In CSPF, the horizontal plane is divided into close hexagons to record the historical moving information of pedestrian, and the information are used in particle filter to calculate the compensation amount of heading error. The CSPF method was applied to the inertial pedestrian system, and the experimental results indicate that the CSPF method can improve the heading and navigation accuracy of inertial pedestrian navigation. Compared to classic inertial pedestrian navigation method, the horizontal error in 2D walking test is improved to 1.7% from 5.3%, and the horizontal error in 3D walking test is improved to 2.9% from 4.2%.

pedestrian navigation; inertial navigation; particle filter; cellular structure

1005-6734(2014)05-0576-04

10.13695/j.cnki.12-1222/o3.2014.05.004

U666.1

A

2014-05-28；

2014-08-28

國家自然科學(xué)基金（61374115，61328301，61104188，61273057，61174197）；江蘇高校優(yōu)勢學(xué)科建設(shè)工程項目

曾慶化（1979—），男，副教授，從事慣性導(dǎo)航及組合導(dǎo)航技術(shù)研究。E-mail：zengqh@nuaa.edu.cn