[发明专利]对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置

权利要求书

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

其中，所述判定步行者是否处于零速状态包括以下步骤：

步骤1：进行步频计算，获取步伐频率：

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

步骤1-2：对加速度能量Eg进行低通滤波，得到E′g；

步骤1-3：对低通滤波后的E′g进行波峰与波谷判决，得到初步判决后的波峰与波谷序列；

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

步骤1-5：对有效波谷间隔取倒数，得到步伐频率vf；

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

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

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

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

步骤2-4：计算比例系数R_p，R_p＝k₀·v_r+α₀，其中，k₀与α₀为提前预设的经验值；

步骤3：对运动状态做判定：当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)＝-fU，F(4，3)＝fN，

F(5，1)＝fU，F(5，3)＝-fE，

F(6，1)＝-fN，F(6，2)＝fE，

观测方程为

y_k＝Hz_k+v_k，

其中，H为观测矩阵，vk为噪声矩阵；

当检测出零速状态时，采用磁力计航向角、零速度和零角速度作为观测量对系统状态进行修正，系统的观测量为zk＝[ΔφU，δVE，δVN，δVU，εX，εY，εZ]，观测矩阵为

当检测出非零速状态时，仅利用磁力计航向角信息对系统状态进行修正，观测矩阵为H＝[[0，0，1]01×3 01×2 01×3 01×3]。

2.一种利用如权利要求1所述对阈值平滑自适应调整的零速修正步行者轨迹的推算方法的装置，其特征在于，包括：

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

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

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