基于腰部MEMS-IMU的室内行人导航航向反馈修正算法

路永乐，刘洪志，郭俊启，陈自然，向高军，刘 宇

（重庆邮电大学 光电信息感测与传输技术重庆市重点实验室，重庆 400065）

行人导航系统（Pedestrian Navigation System，PNS）是一种实现对行人实时位置信息跟踪的导航装置[1]。根据导航环境，可将行人导航系统分为室外导航和室内导航。室内导航又可分为基于射频信号的定位技术和包含传感器的定位技术[2]。惯性导航属于后者，其特点是无需依靠外界信号，自主性强，但误差随时间累积。随着MEMS技术的快速发展，惯性导航朝着低成本，低功耗，轻质量的方向发展[3]，并得到了快速而广泛的应用。

由于MEMS陀螺仪自身漂移使航向角误差随时间累积[4-6]，为了减少纯惯性室内导航航向角误差，德国学者采用粒子滤波和后期地图匹配处理的方法得到最优导航结果[7]。此方法需要用到后期的地图匹配，不能满足实时定位的需求。美国Borestein J.等人提出启发式漂移消除算法（Heuristic Drift Elimination）[8]，该方法根据室内环境将其分为四个或者八个主方向，利用行人航向与主方向之间的差值作为反馈系统，将反馈参数对角速度信息进行修正，从而修正航向。A. R. Jiménez等人在此基础上进一步做了改进，提出了改进型启发式漂移算法（Improved Heuristic Drift Elimination）[9]，将姿态角和主方向进行扩展卡尔曼滤波来获取姿态信息。这两种算法都是基于足部导航方式实现，Luis E. Diez等人提出了基于腕部的PDR （Pedestrian Dead Reckoning）导航方式的加强型的启发式漂移算法（Enhancing Improved Heuristic Drift Elimination）[10]。

本文在启发式漂移算法的基础上，提出了基于腰部佩戴方式的行人导航装置的航向反馈修正算法，通过对行人直行的角速度和航向信息进行分析，设计了一种行人直行的判别方法，并利用此条件作为反馈修正的基础，将楼道和走廊等主方向和非主方向上直行的航向均值信息作为反馈修正的先验条件，利用行人航向和主方向差值最为AUKF的状态量，从而实现航向修正，提高了室内行人导航精度。

1 系统介绍

本系统采用嵌入式系统作为行人自主导航硬件测试平台。该嵌入式系统集成了多个传感器件，如三轴MEMS加速度、三轴MEMS陀螺仪和MEMS气压计等。

导航系统所采用的行人定位导航算法框架如图 1所示。算法流程为首先对加速度计等传感器件的原始数据进行滤波预处理，然后分别对加速度计、陀螺仪进行安装误差校准和刻度因子校正，对处理后的传感器数据利用四元数和欧拉角之间的转换算法以及扩展卡尔曼滤波（EKF）算法获得航姿角信息，以及步长、步态和步数等信息，带入到PDR算法获得行人二维位置信息和气压计解算的高度信息，实现行人的三维实时定位。

图1 系统框架Fig.1 system framework

2 行人室内导航算法

本系统采用的导航坐标系为当地地平坐标系，简称n系，x、y、z方向分别指向0°方向、90°方向和重力反方向。载体坐标系为b系，x、y、z方向分别为载体的前、左、上方向。本文中分别代表第i个采样时刻n系和b系的三轴加速度值。采样频率均为50 Hz。

本算法主要利用三轴加速度进行步数检测和步长估计，与航向信息一起带入到PDR算法中得到行人二维坐标，再与高度信息结合得到三维位置信息。因高度信息不影响航向精度，故本文不对高度解算进行展开。

2.1 步数检测

图2 目标加速度波峰检测图Fig.2 Target acceleration peak detection

2.2 步长估计

Harvey Weinderg[11]提出步长与加速度的最大最小幅度之差呈一定相关性，故采用如下公式计算步长：

2.3 航姿角解算

2.3.1 姿态角的初始解算

2.3.2 欧拉角与四元数的相互转换

2.4 航迹推算算法

3 航向反馈修正算法

HDE算法是在 99%的室内环境下走廊和过道是矩形结构的基础上提出的，并将矩形结构的四个方向命名为主方向。本文在该算法的基础上建立了基于腰部佩戴方式的航向反馈修正算法。本论文中嵌入式定位模块的z轴与脊柱平行置于腰间，佩戴方式如图3所示。

置于腰部的行人运动模式判别不同于足部。本文提出了针对腰间采集数据方式的行人行走模式的判别方法，为了简化模型，我们将行人在室内的行走模式分为直行、非直行。而根据行人是否在主方向上直行，又可将行人的行走模式分为在主方向的直行和在非主方向的直行，而其他情况统一为曲线行走。

图3 消防单兵定位装置佩戴示意图Fig.3 Wearing mode for fire-fighting position device

3.1 直行判定

在本算法中，行人直行是作为反馈修正的基础条件，所以对行人的直行判定方式成为了本论文的重点，根据行人在直行、转向和弯型走廊情况下z轴角速度的采集，分别给出图4~6所示的变化曲线。

考虑行人在短时内航向角不会出现较大偏差，因而将航向角融入到直行判定中。

1）加速度滑动方差检测法（Accelebration Moving Variance Detector, AMVD）检测步态：行人在有步态和无步态过程中加速度波动幅度差别较大，可设置一定的滑动窗口求取方差和适当的方差阈值来对行人是否处于行走状态进行区分，公式如下：

图4 直行情况下的z轴角速度和航向角变化图Fig.4 Variations of z-axis angular velocity and heading angle in the case of going straight

图5 转向情况下的z轴角速度和航向角变化图Fig.5 Variations of z-axis angular velocity and heading angle in the case of steering

图6 弯道行走情况下的z轴角速度和航向角变化图Fig.6 Variations of z-axis angular velocity and heading angle in the case of curve walking

2）三步航向差值判定直行：行人在直行过程中，航向角变化较小，故利用三步间航向差值进行直行判定。

3）角速度判定直行：为了与三步的航向差值判直行相对应，在角速度判直行中也采集连续运动的三步间隔内所记录的角速度值。从图4~6中可以看出，在转向中角速度值明显大于弯道和直行，故记录在这段时间内角速度绝对值小于阈值的个数占总个数的比例，记作 R1，并对该比例设定阈值 r1。当小于 r1时，认定是转向，A3记为0，大于r1时则认定为弯道行走或者直行，A3记为1，其公式如下所示：

在弯道行走中由于行人朝着某一个方向转动，故采集到的角速度值会更多地偏向正或者负的方向。如图6中，角速度分布在正值的较多。而行人在直行过程中角速度在正负的分布情况较为均匀，如图4所示。故在记录A3的同时，分别记录大于零和小于零的角速度的个数占总个数的比例，记做R2和R3。选取R2和R3中较大的比例参与直行的判定，并对此比例设定阈值r2，当大于r2时，A4记为0，当小于r2时，A4记为1，其公式如下所示：

通过对不同行人进行行走测试，当ωth为 60，r1为97%，r2为65.5%时能准确地判断行人直行状态。

当A1、A2、A3、A4同时为1时检测到行人直行，用逻辑“与”来表示：

Flag_straight为直行标志位，当为1时代表直行，当为0时代表非直行。

3.2 主方向上直行

1）主方向范围设置：根据行人的设定直行晃动角度范围range和主方向航向，当前航向角与初始时刻航向角（即 0°）的差值为-range（即 360-range）到range，90-range到90+range，180-range到180+range，270-range到270+range的角度范围内时，即认为处于主方向范围内，如图7中虚线所示。

图7 主方向和主方向航向范围Fig.7 Main direction and its heading range

2）主方向直行：行人在主方向上且直行，用下式表示：

该输出值参与位置解算，并作为下一时刻航向初始值参与四元数和欧拉角解算，从而实现反馈修正的目的。

3.3 非主方向上直行

当在非主方向上且行人直行时，满足非主方向直行条件，用如下公式表示：

图8 航向反馈流程图Fig.8 Flow chart of heading feedback

4 实验验证

本实验所用的传感器部分参数如表1所示。

表1 传感器部分参数Tab.1 Some parameters of sensors

4.1 矩形室内环境

本次实验地点选取重庆邮电大学一教学楼，结构如图9所示，该楼共有四层。规定统一的路线，起点→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→2(ABC)→3(DEF)→2(ABC)→3(DEF)→A→终点，其中2、3、4分别代表第二、三、四层，括号中内容为行走顺序点。出发时的航向为0°，三维坐标为(0, 0, 0)，并规定行人结束时方向与出发时一致，全长1000 m。

图9 结构示意图Fig.9 Structure sketch map

图10 是行人在行走30 min过程中真实航向角、无反馈修正航向角和反馈修正航向角的对比图（为了防止在图中0°附近有突跳的航向值，故将0~20°的值进行上移，整个航向角范围为20°~380°）。

图10 航向对比图Fig.10 Heading comparison chart

通过图 10实验对比分析，在无航向角反馈修正时，航向角逐渐偏离真实航向角，且差值随时间的增加而增加。当行人到达终点时，无反馈修正航向角为318.3°，与出发点差值已达到41.7°，超出了行人行走中正常航向偏差。而通过航向反馈修正算法后，达到终点时反馈修正航向角为 6.3°，保持在行人正常的身体摆动角度范围内，是行人行走中正常的航向偏差。

图11和图12分别为无反馈修正和有反馈修正的行人轨迹图。无修正的航向角在15 min后轨迹严重偏离真实路线。航向反馈修正轨迹在30 min行走过程中与实际路线保持一致。故该算法将精准的航向解算时间提升了两倍以上。表2为不同行人在相同路线下的测试结果，导航精度均在1%以内。

图11 无航向反馈修正行走效果图Fig.11 Walking effect before heading feedback correction

图12 航向反馈修正行走效果图Fig.12 Walking effect after heading feedback correction

表2 不同行人测试结果Tab.2 Different pedestrian test results

4.2 非矩形结构室内环境

本实验选择重庆邮电大学信科二楼作为实验环境。图 13(a)中含圆弧形室内环境，当再次走到矩形环境下，航向得到修正。图13(b)中含非四个主方向的直线环境，当在折线方向时轨迹较为平直，在主方向时也有修正作用，说明该算法适应能力较强。

图13 非矩形结构室内环境Fig.13 Non-rectangular structure indoor environment

5 结 论

本文针对基于 MEMS-IMU的行人室内惯性导航下，航向角误差随时间累积的问题，提出了航向反馈修正算法。行人在主方向和非主方向上直行时，将航向角进行反馈，经自适应无迹卡尔曼滤波后得到最优航向角参与PDR算法和作为下一时刻的初始航向角，减少了航向的累积误差，并提升了导航精度和导航时间，具有很强的工程应用价值。

（

）：

[1] Zhang H, Yuan W, Shen Q, et al. A handheld inertial pedestrian navigation system with accurate step modes and device poses recognition[J]. IEEE Sensors Journal,2015, 15(3): 1421-1429.

[2] 陈伟. 基于GPS和自包含传感器的行人室内外无缝定位算法研究[D]. 合肥: 中国科学技术大学, 2010.Chen W. Research on GPS/Self-Contained sensors based seamless outdoor/indoor pedestrian positioning algorithm[D]. Hefei: University of Science and Technology of China, 2010.

[3] Perlmutter M, Breit S. The future of the MEMS inertial sensor performance, design and manufacturing[C]//2016 DGON Inertial Sensors and Systems (ISS). 2016: 1-12.

[4] Kang L, Ye L, Song K, et al. Attitude heading reference system using MEMS inertial sensors with dual-axis rotation[J]. Sensors, 2014, 14(10): 18075-18095.

[5] Collin J. MEMS IMU carouseling for ground vehicles[J].IEEE Transactions on Vehicular Technology, 2015, 64(6):2242-2251.

[6] Zhang Y S, Yang X, Xing X M, et al. The standing calibration method of MEMS gyro bias for autonomous pedestrian navigation system[J]. The Journal of Navigation, 2017, 70(3): 607-617.

[7] Robertson P, Angermann M, Krach B, et al. Slam dance:inertial-based joint mapping and positioning for pedestrian navigation[J]. Inside GNSS, 2010, 5: 48-59.

[8] Borenstein J, Kwanmuang L O S. Heuristic reduction of gyro drift for personnel tracking systems[J]. Journal of Navigation, 2009, 62(1): 41-58.

[9] Jimenez A R, Seco F, Zampella F, et al. Improved heuristic drift elimination (iHDE) for pedestrian navigation in complex buildings[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2011: 1-8.

[10] Diez L E, Bahillo A, Bataineh S, et al. Enhancing improved heuristic drift elimination for wrist-worn PDR systems in buildings[C]//Vehicular Technology Conference (VTCFall). IEEE, 2016: 1-5.

[11] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J]. Analog Devices AN-602 application note, 2002, 2(2): 1-6.

[12] 胡高歌, 高社生, 赵岩. 一种新的自适应 UKF算法及其在组合导航中的应用[J]. 中国惯性技术学报, 2014,22(3): 357-361, 367.Hu G G, Gao S S, Zhao Y. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 357-361, 367.