融合水平仪、气压计的行人位置估计算法*

王晓甜，付梦媛，毛永毅

(西安邮电大学电子工程学院，陕西 西安 710061)

随着技术的发展，人们对室内定位技术的精度、成本、普适性等方面提出了更高的要求[1-4]。 目前，室内二维定位已经无法满足人们的日常需求。 现在的室内环境以多楼层场景居多，针对多楼层的三维定位受到越来越多的关注[5-8]。

目前，三维定位技术大都利用接收信号衰落特征结合地图环境建立指纹库来完成定位[9-10]，由于信号穿墙衰减较大，前期工作复杂且建立指纹库后需持续更新，该方法多用于单个楼层或相邻两个楼层的定位。 对于三层及以上的楼层，可以通过微机电系统(Micro Electromechanical System，MEMS)利用行人航迹推算(Pedestrian Dead Reckoning，PDR)算法完成定位[11-14]，通过对人员在行进过程中的加速度、角速度进行两次积分得到行人的位置信息。现有PDR 算法存在累积误差大、楼层识别不够准确等问题。 为了解决上述问题，本文融合水平仪、气压计和PDR 算法共同完成对室内行人所处楼层位置的估计，同时水平仪为PDR 算法提供了新的数据来源，对PDR 算法累计误差进行动态校正，从而提高多楼层三维定位算法的精度。

1 融合水平仪的行人航迹推算算法

PDR 算法如图1 所示，在获得上一时间状态的位置信息的基础上，通过推算或实测得到的运动距离和方向变化来计算当前的位置。

图1 PDR 算法

PDR 算法需要获得目标运动时的方位角变化、

步数与步长。 如图2 所示，人体在运动时存在单复步的区别。 单步为双脚交替的一个跨步，而复步则为单侧足部进行的一个跨步。 在运动分析、步长推算时需要进行明确区分。 行走步数Nstride、步长Lstride，行人行走的距离L的关系为:

图2 单复步关系

通过水平仪对步态周期进行检测和记录。 使用峰值检测法对步伐进行检测，通过步频和一个步态周期内的水平仪角度数据可以得到行人的步长。

式中:a，b和c是可依据环境进行设置参数；F为行走频率；βmax和βmin为一个步态周期的水平仪数据的最值。 行人运动时除了需要获得每次递推时的步数和步长以外，还需要获取递推前后的方位角的变化，α为方位角，Δα为方位角变化值:

第k次递推的坐标(xk，yk)可由前一次已知的坐标进行递推得到，其公式如下，式中L为步行位移，α为方位角:

大气压与高度的关系模型如式(7)和式(8)所示:

Tm表示两个等压面的平均温度，T0表示基点温度，T则为测量值。 在室内环境下可认为Tm＝T0＝T。 基准点高程为H0、压强为P0及气温为T。

假设建筑中的楼梯具有不同的高度，且每个台阶高度可变。 通过式(7)行人起点和终点的高度获得高差ΔH；若假设楼梯每个台阶高度相同，即每步高度都可以视为常数。 计算相邻台阶的高度增量Hinc的平均值Ha和标准偏差DH，当｜Hinc-Ha｜＞DH时，通过相邻的高度校正Hinc。 得到行人上下楼n步的高度变化为:

使用线性系统状态方程，经过系统地输入输出观测数据，得到系统状态的最优估计的算法。 因为系统的噪声影响包含在观测数据中，所以此最优估计过程也可作为滤波过程。 移动目标的状态矢量表示如下:

移动目标状态可由式(11)表示:

式中:wi(i∈1，2，3，4，5)为高斯系统噪声。

行人在室内多楼层位置估计原理图如图3 所示，流程如下:①按照行人不同运动状态下的步频区间设置水平仪单位时间内的采样数。 ②得到水平仪采样数据后，对其截取整周期数据进行预处理。 滤波后分析提取特征值:角度区间，步频和步长等。 ③通过磁力计参数和陀螺仪参数计算出方位角。 ④结合PDR 算法，经气压计辅助，利用式(11)计算行人在室内每一步的位置坐标，得到其所在建筑物内的位置变化情况。

图3 行人在室内多楼层位置估计原理图

2 实验及结果分析

数据采集装置为Lenovo 小新Air 计算机一台，具有磁力计、陀螺仪、水平仪及加速度计软件的手机一部，TP-LINK TP-WDR5620 路由器一个。 数据采集时将手机置于右脚脚踝内侧，采集的数据通过路由器传送至计算机。 实验选取5 名年龄在18 岁～50 岁之间，身高在160 cm～185 cm 之间的志愿者进行实验，行走平均速度3 km/h，上下楼平均速度为0.5 步/s。 实验分别在两个地点进行，分别针对二维定位和楼层定位进行研究。

实验地点一为学校2 号实验楼五楼，如图4 所示，主要对二维定位进行研究。 实验分为两组:①实验者使用改进的基于水平仪的PDR 算法以东南角开始，固定起点与终点，沿着实验楼走廊绕行，分析该过程中行人的位置，与计划路线进行对比；②实验者在同一地点同样环境下使用传统PDR 算法以相同的路径绕行，与本文方法进行比较与分析。

图4 实验楼结构图

实验地点二为学校长智2 号宿舍楼，如图5 所示，主要对三维定位进行研究。 宿舍楼共五层，相邻楼层间的楼梯存在拐角，台阶高度为0.15 m，连续台阶数为10。 该地点实验分为两组:①在实验楼内按照计划路线连续地运动，同时记录运动过程中水平仪和气压计的变化情况，分析该过程行人的位置，与计划路线进行对比。 ②在第一组实验基础上，记录测试过程中实验者的位置，得到实验者位置坐标，分析室内楼层定位精确度。

图5 宿舍楼内部结构图

2.1 二维定位实验结果

2.1.1 基于水平仪的二维位置估计

按照图6 路径，对实验者行走路径进行分段，实验者从起点开始，依次通过①～⑥。 实验进行20次，随机选取其中一次测试的记录数据，如图7所示。

图6 实验地点路径规划及分段

图7 测试中水平仪及航向变化数据

(1)根据实验者的水平仪数据变化区间、周期及频率等数据，结合运动状态识别方法，可以判断出实验者运动状态、步数和速度。

(2)根据水平仪记录可看出实验者在第23 s、88 s、107 s、145 s 处时的数据周期变化规律发生明显改变，同时刻对应航向数据也发生明显变化。 与行人实际运动进行比对，发现处于该时刻时行人发生明显方向改变，改变的方向角度可通过陀螺仪直接测量得到。

(3)通过航向数据得到实验者测试过程中的方向变化，通过对水平仪采样数据进行处理后得到特征值步长，然后通过PDR 算法可以得到实验者的详细位置。

随机选取其中一次测试的分段记录数据如表1所示。 随着实验者行走距离的增加，对于其位置估计的误差也随之增大。 当行走的距离不超过30 m时误差小于5%，对距离估计的精确度较高；随着步数的增加，距离估计的误差也随之增加，当行走距离达到70 m 时，定位偏差达将达到7 m，误差高于10%。

表1 测试过程中的数据及误差对比

由图8 可见，测试路径中有三个直角转弯，两个锐角转弯，角度不同的转弯也会造成一定的误差累积。 第1～2 段轨迹较为笔直，并未出现明显地方向偏差和抖动；从第3 段开始，第二个转弯后，轨迹发生轻微偏移；第5 段出现较为明显的方向波动，导致第6 段也相应地出现偏移。 路径长度相同的直线路径实验和带有拐角的路径实验产生的测试结果会有差异，因为其与实验时的路径结构有关。 当路径中存在拐角时，实验者行走过程中会产生角度变化，由于在行走过程中实验者很难保证在每一个转弯时的转弯半径都相同，也会导致在路径实验中产生距离误差。

图8 实验者测试轨迹图

2.1.2 二维位置估计的性能分析对比

使用本文算法和传统PDR 算法进行对比，以相同的路径沿着实验楼走廊绕行一周为一组，进行五组实验。 通过加速度计、磁力计及陀螺仪数据得到测试过程中的步长、步频及方向变化。 数据经滤波处理后利用两种算法得到运动轨迹，两种算法和计划行进路径对比如图9 所示。

图9 两种算法行人轨迹对比

实线代表实验者测试的实际路径，在整个的测试过程中实验者做3 km/h 的匀速运动。 当实验者经过固定位置点时，将对其位置进行校对和修正。虚线对应为实验者使用传统PDR 算法得到的轨迹路线，行走过程中位置估计最大误差达到7 m。 双划线对应的是实验者使用本文算法得到的轨迹路线，该过程中的最大行人位置误差为3 m。 从图9的定位轨迹能够看出，使用传统PDR 算法进行定位的行人轨迹波动大且误差分布不均匀。 融合水平仪与PDR 后，行人定位轨迹比较平滑，其测试轨迹与传统PDR 算法相比更靠近真实路线。

多次重复该实验，传统PDR 算法的定位平均误差在3m 左右，最大误差为7 m。 本文算法的定位平均误差在1.3 m 左右，最大误差为3 m。 通过图10(a)的误差分析可知，传统PDR 的定位误差在行走距离达到20 m 时开始加速增大，且行走距离越远，误差的增加越大。 基于水平仪的PDR 算法的定位误差整体较小，且其误差增速在同等条件下也低于传统PDR 算法。 三种算法的定位误差累积概率分布如图10(b)所示，本文算法的定位效果优于传统PDR 定位算法。 其中，本文算法定位的平均误差为1.3 m，90%的定位误差小于2 m，60%的误差小于1.5 m。

图10 两种定位方法的误差对比

2.2 多楼层定位实验及结果

2.2.1 运动状态识别

为验证本文方法多楼层定位的效果，设计如下实验:在图5 宿舍楼内，首先从三楼楼梯处开始上楼至五楼，后穿过40 m 走廊沿另一侧楼梯下楼至四楼楼梯处结束，重复10 次实验，随机选取一次测试过程中的水平仪数据和气压计数据，如图11 所示。 观察水平仪数据、气压计和航向角数据变化，分析得到:

图11 水平仪数据、气压计数据及方向变化数据

①根据水平仪记录可看出实验者在第10 s、22 s时的数据周期变化规律发生明显改变，同时刻对应航向数据也发生明显变化，变化在180°左右，32 s处航向变化在90°左右。 与行人实际运动进行比对，行人处于楼梯拐角处，发生明显方向改变，观察气压计的变化情况发现该时刻气压并未发生改变，因为行人处于拐角处时高度并未发生变化。 第45 s～75 s 处有方向上的波动，为行走时腿部抖动产生的噪声所致，这段时间内运动方向未发生改变。 第76 s、90 s 处航向数据也发生明显变化。

②通过航向角数据得到实验者测试过程中的方向变化、通过对水平仪采样数据进行处理后得到特征值步长以及通过气压计得到行人运动时的高度变化，结合PDR 算法，可以得到行人在多层建筑物内的详细位置。

结合表2 分析可得:0～45 s 目标处于上楼状态，连续上台阶步数为5 步，可知连续台阶数为10。且上楼过程中经历了3 个拐角，故而初步判定为上楼2 层；46 s～75 s 目标处于行走状态，行走步数为30，水平仪角度变化无明显波动，判断此时目标穿过走廊；76 s 之后，目标开始下楼，途经一个拐角，初步判定为下楼1 层。

表2 测试过程中的数据特征对比

2.2.2 多楼层定位实验结果

综合上下楼路径数据，结合二维位置估计及气压计测高对行人行走过程中的位置进行估计，其结果如图12 所示。 其中图12(a)为实验者在宿舍楼内行走的轨迹图。 从轨迹图中可清晰观察到行人的高度变化和移动趋势。 采用基于水平仪和气压计的PDR 算法所估计出的路径连续性更加明显和平滑。图12(b)为上楼过程中的轨迹图，本文方法能够很好地分析实验者上楼和下楼时每一次脚部与台阶的接触，能够较为直观地描述行人在楼梯上的脚部运动规律，可观察出行人在上楼时连续的单侧步数为5，依据人体上楼时双脚交替的运动习惯，可得连续楼梯数为10。 所以通过行人上楼的轨迹可判断测试者的轨迹中的台阶数量和位置。

图12 室内行人上下楼位置估计

上下楼过程中，行人的运动状态切换较为频繁，包括行走、上楼和下楼三种状态，因此导致多楼层实验中出现的位置误差、方向误差和距离误差均比单楼层实验中的误差大。 虽然测试人员在每次实验中都沿着同一路线行走，但由于运动的随意性，在行走过程中难免出现轻微的方向偏移和速度的不均，导致偏离预设路径，特别是在路径中的转弯处，行走过程中方向的改变更容易出引起偏航。

3 结束语

本文提出了结合水平仪和气压计的行人位置估计算法。 通过磁力计、陀螺仪结合水平仪阈值可以判定行人运动状态及行人位置变化；结合行人运动状态和气压计得到目标行人的高度及楼层；利用PDR 算法最终得到行人位于多层楼内的位置。 实验证明在噪声环境下本文算法的定位精度比传统PDR 算法有明显提高。