2019 年 第 38 卷 第 1 期 传感器与微系统（ Transducer and Microsystem Technologies） 137

DOI： 10． 13873 / J． 1000 —9787（ 2019） 01 —0137 —04

*
基于惯性传感器的行人室内定位算法
寇彩云，张会清，王 普
（ 北京工业大学 信息学部，北京 100124）

摘 要： 针对室内行人定位的运动特征和惯性器件 MPU6050 每次固定位置的水平差异所造成的定位精

度降低的问题，提出了一种基于零偏置校正算法。利用惯性器件在行人静态时所测得的数据，将行人动态
行走的数据减去静态时的均值数据，减小了水平位置误差。针对惯性器件本身的漂移误差在二重积分导
航算法中因积累而导致定位不准确的问题，提出了一种基于改进零速度修正算法和频域积分算法。根据
手机电子罗盘和惯性器件内置陀螺仪的优缺点，将其数据进行融合，由滑动均值滤波和角度补偿算法得到
更准确的行人的航向角。将位移数据与方向数据进行结合得到行人运动轨迹，并进行仿真验证。实验结
果表明： 所提出的算法能够有效提高室内定位的精度。
关键词： 室内定位； MPU6050； 零速度修正； 频域积分
中图分类号： TN965 文献标识码： A 文章编号： 1000 —9787（ 2019） 01 —0137 —04

Indoor positioning algorithm of pedestrian based on

inertial sensor*
KOU Cai-yun，ZHANG Hui-qing，WANG Pu
（ School of Information，Beijing University of Technology，Beijing 100124，China）

Abstract： Aiming at motion characteristics of indoor positioning pedestrian and positioning precision reducing
problem of inertial device MPU6050 caused by the level difference of each fixed position，a zero bias correction
algorithm is proposed． The algorithm uses data obtained by inertial devices in pedestrian static state to process the
data by means of mean value，that is dynamic walking data of pedestrian is subtracted from the mean value of static
data，which reduces error of horizontal position． Aiming at the problem that the drift error of the inertial device
itself is accumulated in the double integral navigation algorithm and result in accurate localization，an improved
zero velocity correction algorithm and a frequency domain integral algorithm are proposed． According to the
advantages and disadvantages of mobile phone electronic compass and inertial device built-in gyroscope，fuse their
data，more accurate pedestrian heading angle can be obtained with the moving average filtering and angle
compensation algorithm，movement trajectory of pedestrian is obtained by combining the displacement data with
directional data and simulation verification is carried out． Experimental result show that the proposed algorithm
can effectively improve the precision of indoor positioning．
Keywords： indoor positioning； MPU6050； zero velocity correction； frequency domain integration

0 引 言 鞋绑式惯性传感器为基础，并应用零速度算法修正的惯性
［6］
目前，以微机电系统 （ micro-electro-mechanical system， 定位系统 ，且其设备成本低、短距离定位导航准确度高。
MEMS） 惯性传感器进行室内定位 ［1～ 4］
的技术可以分为两 国内很多高校如西北工业大学、国防科技大学等也进行了

类，以行人运动姿态为研究的行人航迹推测和以惯性导航 一些基础理论、算法的研究实验工作。导航、制导与控制专

解算算法的行人室内定位。Levi Ｒ W 和 Judd T 在 1996 年 业的科研人员和一些公司也在逐步跟随国外的研究步伐。

提出了行人航迹推算法
［5］
，根据人员行走的步态特征，通 由于鞋绑式的惯性器件固定于脚面，每次固定的水平

过加速度传感器获得室内人员的运动数据，通过算法估计 位置不同会导致采集的数据有误差，本文提出采用零偏置

行人的单步步长和行走的步数，由陀螺仪获得人体姿态信 校正算法解决上述问题； 在进行长距离加速度积分时会有

息。2012 年瑞典皇家工学院提出了一个开源的、实时的以 累积误差的存在，本文采用改进的零速度检测算法和频域

收稿日期： 2017—11—10
* 基金项目： 国家自然科学基金资助项目（ 61640312） ； 北京市自然科学基金资助项目（ 4172007）
 138 传 感 器 与 微 系 统 第 38 卷

{
积分算法分解决上述问题。 1，| a b | ＜ th
 | a b | = ［a b （ 1） 2 + a b （ 2） 2 + a b （ 3） 2 ］，c = k amax

1 数据采集  k 槡 k k k 1
0，其他

 2
{
将惯性器件尽量呈水平状态固定到脚面上，安卓手机 k +s
2
1，| σ a b | ＜ th σamax
1
 σ a bk = 2s + 1 ∑ （ a k － a k ） ，c2 =
b b 2 k

开发的 APP 软件通过 蓝 牙 连 接 到 惯 性 器 件，将 行 人 行 走

 j = k －s
0， 其他

{
时的三轴加速度数据，手机 内 置 的 电 子 罗 盘 的 方 向 数 据 b
 | ωb | = ［ωb （ 1） 2 + ωb （ 2） 2 + ωb （ 3） 2 ］，c = 1，| ωk | ＜ thωmax
和三轴角速度数据通过蓝 牙 传 输 到 手 机 上 并 进 行 存 储。  k 槡 k k k 3
0， 其他


{
如图 1 。

2
k +s 1，| σ ω b | ＜ th σωmax
1
 σ2ω b = 2s + 1 ∑ （ ωbj － ωbk ） 2 ，c4 =
k

 k j = k －s
0，其他
（ 2）

式中 a bk （ 1） ～ a bk （ 3） 为三轴加速度； th amax 为阈值； a bj ，a bk 分

别为三 轴 合 加 速 度 和 合 加 速 度 的 均 值； th σamax 为 阈 值；
b b b b
ω k （ 1） ～ ω k （ 3） 为三轴角速度； th ωmax 为阈值； ω j ，ω k 分别为三
轴合角速度和合角速度的均值； th σωmax 为阈值； 其中 th amax ，
th σamax ，th ωmax 和 th σωmax 由 MATLAB 仿真得到。
最后，将单个条件结果进行“与”运算，求出脚着地，即
图1 Android 手机数据采集画面
“零速度”的时刻 c = c1 ＆c2 ＆c3 ＆c4 。
2 数据处理
对处于零速度时刻的数据进行置零校正。
2． 1 零偏置校正算法
2． 3 频域积分
由于惯性器件采用鞋绑式，每次固定的位置无法达到
［7］
将校正后的数据进行没有累积误差频域积分 ，进行
理想的水平状态，造成了静止状态时所测得的三轴加速度
傅里叶逆变换后得到行人更准确的位移信息。
值不为零，且对零速度检测算法的阈值设定也会因每次固
设一次积分前的信号为 v（ n） （ 0 ＜ n ＜ N － 1） ，进行傅里
定位置不理想而造成阈值每次都会变，阈值的变化也会造
叶变换后为
成最终结果的误差。 N －1
2π
j kn
V（ k） = ∑ v（ n） e N = a k + b k j，
0 ＜ n ＜ N －1 （ 3）
零偏置校正算法是对每次测量数据的初始静态数据求 n =0

均值，行人行 走 时 的 动 态 数 据 减 去 初 始 静 态 数 据 均 值， 将每个频率分量的信号值 V（ k） 转换为一次积分后有

{
以此保证静态时三轴加速 度 数 据 为 零，满 足 惯 性 器 件 理 Ak π
D（ k） = d1k + d2k j，d1k = cos（ φ k － ）
ω 2
想状态下的 水 平 固 定 位 置 状 态。因 为 采 集 数 据 的 设 备 （ 4）
Ak π
每秒采集 100 组数据，为了 保 证 采 集 到 的 数 据 为 静 止 状 d2k + sin（ φ k － ）
ω 2
态的数据，所 以 本 文 取 采 集 数 据 时 的 前 50 个 数 取 求 均 2 2
式中 Ak = 槡ak + b k ，φ k = arctan（ b k / ak ） ，ω = 2πk / T。
值作为初始误差
再将 D（ k） 逆变换，即可得到输入信号 v（ n） 的一次精
1
x珋= （ x1 + x2 + … + x n ） ，n = 1，
2，3，…，
50 （ 1） 准的积分。
n
2． 4 均值滤波与角度补偿算法
以采集的数据减去初始误差作为行人真实行走数据。
［8］
首先用 8 字校正法校正手机中的电子罗盘 ，手机电
2． 2 改进零速度检测算法
子罗盘的优点是能提供绝对的航向值且使用方便。对电子
积分误差会随着时间的推移而累积，用改进的条件判
罗盘获取的数据首先使用滑动均值滤波，减小数据的波动，
断法进行零速度检测，当检测到行人运动的“零速度”时，触
由于电子罗盘本身的原因，在向南方向行走时，所获取的电
发零速度修正，对数据进行置零校正； 对行人运动的三轴加
子罗盘的数据会突然地跳变，对获取的航向会有很大的影
速度和三轴角速度进行阈值设定，当小于一定范围时视为 响，针对此问题采用角度补偿的方法进行校正。滑动均值
静止状态。 处理的阶数选为 50，当前后时刻的角度突然 地 跳 变 超 过
由于脚着地的时候，只受到重力的作用，此时利用合加 180°时，是在向南的方向，此时在［－ 180°，－ 170°］之间产
速度的幅值、方差、合角速度的幅值和方差在给定的阈值范 生一个随机的数作为此时的航向角，当前后时刻的角度跳
围内来判断静止时刻。 变超过 30°而小于 180° 时，则使用陀螺仪的角度作为此时
第1 期 寇彩云，等： 基于惯性传感器的行人室内定位算法 139

的航向角。
3 定位算法设计
系统使用了集成蓝牙 HC—06 模块和 MPU6050 惯性传
感器，集成了加速度计、角速度计和磁力计，不需要借助于
外界任何的信号与装置，且结构简单、抗干扰性强等优点。
根据行人的运动特点，系统采用鞋绑式 MPU6050 惯性传感
器和手机相结合的数据采样方式。模块固定在行人脚面，
图4 零速度校正和姿态角验证结果
行人手持手机正对行走方向。定位算法的流程如图 2。

图5 x 和 y 轴加速度频域积分验证

图2 行人导航定位算法流程
4 实验测试仿真
算法设计验证及分析平台为 MATLAB Ｒ2010a。
1） 零偏置校正算法验证
校正结果如图 3 所示，经过零偏置校正后，静止状态下
加速度数据值基本为零，数据准确度得到了提升，对零速度
图6 行人航向角结果
校正的阈值设定也提供了方便。
x，y 轴积分数据作为横纵坐标画出其轨迹，如图 7 所示。

图3 零偏置校正结果
2） 改进的零速度校正算法验证
对所采集的加速度、角速度、加速度方差、角速度方差
进行阈值设定，当零速度时刻 c = 1 时，将这一时刻的加速
度值置零，所得到零速度检测结果和姿态角检测如图 4。
3） 频域积分验证
对行人行走的二维平面的 x 轴和 y 轴的速度数据进行
离散化并进行积分，积分结果如图 5 所示。
4） 均值滤波与角度补偿算法验证得到的行人航向角数
据如图 6 所示。
对行人的 x，y 轴加速度数据进行时域内二次积分，将
图7 无向时域和频域积分
由图 7 可以看出，时域积分画出的无向运动轨迹褶皱
较多，而频域积分画出的无向运动轨迹则比较顺滑流畅，因
为行人在行走过程中是按直线行走，不会不停地左右变动，
因此频域积分数据更准确一些。
图 8 为改进前轨迹。图 9 为经过零速度检测算法，时
域和频域积分算法，航向角使用滑动均值滤波处理后的行
人行走的轨迹（ 算法 1） ，从图中可以看出在行人行走时偏
离了实际的路线。但频域积分比时域积分更接近实际路
线。图 10 为经过改进零速度检测算法，频域积分算法，航
向角使用滑动均值滤波和角度补偿算法处理后的行人行走
2
的轨迹（ 算法 2） ，其中，加速度幅值取 2． 4 m / s ，加速度方差
2
取 0． 7 m / s ，角速度幅值取 25° / s，角速度方差取2 000° / s；
由图可知，所得的运动结果比改进算法前更准确。
 140 传 感 器 与 微 系 统 第 38 卷

对加速度数据的影响，并提出了运用改进零速度检测算法
和频域积分算法减小累积误差对运动结果的影响。并提出
了运用滑动均值滤波和角度补偿法处理电子罗盘的数据，
结合陀螺仪得到更准确的行人航向角，测试表明，零偏置校
正算法提高了原始采样数据的准确度，改进零速度检测算法
和频域积分减小了累积误差，
滑动均值滤波和角度补偿算法
图8 改进算法前轨迹 提高了航向角的准确度，将算法融合后得到行人运动轨迹，
实验结果表明，
本文算法提升了行人运动轨迹的准确度。
参考文献：
［1］ 赵 锐，钟 榜，朱祖礼． 室内定位技术及应用综述［J］． 电子
2014（ 3） ： 154 － 157．
科技，
［2］ 蔡春龙，刘 翼，刘一薇． MEMS 仪表惯性组合导航系统发展
现状 与 趋 势 ［J］． 中 国 惯 性 技 术 学 报，2009，17 （ 5 ） ：
图9 算法 1 轨迹 562 － 567．
［3］ 李 旭 辉． MEMS 发 展 应 用 现 状［J］． 传 感 器 与 微 系 统，
2006 ，25（ 5） ： 7 － 9．
［4］ Bortz J E． A New mathematical formulation for strap down inertial
navigation［C］∥IEEE Transactions on Aerospace and Electronic
Systems，
1971： 61 － 66．
［5］ Levi Ｒ W，Judd T． Dead reckoning navigational system using

图 10 算法 2 轨迹 accelerometer to measure foot impacts： US5583776［P］． 1996—

表 1 为实际值、算法改进前、算法 1、算法 2 各拐点值与 10—12．
实际值误差绝对值。本文所用的算法有效提高了运动轨迹 ［6］ Nilsson J，Skog I，Handel P，et al． Foot-mounted INS for every-

的准确度。 body—An open-source embedded implementation［C］∥Position

表1 拐点误差绝对值对比 Location and Navigation Symposium（ PLANS） ，IEEE / ION，

2012．
［7］ 王 济，胡 晓． MATLAB 在振动信号处理中的应用［M］． 北
拐点 改进前算法 算法 1 算法 2
京： 中国水利水电出版社，
2006： 104 － 107．
起点 （ 0，
0） （ 0，
0） （ 0，
0）
［8］ 李 艳，管 斌，王成宾． 电子磁罗盘航向角误差推导及分
拐点 1 （ 15，
5． 0） （ 1，
7． 0） （ 0，
0）

拐点 2 （ 17，
8． 1） （ 2，
6． 1） （ 0，
0． 2） 析［J］． 仪器仪表学报， 34（ 7） ： 1566 － 1572．
2013，

拐点 3 （ 1，
5． 1） （ 1，
0． 1） （ 0，
0． 1） 作者简介：
终点 （ 2，
7． 5） （ 1，
0） （ 0，
2． 0） 寇彩云（ 1989 － ） ，女，硕士研究生，主要研究方向为室内定位。
张会清（ 1968 － ） ，男，博士，副教授，主要研究 领 域 为 室 内 定
5 结束语
位，E—mail： bgdbjut@ 163． com
本文分析了基于零偏置校正算法、改进零速度检测算
王 普（ 1962 － ） ，男，教授，博士生导师，主要研究领域为工业
法和频域积分算法的原理和实现方法，指出了零偏置校正 过程控制与优化，复杂系统控制和计算机控制系统等。
檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸檸
（ 上接第 136 页） ［13］ 杨万海． 数据融合及其应用［M］． 西安： 西安电子科技大学出
［10］ Pikora K，Ehlers F． Analysis of the FKIE passive radar dataset 版社，
2004．

with GMPHD and GMCPHD［C］∥The 16th International Confe- ［14］ 权太范． 信息融合： 神经网络——模糊推理理论与应用［M］．

rence on Information Fusion，IEEE，Istanbul，

2013： 272 － 279． 北京： 国防工业出版社，
2002．
［15］ 徐 毓，李 锋，金 以 慧． 基 于 聚 类 融 合 的 多 目 标 跟 踪 算
［11］ Daniei C，Vo B N． Convergence analysis of the Gaussian mixture
法［J］． 传感器技术， 21（ 7） ： 31 － 34．
2002，
PHD filter［J］． IEEE Transactions on Signal Processing，2007，
作者简介：
55（ 4） ： 1204 － 1212．
董 青（ 1994 － ） ，女，硕士研究生，主要研究方向为多源信息
［12］ 车志宇，王海鹏，董云龙． 基于概率最近邻域算法的分布式多
融合，目标跟踪，E—mail： 15991947116@ 163． com。
传感器多目标跟踪算法［J］． 海军航空工程学院院报，
2009， 胡建旺（ 1967 － ） ，男，教授，硕士研究生导师，主要研究领域为
24（ 2） ： 202 － 206． 信息融合，目标跟踪，E—mail： ccehjw_101367@ 163． com。