对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置与流程

本发明涉及通信技术领域，尤其涉及一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置。

背景技术：

在室内、丛林等环境中，卫星定位系统由于受遮挡而无法实现定位，在上述场景中的定位应用中，基于惯性元件的步行者轨迹推算方案由于可以不受外界环境影响而实现全场景定位，因此越来越引起人们的广泛关注。在基于惯性元件进行步行者轨迹推算的方案中，应用比较多的是零速修正算法。该算法利用步行者脚部着地时的零速区间获得运动观测量而对运动参数进行修正，抑制陀螺仪漂移而提高定位精度。由于零速修正算法对零速区间检测的精度要求很高，因此零速检测方法直接决定了系统的整体性能。

传统的零速检测方法基于补偿后的加速度计与陀螺仪测量数据，利用联合阈值的方式进行判断，但由于所采用的阈值为固定的经验值，无法适应人的各种形态运动而导致精度较差；基于Neyman-Pearson准则的方法采用极大似然估计方法对零速状态进行估计，但实现过于复杂难以在装置中获得实际应用；目前应用的阈值自适应调整方法一般为对人的行走与跑步两种状态做判断，并以固定比例设置阈值，但该比例仍为提前设定，因此对快走与慢跑等复杂步态存在误判的可能，从而导致精度降低。

技术实现要素：

鉴于上述的分析，本发明旨在提供一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置，可完全或者至少部分地解决上述问题。

为解决上述问题，本发明主要是通过以下技术方案实现的：

本发明一方面提供了一种阈值自适应调整零速检测步行者轨迹的推算方法，该方法包括：获取步行者脚部的加速度数据、角速度数据以及磁场数据，对所述加速度数据、所述角速度数据以及所述磁场数据进行补偿，并对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率；

根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并根据当前步的滑动窗口对补偿后的角速度数据，计算对应的第二角速度能量均值；

根据计算得到的步伐频率计算步伐频率对应的比例系数；

根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定。

进一步地，所述对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率，具体包括：

计算加速度能量其中，a_x,a_y,a_z分别为加速度在x轴、y轴以及z轴的分量；

对加速度能量E_g进行低通滤波，得到滤波后的加速度能量E′_g；

对低通滤波后的加速度能量E′_g进行波峰与波谷初步判决，得到初步判决后的波峰与波谷序列；

对初步判决后的波谷进行二次判决，确定有效波谷和无效波谷；

对有效波谷间隔取倒数得到步行者的步伐频率v_f。

进一步地，在初步判决后的波谷进行二次判决，确定有效波谷和无效波谷，具体包括：

在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定该波谷为有效波谷；否则，为无效波谷。

进一步地，根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并据根据当前步的滑动窗口对补偿后的角速度数，计算对应的第二角速度能量均值，具体包括：

计算第i个采样点的角速度模值||w_i||为：其中，w_x,w_y,w_z分别为角速度在x轴、y轴以及z轴的分量；

设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：

计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：

进一步地，根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定，具体包括：

对步行者的运动状态做判定：当T＝1时，认为当前处于零速状态，比例系数R_p＝k₀·v_f+α₀，其中，k₀与α₀为提前预设的经验值。

本发明另一方面还提供了一种自适应阈值零速检测步行者轨迹的推算装置，该装置包括：

第一计算单元，用于获取步行者脚部的加速度数据、角速度数据以及磁场数据，对所述加速度数据、所述角速度数据以及所述磁场数据进行补偿，并对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率；

第二计算单元，用于根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并根据当前步的滑动窗口对补偿后的角速度数据，计算对应的第二角速度能量均值，根据计算得到的步伐频率计算步伐频率对应的比例系数；

第三计算单元，用于根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定。

进一步地，所述第一计算单元还用于，计算加速度能量其中，a_x,a_y,a_z分别为加速度在x轴、y轴以及z轴的分量；对加速度能量E_g进行低通滤波，得到滤波后的加速度能量E′_g；对低通滤波后的加速度能量E′_g进行波峰与波谷初步判决，得到初步判决后的波峰与波谷序列；对初步判决后的波谷进行二次判决，确定有效波谷和无效波谷；对有效波谷间隔取倒数得到步行者的步伐频率v_f。

进一步地，所述第一计算单元还用于，在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定该波谷为有效波谷；否则，为无效波谷。

进一步地，所述第一计算单元还用于，计算第i个采样点的角速度模值||w_i||为：其中，w_x,w_y,w_z分别为角速度在x轴、y轴以及z轴的分量；设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：

进一步地，所述第三计算单元还用于，对步行者的运动状态做判定：当T＝1时，认为当前处于零速状态，比例系数R_p＝k₀·v_f+α₀，其中，k₀与α₀为提前预设的经验值。

本发明有益效果如下：

本发明通过对原始数据进行补偿，获取可靠的原始数据，通过对补偿后的加速度数据进行低通滤波以及二次判决，进行步伐频率的提取，然后将步伐频率作为输入量对角速度能量均值的阈值进行调整，从而达到阈值平滑自适应调整的目的。与目前通用的阈值自适应调整方法相比，可进行阈值平滑调整，从而适应步行者的各类步伐频率的行动，克服了对快走与慢跑等情况误判的问题，从而提高系统定位精度。

本发明的其他特征和优点将在随后的说明书中阐述，并且部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

图1为本发明实施例的一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法的流程示意图；

图2为本发明实施例的另一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法的流程示意图；

图3为本发明实施例的再一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法的流程示意图；

图4为应用本发明实施例的所述方法得到的沿运动场运动的轨迹图；

图5为应用本发明实施例的所述方法得到的另一种沿运动场运动的轨迹图；

图6为本发明实施例的一种对阈值平滑自适应调整的零速修正步行者轨迹的推算装置的结构示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。为了清楚和简化目的，当其可能使本发明的主题模糊不清时，将省略本文所描述的器件中已知功能和结构的详细具体说明。

本发明提供了一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法，本发明通过对原始数据进行补偿，获取可靠的原始数据，通过对补偿后的加速度数据进行低通滤波以及二次判决，进行步伐频率的提取，然后将步伐频率作为输入量对角速度能量均值的阈值进行调整，从而达到阈值自适应调整的目的。与目前通用的阈值自适应调整方法相比，可进行阈值平滑调整，从而适应步行者的各类步伐频率的行动，克服了对快走与慢跑等情况误判的问题，从而提高系统定位精度。以下结合附图以及几个实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不限定本发明。

本发明实施例提供了一种对阈值平滑自适应调整的零速修正步行者轨迹的推算方法，参见图1，该方法包括：

S101、获取步行者脚部的加速度数据、角速度数据以及磁场数据，对所述加速度数据、所述角速度数据以及所述磁场数据进行补偿，并对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率；

S102、根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并根据当前步的滑动窗口对补偿后的角速度数据，计算对应的第二角速度能量均值；

S103、根据计算得到的步伐频率计算步伐频率对应的比例系数；

S104、根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定。

也就是说，本发明通过对原始数据进行补偿，获取可靠的原始数据，通过对补偿后的加速度数据进行低通滤波以及二次判决，进行步伐频率的提取，然后将步伐频率作为输入量对角速度能量均值的阈值进行调整，从而达到阈值自适应调整的目的。与目前通用的阈值自适应调整方法相比，可进行阈值平滑调整，从而适应步行者的各类步伐频率的行动，克服了对快走与慢跑等情况误判的问题，从而提高系统定位精度。

具体来说，本发明实施例是通过位对准误差补偿来对获取步行者脚部的加速度数据、角速度数据以及磁场数据进行补偿。当然，本领域的技术人员也可以通过其他的方法来对获取的各个数据进行补偿。

具体实施是，本发明所述对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率，具体包括：

计算加速度能量(其中，a_x,a_y,a_z分别为加速度在x轴、y轴以及z轴的分量)；

对加速度能量E_g进行低通滤波，得到滤波后的加速度能量E′_g；

对低通滤波后的加速度能量E′_g进行波峰与波谷初步判决，得到初步判决后的波峰与波谷序列；

对初步判决后的波谷进行二次判决，确定有效波谷和无效波谷；

对有效波谷间隔取倒数得到步行者的步伐频率v_f。

本发明实施例在初步判决后的波谷进行二次判决，确定有效波谷和无效波谷，具体包括：

在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定该波谷为有效波谷；否则，为无效波谷。

本发明实施例所述根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并据根据当前步的滑动窗口对补偿后的角速度数，计算对应的第二角速度能量均值，具体包括：

计算第i个采样点的角速度模值||w_i||为：(其中，w_x,w_y,w_z分别为角速度在x轴、y轴以及z轴的分量)；

设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：

计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：

本发明实施例所述根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定，具体包括：

对步行者的运动状态做判定：当T＝1时，认为当前处于零速状态，比例系数R_p＝k₀·v_f+α₀，其中，k₀与α₀为提前预设的经验值。

如图2所示，本发明提供一种自适应阈值零速检测的步行者轨迹推算方法，该方法的主要步骤包括：在步行者脚部固定微机电系统(MEMS)惯性传感元件，包括加速度计、陀螺仪以及磁力计；系统开始工作后，首先选定起始工作点(原点)，设定初始状态；然后获取由所述MEMS传感器所提供的传感数据，并进行补偿；随后，利用自适应阈值零速检测方法，判定步行者是否处于零速状态；若是，则基于扩展卡尔曼滤波对系统状态进行校正，并进行系统状态递推；若否，则直接进行系统状态递推；循环执行步行者零速状态判定、基于扩展卡尔曼滤波对系统状态进行更新、系统状态递推三个步骤；最后，输出定位结果。

如图3所示，本发明所提供的步行者零速状态判定方法，首先需要进行步频计算，主要步骤包括：

计算加速度能量

对加速度能量E_g进行低通滤波，得到E′_g；

对低通滤波后的E′_g进行波峰与波谷判决，得到初步判决后的波峰与波谷序列；

对初步判决后的波峰与波谷序列进行二次判决，判决方法为：在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定此波谷为有效波谷；否则，视为无效波谷；

对有效波谷间隔取倒数，即可得到步频v_f。

计算得到步频v_f之后，以其为参考计算阈值比例系数；并基于角速度能量均值，以前一步角速度能量均值与所述比例系数的乘积为阈值，进行零速状态检测，具体步骤包括：

计算第i个采样点的角速度模值||w_i||为：

设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：

计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：

计算比例系数R_p：R_p＝k₀·v_f+α₀(其中，k₀与α₀为提前预设的经验值)；

对运动状态做判定：

当T＝1时，认为当前处于零速状态。

总体来说，本发明实施例所述的方法包括：

在步行者脚部固定MEMS传感器，获取脚部的加速度、角速度以及磁场等原始传感数据；

所述原始传感数据需经过相关补偿方法进行补偿；

补偿后的加速度数据需进行能量计算、低通滤波与两次判决过程，以确定步行者的步伐频率；

补偿后的角速度数据需根据前一步的采样区间，计算对应的角速度能量均值；

补偿后的角速度数据需根据当前步的滑动窗口，计算对应的角速度能量均值；

依据所计算的步伐频率，计算对应的比例系数；

依据前一步角速度能量均值与所述比例系数的乘积计算自适应阈值；

比较当前步滑动窗口内的角速度能量均值与所述自适应阈值，进行步行者的零速状态判定。

图5和图6为利用本发明提供的方法对步行者轨迹进行推算的结果图，本发明采用了荷兰Xsens公司的MTI 100模块进行了两组步行实验。图5对应的是实验的场地为威海北洋电气集团股份有限公司的篮球场，沿篮球场白线行走，总路程约505米，回到起始点，始终点误差为0.87米，定位精度为0.17％。图6对应的是实验的场地为山东大学(威海)体育场，沿跑道行走，回到起始点，总路程>800米，始终点误差为2.89米，定位精度为0.36％。

总体来说，通过MEMS传感器的应用，可以在室内、丛林等卫星定位系统受遮挡而失效的环境中实现定位；通过对传感器原始数据进行补偿、对补偿后的加速度数据进行低通滤波以及二次判决、角速度能量均值计算、阈值自适应调整等过程，可以在步行者行走、跑步、快走、慢跑等各类行动条件下实现零速状态的准确判决，从而有效提高定位精度。

与图1所示的方法相对应的，本发明实施例还提供了一种对阈值平滑自适应调整的零速修正步行者轨迹的推算装置，参见图6，该装置包括：

第一计算单元，用于获取步行者脚部的加速度数据、角速度数据以及磁场数据，对所述加速度数据、所述角速度数据以及所述磁场数据进行补偿，并对补偿后的加速度数据进行能量计算、低通滤波与两次判决，确定步行者的步伐频率；

第二计算单元，用于根据前一步的采样区间对补偿后的角速度数据，计算对应的第一角速度能量均值，并根据当前步的滑动窗口对补偿后的角速度数据，计算对应的第二角速度能量均值，根据计算得到的步伐频率计算步伐频率对应的比例系数；

第三计算单元，用于根据所述第一角速度能量均值与所述比例系数的乘积计算得到自适应阈值，根据所述第二角速度能量均值与所述自适应阈值进行步行者的零速状态判定。

具体实施时，本发明实施例所述第一计算单元还用于，计算加速度能量(其中，a_x,a_y,a_z分别为加速度在x轴、y轴以及z轴的分量)；对加速度能量E_g进行低通滤波，得到滤波后的加速度能量E′_g；对低通滤波后的加速度能量E′_g进行波峰与波谷初步判决，得到初步判决后的波峰与波谷序列；对初步判决后的波谷进行二次判决，确定有效波谷和无效波谷；对有效波谷间隔取倒数得到步行者的步伐频率v_f。

进一步地，本发明实施例所述第一计算单元还用于，在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定该波谷为有效波谷；否则，为无效波谷。

进一步地，本发明实施例所述第一计算单元还用于，计算第i个采样点的角速度模值||w_i||为：(其中，w_x,w_y,w_z分别为角速度在x轴、y轴以及z轴的分量)；设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：

具体实施时，本发明实施例所述第三计算单元还用于，对步行者的运动状态做判定：当T＝1时，认为当前处于零速状态，比例系数R_p＝k₀·v_f+α₀，其中，k₀与α₀为提前预设的经验值。

本实施例提供一种自适应零速点检测的步行者轨迹推算系统，通过固定在步行者脚部的MEMS惯性传感模块进行定位，其中，所述的MEMS惯性传感模块包括三轴加速度计、三轴陀螺仪以及三轴磁力计。

具体实施时，本发明对所述步行者轨迹推算的装置也可包括惯性传感模块、初始状态设置模块、传感数据补偿模块、判断模块、估计模块、系统递推模块以及输出模块。

本发明涉及的方法，包括如下步骤：

将所述的MEMS惯性传感模块固定于步行者的脚部；

所述初始状态设置模块用于设置系统初始状态；

所述传感数据补偿模块按照既定的数据补偿方法，对惯性传感模块的原始数据进行补偿，并利用基于椭球假设的最小二乘法对磁力计数据进行三维校正；

所述判断模块用于对步行者的运动状态进行判断，以确定其是否处于零速状态，首先进行步伐频率计算，具体步骤包括：1)计算加速度能量2)对加速度能量E_g进行低通滤波，得到E′_g；2)对低通滤波后的E′_g进行波峰与波谷判决，得到初步判决后的波峰与波谷序列；3)对初步判决后的波峰与波谷序列进行二次判决，判决方法为：在初步判决后的波谷周围寻找初步判决波峰，若存在初步判决波峰，则判定此波谷为有效波谷；否则，视为无效波谷；4)对有效波谷间隔取倒数，得到步伐频率v_f。得到步伐频率后，进行并基于角速度能量均值，并以前一步角速度能量均值与所述比例系数的乘积为阈值，进行零速状态检测，具体步骤包括：1)计算第i个采样点的角速度模值||w_i||为：2)设(m,n)为第s步的采样区间，计算第s步的角速度能量均值为：3)计第s+1步内的第i个采样点角速度模值为||w_i||，陀螺仪噪声方差为滑动窗口宽度为W，计算在滑动窗口(n,n+W-1)内角速度能量均值为：4)计算比例系数R_p：R_p＝k₀·v_f+α₀(其中，k₀与α₀为提前预设的经验值)。最后，对运动状态做判定：当T＝1时，认为当前处于零速状态。

所述估计模块基于扩展卡尔曼滤波器对位置信息误差进行估计，具体方法如下：

建立卡尔曼滤波的系统状态方程为选取系统状态变量为其中：φ_E、φ_N、φ_U为俯仰角误差、横滚角误差和方位角误差；δV_E、δV_N、δV_U分别为东向、北向和天向速度误差；δλ为纬度、经度误差；ε_X、ε_Y、ε_Z分别为陀螺东北天向的零漂；分别为加速度计东北天向的偏置；W(t)＝[w_gx w_gy w_gz w_ax w_ay w_az]^T高斯白噪声，且E[W(t)]＝0，cov[W_k,W_j]＝E[W_kW_j^T]Q_kδ_kj(Q_k为系统噪声方差阵)。

其中，

F_A为8×8阶矩阵，其中非零元素如下：

F(4,2)＝-f_U

F(4,3)＝f_N

F(5,1)＝f_U

F(5,3)＝-f_E

F(6,1)＝-f_N

F(6,2)＝f_E

2)观测方程为

y_k＝Hz_k+ν_k

其中，H为观测矩阵，ν_k为噪声矩阵。

当运用本发明提供的零速检测方案检测出零速状态时，采用磁力计航向角、零速度和零角速度作为观测量对系统状态进行修正，系统的观测量为z_k＝[Δφ_U,δV_E,δV_N,δV_U,ε_X,ε_Y,ε_Z]，观测矩阵为

当检测出非零速状态时，仅利用磁力计航向角信息对系统状态进行修正，系统的观测量为，观测矩阵为

H＝[[0,0,1] 0_1×3 0_1×2 0_1×3 0_1×3]。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。