CN105444763A - 一种imu室内定位方法 - Google Patents

Abstract

本发明提供了一种IMU室内定位方法，包括：步数检测：通过加速度传感器获取Z轴方向的加速度数据，利用步数检测算法计算行走步数；自适应步长估计：根据沿着行走方向的相关数据，结合自适应步长估计算法得出用户行走一步的距离，所述数据包括加速度和步频；航向估计：通过方向传感器和陀螺仪计算得出用户的行走方向；室内定位：基于几何中心的部分重采样粒子滤波算法对用户进行室内定位。本发明计算得到的步数和步长更加精准，并且在本发明中设计粒子滤波算法完成了IMU室内定位信息的整合和优化，针对粒子滤波的出现的贫化和退化问题，提出了基于几何中心的粒子滤波重采样算法，减少了计算复杂度，提高了定位精度。

Description

一种IMU室内定位方法

技术领域

本发明属于室内定位技术领域，尤其涉及一种IMU室内定位方法及系统。

背景技术

随着物联网和智慧城市建设的兴起和快速发展，人们对室内位置服务例如目标发现、医疗服务和智能家居等的需求与日俱增。

传统的GPS和蜂窝网定位技术在室外能达到较高的定位精度，但是在室内环境下，由于射频信号受到阻隔而无法有效定位。

目前常用的室内定位技术主要是基于各种无线网络，如WLAN、RFID、WIFI等，利用接收信号强度(ReceivedSignalStrength，RSS)来实现室内定位。但是由于多径效应、墙壁衰减、人的走动等的影响，室内环境比较复杂，RSS的变化比较大，因此基于无线网络的室内定位精度受到限制，定位精度很少能达到1米以下，并且这些定位技术需要辅助相应的信号基站。

基于以上原因，考虑使用IMU(InertialMeasurementUnit，IMU)定位，它利用惯性测量单元推算下一时刻用户的位置，估计行走轨迹，不需要辅助信号基站，具有完全自主、不受外界环境影响、数据更新速度快等特点。但是，目前传统IMU室内定位的航位推算模型中还存在数据处理误差大，定位精度低的问题。

发明内容

本发明的目的在于提供一种基于改进航位推算模型和粒子滤波的IMU室内定位系统，属于室内定位领域，旨在改进航位推算模型和粒子滤波的基础上设计出室内定位系统，减少室内定位误差。

本发明是这样实现的，一种IMU室内定位方法，该方法包括以下步骤：

S1、步数检测：通过加速度传感器获取Z轴方向的加速度数据，利用步数检测算法计算行走步数；

S2、自适应步长估计：根据沿着行走方向的相关数据，结合自适应步长估计算法得出用户行走一步的距离，所述数据包括加速度和步频；

S3、航向估计：通过方向传感器和陀螺仪计算得出用户的行走方向；

S4、室内定位：基于几何中心的部分重采样粒子滤波算法对用户进行室内定位。

优选地，在步骤S1中，所述步数检测算法包括以下步骤：

对Z轴方向的加速度值进行滤波去噪、峰谷值检测算法，得到Z轴加速度的波形图；

对此波形进行窗口检测算法，对不满足窗口检测的波形段标记为无效步伐；

通过对波形进行DTW算法检测，通过检测相邻的波峰和波谷的相近度来确定其是否为真实的一步。

优选地，在步骤S2中，所述自适应步长估计算法包括以下步骤：

(1)初始化：在未采集到用户的步伐相关数据前，先使用通用步长模型的α和β，根据步频和步长模型获得步长；

(2)自适应估计：在收集到i-1时刻步伐相关数据后，根据其步长与步频的关系，利用线性回归可以得到i时刻的α和β，再根据i时刻的步频可得出相应的步长。

优选地，在步骤S4中，所述部分重采样粒子滤波算法包括以下步骤：

(1)根据预先设定的粒子初始化状态分别对粒子进行初始化t＝0，根据p(x₀)生成N个粒子p(x₀)分布通常为高斯白噪声分布；

(2)设定t＝t+1，根据状态转移公式采样粒子样本 表示t时刻的第i个粒子；对每个粒子分配相应的权值其中是已知t-1时刻的状态量去估计t时刻状态量的后验概率密度函数，是已知t时刻的状态量去估计t时刻的观测量的后验概率密度函数，表示粒子对应的权重；

对每个粒子的权重进行归一化：

${\mathrm{\ω}}_t^i=\frac{{\mathrm{\ω}}_t^i}{\underset{j=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_t^i};$

(3)基于几何中心部分重采样的原理对A类和C类粒子进行重采样，并重新分配权重,对A类和C类粒子根据其加权后的权重进行重采样得到N_s个新粒子，并对其重新分配权重1/N_s；其中，所述A类粒子为距离小于Tl的粒子，所述C类粒子为距离大于Th的粒子；

(4)根据新生成的粒子进行状态估计

针对现有技术的不足，本发明提供了一种基于改进航位推算模型和粒子滤波的IMU室内定位方法及系统。本发明针对原航位推算模型数据处理误差大，设计了步数检测算法和自适应步长算法；粒子滤波算法完成了IMU定位系统的信息整合和优化过程，针对其自身粒子出现的贫化和退化问题，本发明提出了基于几何中心的部分重采样粒子滤波算法。本发明IMU室内定位过程如图1所示；其中，改进的航位推算模型包括步数检测算法和自适应步长算法。

通常，步数检测算法的一般过程为：

(1)滤波去噪：为了表现信号的周期性，步数检测算法首先使用低通FIR数字滤波器去除高频率噪声。低通频率下限设为3Hz；

(2)峰谷值检测：峰谷值检测用于计算一个单步内的一个峰值和谷值，判断其是否完成一步，如图2。

通过对加速度数据平滑处理后，算法找到波形的峰值和谷值去判断是否完成一步。但是，并不是每一个符合峰谷值特征的波形都是真实的一步，手机的偶尔抖动也会产生错误的一步。本发明通过增加时间窗口检测和动态时间规整(DTW)距离算法来减少错误的估计。

时间窗口检测：时间窗口用于排除无效抖动。假设用户最快的行走速度为每秒三步，最慢的步行速度为每两秒一步。因此，两个有效的步数之间时间间隔在时间窗口[0.3s～2.0s]之内，如果时间间隔小于0.3s或者大于2s的步伐是无效步伐。

DTW算法：DTW已经被广泛作为检测两个离散波形的相似度的有效方法。低的DTW距离代表着高相似度。考虑到当一个人正常行走时是左右脚交替前行，所以可以看成是相似的波形在重复。基于此，设计了DTW算法来确定通过峰谷值检测的一步是真正的一步。具体操作过程为：假设通过峰谷值检测的步行{S1,S2,S3,...,Sn}。那么，对于Si，算法可以计算Si与Si-2之间的DTW距离。如果它低于给定的门限值，那么Si看做是真实的一步，否则视为无效的一步。

自适应步长估计算法：基于步长与步频、加速度的关系，本发明选择步长模型：

L_g＝α×f+β×ν+γ；

其中，L_g是步长，f是步频(即每秒行走的步数)，α、β和γ是可变参数，在每一步完成之后可根据相关数据对其修正：

f_i＝1/(t_i-t_i-1)；

$v_i=\underset{t=t_{i-1}}{\overset{t_i}{\Σ}}\[{\left(a_t-\stackrel{\‾}{a_i}\right)}^2/N\];$

其中，t_i是检测到行走完第i步的时间。a_t是时刻t的加速度值，是一步之内的加速度平均值，N是一步内的信号输出个数。

本发明的自适应步长估计算法过程具体为：

(1)初始化：在未采集到用户的步伐相关数据前，先使用通用步长模型的α和β，根据步频和步长模型获得步长；

(2)自适应估计：在收集到i-1时刻步伐相关数据后，根据其步长与步频的关系，利用线性回归可以得到i时刻的α和β，再根据i时刻的步频可得出相应的步长。

此外，相对于本发明改进粒子滤波算法部分而言，现有粒子滤波算法几乎都是对所有粒子进行的完全重采样，这种“过度”重采样极易引发粒子贫化。若将完全重采样用部分重采样替代，则参与重采样的粒子有助于缓解退化问题，未参与重采样的粒子有利于保持粒子的多样性。基于这种思想，本发明提出了基于几何中心的部分重采样粒子滤波算法，先通过几何方法寻找粒子集的中心，按照粒子与其中心的欧式距离的大小确定需要重采样的粒子，解决了粒子滤波的贫化和退化的问题，最后通过为了保证滤波算法的有效性通过设置滤波阀值，高于此阀值说明滤波算法失去其有效性，重新初始化重新滤波。

本发明的粒子滤波系统模型为：

X_t＝f_t(X_t-1,υ_t-1)；

Z_t＝h_t(X_t,μ_t)；

其中，X_t和Z_t分别是t时刻的状态量和观测量，υ_t-1和μ_t分别为系统过程噪音和观测噪音，其概率密度函数相互独立其已知。滤波的目的就是根据给定的状态初值X₀和序贯获取的量测序列Z_1:t＝{Z₁,Z₂,...,Z_t}递推估计X_t。粒子滤波通过状态方程来预测状态的先验概率模型，再用观测值对其进行修正，得到状态的后验概率模型，从而得到系统状态值的最优估计。

粒子滤波的思路就是从选取的重要性密度函数q(X_t|X_0:t-1,Z_1:t)中抽取N个加权粒子经过权值归一化及再抽样来估计t时刻的后验概率分布：

$p\left(X_t\right|Z_{1:t})=\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_t^i\mathrm{\δ}(X_t-X_t^i);$

其中，δ为Dirac函数。

基于几何中心的部分重采样的主要思想是：通过几何方法找到粒子集的中心点，使中心点与各个粒子的欧式距离之和为最小。设置距离阀值Th、Tl(0<Tl<Th)，关于如何设置在下文中说明。根据粒子与中心点的距离，粒子可以分为3类：

A类:距离小于Tl的粒子；

B类:距离大于Tl小于Th的粒子；

C类:距离大于Th的粒子。

B类的粒子是比较稳健的，不需要重采样，因此重采样只需要对A类和C类的粒子进行，如图3。

距离阀值的大小选择对于重采样算法的计算时间、粒子的多样性以及粒子滤波的性能有着重要影响，设Th＝0.8L,Tl＝0.2L。

假设A类和C类粒子的总数目为N_s，在重采样之前A类和C类粒子的权重为其中是粒子的权重。易知，A类的权重大，C类的权重小，为了减少每次重采样中C类粒子的损失，我们对A类和C类粒子群权重取平均得再将与进行组合，得到新的粒子群公式如下：

$\stackrel{\&OverBar;}{{\mathrm{\&omega;}}_t}=\frac{\underset{i=1}{\overset{N_s}{\&Sigma;}}{\mathrm{\&omega;}}_t^i}{N_s};$

$w_t^i=\frac{K-1}{K}{\mathrm{\&omega;}}_t^i+\frac{\stackrel{\&OverBar;}{{\mathrm{\&omega;}}_t}}{K};$

其中，K(1≤K≤∞)是比例系数，K值的选择将会影响到滤波效果，当增大K值时，小权重粒子将更多的被抛弃；当减少K值时，将保留更多粒子来增大信息的采集量。如何选择K值需要根据具体应用合理选择，例如，当在地形变化剧烈时，增大K值可减少错误信息的干扰；当地形变化平缓时，可减少K值保留更多粒子增大新信息的采集量。在本发明中，令K＝3。对A类和C类粒子根据其权重进行重采样得到N_s个新粒子，并对其重新分配权重1/N_s。

基于几何中心的部分重采样粒子滤波算法具体实现步骤如下：

(1)根据预先设定的粒子初始化状态分别对粒子进行初始化t＝0，根据p(x₀)生成N个粒子p(x₀)分布通常为高斯白噪声分布；

(2)设定t＝t+1，根据状态转移公式：

$q\left(X_t^i\right|X_{t-1}^i,Z_{t-1});$

采样粒子样本对每个粒子分配相应的权值：

${\mathrm{\&omega;}}_t^i\&Proportional;\frac{{\mathrm{\&omega;}}_{t-1}^{i}p\left(Z_t|X_t^i\right)p\left(X_t^i|X_{t-1}^i\right)}{q\left(X_t^i|X_{t-1}^i,Z_t\right)};$

对每个粒子的权重进行归一化：

${\mathrm{\&omega;}}_t^i=\frac{{\mathrm{\&omega;}}_t^i}{\underset{j=1}{\overset{N}{\&Sigma;}}{\mathrm{\&omega;}}_t^i};$

(3)基于几何中心部分重采样的原理对A类和C类粒子进行重采样，并重新分配权重,对A类和C类粒子根据其加权后的权重进行重采样得到N_s个新粒子，并对其重新分配权重1/N_s；

(4)根据新生成的粒子进行状态估计

相比于现有技术的缺点和不足，本发明具有以下有益效果：针对传统IMU室内定位的航位推算模型中还存在数据处理误差大，定位精度低的问题，本发明设计了步数检测算法和自适应步长算法来改进航位推算模型，使得到的步数和步长更加精准，并且在本发明中设计粒子滤波算法完成了IMU室内定位信息的整合和优化，针对粒子滤波的出现的贫化和退化问题，提出了基于几何中心的粒子滤波重采样算法，减少了计算复杂度，提高了定位精度。

附图说明

图1为本发明IMU室内定位过程构架结构图；

图2为步数检测算法中峰谷值检测结果示意图；

图3为粒子分类结果示意图；

图4是本发明实施例中单次试验队系统状态的估计图；

图5是本发明实施例中100次试验不同重采样算法RMSE的比较图；

图6是本发明实施例中室内定位实验环境示意图；

图7是本发明实施例中定位算法实验效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

IMU室内定位算法包含以下模块：步数检测，自适应步长估计，航向估计和室内定位，如图1。该定位算法通过IMU提供的传感器数据实时获得用户在室内的位置。本文是通过在apple5c智能终端上利用加速度传感器、陀螺仪、方向传感器等硬件设备进行系统开发的。定位信息数据如加速度、陀螺仪和方向传感数据等的数据采样率是100Hz。

IMU测量单元实现行走轨迹跟踪：通过加速度传感器获取Z轴方向的加速度数据，通过步数检测算法对加速度数据的分析处理，得出是否完成真实的一步，然后利用沿着行走方向的加速度和步频等数据并结合自适应步长估计算法得出行走一步的距离，行走方向由方向传感器提供。行走位移可以通过步数乘以步长，再加上行走方向就能推算下一时刻的位置。在本发明中利用步数检测算法计算行走步数，利用自适应步长估计算法计算每一步的行走步长，行走方向由方向传感器计算得出。

基于几何中心的部分重采样粒子滤波算法具体实现步骤如下：

(1)根据预先设定的粒子初始化状态分别对粒子进行初始化t＝0，根据p(x₀)生成N个粒子p(x₀)分布通常为高斯白噪声分布；

(2)设定t＝t+1，根据状态转移公式：

$q\left(X_t^i\right|X_{t-1}^i,Z_{t-1});$

采样粒子样本对每个粒子分配相应的权值：

${\mathrm{\&omega;}}_t^i\&Proportional;\frac{{\mathrm{\&omega;}}_{t-1}^{i}p\left(Z_t\right|X_t^i)p\left(X_t^i\right|X_{t-1}^i)}{q\left(X_t^i\right|X_{t-1}^i,Z_t)};$

对每个粒子的权重进行归一化：

${\mathrm{\&omega;}}_t^i=\frac{{\mathrm{\&omega;}}_t^i}{\underset{j=1}{\overset{N}{\&Sigma;}}{\mathrm{\&omega;}}_t^i};$

(3)基于几何中心部分重采样的原理对A类和C类粒子进行重采样，并重新分配权重,对A类和C类粒子根据其加权后的权重进行重采样得到N_s个新粒子，并对其重新分配权重1/N_s；

(4)根据新生成的粒子进行状态估计

为了验证基于几何中心部分重采样的粒子滤波算法的有效性，将多项式重采样算法、残差重采样算法、分层重采样算法与本发明提出的算法进行仿真对比，对以上算法做M＝100仿真试验，为了更好地评价算法的性能，定义M次蒙特卡洛试验均方根误差为：

$RMSE={\&lsqb;\frac{1}{M}\underset{k=1}{\overset{M}{\&Sigma;}}{\left(X_t^k-\stackrel{\&OverBar;}{X_t^k}\right)}^2\&rsqb;}^{1/2};$

其中，和分别表示目标状态量的真实值和估计值。图4为某次独立试验上文提到的4种算法对系统状态的跟踪估计图。图5为4种算法经100次试验后，获得的均方根误差曲线图。

本发明实施例以图书馆走廊为实验环境，如图6，虚线为行走路线。该区域大小为9m*9m的矩形区域。本发明所使用的实验设备是苹果的5S智能手机，其中包括IMU套件。在实验过程中由不同年龄段和生理特性的实验员将绕走廊步行一遍，实验员每走一步，记录相关数据，每个实验员总过需要做5次实验。IMU的采样率是100Hz。图7是其中一次实验推算路线与实际路线的比较。从图7看出，使用本发明算法所得到的坐标值与实际坐标值非常接近，定位误差控制在1m以内，较好的解决了室内定位环境中的各种干扰因素。而使用传统室内定位方法所得到的坐标与实际坐标相距较远，误差为3～5m，定位误差比较大。对比可知本发明提出的IMU定位算法改善了定位效果，提高了定位精度，并且此定位成本较低，易于推广和被大众所接受。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种IMU室内定位方法，其特征在于，该方法包括以下步骤：

S1、步数检测：通过加速度传感器获取Z轴方向的加速度数据，利用步数检测算法计算行走步数；

S2、自适应步长估计：根据沿着行走方向的相关数据，结合自适应步长估计算法得出用户行走一步的距离，所述数据包括加速度和步频；

S3、航向估计：通过方向传感器和陀螺仪计算得出用户的行走方向；

S4、室内定位：基于几何中心的部分重采样粒子滤波算法对用户进行室内定位。

2.如权利要求1所述的IMU室内定位方法，其特征在于，在步骤S1中，所述步数检测算法包括以下步骤：

对Z轴方向的加速度值进行滤波去噪、峰谷值检测算法，得到Z轴加速度的波形图；

对此波形进行窗口检测算法，对不满足窗口检测的波形段标记为无效步伐；

通过对波形进行DTW算法检测，通过检测相邻的波峰和波谷的相近度来确定其是否为真实的一步。

3.如权利要求1所述的IMU室内定位方法，其特征在于，在步骤S2中，所述自适应步长估计算法包括以下步骤：

(1)初始化：在未采集到用户的步伐相关数据前，先使用通用步长模型的α和β，根据步频和步长模型获得步长；

(2)自适应估计：在收集到i-1时刻步伐相关数据后，根据其步长与步频的关系，利用线性回归可以得到i时刻的α和β，再根据i时刻的步频可得出相应的步长。

4.如权利要求1所述的IMU室内定位方法，其特征在于，在步骤S4中，所述部分重采样粒子滤波算法包括以下步骤：

(1)根据预先设定的粒子初始化状态分别对粒子进行初始化t＝0，根据p(x₀)生成N个粒子p(x₀)分布通常为高斯白噪声分布；

(2)设定t＝t+1，根据状态转移公式采样粒子样本 表示t时刻的第i个粒子；对每个粒子分配相应的权值其中是已知t-1时刻的状态量去估计t时刻状态量的后验概率密度函数，是已知t时刻的状态量去估计t时刻的观测量的后验概率密度函数，表示粒子对应的权重；

对每个粒子的权重进行归一化：

${\mathrm{\&omega;}}_t^i=\frac{{\mathrm{\&omega;}}_t^i}{\underset{j=1}{\overset{N}{\&Sigma;}}{\mathrm{\&omega;}}_t^i};$

(3)基于几何中心部分重采样的原理对A类和C类粒子进行重采样，并重新分配权重,对A类和C类粒子根据其加权后的权重进行重采样得到N_s个新粒子，并对其重新分配权重1/N_s；其中，所述A类粒子为距离小于Tl的粒子，所述C类粒子为距离大于Th的粒子；

(4)根据新生成的粒子进行状态估计