CN110487273B - 一种水平仪辅助的室内行人轨迹推算方法 - Google Patents

Abstract

本发明属于室内定位领域，涉及传感器信号处理、粒子滤波算法、行人航位推算等技术，具体为一种水平仪辅助的室内行人轨迹推算方法，用以克服传统PDR方法中手机姿态不稳定带来的航向信息不可用的问题。本发明首先利用手机内置水平仪检测手机姿态，获取可靠的行人航向信息，接着分析各AP的Wi‑Fi接收信号强度获取行人粗略位置，然后利用手机地磁信息检测行人是否发生转弯动作，并结合实际地图信息更新行人航向，最终融合行人步频、步长、航向三种信息推算行人运动轨迹；相对一般的手机惯性导航而言，本发明在导航过程中不需要保持手机姿态相对人体姿态不变，实时更新行人航向，技术简单，实用性强，适用于室内众包指纹建库领域。

Description

一种水平仪辅助的室内行人轨迹推算方法

技术领域

本发明属于室内定位领域，涉及传感器信号处理、粒子滤波算法、行人航位推算(Pedest rian Dead Reckoning，PDR)等技术，具体为一种水平仪辅助的室内行人轨迹推算方法。

背景技术

在PDR技术中，步频、步长、航向三种信息至关重要；其中，步频、步长的研究较为成熟，而如何提高航向信息获取的准确度，仍是研究热点。目前，主流思路是通过四元数法，结合陀螺仪信息解算航向，该方法要求手机一直保持相对人体固定的平端姿态，运算量大，硬件要求高，容易累积误差，造成航向信息不可用，进而导致PDR算法失效。

目前主流的航向测算方法通常要求手机一直保持固定的平端姿态，以保证地磁数据，陀螺仪数据准确，适用于四元数方法的计算。但现实情况下，手机姿态多样，可能打电话，可能放包里，可能平放看视频，这种限制条件严重影响了用户体验，使得目前手机PDR算法不具有实际应用价值。

在室内环境中，行人运动状态一般分为长时间直线行走与短时间直角转弯。研究发现，手机最常出现的是机身平放、头部朝前的姿态，此时，手机获得的地磁信息可以较好的表征行人航向。文献“A.Rai,K.K.Chintalapudi,V.N.Padmanabhan,and R.Sen,“Zee:Zero-effort crowdsourcing for indoor localization,”in Proc.ACM MobiCom,2012,pp.293–304.”指出，室内环境中，墙壁、走廊可以辅助定位。根据上述背景，本发明提出了一种融合实际环境信息的行人运动轨迹推算方法。

发明内容

本发明的目的在于针对传统PDR方法中手机姿态不稳定带来的航向信息不可用的问题，提出了一种水平仪辅助的室内行人轨迹推算方法；该方法首先利用手机内置水平仪检测手机姿态，获取可靠的行人航向信息，接着分析各AP的Wi-Fi接收信号强度(ReceivedSignal St rength，RSS)获取行人粗略位置，然后利用手机地磁信息检测行人是否发生转弯动作，并结合实际地图信息更新行人航向，最终融合行人步频、步长、航向三种信息推算行人运动轨迹。

为实现上述目的，本发明采用的技术方案为：

一种水平仪辅助的室内行人轨迹推算方法，包含以下具体步骤：

步骤1.数据预处理

通过加速度传感器、陀螺仪传感器、地磁传感器、重力传感器获取数据，并将所得到的数据做滤波平滑处理；

步骤2.计算步频

2-1.加速度数据预处理，设置滑动时间窗口ΔT，采样频率f；

对每一个采样点计算：

其中，a_x、a_y、a_z分别为X轴、Y轴、Z轴的加速度，均由加速度传感器测量得到；

2-2.搜索滑动时间窗口ΔT内的所有波峰，并依次标记；

2-3.去除伪波峰，得到滑动时间窗口ΔT内所有有效波峰：location＝[l₁,l₂,l₃,...,l_i,...,l_n,]^T，n为有效波峰总数，则滑动时间窗口ΔT内的步数为n；采用三阈值法去除伪波峰，具体为：

2-3-1.设置合加速度阈值为1.2g：当合加速度a＜1.2g，认定为伪波峰；

2-3-2.设置波峰阈值：计算时间窗口内的三个最大值的平均值A_max，三个最小值的平均值A_min，取A_max和A_min的平均值average为波峰阈值，当小于波峰阈值时，认定为伪波峰；

2-3-3.设置采样点阈值M：M＝0.2f；当相邻两个波峰之间采样点数小于M，认定第二个波峰为伪波峰；否则，两个波峰均为有效波峰；

步骤3.计算步长

3-1.计算

得到第i步的起始点，同时作为第i-1步的终止点；进而得到所有起始点、终止点的集合：

RANGE＝[range_1,range_2,...,range_i,...,range_n,range_L]^T；

其中，range_1表示滑动时间窗口ΔT内第1个采样点、作为第1步的起始点，range_L表示滑动时间窗口ΔT内最后1个采样点，作为第n步的终止点；

3-2.计算每一步的步长：

其中，Q_i为第i步的采样点总数，a_q表示第i步中第q个采样点的合加速度，k、b为预设常数；

步骤4.基于水平仪辅助计算行人航向角

4-1.将每个时刻对应的时间戳t、方位角α、俯仰角β、翻滚角χ、X轴重力分量G_x、Y轴重力分量G_y、Z轴重力分量G_z组成向量：GRAND＝[t,α,β,χ,G_x,G_y,G_z]^T；其中，方位角α由地磁传感器测量得到，俯仰角β、翻滚角χ由陀螺传感器测量得到，X轴重力分量G_x、Y轴重力分量G_y、Z轴重力分量G_z由重力传感器测量得到；

针对时刻t，若满足条件：俯仰角β、翻滚角χ均小于预设阈值β_th，且重力X轴分量G_x小于预设阈值G_th，则认定时刻t手机处于平放姿态；从零时刻开始检测，当连续检测手机处于平放姿态超过时间阈值t_th，则实现有效采样，将该时间段内所有时刻方位角的平均值作为预测初始行人航向角；

4-2.结合室内地图和AP布局得到所有可能航向；将预测初始行人航向角与所有可能航向比较，得出与预测初始行人航向角最接近的可能航向，将该可能航向作为初始行人方向；

5.转弯检测

5-1.设定采集窗口ΔT_orien，计算每个采集窗口ΔT_orien范围内方位角α的平均值orien；

5-2.计算前后两个orien的变化幅度，若超过阈值80度，则判定为发生转弯动作；

5-3.实时检测行人位置上所有AP的信号强度，选定信号强度最大的AP所在的拐点为转弯时刻行人最接近的拐点，在最接近的拐点处向信号强度最大的AP转向，并更新行人航向角；

步骤6.根据步骤2～步骤5得到的步频、步长、行人航向角信息，采用粒子滤波算法得到行人轨迹推算。

本发明的有益效果在于：

本发明提供一种水平仪辅助的室内行人轨迹推算方法，通过手机内置水平仪检测手机姿态，当检测到手机处于平放姿态时，记录下此时的地磁数据作为行人航向，并一直沿用到检测出行人发生转弯动作之前；在检测到行人发生转弯动作之后，首先利用AP信号强度信息，粗略获取行人大致位置，匹配到行人当前所在的拐角，接着结合地图信息，将当前行人航向自动匹配到行人所在拐角的转弯后的方向，并一直沿用到下个转弯动作发生之前，如此迭代，实时更新行人航向信息。相对一般的手机惯性导航而言，本发明在导航过程中不需要保持手机姿态相对人体姿态不变，通过融合RSS信号强度与粒子滤波算法，实时更新行人航向，技术简单，实用性强，适用于室内众包指纹建库领域。

附图说明

图1为本发明水平仪辅助的室内行人轨迹推算方法的流程示意图。

图2为本发明基于水平仪的航向更新算法流程图。

图3为本发明实施例中水平仪检测性能图。

图4为本发明实施例中实验场景示意图。

图5为本发明实施例中手机姿态示意图。

具体实施方式

下面结合附图与实例对本发明的技术方案做进一步的详细说明。

本实施例提供一种水平仪辅助的室内行人轨迹推算方法，其流程如图1所示，具体包括以下步骤：

1.数据预处理

使用安卓***提供的接口注册加速度传感器、陀螺仪传感器、地磁传感器、重力传感器的监听，实时监听获取三个传感器的数据变化，并将所得到的数据做滤波平滑处理；

当用户移动时，利用加速度传感器、陀螺仪传感器、地磁传感器，得到用户的步频、步长以及方向角信息，从而得到PDR定位的结果，最后通过粒子滤波算法融合各数据得到用户的最佳定位结果；

2.三阈值法测步频

2-1.加速度数据预处理，设置ΔT为一个滑动时间窗口，下文均在一个ΔT的时间范围内分析步数、步长、航向信息，本实施例中，ΔT＝3秒；

取

为处理后的加速度数据，其中，a_x、a_y、a_z分别为X轴、Y轴、Z轴的加速度，均由加速度传感器测量得到；正常人前进一步的时间范围是0.2s至1s，大量实验发现，将采样频率设为50Hz，可以通过加速度信息有效刻画行人步频状态，避免步数漏检问题的出现，故本实施例中，采样频率设为50Hz；

2-2.依次分析ΔT范围内的每一个加速度采样点数据，若当前加速度采样点大于前一个且小于后一个采样点，则记当前采样点所在位置为一个波峰，依次得到ΔT范围内的所有波峰位置；

2-3.通过三阈值法，分析所有波峰数据，去除伪波峰；具体为：

2-3-1.设置合加速度阈值为1.2g：当合加速度a＜1.2g，认定为伪波峰；

2-3-2.设置波峰阈值：避免抖动干扰，将一个时间窗口内的三个最大值的平均值设为A_max，三个最小值的平均值设为A_min，取A_max和A_min的平均值average为波峰阈值，当小于波峰阈值时，认定为伪波峰；

2-3-3.设置采样点阈值M：M＝0.2f、f为采样频率；从前一个波峰到下一个波峰，也就是2步之间，至少间隔0.2秒，在50Hz采样率的情况下即对应10个采样点；当相邻两个波峰之间采样点数小于M，认定第二个波峰为伪波峰；否则，两个波峰均为有效波峰；

2-4.通过“三阈值法”得到ΔT时间段内所有有效波峰：location＝[l₁,l₂,l₃,...,l_i,...,l_n,]^T，n为有效波峰总数，则ΔT时间段内的步数为n；

3.公式法测步长，通过有效波峰位置，分析每一步的起始与终止时刻；

3-1.计算

得到第i步的起始点，同时作为第i-1步的终止点；

RANGE＝[range_1,range_2,...,range_i,...,range_n,range_L]^T；

需要说明是，range_1表示ΔT时间段内第1个采样点、作为第1步的起始点，range_2即是第1步的终止点、也作为第2步的起始点，依次类推，range_n作为第n步的起始点，range_L表示ΔT时间段内最后1个采样点，作为第n步的终止点；

3-2.计算每一步的步长，步长测量主要利用步长与加速度间的线形关系：

其中，Q_i为第i步的采样点总数(包括两端点)，a_q表示第i步中第q个采样点的合加速度，k、b为预设常数、通过最小二乘法拟合得到；

4.基于水平仪辅助的行人航向角算法

行人在运动过程中，手机姿态变化幅度很大，可能放在口袋，可能在耳边接听电话，可能随手臂摆动，地磁数据难以准确的实时表征行人实际的前进方向；如图5-1所示，分析不同手机姿态发现，行人在使用手机阅读屏幕时，手机通常处于平放姿态，机头朝前，此时的地磁数据可以提供绝对航向值，且不需要经过积分运算，可以较好的表征行人实际的前进方向；而在室内环境中，行人运动状态可分为长距离直线行走与短时间直角转弯两种主要状态，若能检测出手机平放姿态，此时的方向数据可以一直沿用到检测出行人发生转弯动作之前，据此，结合室内墙壁、走廊等客观条件限制，通过粒子滤波算法，即可较好的跟踪行人运动轨迹；

水平仪作用是检测手机在运动中是否处在水平放置的姿态，若是，则此时的地磁方向数据，可以较好的表征出行人的运动方向，具体过程如下：

Android可以直接调用地磁传感器测量方位角，陀螺传感器测量俯仰角、翻滚角，其中，航向角即为地磁数据的前进方向(-180度到180度)，俯仰角表示手机窄边抬起或放下的角度(-90度到90度)，翻滚角表示手机宽边抬起或放下的角度(-90度到90度)；

手机在地球坐标系上的三轴重力分量也可以通过Android的规定直接调用重力传感器测量得到，若手机处于完全水平放置的状态，X轴、Y轴的重力分量均为零，Z轴的重力分量为g(取9.8)；手机窄边竖立，机头朝上，则Y轴重力分量为g，其余两方向分量为零；同理，可得手机宽边侧立，X轴重力分量为g，其余两轴分量为零(反方向就是-g)；

由此，为了检测出手机平放姿态，本发明提出数据阈值、时间阈值两个标准：

数据阈值：手机平放时，翻滚角、俯仰角应小于β_th，三轴重力分量方面，用户在使用手机时，不可能做到完全水平，如图5-1，5-3，但是多为手机窄边与水平方向产生角度误差，手机宽边较少与水平方向产生角度误差，如图5-2、5-4，所以，重力分量的X轴，即手机窄边的上下摆动角度对重力产生的影响，对于手机平放姿态的判断很敏感；所以，X轴重力分量应小于阈值G_th；当同时满足翻滚角、俯仰角均小于β_th与X轴重力分量应小于阈值G_th时，认为手机处于平放姿态；本实施例中，β_th为10-20度，G_th为3；

时间阈值：手机在口袋中、随手臂摆动时，可能出现短暂的平放姿态，持续时间较短，由于采样误差的影响，此时的地磁方位数据不可信；所以，只有在检测出手机处于平放姿态，且持续了一段时间之后，此时的地磁方位数据才能较好的表征行人运动方向；因此，设定持续时间阈值t_th；本实施例中，t_th为3秒；如图3所示，本实施例中，在采样点210到采样点500的时间段内，检测手机姿态为水平平端放置，而此时的地磁航向信息也维持稳定可用，证明检测有效；

4-1.收集每个时刻，对应的时间戳t、方位角α、俯仰角β、翻滚角χ、重力X轴分量G_x、Y轴分量G_y、Z轴分量G_z；组成向量GRAND＝[t,α,β,χ,G_x,G_y,G_z]^T；

4-2.若时刻t的俯仰角β、翻滚角χ小于阈值β_th，重力X轴分量G_x小于阈值G_th，则记时刻t的标志flag_t＝1；

4-3.以采样频率50Hz次，若检测到连续150个(1000t/Δt)采样点的flag_t＝1，则记为有效采样，计算该段时间的方位角平均值Δα作为预测初始行人方向角；

4-4.结合室内地图和AP布局得到所有可能航向，并缓存；

本实施例中，设定环境如图4所示，在矩形四个拐点设置四个AP；每条边对应正反两个方向，即行人可能存在四种航向，缓存此四种航向；当检测到用户水平仪信息可用时，将预测初始行人方向角与缓存四种航向比较，得出最接近的可能航向，将该可能航向作为初始行人方向；

5.转弯检测

5-1.设定时间窗口ΔT_orien，实时采集窗口ΔT_orien范围内的方位角α，取均值得到orien；

5-2.计算前后两个orien的变化幅度，若超过阈值80度，则判定为发生转弯动作；如图3所示，在采样点900附近，地磁指南针数据有明显台阶式变化，判断为发生转弯动作；

5-3.实时检测行人位置上四个拐点AP的信号强度，信号强度最大的AP所在的拐点，即为此时行人最接近的拐点，在最接近的拐点处向信号强度最大的AP转向；并更新行人方向；

6.基于粒子滤波算法的PDR航位推算，经过上述收集的数据，得出了用户运动过程中完整且与时刻相对应的步频、步长、行人方向信息，下面利用粒子滤波算法与地图融合算法绘制用户运动轨迹；具体过程为：

粒子携带5个参数，p＝(x,y,t,w,L,θ)，其中，x、y分别代表该粒子在地图中的横、纵坐标；t代表时刻；w代表该粒子的权值，权值越大，代表行人在该位置的可能性越大；L代表该时刻行人移动步长；θ代表此刻行人方向；

6-1.在初始时刻，设定所有粒子的权值相同；当行人进入室内空间后，粒子滤波开始迭代，不同位置的粒子的实际位置、权值根据下列算法不断迭代；

6-2.粒子位置变化根据步频、步数、行人方向信息确定：

其中，L_k表示第k步的步长，θ_k表示第k步范围内的航向值、x_k、y_k表示第k步在地图X轴、Y轴方向上的位移；

6-3.由于室内空间限制，当粒子位置前后变化轨迹与墙壁线条有交点，其权值会被立即置零：

6-4.每次迭代后，对粒子进行重采样，低权重的粒子被认为距离真实用户位置较远，重新分配粒子能够将有限的粒子集中在置信度更高的区域中，得到所有粒子新权值，计算

得到迭代后的横坐标，纵坐标同理；权值越大的地方，就是行人越可能出现的地方；

6-5.结合时间信息，得到行人的移动轨迹。

需要说明的是，上述步骤3、步骤4可以并行处理。

以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。

Claims (1)

1.一种水平仪辅助的室内行人轨迹推算方法，包含以下具体步骤：

步骤1.数据预处理

通过加速度传感器、陀螺仪传感器、地磁传感器、重力传感器获取数据，并将所得到的数据做滤波平滑处理；

步骤2.计算步频

2-1.加速度数据预处理，设置滑动时间窗口ΔT，采样频率f；

对每一个采样点计算：

其中，a_x、a_y、a_z分别为X轴、Y轴、Z轴的加速度，均由加速度传感器测量得到；

2-2.搜索滑动时间窗口ΔT内的所有波峰，并依次标记；

2-3.去除伪波峰，得到滑动时间窗口ΔT内所有有效波峰：location＝[l₁,l₂,l₃,...,l_i,...,l_n,]^T，n为有效波峰总数，则滑动时间窗口ΔT内的步数为n；采用三阈值法去除伪波峰，具体为：

2-3-1.设置合加速度阈值为1.2g：当合加速度a＜1.2g，认定为伪波峰；

2-3-2.设置波峰阈值：计算滑动时间窗口ΔT内的三个最大值的平均值A_max，三个最小值的平均值A_min，取A_max和A_min的平均值average为波峰阈值，当小于波峰阈值时，认定为伪波峰；

2-3-3.设置采样点阈值M：M＝0.2f；当相邻两个波峰之间采样点数小于M，认定第二个波峰为伪波峰；否则，两个波峰均为有效波峰；

步骤3.计算步长

3-1.计算

得到第i步的起始点，同时作为第i-1步的终止点；进而得到所有起始点、终止点的集合：

RANGE＝[range_1,range_2,...,range_i,...,range_n,range_L]^T；

其中，l_i表示滑动时间窗口ΔT内第i个有效波峰，range_1表示滑动时间窗口ΔT内第1个采样点、作为第1步的起始点，range_L表示滑动时间窗口ΔT内最后1个采样点，作为第n步的终止点；

3-2.计算每一步的步长：

其中，Q_i为第i步的采样点总数，a_q表示第i步中第q个采样点的合加速度，k、b为预设常数；

步骤4.基于水平仪辅助计算行人航向角

4-1.将每个时刻对应的时间戳t、方位角α、俯仰角β、翻滚角χ、X轴重力分量G_x、Y轴重力分量G_y、Z轴重力分量G_z组成向量：GRAND＝[t,α,β,χ,G_x,G_y,G_z]^T；其中，方位角α由地磁传感器测量得到，俯仰角β、翻滚角χ由陀螺传感器测量得到，X轴重力分量G_x、Y轴重力分量G_y、Z轴重力分量G_z由重力传感器测量得到；

针对时刻t，若满足条件：俯仰角β、翻滚角χ均小于预设阈值β_th，且重力X轴分量G_x小于预设阈值G_th，则认定时刻t手机处于平放姿态；从零时刻开始检测，当连续检测手机处于平放姿态超过时间阈值t_th，则实现有效采样，将该时间段内所有时刻方位角的平均值作为预测初始行人航向角；

4-2.结合室内地图和AP布局得到所有可能航向；将预测初始行人航向角与所有可能航向比较，得出与预测初始行人航向角最接近的可能航向，将该可能航向作为初始行人方向；

步骤5.转弯检测

5-1.设定采集窗口ΔT_orien，计算每个采集窗口ΔT_orien范围内方位角α的平均值orien；

5-2.计算前后两个orien的变化幅度，若超过阈值80度，则判定为发生转弯动作；

5-3.实时检测行人位置上所有AP的信号强度，选定信号强度最大的AP所在的拐点为转弯时刻行人最接近的拐点，在最接近的拐点处向信号强度最大的AP转向，并更新行人航向角；

步骤6.根据步骤2～步骤5得到的步频、步长、行人航向角信息，采用粒子滤波算法得到行人轨迹推算。

Images