基于蜂窝网格粒子滤波的行人导航航向估计方法

曾庆化，万骏炜，刘建业，黄 凯，顾姗姗

（南京航空航天大学 自动化学院，南京 210016；2. 中航工业陕西宝成航空仪表有限责任公司，宝鸡 721006）

基于蜂窝网格粒子滤波的行人导航航向估计方法

曾庆化1，万骏炜1，刘建业1，黄 凯2，顾姗姗1

（南京航空航天大学 自动化学院，南京 210016；2. 中航工业陕西宝成航空仪表有限责任公司，宝鸡 721006）

为了提高低成本行人惯性导航系统的实时导航精度，针对行人惯性导航系统航向角发散问题，提出一种适用于实时行人导航系统的蜂窝网格粒子滤波算法（CSPF，cellular structure particle filter）。该方法将二维平面划分为紧密相连的蜂窝六边形区域，记录行人运动的历史轨迹信息，采用粒子滤波方法对航向误差补偿量进行估计。蜂窝网格粒子滤波算法在行人惯性导航系统的实验结果表明，引入了蜂窝网格粒子滤波的行人惯性导航方法与常规方法相比能有效改善实时行人惯性导航精度。在二维行走实验中，水平位置误差百分比由5.3%改进到1.7%；三维行走实验中，水平误差百分比由4.2%改进到2.9%。

行人导航；惯性导航；蜂窝结构；粒子滤波

行人导航的概念提出于20世纪80~90年代，随着卫星导航系统的发展，基于卫星导航的行人导航系统正在被广泛的应用[1]。当行人处于信号遮蔽的环境下，利用卫星导航技术的行人导航定位方式无法有效定位跟踪[2]，因此仅依赖卫星导航技术无法实现行人实时无缝导航。为了解决无卫星导航情况下的行人导航问题，美国Draper实验室在上世纪末首先提出了将MEMS惯性测量组件安置于人体足部进行行人移动导航的方法。此方法使用零速修正技术，很好地解决了MEMS导航系统误差发散问题，近些年正广泛被国内外学者研究[3-5]。

然而，行人惯性导航系统中存在航向角发散问题，由于航向角的不可观测性，这个关键问题一直未能得到非常有效地解决，航向角的误差成为整个导航系统误差的主要来源[6]。为解决航向角发散的问题，国内外学者进行了相关的研究，现有方法大多需要借助附加信息，如：建筑信息融合算法[7]、磁航向辅助算法[8]等，这些附加信息不仅增加了系统的体积、功耗，而且更易受到客观条件情况或者外界恶劣环境影响，从而大大限制了行人导航系统的实际应用。

为了提高基于纯惯性技术的行人导航系统精度，德国宇航局的学者创新的提出将区域划分的算法，采用粒子滤波和后期地图匹配处理的方法得到最优导航结果[9]。该方法数据计算量较大，适用于后期处理分析，且对处理器的运算能力要求高，不适合低成本处理器的应用。本文在对上述各种行人导航方法进行深入研究的基础上，取长补短，将区域划分的概念和粒子滤波的思想引入行人导航系统的航向角估计和修正中，提出并实现CSPF算法，最终提高了行人惯性实时导航精度。实验表明，新方法能够有效地在线修正航向角并取得良好的实时导航结果，可以满足实时行人导航需要。

1 基于CSPF方法的行人导航系统原理

粒子滤波（PF-particle filter）是利用蒙特卡洛方法递推贝叶斯估计的一种滤波方法[10]，其核心思想是采用一组具有相应权值的随机样本集合（通常称为粒子）逼近所需的后验概率密度。CSPF算法采用区域划分思想，将行人行走区域划分为紧密连接的蜂窝六边形。捷联惯导解算模块向滤波器提供行人步长和方位信息，这些量将作为行人行走历史信息被保存在系统中，作为滤波器参数调整的依据。

1.1 行人导航算法总体框架

传统行人导航算法包含捷联惯导算法和零速修正算法（SINS+ZUPT）。传统捷联导航算法在足部运动过程中，利用惯性器件信息解算，当检测到零速时刻，系统速度被置零，系统的横滚俯仰角使用加速度计信息进行重新计算，以抑制水平姿态角度误差的发散。但是对于系统航向角度，传统算法无法进行有效抑制。

本文研究的包含CSPF航向修正的行人导航算法在传统算法的基础上引入基于蜂窝网格形状划分的粒子滤波算法。基于蜂窝网格形状划分的粒子滤波算法可以综合考虑行人行走的历史信息，进行航向角的修正计算。引入CSPF算法后，行人导航算法总体框架如图1所示。

相邻零速修正时刻之间的捷联惯导输出位置之差可以计算出行人位移矢量，行人的位移矢量作为航位推算算法（DR，Dead-Reckoning）的输入量。航位推算算法过程中，系统记录行人行走信息，这些历史信息将在粒子滤波解算中被使用，以得到航向角的补偿量。航向角补偿量被实时反馈至捷联解算部分，用于航向角的修正。

图1 基于CSPF算法的行人导航算法结构Fig.1 Pedestrian navigation algorithm structure based on CSPF algorithm

1.2 历史行走信息的记录方法

蜂窝六边形有效地将行人行走区域进行无缝划分后，可通过图2形式对六边形及进行编码。图中表示了经过一步行走之后，行人的位置从区域（1,2）穿过第 3边进入区域（2,3）的情况。计数值进行表示这种穿越信息，如图 2所示的情况发生后，的值累加一次，其余的情况类似。通过计数值可以有效实现行人行走信息的记录。

图2 区域划分示意图Fig2 Schematic zoning

1.3 DR+CSPF算法融合研究

捷联惯导解算可以实时量测提取步长信息，这些信息可以换算出行人二维位置（E, N）状态递推方程：

式中：S为一步步长，φ为人体的方位角。此外图中还表示了置于足部的系统航向角Ψ。φ应用在航位推算算法中，表示行人步长在二维空间中的方向，而Ψ则表示载体/器件的航向角，示意图如图3所示，其中E、N表示地理坐标系的东向和北向，xb、yb表示载体（脚）坐标系。

SINS+ZUPT算法量测到一步信息{ Sk-1,φk-1}后，可以认为方位角φ中存在误差，误差范围与φ的变化情况相关。采样得到各个粒子的方位角：

图3 行人步进示意图Fig.3 Stepping schematic of pedestrian

得到步长信息和方位角信息后，各粒子二维位置状态一步递推方程如下：

将历史信息引入更新粒子权值：

更新完成粒子的权值之后，即可计算出方位角的滤波估计值：

式中，k为补偿系数。求得系统航向角的补偿量之后实时反馈回系统，进行航向角误差的在线补偿以提高实时导航系统的精度。

2 CSPF方法实现研究

粒子穿越六边形蜂窝网格边界的过程存在多种形式[11]，考虑到研究效率和行走实际，本文实现的CSPF算法以粒子穿越网格区域边界的可能性为6种（6边）为例，且假设粒子每次最多只能穿越一个网格区域边界。

CSPF算法在实现过程中的流程示意图如图4所示。

根据捷联系统的输出进行粒子位置的更新操作，接着判断粒子的移动穿越情况并进行滤波计算得出航向角滤波估计，最后得出航向角修正量并反馈回捷联系统等待下一个粒子位置更新周期的到来。

图4 CSPF算法实现过程Fig.4 Implementation process of CSPF algorithm

3 实验结果分析

为了验证本文介绍的CSPF算法的有效性，本文进行了行人行走实验验证。实验过程中，低成本MEMS惯性器件MPU-6050置于足部测量行人运动过程中的足部运动信息，器件输出频率选择为100 Hz。实验场地为南京航空航天大学明故宫校区四号楼内部，起始和终点位置选为同一点以便分析。取其中两组实验数据进行分析说明。试验 1：水平二维行走的导航结果对比图（见图5）；试验2：三维行走（含上下楼梯）的水平二维位置导航结果对比图（见图6）。

从导航结果对比分析可知，采用本文介绍的CSPF算法之后，航向角的漂移均得到有效的抑制，导航结果明显改进，二维行走实验的二维导航结果误差由5.3%改善为1.7%，三维行走实验的二维导航结果误差从4.2%改善为2.9%。实验结果验证了CSPF算法的可用性。

表1 导航参数对比Tab.1 Navigation parameter comparison

图5 二维行走实验导航结果对比Fig.5 Comparison for 2D-walking navigation results

图6 三维行走实验导航结果对比Fig.6 Comparison for 3D-walking navigation results

4 结 论

针对行人惯性导航系统中航向角误差漂移问题，本文提出并实现一种基于CSPF方法的行人导航系统航向角修正算法。该算法具有计算量小，可实现性强、实时性能好等特点。将该方法应用行人行走实验结果表明，该算法可以有效改善行人导航系统中的航向角，导航结果明显更接近实际行走路线曲线。本方法可以实现不依赖于外部信息源进行航向角的修正，具有较好的自主性和工程应用参考价值。

（

）：

[1] 许睿. 行人导航系统算法研究与应用实现[D]. 南京：南京航空航天大学，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 惯性技术的鞋式个人导航系统[J]. 中国惯性技术学报，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] 谢波，江一夫，严恭敏，等. 个人导航融合建筑平面信息的粒子滤波方法[J]. 中国惯性技术学报，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] 钱伟行，朱欣华，苏岩. 基于足部微惯性/地磁测量组件的个人导航方法[J]. 中国惯性技术学报，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] 张共愿，赵忠. 粒子滤波及其在导航系统中的应用综述[J]. 中国惯性技术学报，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

国家自然科学基金（61374115，61328301，61104188，61273057，61174197）；江苏高校优势学科建设工程项目

曾庆化（1979—），男，副教授，从事惯性导航及组合导航技术研究。E-mail：zengqh@nuaa.edu.cn