CN106225801A - 一种基于惯性传感的人员步长估算的方法 - Google Patents

Abstract

本发明公开了一种基于惯性传感的人员步长估算的方法，步骤(1)、使用微电子机械系统采集室内人员运动原始数据，步骤(2)、利用四元数生成坐标转换矩阵；步骤(3)、将人员连续的运动划分为每一个单步，计算单步步长，步骤(4)、累加总位移得到整个运动步长。与现有技术相比，本发明能够：1)避免加速度二次积分计算步长的误差累积，从而降低了步长计算误差的累积；2)对人员单步步长计算和行进总位移计算，因此具有很好的可靠性和稳定性；3)提高了零速率修正准确率，从而提高了步长计算的精度。

Description

一种基于惯性传感的人员步长估算的方法

技术领域

本发明涉及室内定位技术领域，特别是一种室内定位步长计算的方法。

背景技术

定位技术，简单的说就是获取物体的位置信息。伴随着社会的发展和科技的进步，人们在生活和工作中对定位和导航的需求不断增长。全球卫星定位系统很好地完成了室外空旷环境下的定位；而对于室内和卫星信号衰弱的环境，运用惯性导航技术，可以实现人员的定位和跟踪。

近年来，随着微电子机械系统(Micro-Electrical-Mechanical Systems，MEMS)的性价比不断提高，行人航位推算法(PDR)被认为是目前最有前景的人员定位技术方案之一。PDR定位主要包括步长计算、方向估计等技术，利用MEMS采集人员行进的加速度、四元数等运动数据，通过适当的算法处理，得到人员行进的步长和方向，从而推算出人员的位置。因此，步长估计的精度对PDR系统的定位精度有至关重要的作用。

而PDR系统中MEMS本身的误差(如：测量噪声、陀螺仪偏差等)以及步长计算的累积误差是影响步长计算精度的关键因素之一。随着国内外学者研究的深入，已有学者在2005年提出利用零速率修正技术来降低累积误差。该技术首先进行步态检测，将人员的连续运动划分为每一个运动单步，将每一个单步在起始时刻的速度设置成零，以达到每一个单步都减少误差的累积，很好地提高步长计算的精度。然而，由于人员行走的不确定性，假设MEMS置于人员右脚脚跟，当人员左脚行走时，很难保证右脚完全静止，造成了MEMS采集的数据不准确。也有很多研究者提出利用现有的基础设施(如：WiFi、RFID、监视摄像头等)来修正积分误差，但是该方案依赖于定位环境的基础设施，很难在特殊环境(如：火灾现场、矿难现场等)使用。

发明内容

基于现有技术存在的缺陷，本发明提出了一种基于惯性传感的人员步长估算的方法，在人员航位推算过程中采用双零速率修正技术来进行步长估计和速率纠正的算法。

本发明提出了一种基于惯性传感的人员步长估算的方法，该方法包括以下步骤：

步骤1、使用微电子机械系统采集室内人员运动的基于MEMS载体坐标系的三轴加速度[a_N a_E a_D]以及四元数作为原始数据，并进行原始数据初期滤波和校正；

步骤2、利用四元数生成坐标转换矩阵，坐标转换矩阵计算公式如下：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

其中，[q₀ q₁ q₂ q₃]表示四元数；

再运用坐标转换矩阵，把基于MEMS的载体坐标系的加速度转换到地理坐标系，转换矩阵如下：

$\left[\begin{array}{c}{a}_N\\ a_E\\ a_D\end{array}\right]=R*\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$

其中，[a_N a_E a_D]表示地理坐标系下三轴加速度，R表示坐标转换矩阵，[a_x a_y a_z]表示地理坐标系下三轴加速度；

步骤3、利用Dual-ZUPT算法得到双零速率修正点即F-ZUPT点和S-ZUPT点，将人员连续的运动划分为每一个单步，单步步长为F-ZUPT点到S-ZUPT点行进的距离，计算公式如下：

$\left[\begin{array}{c}{S}_s^N\\ S_s^E\\ S_s^D\end{array}\right]=\left[\begin{array}{c}{S}_0^N\\ S_0^E\\ S_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^D\mathrm{\δ}t\end{array}\right]$

其中，表示S-ZUPT点的三轴位移，表示F-ZUPT点的三轴位移，计算时被设为0，[V_i ^N V_i ^E V_i ^D]表示i时刻的三轴速度，计算公式如下：

$\left[\begin{array}{c}{V}_s^N\\ V_s^E\\ V_s^D\end{array}\right]=\left[\begin{array}{c}{V}_0^N\\ V_0^E\\ V_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^D\mathrm{\δ}t\end{array}\right]$

其中：表示S-ZUPT点的三轴速度，[V₀ ^N V₀ ^E V₀ ^D]表示F-ZUPT点的三轴速度，计算时被设为0，表示i时刻的三轴加速度；

步骤4、根据双零速率修正点来计算每一个单步的步长，然后，累加总位移得到整个运动步长：

$\begin{array}{c}L={\mathrm{\ΣL}}_{unit}\\ =\mathrm{\Σ}\sqrt{{\left(S_s^N\right)}^2+{\left(S_s^E\right)}^2+{\left(S_s^D\right)}^2}\end{array}$

其中：L表示人员运动的总位移，L_unit表示人员的单步步长，表示S-ZUPT点的三轴位移。

所述步骤3中的所述F-ZUPT点和S-ZUPT点的选取，包括以下步骤：

如果i时刻的三轴加速度满足下式，则i时刻为脚尖离地时刻：

$\left\{\begin{array}{c}\left|\frac{{\mathrm{\δa}}_i^N}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^E}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^D}{\mathrm{\δ}t}\right|\≥A_{th}\\ \left|a_i^N\right|+\left|a_i^E\right|+\left|a_i^D\right|\≤a_{th}\end{array}\right.$

其中，t表示采样间隔，表示i时刻的三轴加速度，A_th和a_th分别表示加速度斜率阈值和加速度阈值，F-ZUPT点在此时刻选取；

当人员脚跟触地时，三轴加速度在这个瞬间就会产生一个拐点，即满足式子：

$\frac{{\mathrm{\δa}}_i^n}{\mathrm{\δ}t}=0$

将此式定义为触地瞬间时间组(lpg)，S-ZUPT点在(lpg)中进行选取。

与现有技术相比，本发明具有以下积极的技术效果：

1)避免加速度二次积分计算步长的误差累积，从而降低了步长计算误差的累积；2)对人员单步步长计算和行进总位移计算，因此具有很好的可靠性和稳定性；3)提高了零速率修正准确率，从而提高了步长计算的精度。

附图说明

图1为MEMS固定位置和室内定位步长计算方法的流程图；

图2为人员行走步态周期和加速度信号的分解。

具体实施方式

为了能进一步了解本发明的发明内容、特点及功效，下面将结合附图对本发明实施方式作进一步地详细描述。

本发明的整体思路主要在于步态的检测和对步态周期内加速度信号的分解处理，采用双零速率检测点来定位步长，提高步长计算的精度。

一、本发明的一种基于惯性传感的人员步长估算的方法整体流程的各个步骤具体说明如下：

本发明选用的微电子机械系统(MEMS)为PNI公司生产的SPACEPOINT SCOUT，内置三轴加速度计、三轴陀螺仪以及三轴磁力计。通过对不同MEMS放置位置的研究比较，从实验装置复杂度、实验成本以及对人员行走影响等方面考虑，将MEMS放置在行人的脚后跟位置进行步长估计是较为合理的(如图1所示)，有利于行人自由行走时，采集行人运动的三轴加速度以及四元数。

步骤2、由于环境噪声干扰、传感器自身误差等因素，采集的传感器数据需要进行前期的滤波和校准，提高原始数据的精度，然后利用采集的四元数来生成坐标转换矩阵，如下：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

其中，[q₀ q₁ q₂ q₃]表示四元数；

运用坐标转换矩阵，将基于MEMS载体坐标系的三轴加速度[a_N a_E a_D]转换到地理坐标系，得到地理坐标系的三轴加速度[a_x a_y a_z]，方法如下：

$\left[\begin{array}{c}{a}_N\\ a_E\\ a_D\end{array}\right]=R*\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$

根据人员行走的规律性和周期性，对人员行走的步态周期和相应的三轴加速度进行分解，如图2所示：

将步行的过程进行分解，每一个单步分为摆动期和站立期，其中包括四个特殊时刻：脚跟抬起、脚尖离地、脚跟着地和脚面完全贴地。在Dual-ZUPT算法中，脚尖离地时刻为第一个零速率修正点(First Zero Velocity Update,F-ZUPT)，脚跟着地时刻为第二个零速率修正点(Second Zero Velocity Update,S-ZUPT)。接下来将详细说明F-ZUPT和S-ZUPT选取方法。

对于F-ZUPT，如果i时刻的三轴加速度满足下式，则i时刻为脚尖离地时刻(Toe-off时刻)：

$\left\{\begin{array}{c}\left|\frac{{\mathrm{\δa}}_i^N}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^E}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^D}{\mathrm{\δ}t}\right|\≥A_{th}\\ \left|a_i^N\right|+\left|a_i^E\right|+\left|a_i^D\right|\≤a_{th}\end{array}\right.$

其中：t表示采样间隔，表示i时刻的三轴加速度，A_th和a_th分别表示加速度斜率阈值和加速度阈值。

当人员脚跟触地时，三轴加速度在这个瞬间就会产生一个拐点，即满足式子：

$\frac{{\mathrm{\δa}}_i^n}{\mathrm{\δ}t}=0$

将此式定义为触地瞬间时间组(lpg)，S-ZUPT点在(lpg)中进行选取。

完成F-ZUPT点和S-ZUPT点的选取之后，在Dual-ZUPT算法中，定位单步步长为F-ZUPT点到S-ZUPT点行进的距离，计算方法如下：

$\left[\begin{array}{c}{S}_s^N\\ S_s^E\\ S_s^D\end{array}\right]=\left[\begin{array}{c}{S}_0^N\\ S_0^E\\ S_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^D\mathrm{\δ}t\end{array}\right]$

其中，表示S-ZUPT点的三轴位移，表示F-ZUPT点的三轴位移，计算时被设为0，[V_i ^N V_i ^E V_i ^D]表示i时刻的三轴速度，计算方法如下：

$\left[\begin{array}{c}{V}_s^N\\ V_s^E\\ V_s^D\end{array}\right]=\left[\begin{array}{c}{V}_0^N\\ V_0^E\\ V_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^D\mathrm{\δ}t\end{array}\right]$

其中：表示S-ZUPT点的三轴速度，表示F-ZUPT点的三轴速度，计算时被设为0，表示i时刻的三轴加速度。

最后，进行单步计算和总位移累积

$\begin{array}{c}L={\mathrm{\ΣL}}_{unit}\\ =\mathrm{\Σ}\sqrt{{\left(S_s^N\right)}^2+{\left(S_s^E\right)}^2+{\left(S_s^D\right)}^2}\end{array}$

其中：L表示人员运动的总位移，L_unit表示人员的单步步长。

二、本发明一种室内定位步长计算的方法测试结果。

(1)将IMU固定在测试者脚后跟位置，参加实际环境里的行进测试。3名测试者(年龄从20到27岁，身高从1.55到1.82米，体重从44.6到85.5千克)熟悉了测试条件和方法之后，选择自己最舒服和最习惯的步行方式完成实验。

如下实验后，根据图片的像素点和长度的关系，精确计算出每一个单步的步长，并分别用ZUPT算法和Dual-ZUPT算法进行数据处理，实验结果如表1所示。

表1、实验者步长估计结果

表1

从表中可以看出，相比于ZUPT算法9.58％的平均误差，Dual-ZUPT算法的平均误差仅为2.82％。说明Dual-ZUPT算法可以有效地在每一个单步减小实验误差，提高实验的精度。因此，减少人员行走的随机性和不确定性可以提高单步步长计算的精度。

Claims (2)

1.一种基于惯性传感的人员步长估算的方法，其特征在于，该方法包括以下步骤：

步骤(1)、使用微电子机械系统采集室内人员运动的基于MEMS载体坐标系的三轴加速度[a_N a_E a_D]以及四元数作为原始数据，并进行原始数据初期滤波和校正；

步骤(2)、利用四元数生成坐标转换矩阵，坐标转换矩阵计算公式如下：

$R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

其中，[q₀ q₁ q₂ q₃]表示四元数；

再运用坐标转换矩阵，把基于MEMS的载体坐标系的加速度转换到地理坐标系，转换矩阵如下：

$\left[\begin{array}{c}{a}_N\\ a_E\\ a_D\end{array}\right]=R*\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$

其中，[a_N a_E a_D]表示地理坐标系下三轴加速度，R表示坐标转换矩阵，[a_x a_y a_z]表示地理坐标系下三轴加速度；

步骤(3)、利用Dual-ZUPT算法得到双零速率修正点即F-ZUPT点和S-ZUPT点，将人员连续的运动划分为每一个单步，单步步长为F-ZUPT点到S-ZUPT点行进的距离，计算公式如下：

$\left[\begin{array}{c}{S}_s^N\\ S_s^E\\ S_s^D\end{array}\right]=\left[\begin{array}{c}{S}_0^N\\ S_0^E\\ S_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{V}_i^D\mathrm{\δ}t\end{array}\right]$

其中，表示S-ZUPT点的三轴位移，表示F-ZUPT点的三轴位移，计算时被设为0，[V_i ^N V_i ^E V_i ^D]表示i时刻的三轴速度，计算公式如下：

$\left[\begin{array}{c}{V}_s^N\\ V_s^E\\ V_s^D\end{array}\right]=\left[\begin{array}{c}{V}_0^N\\ V_0^E\\ V_0^D\end{array}\right]+\left[\begin{array}{c}\underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^N\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^E\mathrm{\δ}t\\ \underset{i=F-ZUPT}{\overset{S-ZUPT}{\mathrm{\Σ}}}{a}_i^D\mathrm{\δ}t\end{array}\right]$

其中：表示S-ZUPT点的三轴速度，表示F-ZUPT点的三轴速度，计算时被设为0，表示i时刻的三轴加速度；

步骤(4)、累加总位移得到整个运动步长：

$\begin{array}{c}L={\mathrm{\ΣL}}_{unit}\\ =\mathrm{\Σ}\sqrt{{\left(S_s^N\right)}^2+{\left(S_s^E\right)}^2+{\left(S_s^D\right)}^2}\end{array}$

其中：L表示人员运动的总位移，L_unit表示人员的单步步长，表示S-ZUPT点的三轴位移。

2.根据权利要求1所述的一种基于惯性传感的人员步长估算的方法，其特征在于，所述步骤(3)中的所述F-ZUPT点和S-ZUPT点的选取，包括以下步骤：

如果i时刻的三轴加速度满足下式，则i时刻为脚尖离地时刻：

$\left\{\begin{array}{c}\left|\frac{{\mathrm{\δa}}_i^N}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^E}{\mathrm{\δ}t}\right|+\left|\frac{{\mathrm{\δa}}_i^D}{\mathrm{\δ}t}\right|\≥A_{th}\\ \left|a_i^N\right|+\left|a_i^E\right|+\left|a_i^D\right|\≤a_{th}\end{array}\right.$

其中，t表示采样间隔，表示i时刻的三轴加速度，A_th和a_th分别表示加速度斜率阈值和加速度阈值，F-ZUPT点在此时刻选取；

当人员脚跟触地时，三轴加速度在这个瞬间就会产生一个拐点，即满足式子：

$\frac{{\mathrm{\δa}}_i^n}{\mathrm{\δ}t}=0$

将此式定义为触地瞬间时间组(lpg)，S-ZUPT点在(lpg)中进行选取。