CN104931049A - 一种基于运动分类的行人自主定位方法 - Google Patents

Abstract

本发明公开了一种基于运动分类的行人自主定位方法，包括以下步骤：步骤1，将行人身上可穿戴IMU输出的数据分类，自适应识别IMU放置位置：脚、腰、胸；步骤2，通过分析可穿戴IMU输出的数据，确定划分运动分类的阈值，将静止、走、跑等运动分类；步骤3，行人运动过程中零瞬态时刻的识别捕捉；步骤4，通过捷联惯性导航的导航解算，获取行人姿态、速度、位置信息；步骤5，融合以上IMU位置识别、运动分类结果，以及零瞬态检测结果的自适应滤波设计；步骤6，更新行人运动过程中的导航结果。本发明解决了运动数据分类问题，并且通过运动分类，不同的运动类型对应不同的步长模型，提高了定位的精度。

Description

一种基于运动分类的行人自主定位方法

技术领域

本发明属于导航定位技术领域，尤其涉及一种基于运动分类的行人自主定位方法。

背景技术

行人自主导航系统(包括MEMS三轴磁力计、MEMS三轴加速度计、MEMS三轴陀螺仪)主要用于个人在已知或未知条件下的自主导航和实时定位，辅助完成各类型的紧急救援任务。当火灾、地震等紧急意外发生时，事故发生现场可能会存在可视性降低、固有环境改变等不利于救援的情况，营救人员无法快速准确的辨识自身位置。此时，行人导航系统提供的定位信息可为营救人员提供有效的技术保障。

现有的具有单兵导航定位功能的产品绝大多数主要依靠GPS(Global Positioning System)进行定位，但当GPS信号缺失时，系统将无法工作，继而无法满足行人自主导航系统自主、实时、稳定的定位要求。因此，研究在无GPS情况下的单兵自主定位技术具有一定的应用价值。基于MEMS惯性测量技术的行人自主导航系统工作时不依赖于任何外部信息，具有良好的抗干扰性，因此研究基于MEMS惯性测量技术的行人自主导航技术具有良好的应用价值。在现有的行人自主导航应用中主要有两种方法：一种方法即是著名的鞋式捷联式导航算法。将IMU单元绑在鞋上，通过处理IMU输出的加速度、角速度值，预测运动载体的姿态信息和位置。另一种是采用PDR方法，通过加速度输出值来检测出行走的步数，移动目标的位置由预测的近似步长和通过磁场或者陀螺仪判断出的方向共同决定。虽然采用运动学近似估计步长，能够避免由加速度值双积分得到距离而引起的位置错误，但这种经验性的方法由于不能识别不同人的步态变化，所以当采用与以往不同的方式进行移动时将会失败，并且在非正常的环境下，例如人多拥挤的环境或者上下山等情况时也会完全失败。

发明内容

本发明的目的在于提供一种基于运动分类的行人自主定位方法，意在设计一种能够根据行人的停止、行走、奔跑等运动步态特征所提供相应的导航信息并及时给予相关的补偿校正，之后较为精准地定位的行人自主导航定位技术。

为解决上述技术问题，本发明采用的技术方案是：

一种基于运动分类的行人自主定位方法，包括以下步骤：

步骤1，将行人身上可穿戴IMU输出的数据分类，自适应识别IMU放置位置：脚、腰、胸；

步骤2，通过分析可穿戴IMU输出的数据，确定划分运动分类的阈值，将静止、走、跑等运动分类；

步骤3，行人运动过程中零瞬态时刻的识别捕捉；

步骤4，通过捷联惯性导航的导航解算，获取行人姿态、速度、位置信息；

步骤5，融合以上IMU位置识别、运动分类结果，以及零瞬态检测结果的自适应滤波设计；

步骤6，更新行人运动过程中的导航结果。

与现有技术相比，本发明的有益效果是：

(1)本发明提供的基于运动分类的行人自主定位方法，通过分析IMU输出的数据，自适应识别IMU放置的位置，以便选择不同的数学模型。

(2)本发明提供的基于运动分类的行人自主定位方法，通过对数据进行短时傅里叶(STFT)变换以及功率谱分析，设定好阈值，将运动分类，方便步长的计算，提高定位的精度。

(3)本发明提供的基于运动分类的行人自主定位方法，采用最佳状态假设性检验法，判断行人是否处在静止状态。当人体静止时，理论上IMU的速度和角速度几乎为零，然而由于传感器的量测误差、噪声以及算法误差等，此时速度计算值和陀螺仪的加速度输出并不为零。把它们作为卡尔曼滤波器的量测值来进行误差估计，即ZUPT和ZARU的应用。这样可以提高静止检测的准确度。

附图说明

图1为基于运动分类的行人自主定位方法流程图；

图2为捷联惯性导航算法流程图

图3为基于运动分类的行人自主定位方法轨迹图

具体实施方式

本发明提供的一种基于运动分类的行人自主定位方法流程如图1所示，具体如下：

步骤1，将行人身上可穿戴IMU输出的数据分类，自适应识别IMU放置位置：脚、腰、胸；

IMU放置在脚、腰、胸等不同的位置，加速度计输出的比力值以及陀螺仪输出的角速度是有差别的，通过分析数据可以将采集到的数据自适应分类，为后续的数据处理做准备。

步骤2，通过分析可穿戴IMU输出的数据，确定划分运动分类的阈值，将静止、走、跑等运动分类；

定义加速度的合成幅值为：

$\left|a_k\right|=\sqrt{a_k^b{\left(x\right)}^2+a_k^b{\left(y\right)}^2+a_k^b{\left(z\right)}^2}---\left(15\right)$

$C1=\left\{\begin{array}{cc}1& {\mathrm{th}}_{a\min }<\left|a_k\right|<{\mathrm{th}}_{a\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(16\right)$

th_amin为加速度合成幅值的检测阈值的最小值，th_amax为加速度合成幅值的检测阈值的最大值。

定义加速度计输出的局部方差为：

${\mathrm{\&sigma;}}_{a_k^b}^2=\frac{1}{2s+1}\underset{q=k-s}{\overset{k+s}{\mathrm{\&Sigma;}}}{(a_q^b-{\stackrel{\&OverBar;}{a}}_k^b)}^2---\left(17\right)$

其中，s为半窗口采样数量，k，q表示某一时刻。

$C2=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{a_k^b}>{\mathrm{th}}_{{\mathrm{\&sigma;}}_a}\\ 0& \mathrm{otherwise}\end{array}\right.---\left(18\right)$

表示加速度计输出的局部方差的检测阈值的最小值。

定义陀螺仪输出的合成幅值为：

$\left|{\mathrm{\&omega;}}_k\right|=\sqrt{{\mathrm{\&omega;}}_k^b{\left(x\right)}^2+{\mathrm{\&omega;}}_k^b{\left(y\right)}^2+{\mathrm{\&omega;}}_k^b{\left(z\right)}^2}---\left(19\right)$

$C3=\left\{\begin{array}{cc}1& \left|{\mathrm{\&omega;}}_k\right|<{\mathrm{th}}_{\mathrm{\&omega;}\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(20\right)$

th_ωmax表示陀螺仪输出的合成幅值的检测阈值的最大值。

定义磁强计的输出合成幅值为：

$\left|m_k\right|=\sqrt{m_k^b{\left(x\right)}^2+m_k^b{\left(y\right)}^2+m_k^b{\left(z\right)}^2}---\left(21\right)$

$C4=\left\{\begin{array}{cc}1& {\mathrm{th}}_{\mathrm{mag}\min }<\left|m_k\right|<{\mathrm{th}}_{\mathrm{mag}\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(22\right)$

th_mag min表示磁强计的输出合成幅值的检测阈值的最小值，th_mag max磁强计的输出合成幅值的检测阈值的最大值。

将数据进行短时傅里叶变换(STFT)并进行谱分析，确定C1、C2、C3、C4中的阈值，动态调节其阈值来划分不同的运动。四个条件之间采用“与”的逻辑，即只有当四个条件的判断结果都为1时才认为是处于某一种运动状态。即

将数据进行短时傅里叶变换(STFT)并进行谱分析，确定C1、C2、C3、C4中的第一组阈值th_amin1、th_amax1，th_ωmax1，th_mag min1、th_mag max1。若此时

C1₁∩C2₁∩C3₁∩C4₁＝1   (23)

则说明此时处于静止状态。

将数据进行短时傅里叶变换(STFT)并进行谱分析，确定C1、C2、C3、C4中的第二组阈值th_amin2、th_amax2，th_ωmax2，th_mag min2、th_mag max2。若此时

C1₂∩C2₂∩C3₂∩C4₂＝1   (24)

则说明此时处于行走状态。

将数据进行短时傅里叶变换(STFT)并进行谱分析，确定C1、C2、C3、C4中的第三组阈值th_amin3、th_amax3，th_ωmax3，th_mag min3、th_mag max3。若此时

C1₃I C2₃I C3₃I C4₃＝1   (25)

则说明此时处于跑步状态。

步骤3，行人运动过程中零瞬态时刻的识别捕捉；

静止检测是实现速度误差清零和抑制位置误差的前提，也是个人导航系统的关键技术。为保证其检测的准确性，同时利用加速度计和陀螺仪的输出数据，采用最佳状态假设检验法，判断人体运动状态。这种检测方法可以理解为一个二元假设检验问题。与(R³表示是三维的矢量)分别表示n时刻测得的加速度矢量与角速度矢量。行人的运动状态处于静止或是运动，其量测序列与是不同的。如果行人处于静止状态，则存在：

$T(z_n^a,z_n^{\mathrm{\&omega;}})<\mathrm{\&gamma;}---\left(26\right)$

其中是检验统计量，g为检验阈值。

式中，挝R¹，R¹(R¹表示是一维的数)，表示表示陀螺和加速度计的测量噪声，||a||²＝a^Ta，(g)^T为转置算子，W表示窗的大小，g表示重力加速度，n表示n时刻，表示k时刻测得的加速度矢量，表示k时刻测得的角速度速度矢量。

表示加速度采样的均值：

g取值由下式确定：

L(z_n)＝p(z_n；H₁)/p(z_n；H₀)   (30)

式中，H₀、H₁是假设性检验的两个假设，H₀表示IMU是运动的，H₁表示IMU是静止的。p(z_n；H₁)、p(z_n；H₀)表示这两种假设的观测值的概率密度函数，a表示一个给定的值。

步骤4，通过捷联惯性导航的导航解算，获取行人姿态、速度、

位置信息；

具体步骤如下：

针对人体的运动建立状态转移模型以及进行捷联惯性导航系统位置和姿态的解算；

在进行行人室内定位的过程当中，其状态模型是非线性的，但是可以运用状态估计的方式将其线性化。在此，本发明设立一个15维的状态误差向量，在其k时刻的表达式如下：

其中，分别表示为姿态误差，陀螺仪零偏误差，位置误差，速度误差以及加速度计零偏误差。之后根据上述状态误差向量矩阵所得的线性化状态转移模型如下所示：

${\mathrm{\&delta;x}}_{k|k-1}=f_k{\mathrm{\&delta;x}}_{k-1|k-1}+w_{k-1}---\left(32\right)$

其中dx_k|k-1为所预测的状态误差，dx_k-1|k-1是在k-1时刻滤波后的状态误差，w_k-1是过程噪声，其用一个协方差矩阵表示为：

$Q_K=E\left(w_k{w}_k^T\right)---\left(33\right)$

而针对人体运动状态的特殊性，所建立的相关状态转移矩阵φ_k(15×15)如下所示：

${\mathrm{\&phi;}}_k=\left[\begin{array}{ccccc}I& \mathrm{\&Delta;t}\&CenterDot;C_{b_{k|k-1}}^n& 0& 0& 0\\ 0& I& 0& 0& 0\\ 0& 0& I& \mathrm{\&Delta;t}\&CenterDot;I& 0\\ -\mathrm{\&Delta;t}\&CenterDot;S\left({\mathrm{\&alpha;}}_k^{\&prime;n}\right)& 0& 0& I& \mathrm{\&Delta;t}\&CenterDot;C_{b_{k|k-1}}^n\\ 0& 0& 0& 0& I\end{array}\right]---\left(34\right)$

其中为一个加速度的斜对称矩阵，其作用是用来估计传感器的俯仰角及横滚角的。其具体表示为：

$S\left({\mathrm{\&alpha;}}_k^{\&prime;n}\right)=\left[\begin{array}{ccc}0& -a_{\mathrm{zk}}& a_{\mathrm{vk}}\\ a_{\mathrm{zk}}& 0& -a_{\mathrm{xk}}\\ -a_{\mathrm{vk}}& a_{\mathrm{xk}}& 0\end{array}\right]---\left(35\right)$

其中是已转移到导航坐标系下的已被校正过零偏误差的加速度值。其具体表示为：

${\mathrm{\&alpha;}}_k^{\&prime;n}=C_{b_{k|k-1}}^n\&CenterDot;{\mathrm{\&alpha;}}_k^{\&prime;b}=(a_{\mathrm{xk}},a_{\mathrm{vk}},a_{\mathrm{zk}})---\left(36\right)$

状态转移模型建立好后，便可通过固连于行人的陀螺仪、加速度计，利用捷联惯性导航原理，计算行人的实时位置、速度和姿态。针对于室内环境的捷联惯性导航算法，具体流程如图2所示：

2-1：为捷联惯性测量器件。2-1-1：为固连于载体的加速度计，用于测量载体三轴的加速度值；2-1-2：为固连于载体的陀螺仪，用于测量载体三轴的角速度值；

2-2：为姿态计算机，用其算出各姿态角；

2-3：为比力测量值的分解模块，用其将比力由载体坐标系转换至惯性坐标系；

2-4：为重力计算机，其作用是进行重力补偿；

2-5：为导航计算机，其作用是算出速度及位置的估计值。

导航更新解算采用传统的捷联惯性导航计算方法。这里需要指出的是，初始对准过程中，由于选用的小型MEMS陀螺仪的精度较低，不能有效敏感地球的自转角速度，因此惯性自主定位测量单元的初始对准可简化为：通过水平加速度计的输出估算俯仰角θ和滚转角γ，然后由地磁传感器的输出估算出偏航角φ：计算出的导航坐标系下的磁场强度，从而算出偏航角φ，然后完成初始对准。具体步骤如下：首先，在北东地为导航坐标系的情况下，根据获得的x、y、z轴向上的加速度计信息，估算俯仰角θ和滚转角γ：

$\mathrm{\&theta;}=\arcsin \frac{{-\mathrm{\&alpha;}}_k^b\left(x\right)}{\sqrt{{\mathrm{\&alpha;}}_k^b{\left(x\right)}^2+{\mathrm{\&alpha;}}_k^b{\left(y\right)}^2+{\mathrm{\&alpha;}}_k^b{\left(z\right)}^2}}---\left(37\right)$

$\mathrm{\&gamma;}=\arctan \frac{{\mathrm{\&alpha;}}_k^b\left(y\right)}{{\mathrm{\&alpha;}}_k^b\left(z\right)}---\left(38\right)$

其中，为经过补偿后的加速度计输出值。

根据计算出俯仰角θ和滚转角γ，计算导航坐标系下的磁场强度为：

$B_k^n=\left[\begin{array}{ccc}\cos {\mathrm{\&theta;}}_k& 0& -\sin {\mathrm{\&theta;}}_k\\ 0& 1& 0\\ -\sin {\mathrm{\&theta;}}_k& 0& \cos {\mathrm{\&theta;}}_k\end{array}\right]\&CenterDot;\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\&gamma;}}_k& -\sin {\mathrm{\&gamma;}}_k\\ 0& \sin {\mathrm{\&gamma;}}_k& {\cos \mathrm{\&gamma;}}_k\end{array}\right]\&CenterDot;B_k^b---\left(39\right)$

式中，表示在载体坐标系下的磁场强度，θ_k、γ_k为k时刻的俯仰角、横滚角。

则可得到偏航角为：

$\mathrm{\&phi;}=\arctan \frac{B_k^n\left(z\right)\cos \mathrm{\&gamma;}-B_k^n\left(y\right)\sin \mathrm{\&gamma;}}{B_k^n\left(x\right)\cos \mathrm{\&theta;}+B_k^n\left(y\right)\sin \mathrm{\&theta;}+B_k^n\left(z\right)\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}}---\left(40\right)$

从而完成了系统的初始对准，获得行人坐标系(b系)到导航坐标系(n系)的方向余弦矩阵：

$\begin{array}{c}{C}_n^h={\left(C_h^n\right)}^T={\left(C_h^n\right)}^{-1}\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&phi;}& \sin \mathrm{\&phi;}\\ 0& -\sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\end{array}\right]\&CenterDot;\left[\begin{array}{ccc}\cos \mathrm{\&theta;}& 0& -\sin \mathrm{\&theta;}\\ 0& 1& 0\\ \sin \mathrm{\&theta;}& 0& \cos \mathrm{\&theta;}\end{array}\right]\&CenterDot;\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& \sin \mathrm{\&gamma;}& 0\\ -\sin \mathrm{\&gamma;}& \cos \mathrm{\&gamma;}& 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}\cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&theta;}\\ -\cos \mathrm{\&phi;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&phi;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&phi;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&phi;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&phi;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&phi;}\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&phi;}\cos \mathrm{\&theta;}\end{array}\right]\end{array}---\left(41\right)$

完成初始对准后，根据获得的角速率信息，利用四元数方法，对行人的运行姿态进行更新，四元数更新算法如下，首先计算角增量Δ：

$\mathrm{\&Delta;}=\sqrt{{\left({\mathrm{\&omega;}}_x{T}_m\right)}^2+{\left({\mathrm{\&omega;}}_y{T}_m\right)}^2+{\left({\mathrm{\&omega;}}_z{T}_m\right)}^2}---\left(42\right)$

其中，Δ为角增量，ω_x、ω_y、ω_z分别为三轴角速度标量值，T_m为采样时间。之后进行四元数更新：

$\left\{\begin{array}{c}{q}_{1|k+1}=(1-\frac{{\mathrm{\&Delta;}}^2}{8}+\frac{{\mathrm{\&Delta;}}^4}{384})q_{1|k}-{\mathrm{\&omega;}}_x{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{2|k}-{\mathrm{\&omega;}}_y{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{3|k}-{\mathrm{\&omega;}}_z{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{4|k}\\ q_{2|k+1}=(1-\frac{{\mathrm{\&Delta;}}^2}{8}+\frac{{\mathrm{\&Delta;}}^4}{384})q_{2|k}+{\mathrm{\&omega;}}_x{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{1|k}+{\mathrm{\&omega;}}_z{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{3|k}-{\mathrm{\&omega;}}_y{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{4|k}\\ q_{3|k+1}=(1-\frac{{\mathrm{\&Delta;}}^2}{8}+\frac{{\mathrm{\&Delta;}}^4}{384})q_{3|k}+{\mathrm{\&omega;}}_y{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{1|k}-{\mathrm{\&omega;}}_z{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{2|k}+{\mathrm{\&omega;}}_x{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{4|k}\\ q_{4|k+1}=(1-\frac{{\mathrm{\&Delta;}}^2}{8}+\frac{{\mathrm{\&Delta;}}^4}{384})q_{4|k}-{\mathrm{\&omega;}}_x{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{3|k}+{\mathrm{\&omega;}}_y{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{2|k}-{\mathrm{\&omega;}}_z{T}_m(0.5-\frac{{\mathrm{\&Delta;}}^2}{48})q_{1|k}\end{array}\right.---\left(43\right)$

其中，q_1|k+1为k+1时刻四元数的第一个值，之后的以此类推；q_1|k为k时刻四元数的第一个值，之后的依旧以此类推。然后将其四元数归一化：

$A=\sqrt{{q_{1|k+1}}^2+{q_{2|k+1}}^2+{q_{3|k+1}}^2+{q_{4|k+1}}^2}---\left(44\right)$

$\left\{\begin{array}{c}{q}_{1|k+1}^{\&prime;}=\frac{q_{1|k+1}}{A}\\ q_{2|k+1}^{\&prime;}=\frac{q_{2|l+1}}{A}\\ q_{3|k+1}^{\&prime;}=\frac{q_{3|k+1}}{A}\\ q_{4|k+1}^{\&prime;}=\frac{q_{4|k+1}}{A}\end{array}\right.---\left(45\right)$

其中A为k+1时刻四元数的平方和；q′_1|k+1为k+1时刻四元数的第一个归一化的值，之后的以此类推。然后便可得到方向余弦矩阵，如下：

$C_b^n=\left[\begin{array}{ccc}{\left(q_{1|k+1}^{\&prime;}\right)}^2+{\left(q_{2|k+1}^{\&prime;}\right)}^2-{\left(q_{3|k+1}^{\&prime;}\right)}^2-{\left(q_{4|k+1}^{\&prime;}\right)}^2& 2(q_{2|k+1}^{\&prime;}{q}_{3|k+1}^{\&prime;}-q_{1|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;})& 2(q_{2|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;}+q_{1|k+1}^{\&prime;}{q}_{3|k+1}^{\&prime;})\\ 2(q_{2|k+1}^{\&prime;}{q}_{3|l+1}^{\&prime;}+q_{1|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;})& {\left(q_{1|k+1}^{\&prime;}\right)}^2-{\left(q_{2|k+1}^{\&prime;}\right)}^2+{\left(q_{3|k+1}^{\&prime;}\right)}^2-{\left(q_{4|k+1}^{\&prime;}\right)}^2& 2(q_{3|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;}-q_{1|k+1}^{\&prime;}{q}_{2|k+1}^{\&prime;})\\ 2(q_{2|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;}-q_{1|k+1}^{\&prime;}{q}_{3|k+1}^{\&prime;})& 2(q_{3|k+1}^{\&prime;}{q}_{4|k+1}^{\&prime;}+q_{1|k+1}^{\&prime;}{q}_{2|k+1}^{\&prime;})& {\left(q_{1|k+1}^{\&prime;}\right)}^2-{\left(q_{2|k+1}^{\&prime;}\right)}^2-{\left(q_{3|k+1}^{\&prime;}\right)}^2+{\left(q_{4|k+1}^{\&prime;}\right)}^2\end{array}\right]---\left(46\right)$

之后，则可以得到相应的姿态信息：

$\left\{\begin{array}{c}\mathrm{\&phi;}=\arctan \frac{\left(C_b^n\left(\mathrm{3,1}\right)\right)}{C_b^n\left(\mathrm{3,3}\right)}\\ \mathrm{\&theta;}=\arcsin (-C_b^n\left(\mathrm{3,1}\right))\\ \mathrm{\&gamma;}=\arctan \frac{\left(C_b^n\left(\mathrm{2,1}\right)\right)}{C_b^n(1,1)}\end{array}\right.---\left(47\right)$

根据上述矩阵信息和比力信息对重力进行补偿，获得_n系下的加速度，同时计算速度和位置信息。

r_k|k-1＝r_k-1|k-1+υ_k|k-1·Δt   (50)

步骤5，融合以上IMU位置识别、运动分类结果，以及零瞬态检测结果的自适应滤波设计；

建立捷联惯导的误差方差，其状态向量包括3个姿态误差(俯仰角θ、横滚角γ和偏航角φ)、三周陀螺仪零偏误差、3个位置误差、3个速度误差和三轴加速度计零偏误差共计15维状态空间方程即滤波模型，利用人体运动步态检测获得的触发信息，驱动滤波器，完成各状态向量的估计，反馈校正加速度计输出、陀螺仪输出和相关导航输出结果。状态空间方程为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\end{array}\right.---\left(51\right)$

其中， 为姿态角误差，为陀螺仪零偏误差，为加速度计零偏误差，δυ_k为速度误差，δr_k为位置误差；W(t)为系统过程噪声矩阵，其表达式为： 为方向余弦矩阵，ω^b为载体系角速度，a^b为载体系加速度；G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵。

为得到更高的精度，采用速度辅助修正融合角速度辅助修正(ZUPT+ZARU)，则相应的观测值和观测矩阵为：

Z_k＝[Δw_k Δv_k]^T＝[w_k v_k]^T

$H_k=\left[\begin{array}{ccccc}{O}_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}\\ O_{3\&times;3}& O_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}\end{array}\right]---\left(52\right)$

步骤6，更新行人运动过程中的导航结果。

将滤波器解算出的误差修正信息反馈给惯性测量器件所实测的测量值部分，进行误差消除计算，更新导航结果，进而得出较高精度的室内行人的实时姿态及定位信息。

由上述六步，便可完成这种基于运动分类的行人自主定位方法的设计发明。通过进行一项闭环的惯性测量实验，可验证其定位精度可达到2％以内，其运动轨迹图如附图3所示。

本发明提供的这种基于运动分类的行人自主定位方法，不仅提高了数据处理过程中的自适应性，使得行人自主导航系统更加完善，而且提高了行人导航系统的定位精度。

以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限制。凡采用等同变换或者等效替换而形成的技术方案，均落在本发明的权利保护范围之内。

Claims (6)

1.一种基于运动分类的行人自主定位方法，其特征在于：包括以下步骤，

步骤1，将行人身上可穿戴IMU输出的数据分类，自适应识别IMU放置位置：脚、腰、胸；

步骤2，通过分析可穿戴IMU输出的数据，确定划分运动分类的阈值，将静止、走、跑等运动分类；

步骤3，行人运动过程中零瞬态时刻的识别捕捉；

步骤4，通过捷联惯性导航的导航解算，获取行人姿态、速度、位置信息；

步骤5，融合以上IMU位置识别、运动分类结果，以及零瞬态检测结果的自适应滤波设计；

步骤6，更新行人运动过程中的导航结果。

2.根据权利要求1所述的一种基于运动分类的行人自主定位方法，其特征在于：在步骤1中，通过对加速度计陀螺仪采集到的加速度、角速度分析并加以区分，自适应识别IMU放置的位置：脚、腰、胸。

3.根据权利要求1所述的一种基于运动分类的行人自主定位方法，其特征在于：在步骤2中，通过对运动数据的频域分析，按一定的阈值将运动分类。

定义加速度的合成幅值为：

$\left|a_k\right|=\sqrt{a_k^b{\left(x\right)}^2+a_k^b{\left(y\right)}^2+a_k^b{\left(z\right)}^2}---\left(1\right)$

为加速度合成幅值的检测阈值的最小值，为加速度合成幅值的检测阈值的最大值。

定义加速度计输出的局部方差为：

${\mathrm{\&sigma;}}_{a_k^b}^2=\frac{1}{2s+1}\underset{q=k-s}{\overset{k+s}{\mathrm{\&Sigma;}}}{(a_q^b-{\stackrel{\&OverBar;}{a}}_k^b)}^2---\left(3\right)$

其中，s为半窗口采样数量，k，q表示某一时刻。

表示加速度计输出的局部方差的检测阈值的最小值。

定义陀螺仪输出的合成幅值为：

$\left|{\mathrm{\&omega;}}_k\right|=\sqrt{{\mathrm{\&omega;}}_k^b{\left(x\right)}^2+{\mathrm{\&omega;}}_k^b{\left(y\right)}^2+{\mathrm{\&omega;}}_k^b{\left(z\right)}^2}---\left(5\right)$

$C3=\left\{\begin{array}{cc}1& \left|{\mathrm{\&omega;}}_k\right|<{\mathrm{th}}_{\mathrm{\&omega;}\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(6\right)$

th_ωmax表示陀螺仪输出的合成幅值的检测阈值的最大值。

定义磁强计的输出合成幅值为：

$\left|m_k\right|=\sqrt{m_k^b{\left(x\right)}^2+m_k^b{\left(y\right)}^2+m_k^b{\left(z\right)}^2}---\left(7\right)$

$C4=\left\{\begin{array}{cc}1& {\mathrm{th}}_{\mathrm{mag}\min }<\left|m_k\right|<{\mathrm{th}}_{\mathrm{mag}\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(8\right)$

th_mag min表示磁强计的输出合成幅值的检测阈值的最小值，th_mag max磁强计的输出合成幅值的检测阈值的最大值。

将数据进行短时傅里叶变换(STFT)并进行谱分析，确定C1、C2、C3、C4中的阈值，动态调节其阈值来划分不同的运动。四个条件之间采用“与”的逻辑，即只有当四个条件的判断结果都为1时才认为是处于某一种运动状态。

4.根据权利要求1所述的一种基于运动分类的行人自主定位方法，其特征在于：在步骤3中，利用公式：

$\frac{1}{W}\underset{k=n}{\overset{n+W-1}{\mathrm{\&Sigma;}}}\frac{1}{{\mathrm{\&sigma;}}_a^2}{\left|\right|y_k^a-g\frac{{\stackrel{\&OverBar;}{y}}_n^a}{\left|\right|{\stackrel{\&OverBar;}{y}}_n^a\left|\right|}\left|\right|}^2+\frac{1}{{\mathrm{\&sigma;}}_{\mathrm{\&omega;}}^2}{\left|\right|y_k^a\left|\right|}^2<\mathrm{\&gamma;}---\left(9\right)$

判断出导航系统的零速区间，若上式成立则行人导航系统使用者静止；其中，分别为比力矢量和角速度矢量，有W个时间观测点，从n到n+W-1，表示陀螺和加速度计的测量噪声，||a||²＝a^Ta，(·)^T为转置算子，W表示窗的大小，g表示重力加速度，n表示n时刻，表示k时刻测得的加速度矢量，表示k时刻测得的角速度速度矢量，表示采样的均值：

${\stackrel{\&OverBar;}{y}}_n^a=\frac{1}{W}\underset{k=n}{\overset{n+W-1}{\mathrm{\&Sigma;}}}{y}_k^a---\left(10\right)$

γ取值由下式确定：

$P_{\mathrm{FA}}=\underset{\{z_n:L\left(z_n\right)>\mathrm{\&gamma;}\}}{\&Integral;}p(z_n;H_0){\mathrm{dz}}_n=\mathrm{\&alpha;}---\left(11\right)$

L(z_n)＝p(z_n；H₁)/p(z_n；H₀)         (12)

5.根据权利要求1所述的一种基于运动分类的行人自主定位方法，其特征在于：在步骤4中，利用捷联解算算法，得到行人的姿态、位置、速度信息。

6.根据权利要求1所述的一种基于运动分类的行人自主定位方法，其特征在于：在步骤5中实用的自适应滤波模型为：

$\left\{\begin{array}{c}\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\end{array}\right.---\left(13\right)$

其中， 为姿态角误差，为陀螺仪零偏误差，为加速度计零偏误差，δυ_k为速度误差，δr_k为位置误差；W(t)为系统过程噪声矩阵，其表达式为： $W\left(t\right)=\left[\begin{array}{cc}-C_b^n{\mathrm{\&omega;}}^b& -C_b^n{a}^b\end{array}\right];$ G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵。

为得到更高的精度，采用速度辅助修正融合角速度辅助修正(ZUPT+ZARU)，则相应的观测值和观测矩阵为：

Z_k＝[Δw_k Δv_k]^T＝[w_k v_k]^T

                                    (14)

$H_k=\left[\begin{array}{ccccc}{O}_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}\\ O_{3\&times;3}& O_{3\&times;3}& I_{3\&times;3}& O_{3\&times;3}& O_{3\&times;3}\end{array}\right]$

自适应滤波器主要实现以下三种功能：修正速度误差，修正水平姿态误差，修正运动期间的累积位置误差。