CN104061934A - 基于惯性传感器的行人室内位置跟踪方法 - Google Patents

Abstract

基于惯性传感器的行人室内位置跟踪方法，本发明涉及一种行人室内位置跟踪方法，具体涉及基于惯性传感器的PDR方法和PF融合地图信息算法。本发明是要解决单纯使用PDR对行人位置跟踪时，因为惯性传感器自身所具有的长时漂移性而造成位置估计误差大甚至错误估计等情况。一、根据加速度传感器进行检步及步长估计；二、根据陀螺仪测量数据中的三轴角速度变化进行航向角估计，对航向角进行校正，然后根据校正后的航向角与第一步步长进行航迹推算，最后根据步长、航向角通过PDR方法估计位置：三、通过粒子滤波将地图信息和PDR估计结果融合，即完成了基于惯性传感器的行人室内位置跟踪方法。本发明应用于室内定位技术领域。

Description

基于惯性传感器的行人室内位置跟踪方法

技术领域

本发明涉及一种行人室内位置跟踪方法，具体涉及基于惯性传感器的PDR方法和PF融合地图信息算法。

背景技术

近几年，微电子机械系统(Micro-ElectroMechanical Systems，MEMS)的发展，使得惯性传感器在智能移动终端的应用得到普及。基于惯性传感器的行人室内导航系统，以其无需铺设外部设施的低成本优势，逐渐成为研究热门。该系统的基本原理是采用行人航迹推算法(PDR，Pedestrian Dead-Reckoning)，根据惯性传感器(如加速度计、陀螺仪)的测量数据，主要涉及到检步、估计步长、估计航向角等技术，因此这种系统有时也称为SHSs(Step-and-Heading Systems)。早期采用的是可穿戴设备，将传感器安装于鞋子、头盔、置于口袋、腰部等，行走中脚部能更多的反映运动特征，所以基于脚部传感器能更好的检步，然而需要额外购置专门设备且不便携带，不适用于普通的行人室内导航系统。在行走过程中，加速度传感器会输出一定的行走特征，通过对测量数据进行峰值检测、过零检测、自相关匹配、频谱分析等，其中一种或几种相结合来识别每一步，后两种因为耗时长而导致实时性不强。对于步长估计，最粗略的是直接设定步长为一个常数，因为行人在常速行走下有一个平均步长，但是现实中，步长受一个人的身高、体型、换步频率等因素影响，不可以一概为均值。研究者们普遍接受一个最大最小加速度差值四次根方的公式计算，但也有人提出其它如基于步频和步长有线性关系，穿戴脚部的传感器采用零速更新(ZUPT，Zero-velocity UPdaTe)，基于行人身高等步长估计方法。而航向估计中，最简单的估计方法是将传感器固定于行人，使传感器坐标系与地球坐标系(指南针坐标系，N-E坐标系)三轴重合，这样传感器测得的方位角即是行人的航向角，则经常通过借助重力在加速度传感器三轴上的分量以及磁强计(电子罗盘)来计算求得，而一般情况下，传感器的放置是无固定姿态的(即姿态任意性)，这是需要借助其它方法来估计航向，比如主成份分析法(PCA，Primary Components Analyze)估计航向角。

粒子滤波(PF，Particle Filter)是在Kalman滤波的状态模型的高斯、线性假设条件不满足的情况下兴起的滤波跟踪算法，它通过使用蒙特卡洛法抽样最大后验估计，从而逼近贝叶斯最优估计。目前各种室内定位如WLAN室内定位、超宽带室内定位、基于视觉室内定位、惯性传感器室内定位等都采用粒子滤波来改善定位精度以及解决多目标跟踪问题，并且使用粒子滤波来融合不同定位系统的数据，从而相互辅助提高定位结果。经常采用粒子滤波来将地图信息加入到位置估计中，根据地图信息来调整粒子滤波中的参数，从而不仅提高精度，也防止了跟踪目标出现穿墙等特殊情况的发生。

发明内容

本发明是要解决单纯使用PDR对行人位置跟踪时，因为惯性传感器自身所具有的长时漂移性而造成位置估计误差大甚至错误估计等情况，而提供了基于惯性传感器的行人室内位置跟踪方法。

基于惯性传感器的行人室内位置跟踪方法按以下步骤实现：

一、根据加速度传感器进行检步及步长估计；

二、根据陀螺仪测量数据中的三轴角速度变化进行航向角估计，对航向角进行校正，然后根据校正后的航向角与第一步步长进行航迹推算，最后根据步长、航向角通过PDR方法估计位置：

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

其中，表示第k步的PDR估计位置h_k代表第k步估计的航向角，sLen_k代表第k步的步长；

三、通过粒子滤波将地图信息和PDR估计结果融合，即完成了基于惯性传感器的行人室内位置跟踪方法。

发明效果：

基于惯性传感器的行人位置跟踪方法，借助智能手机中的惯性传感器模块，行人航迹推算法初步估计位置，然后采用粒子滤波算法融合地图信息，为用户提供实时位置信息。在行人航迹推算法实现中，提出一种基于加速度值和时间的PZVT检步算法，通过步长估计，针对陀螺仪的角度积分有长时漂移特点，利用三轴角度变化互相矫正，在实验环境中进行了测试，位置估计误差相对减小。使 用粒子滤波结合地图信息对估计位置进行滤波，在一定程度上提高了位置估算的准确度。

附图说明

图1是基于惯性传感器的PDR原理框图；

图2是手机传感器坐标系、电子罗盘坐标系示图；

图3是基于惯性传感器行人位置跟踪实验环境示意图；

图4是Z轴加速度值以及检步结果；

图5是未校正及校正后航向角对比图；

图6是使用PF和只使用DR的位置跟踪对比图。

具体实施方式

具体实施方式一：本实施方式的基于惯性传感器的行人室内位置跟踪方法按以下步骤实现：

一、根据加速度传感器进行检步及步长估计；

二、根据陀螺仪测量数据中的三轴角速度变化进行航向角估计，对航向角进行校正，然后根据校正后的航向角与第一步步长进行航迹推算，最后根据步长、航向角通过PDR方法估计位置：

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

其中，表示第k步的PDR估计位置h_k代表第k步估计的航向角，sLen_k代表第k步的步长；

三、通过粒子滤波将地图信息和PDR估计结果融合，即完成了基于惯性传感器的行人室内位置跟踪方法。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一具体过程如下：

采用峰值-零值-谷值-时间间隔的方法进行检步，即每行走一步包含1个最大加速度、2个零值、1个最小加速度，并且满足时间间隔合理，按照成人常速行走的速度为2～4步每秒，设置时间间隔下限为250毫秒，S₀表示起步，S_i,i＝1…9表示第i步，步长估计通过下式来获得

$\mathrm{sLen}=1.07\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(2\right)$

其中sLen是估计的步长，acc_i、acc_Ave分别代表每一步中的加速度值、平均加速度值，N表示每步中采集到的数据个数。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中航向角估计方法为：

首先对围绕x、y、z的三轴角速度进行时间积分，分别得到俯仰角、滚动角、方位角，记作Pitch,Roll,Azimuth；

采用(3)对航向角进行第一步校正：

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth

(3)其中，俯仰角Pitch表示围绕着x轴的旋转量，滚动角Roll表示围绕着y轴的旋转量，方位角Azimuth表示围绕着z三轴的旋转量，其中c₁,c₂,c₃为对应的加权系数；

对第一步校正后的航向角进行第二步校正，即进行逐次平滑，具体通过下式计算：

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

mean(·)表示取平均值。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三的通过粒子滤波将地图信息和PDR估计结果融合方法如下：

粒子滤波是一种采用蒙特卡洛方法来逼近后验概率的贝叶斯滤波方法，不要求运动系统是线性或者高斯的；

一、采用的粒子滤波的状态方程为：

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·\begin{array}{}\end{array}\begin{array}{}\end{array}(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\begin{array}{}\end{array}\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

其中，x_k,y_k表示第k步时的位置坐标，x_k-1,y_k-1表示第k-1步的位置坐标，h_k是估计出的航向角，为步长估计误差，为航向角估计误差，l_k表示？，sin(·),cos(·)分别表示求对应角度的正弦值、余弦值；

二、对第k步的所有粒子进行加权值w_i计算：

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

其中表示第k步的PDR估计位置 表示第k步的第i个粒子的位置，σ表示PDR位置估计的标准差，当第k步中生成的某个粒子不在有效位置范围内时，则w_i取0值，否则按照condition_1计算权值；

三、通过粒子滤波将地图信息和PDR估计结果融合：

在得到第k步的所有粒子加权值w_i后，根据w_i计算使用了粒子滤波的第k步的估计位置：

${\mathrm{pos}}_k^{\mathrm{PF}}=\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{\tilde{w}}_i\·{\mathrm{pos}}_k^i---\left(7\right)$

其中，Np代表第k步中的粒子个数，代表第k步中对第i个粒子的估计位置根据式(5)计算得到，为归一化加权值，通过下式计算：

${\tilde{w}}_i=\frac{w_i}{\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{w}_i}---\left(8\right)$

不断的对PDR估计出的位置进行粒子滤波，得到新的估计位置。

动态系统的状态估计一般通过两个方程来描述，分别为状态方程：s_k＝F(s_k-1,n_k-1) (9)

观测方程：m_k＝G(s_k,v_k) (10)

其中F(·),G(·)是系统状态s_k-1,s_k的函数，s_k为k时刻状态变量，m_k为s_k的观测值，系统噪声n_k和观测噪声v_k为概率密度已知的相互独立，且与系统状态独立的随机变量；

其它步骤及参数与具体实施方式一至三之一相同。

下面结合对具体实施方式进行详细说明，基于惯性传感器的行人位置跟踪方法通过下述步骤实现：

一、根据加速度传感器进行检步及步长估计：

结合图2、图3对具体实施方式一做出详细说明。图2是哈尔滨工业大学科学园2A栋12层的平面图示意，基于惯性传感器行人位置跟踪在该楼层的走廊中实验(图中深色填充区域)，走廊宽3米左右，长大约90米。图3中显示的是在实验测试中，测试者手持手机的姿态，以及手机内传感器坐标系(Sensor坐标系)、导航坐标系(E-N坐标系)。

人在行走过程中，身体的不同部位会不同的加速度变化，但是都有规律性，所以可以从中提取信息，来识别是否行走，或者静止等动作。采用峰值-零值-谷值-时间间隔(PZVT)的方法来检步，即每行走一步包含1个最大加速度、2个零值、1个最小加速度(如图4所示)，并且满足时间间隔合理，按照成人常速行走的速度为2-4步每秒，设置时间间隔下限为250毫秒。波形的顶点方框表示检测到的有效步，S₀表示起步，S_i，i＝1…9表示第i步。

实测PZVT检步算法可知，本实施方式所采用的检步算法精度基本在100％，即误检、漏检、多检的情况很少发生。

步长不仅与人的身高有关，还与行走中的步频等因素有关，同一个人在行走中的每一步都有差异，所以步长估计是一个非常具有挑战性的问题。本专利采用公式

$\mathrm{sLen}=k\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(1\right)$

其中sLen是估计的步长，acc_i、acc_Ave分别代表每一步中的加速度值、平 均加速度值，k为每一种方法对应的系数，本发明取值1.07，N表示一步中采集到的数据个数。

二、根据陀螺仪测量数据进行航向角估计。考虑到实际行走中手机并不是一直都处于前面提到的基本姿态，比如走动过程中，会随着行人脚步交替而有轻微的左右摆动，以及由于重心上下起伏导致的角度扰动等，所以采用(3)对其进行第一步校正：

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth (2)

其中Pitch,Roll,Azimuth分别对应图2所示，其中围绕着x、y、z三轴的旋转量分别为俯仰角、滚动角、方位角，记作Pitch、Roll、Azimuth，c₁,c₂,c₃为对应的加权系数，需要在实际场景中测试得到，本实验中赋值为c₁＝1,c₂＝0.3,c₃＝0.1。在此基础上对得到的角度进行第二步校正，即进行逐次平滑，主要是为了防止有毛刺值出现从而影响某一步中的航向角精度，但是平滑仅限于直线行走或者比较稳定的过程，如图5中的曲线的水平段，具体通过下式计算

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

其中，mean(·)表示取平均值。

三、通过粒子滤波将地图信息和PDR估计结果融合。相对于卡尔曼滤波(KF，Kalman Filtering)简化运动模型，从而求得最优解，粒子滤波不对模型进行简化，而是直接计算复杂运动模型的次优解，从而逐渐逼近最优解。粒子滤波是一种采用蒙特卡洛方法来逼近后验概率的贝叶斯滤波方法，不需要设定运动模型为线性模型等初始条件。

动态系统的状态估计一般通过两个方程来描述，分别为

状态方程：s_k＝F(s_k-1,n_k-1) (9)

观测方程：m_k＝G(s_k,v_k) (10)

其中F(·),G(·)是系统状态s_k-1,s_k的函数，s_k为k时刻状态变量，m_k为s_k 的观测值，系统噪声n_k和观测噪声v_k为概率密度已知的相互独立，且与系统状态独立的随机变量。

本实施方式中采用的粒子滤波的状态方程为

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·\begin{array}{}\end{array}\begin{array}{}\end{array}(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\begin{array}{}\end{array}\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

其中，x_k,y_k表示第k步时的位置坐标，h_k是根据具体实施方式二估计出的航向角，为步长估计误差，为航向角估计误差，sin(·),cos(·)分别表示求对应角度的正弦值、余弦值。

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

其中表示第k步的PDR估计位置表示第k步的第i个粒子的位置，σ表示PDR位置估计的标准差。当第k步中生成的某个粒子不在有效位置范围内时(比如穿墙等情况)，则w_i取0值，否则按照condition_1计算权值。

具体实现过程是：首先给定一个初始位置(0，0)，然后检测是否是一个有效步，如果是，则估计该步步长、航向角，根据步长、航向角通过PDR估计粗略位置，结合随机产生的100个粒子(在每一步中都需要重新产生)，计算粒子的位置，然后根据(8)的条件计算每个粒子的权值，对所有粒子位置加权求和得到PF滤波后的位置，然后重复以上过程进行下一位置估计，这样重复计算每一步位置，实现了对行人的位置跟踪。

四、最后在图3中所示的实验环境中通过NEXUS5智能手机采集数据(按照图2中的手机放置姿态，测试者手持手机在实验走廊中行走一圈记录数据)，采集到的数据有加速度值、角速度值以及采样时间，然后在MATLAB2012b中进行仿真计算，根据所提出的算法最终得到如图6所示的曲线。可以看到，经过PZVT检步、航向角校正以及PF融合地图信息的行人位置估计方法相比较于仅仅使用PDR估计位置，提高了估计精确度。

首先给定一个初始位置(0，0)，然后检测是否是一个有效步，如果是，则估计该步步长、航向角，根据步长、航向角通过PDR估计粗略位置，结合随机产生的100个粒子(在每一步中都需要重新产生)，计算粒子的位置，然后根据的条件计算每个粒子的权值，对所有粒子位置加权求和得到PF滤波后的位置，然后重复以上过程进行下一位置估计，这样重复计算每一步位置，实现了对行人的位置跟踪。

Claims (4)

1.基于惯性传感器的行人室内位置跟踪方法，其特征在于它按以下步骤实现：

一、根据加速度传感器进行检步及步长估计；

二、根据陀螺仪测量数据中的三轴角速度变化进行航向角估计，对航向角进行校正，然后根据校正后的航向角与第一步步长进行航迹推算，最后根据步长、航向角通过PDR方法估计位置：

${\mathrm{pos}}_k^{\mathrm{PDR}}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\end{array}\right]+{\mathrm{sLen}}_k\·\left[\begin{array}{c}\cos \left(h_k\right)\\ \sin \left(h_k\right)\end{array}\right]---\left(1\right)$

其中，表示第k步的PDR估计位置h_k代表第k步估计的航向角，sLen_k代表第k步的步长；

三、通过粒子滤波将地图信息和PDR估计结果融合，即完成了基于惯性传感器的行人室内位置跟踪方法。

2.根据权利要求1中所述的基于惯性传感器的行人位置跟踪方法，其特征在于所述步骤一具体过程如下：

采用峰值-零值-谷值-时间间隔的方法进行检步，即每行走一步包含1个最大加速度、2个零值、1个最小加速度，并且满足时间间隔合理，按照成人常速行走的速度为2～4步每秒，设置时间间隔下限为250毫秒，S₀表示起步，S_i,i＝1…9表示第i步，步长估计通过下式来获得

$\mathrm{sLen}=1.07\·\sqrt[3]{{\mathrm{acc}}_{\mathrm{Ave}}},{\mathrm{acc}}_{\mathrm{Ave}}=\frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\left|{\mathrm{acc}}_i\right|}{N}---\left(2\right)$

其中sLen是估计的步长，acc_i、acc_Ave分别代表每一步中的加速度值、平均加速度值，N表示每步中采集到的数据个数。

3.根据权利要求2中所述的基于惯性传感器的行人位置跟踪方法，其特征在于所述步骤二中航向角估计方法为：

首先对围绕x、y、z的三轴角速度进行时间积分，分别得到俯仰角、滚动角、方位角，记作Pitch,Roll,Azimuth；

采用(3)对航向角进行第一步校正：

heading＝c₁·Pitch+c₂·Roll+c₃·Azimuth (3)

其中，俯仰角Pitch表示围绕着x轴的旋转量，滚动角Roll表示围绕着y轴的旋转量，方位角Azimuth表示围绕着z三轴的旋转量，其中c₁,c₂,c₃为对应的加权系数；

对第一步校正后的航向角进行第二步校正，即进行逐次平滑，具体通过下式计算：

${\mathrm{heading}}_i=\mathrm{mean}\left(\underset{k=1}{\overset{i}{\mathrm{\Σ}}}{\mathrm{heading}}_k\right)---\left(4\right)$

mean(·)表示取平均值。

4.根据权利要求3中所述的基于惯性传感器的行人位置跟踪方法，其特征在于所述步骤三的通过粒子滤波将地图信息和PDR估计结果融合方法如下：

粒子滤波是一种采用蒙特卡洛方法来逼近后验概率的贝叶斯滤波方法，不要求运动系统是线性或者高斯的；

一、采用的粒子滤波的状态方程为：

$s_k=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& (l_k+n_k^l)\·\cos (\·)\\ 0& 1& (l_k+n_k^l)\·\sin (\·)\end{array}\right]\·(\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ h_k\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ n_k^h\end{array}\right])---\left(5\right)$

其中，x_k,y_k表示第k步时的位置坐标，x_k-1,y_k-1表示第k-1步的位置坐标，h_k是估计出的航向角，为步长估计误差，为航向角估计误差，l_k表示？，sin(·),cos(·)分别表示求对应角度的正弦值、余弦值；

二、对第k步的所有粒子进行加权值w_i计算：

$w_i=\left\{\begin{array}{c}\exp (-\frac{{\left|\right|{\mathrm{pos}}_k^{\mathrm{PDR}}-{\mathrm{pos}}_k^i\left|\right|}^2}{2{\mathrm{\σ}}^2}),\mathrm{condition}\_1\\ 0,\mathrm{condition}\_2\end{array}\right.---\left(6\right)$

其中表示第k步的PDR估计位置表示第k步的第i个粒子的位置，σ表示PDR位置估计的标准差，当第k步中生成的某个粒子不在有效位置范围内时，则w_i取0值，否则按照condition_1计算权值；

三、通过粒子滤波将地图信息和PDR估计结果融合：

在得到第k步的所有粒子加权值w_i后，根据w_i计算使用了粒子滤波的第k步的估计位置：

${\mathrm{pos}}_k^{\mathrm{PF}}=\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{\tilde{w}}_i\·{\mathrm{pos}}_k^i---\left(7\right)$

其中，Np代表第k步中的粒子个数，代表第k步中对第i个粒子的估计位置根据式(5)计算得到，为归一化加权值，通过下式计算：

${\tilde{w}}_i=\frac{w_i}{\underset{i=1}{\overset{\mathrm{Np}}{\mathrm{\Σ}}}{w}_i}---\left(8\right)$

不断的对PDR估计出的位置进行粒子滤波，得到新的估计位置。