基于惯性传感器与WiFi的室内行人轨迹推算

赵世华，赵 磊

( 山东理工大学 计算机科学与技术学院，山东 淄博 255049)

室内行人轨迹推算即实现室内环境中行人的实时位置确定与行进轨迹的计算与优化，在学术上属于同步定位与建图(Simultaneous Localization And Mapping:SLAM[1-2])的范畴.由于室内环境复杂，动态变化频繁，在工程上还没有一种统一成熟的方案.SLAM主要在贝叶斯的框架下进行讨论，多使用卡尔曼滤波器或者粒子滤波器[3-4].传统的SLAM算法多基于激光测距传感器并应用在机器人上，例如美国斯坦福人工智能实验室(Thrun实验室) 设计的FastSLAM算法[5].由于精度高的传感器比较昂贵并且不适用于行人，本文算法设计仅使用惯性传感器与WiFi信号，在没有精密的激光测距传感器的情况下，需要重点解决惯性传感器无边界的误差积累问题.德国宇航中心(Deutsches Zentrum für Luft-und Raumfahrt ：DLR)的通信和导航研究所设计并实现了FootSLAM[6]，该项技术基于粒子滤波且仅使用惯性测量装置，以行人位置状态变化的概率分布作为建图的基础，在没有视觉和距离测量传感器的情况下实现了行人轨迹推算，并将误差控制在1-3m.本文粒子滤波与底图设计部分参考借鉴了该项技术.与该项技术不同，在传感器数据采集上使用了安卓智能手机，更有利于应用和推广.在WiFi数据的使用上，DLR在FootSLAM的基础上设计了WiSLAM[7]，在行进的过程中使用WiFi信号强度衰减模型推算AP(Wireless Access Point，无线访问接入点)的位置，并使用AP位置进行后验概率计算提升粒子滤波的效果.本文所述算法后验概率的产生同样基于WiFi信号，但使用了一种基于WiFi信号强度变化趋势的WiFi地标策略，不需要推算AP位置，规避了因AP位置推算而产生的误差.综上所述，本文所述算法面向行人，在仅使用惯性传感器与WiFi信号的情况下，控制传感器误差积累，使轨迹收敛，实现行人的轨迹推算.

1 传感器的使用

使用了目前市面上普遍流行的智能手机的惯性传感器，包括加速度计与陀螺仪，根据加速度计和陀螺仪的数据分别可以获得行人的步态信息与每步的旋转角度.手机在使用时，要求平放于手中或者固定于胸前.

实验采用了平行于地面的手机手持姿势，手机传感器坐标系如图1所示，实验中x，y所在平面与地面平行，z轴正方向指向天空，y轴正方向指向身体正前方.由于手机姿态固定，步态检测使用了手机三轴加速度传感器的z轴数据，旋转角度推算使用了手机三轴陀螺仪的z轴数据.

图1 Android手机传感器坐标系Fig.1 Coordinate system of Android phone′s sensor

1.1 使用加速度计进行步态检测

由于手机的加速度计精度不高，并且行走状态下对加速度计的影响因素比较多，对加速度计进行积分求步长误差比较大.本文所述的步态检测为确定行人的步伐状态，即计算每一步的起止时间.使用手机三轴加速度传感器z轴数据进行分步，如图2所示，实验人员行进过程中，加速度计z轴数据呈现规律性变化(为了便于计算，已去除重力影响)，该变化规律反映了行走状态下身体上升和下降姿态的加速度变化.据此进行分步，即获得每一步的起始时间和结束时间.而步长部分采用了固定步长加噪声的方法来表示每一步的移动距离，固定步长由实验人员根据实际情况进行手动设置.

图2 加速度数据步态划分Fig.2 Gait analysis of accelerometer sensor data

利用加速度计可将整个移动过程按步分段，每一步的属性包括移动距离与旋转角度.旋转角度是基于手机三轴陀螺仪z轴数据进行推算的.

1.2 使用陀螺仪计算旋转角度

手机陀螺仪返回的数据表示当前时刻的角速度.通过对陀螺仪进行积分可以推算出某段时间的行进角度变化.根据加速度计的步态检测提供的每一步的起始时间t1与结束时间t2对陀螺仪进行积分可得该步旋转角度为

(1)

通过加速度和陀螺仪分别获得了行人的步态信息和方位信息.给定实验人员初始位置、初始方位和步长的情况下，利用加速度计和陀螺仪可以对实验人员进行粗糙的轨迹推算.如图3所示，白色区域为走廊，黑色表示不可通行区域，真实的行走轨迹为绕图中走廊行走三圈.由于陀螺仪误差的积累，轨迹偏离严重.

图3 直接使用惯性传感器的粗糙定位Fig.3 Rough positioning using sensor data directly

本节通过惯性传感器获得的步态信息与旋转角度作为粒子滤波中状态与状态之间转换的驱动，是产生先验概率的必要条件.状态的后验概率由该位置的WiFi环境计算得来.

1.3 利用WiFi信号构造WiFi地标

在基于WiFi的室内定位算法中，大多数需要设计WiFi强度衰减模型，推算AP位置，在确定AP位置后，需要至少三个AP进行三角定位确定手机位置作为系统观测，提升算法效果，如WiSLAM[7].然而准确地对WiFi信号进行建模是十分困难的，一方面需要考虑不同设备的发射功率，另一方面还要考虑实际空间的阻挡情况对模型的影响.同时不同的手机由于其WiFi接收模块的差异，不能使用同一模型.在WiFi信号的使用方面，采用一种基于WiFi信号强度变化趋势的WiFi地标[8]，该地标不需要推算AP位置并且避免了设备不同或手机不同而造成的影响.

1.3.1 WiFi地标的定义

WiFi地标的三元组定义格式如下

WiFi-Mark：[BSSID，Orientation，Neighboring APS]

单一WiFi地标形成情况如图4所示，如果WiFi地标的定义仅使用一个WiFi，将会在多个位置形成相同的WiFi地标，这样不能满足每个位置地标的独特性.所以在地标的定义时引入临近WiFi，三元组WiFi地标定义如图5所示，中心标有数字1的点即为一个WiFi地标(WiFi-Marks)，其所在长条形区域为路，贯穿该点的箭头表示运动的轨迹和方向，各圆形阴影区域即图中AP Coverage所示区域表示各AP的覆盖情况，MasterAP为对地标生成起决定性作用的一个无线访问节点，当检测到某个WiFi信号的峰值时，WiFi地标生成，该WiFi对应的AP即为MasterAP.BSSID表示AP的物理地址，三元组中的第一项BSSID为MasterAP的物理地址.其中山峰线表示运动过程中MasterAP的强度变化，其横坐标(Displacement)表示行人位置，纵坐标(RSS)表示收到的WiFi信号强度，图中曲线的峰值点即WiFi趋势变化的临界点(Tipping point of RSS trend).WiFi采样时间间隔为500 ms，由于对WiFi信号进行了滤波平滑，所以几乎没有出现连续峰值的情况，算法中认为一旦出现连续峰值则取最后一个作为峰值点. Orientation表示行人通过信号峰值位置前后时刻的面向角.Neighboring APS表示当前位置临近(按强度判断是否临近)AP的物理地址以及该AP的信号强度与Master AP的强度差值，为了减小计算复杂度使用两个临近AP，即取相邻AP中信号强度最大的两个.当行人在包含WiFi信息的环境中行进时，一旦检测到WiFi峰值便会对当前WiFi环境就地生成一个WiFi地标.

图4 单一WiFi地标形成情况Fig.4 The situation of using a single WiFi for the creation of landmarks

图5 三元组WiFi地标定义示意图 Fig.5 Three-element tuple for describing the WiFi-mark

1.3.2 WiFi峰值检测

实验使用的手机型号是华为Honor6，由于噪声的存在需要先对WiFi信号进行平滑.然后根据其变化趋势进行峰值检测.使用了截止频率为0.3 Hz的低通滤波器，并使用斜率检测确定WiFi强度峰值，若出现连续峰值则取最后一个，WiFi信号总数为27个，其中一个WiFi的峰值检测效果如图6所示，行走轨迹为闭环矩形路径绕行三圈，可将WiFi模块记录的时间戳与惯性传感器记录的时间戳进行映射，进而锁定在哪个位置检测到峰值.实验人员的行走轨迹为绕闭环走廊行走三圈，由图可见该WiFi信号亦出现三次规律性变化，峰值也为三个.

图6 其中一个WiFi信号的平滑与峰值检测Fig.6 Smooth and peak detection of a single WiFi

2 粒子滤波

本文算法的核心部分采用了粒子滤波的方法.将人在空间中的状态表示成概率问题，在整个运动过程中，撒下粒子群，所有粒子模拟同一人的运动.每个粒子携带自己的高斯白噪声和传感器漂移.将空间映射为一张栅格底图，栅格大小为0.1 m×0.1 m，栅格底图大小默认为1 000 m×1 000 m.通过传感器提供的驱动信息与栅格底图产生先验概率，每个状态所处位置的WiFi信息提供后验概率(传感器信号和WiFi信号在行走过程中同时获得)，并在此基础上进行算法设计，以使系统可以抑制陀螺仪误差积累，较好的拟合真实的运动轨迹，并具有良好的鲁棒性.如1.1、1.2所述，使用了智能手机来提供加速度计和陀螺仪的数据支持，为状态的改变提供驱动信息，同时使用了一种基于WiFi信号强度变化趋势的地标.

2.1 系统状态初始化

2.2 状态转移

就单一粒子而言，假设k-1时刻的坐标为(xk-1,yk-1)，面向角度为θk-1，任意时刻t向下一时刻位置过度的移动距离为

lt=d+st

式中：st为t时刻的步长白噪声，d为固定步长.

故k-1时刻向k时刻过度的步长为lk-1=d+sk-1.

有运动模型

(2)

k时刻的面向角为

系统的状态方程为

(3)

每次状态传递后都需要将每个粒子的当前状态加入到历史状态队列.

2.3 权重更新

粒子的每一状态转换都需要更新其权重，粒子的权重变化由其上一个状态的权重和该状态下的先验概率和后验概率决定.在栅格地图上，每一个栅格权重的含义为该栅格可以被通过的概率.被通过次数多的栅格更有可能被通过[6].所以每一次的状态转移，都会更新底图栅格的权重wmap，而该权重即为粒子所处状态的先验概率.若对实际环境有所了解，或者有空间地图存在时，我们可以初始化底图栅格权重.障碍物区域的栅格权重为0，一旦粒子运动到该区域，栅格权重不会被更新，而且粒子权重会被置位0，即杀死该粒子.粒子权重是由栅格权重提供的先验概率和WiFi提供的后验概率计算得来，每个粒子和每个栅格都有各自的权重.

本文使用的WiFi地标，在地图上提供了由相互独立的地标组成的地标集，即在运动过程中会生成一系列如1.3.1 WiFi地标定义所示的WiFi信号特征、转角和地标坐标的集合.粒子在某时刻的运动结束后，对手机在该时刻收到的WiFi信号集进行地标合成，若发现新WiFi地标则进行地标位置记录，地标位置为当前时刻系统状态的位置坐标.当某个时刻发生重采样时，初始时刻到该时刻的所有状态将进行调整，WiFi地标的位置也随之进行调整.

WiFi地标的位置时刻随系统状态进行调整，实则将WiFi地标与生成该地标的时刻进行绑定，再次检测到该WiFi地标，则认为这两个时刻的位置应该在同一点.即当前粒子群根据产生该WiFi地标时刻的位置坐标进行权重更新.

(4)

(5)

2.4 重采样

2.5 更新系统状态

本文所述系统状态是指从运动开始到当前时刻的所有状态. 即从运动开始到当前时刻的整条轨迹.每次发生重采样，由于新粒子会继承其父粒子的历史状态并且粒子权重分布发生了变化，故整条轨迹也会优化调整.k时刻的系统状态更新即为0～k每个时刻的位置坐标更新，每个时刻位置坐标为该时刻的粒子群的位置坐标的加权平均，即

(6)

K时刻的系统状态为Xk={s0，s1，…，sk}

当m时刻发生重采样后，相同数目的新粒子群产生，新粒子会继承其父粒子保存的历史轨迹，然而很多新粒子的父粒子是同一权重比较大的粒子，因此，粒子群保存的路径差异变小.继续运动后粒子发散，粒子保存的轨迹差异开始变大，但是差异主要体现在m时刻到当前时刻的一段.所以再次发生重采样后，该段差异缩小明显.反应到实验现象上，可以看到之前的轨迹调整微弱，偏离的轨迹被拉回正确位置.

3 实验效果

在采集时要求采集人员步伐尽量均匀.将固定步长视为可调参数，本次实验设为0.7 m，白噪声的幅值在[-0.1,0.1]之间，粒子群可以消除这方面引起的误差.实验环境为一段闭环的走廊，围绕该闭环路径绕行三圈，手机的手持姿势如本文1所述，平放于手中，全程姿态比较稳定.

如图7所示，栅格底图的权重都为相同的初值，使用2 000粒子运行，可用的WiFi地标为13个.带有圆点的线为仅使用加速度计进行的粗略轨迹推算，每个圆点代表一个状态即两个圆点之间为一步，可以发现轨迹偏离严重.带有星号的线为使用本文算法进行轨迹推算的结果，每个星号为一个状态，两个星号之间为一步.根据实际的行走路径，本文所述算法可以实现控制传感器误差积累，达到推算轨迹与行人轨迹较好拟合的目的.

图7 行人轨迹推算效果 Fig.7 The effect of pedestrian trajectory estimation

4 结束语

基于智能手机的硬件环境，在仅使用惯性传感器和WiFi信号的情况下，实现了室内环境中实验人员固定手机姿态的运动轨迹推算.其中基于WiFi信号强度变化趋势的WiFi地标策略，有效地规避了传统WiFi信号强度衰减模型由不同机型和发射设备以及墙体等障碍物所引起的误差，使用该地标策略的粒子滤波算法可以有效地控制陀螺仪误差无边界的积累，实现推算轨迹与真实运动轨迹的拟合.在数据采集方面，只需要实验人员携带手机在室内环境中行走，规避了一些特定环境中所涉及的隐私等问题，基于智能手机传感器的设计更有利于该算法的推广.

设计非固定姿态手机的室内行人轨迹推算策略，应用上探讨多人协作模式的室内地图的构建，在智能手机的环境下融合更多的信息以提高轨迹推算的准确度，这都是今后研究的方向.

[1]SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence and Pattern Recognition, 1988, 5(5):435-461.

[2] DISSANAYAKE M W M G, NEWMAN P, DURRANT-WHYTE H F, et al. An experimental and theoretical investigation into simultaneous localisation and map building[C]// CORKE P,TREVELYAN J.The Sixth International Symposium on Experimental Robotics VI.London:springer-verag,2000:265-274.

[3]张毅, 郑潇峰, 罗元,等. 基于高斯分布重采样的rao-blackwellized粒子滤波SLAM算法[J]. 控制与决策, 2016, 31(12):2 299-2 304.

[4]NASURIWONG S, YUWAPOOSITANON P. Gaussian kernel posterior elimination for fast look-ahead rao-blackwellised particle filtering for SLAM[J]. Applied Mechanics & Materials, 2015, 781:555-558.

[5]MONTEMERLO M S. Fastslam: a factored solution to the simultaneous localization and mapping problem with unknown data association[J]. Archives of Environmental Contamination & Toxicology, 2015, 50(2):240-248.

[6] ANGERMANN M, ROBERTSON P. FootSLAM: pedestrian simultaneous localization and mapping without exteroceptive sensors—hitchhiking on human perception and cognition[J]. Proceedings of the IEEE, 2012, 100(5):1 840-1 848.

[7] BRUNO L, ROBERTSON P. WiSLAM: improving FootSLAM with WiFi[C]//MOREIRA A, MAUTZ R. 2011 International conference on Indoor Positioning and Indoor Navigation. Piscataway:IEEE,2011:1-10.

[8]SHEN G, CHEN Z, ZHANG P, et al. Walkie-Markie: indoor pathway mapping made easy[C]// FEAMSTER N,MOGUL J.nsdi′13 proceedings of the 10th Usenix Conference on Networked Systems Design and Implementation. CA:USENIX Association Berkeley, 2013:85-98.