CN106482733B - 行人导航中基于足底压力检测的零速修正方法 - Google Patents

Abstract

行人导航中基于足底压力检测的零速修正方法，结合运动中足底压力值、加速度值以及角速度值判定静止区间，以静止区间内的速度值作为卡尔曼滤波器的量测量，并利用卡尔曼滤波估计误差参数并修正速度、位置和姿态误差。本发明通过设定多个静止区间阈值与判定条件，提高了检测静止区间的准确性，有助于高动态下静止区间的检测，同时利用卡尔曼滤波器修正误差，提高了行人导航的定位精度。

Description

行人导航中基于足底压力检测的零速修正方法

技术领域

本发明涉及的是行人导航技术领域，特别涉及一种基于足底压力检测的零速修正方法。

背景技术

在行人导航系统中，主要使用惯性测量方法测量行人的运动轨迹。测量行人运动轨迹的主要步骤包括：将陀螺仪和加速度计安装在人体上，以获取人体运动时的运动参数即角速度和加速度，从而根据求解速度、位置和姿态角的公式计算出行人的行走轨迹。陀螺仪、加速度计自身不可避免的误差导致积分计算后的位置、速度等信息中包含了随时间漂移的误差，为了提高行人导航系统的定位精度，零速修正算法经常被应用于行人导航中。

零速修正算法本身具有局限性，主要包含静止区间检测不准确和运动区间的误差累积。目前静止区间检测常用的方法是通过加速度计和陀螺仪的输出数据来判断行人运动各时刻的加速度模值/角速度模值是否在阈值区间内，但是，在高动态下利用加速度和角速度信息判断静止区间的方法容易出现误判现象。近年来，足底动力学信息广泛应用于人体步态识别领域中，由足底动力学特点可知，足底所受压力随足部运动状态的变化而变化，在行走过程中足底所受的总压力一般呈驼峰形，根据行走过程中足底所受压力与足部状态的映射关系，通过足底压力值可以较准确的检测出静止区间。

导航过程中，加速度计输出值、陀螺仪输出值和解算出的位置、速度、姿态角等信息通常任何时刻都带有噪声，影响整个导航系统的定位精度。卡尔曼滤波技术利用动态量测信息去除噪声的影响，估计出从某种统计意义上误差最小的状态估值。对动态行为的估计，它能实现实时运行状态的估计和预测。针对现有的零速修正方法存在的缺陷，本发明提供了一种应用于行人导航中基于足底压力检测的零速修正方法，实现较复杂运动状态下静态区间的检测，并通过卡尔曼滤波实现行人导航系统的误差修正以提高行人导航系统定位精度。

发明内容

本发明的目的是针对现有技术不足提供一种行人导航中基于足底压力检测的零速修正方法，其主要特征在于结合足底压力值与惯性测量元件的输出数据可以更加准确的判定静止区间，通过卡尔曼滤波实现对行人导航系统中的误差修正，相对于现有的零速修正算法而言，结合足底压力检测的零速修正算法考虑到了人体行走的行为模式和足部运动状态。

本发明所述方法包括下列步骤：

1)在人体足底的前脚掌和后脚跟安装压力传感器，实时采集运动时前脚掌与后脚跟的压力值，将微型惯性测量单元即MIMU固定在人体脚部的踝关节上方，采集运动过程中的加速度和角速度信息，同时减少人体行走状态对MIMU的影响；

2)根据人体步态相位的分析，结合每个离散时刻足底压力值、加速度值、角速度值以及噪声特性设定静止区间的上、下阈值；

3)根据足底压力变化与步态变化的内在联系，结合足底压力传感器、加速度计和陀螺仪分别设定判定脚掌全部落地即静止区间的条件，并根据这些条件所进行的与操作，最终判定出静止区间；

4)零速修正在静止区间内触发卡尔曼滤波器，在检测出的静止区间内人体脚部的运动速度视为零，以此时MIMU所输出的速度值作为卡尔曼滤波器的量测量，利用卡尔曼滤波估计更多的误差参数，从而修正基于MIMU的惯性行人导航系统中的速度误差、位置误差和姿态误差。

所述步骤2)中的步态相位为行走时的支撑相位和摆动相位，由步态相位与足底受力情况内在联系的分析，可知行人处于静止区间时其支撑脚所受的力为人体总质量，根据人体体重和单位窗口内的足底平均压力值设定阈值F_V；根据理想情况下静止区间内脚部的合加速度大小为G即重力加速度，且方向垂直地面向下这一特点，设定加速度幅值的判断阈值为[G_V1,G_V2]；根据单位窗口内平均加速度幅值与噪声特性设定加速度幅值标准差的判断阈值为G_V3；根据单位窗口内角速度幅值与噪声特性设定角速度幅值的判断阈值为ω_V；

所述步骤3)中，设定检测静止区间的4个判断条件分别为C₁、C₂、C₃和C₄。当行人处于静止区间内，脚掌完全贴地，前后掌均受到地面反作用力且支撑脚所受力为人体自重，因此，设定判断条件C₁为

F₁＝f₁＞0&f₂＞0

F₂＝f₁+f₂

式中，f₁为前脚掌所受力，f₂为后脚跟所受力，F₁表示前脚掌与后脚跟均受力时为真，否则为假，F₂为前后掌总受力。判断条件C₂为通过加速度模值进行判定，如下

式中，a_kx、a_ky、a_kz分别为加速度沿三轴方向上的分量，|a_k|为加速度模值。判断条件C₃根据加速度矢量的幅值方差进行判定

是k时刻的平均加速度幅值，s为平滑窗口长度，a_j为加速度采样点，则k时刻加速度的幅值方差为

式中，δ(a_k)为k时刻加速度的幅值标准差，G_V3为判定阈值。利用角速度幅值设定判断条件C₄，角速度幅值表示为

ω_kx、ω_ky、ω_kz分别表示角速度在三轴方向上的分量，|ω_k|为角速度模值，则判断条件C₄表达式为

对C₁、C₂、C₃和C₄条件进行与操作，最终的静止区间的判定条件为：C＝C₁&C₂&C₃&C₄，当四个条件都满足时，C记录为1，表示检测到静止区间，否则记录为0，表示脚部处于运动状态。

所述步骤4)中，通过利用步骤2)、3)所述的静态区间检测方法检测出高动态下行人导航中的静态区间，将此时刻的运动速度视为零，MIMU在静止区间输出的速度值即为MIMU的误差漂移量，以该时刻MIMU所测得的速度值作为量测量通过卡尔曼滤波对其他误差参数进行估计并修正。

卡尔曼滤波离散化后的状态方程和量测方程分别为：

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化系统观测矩阵；φ_k,k-1为离散化状态转移矩阵；H_k为离散化系统量测矩阵；W_k-1和V_k分别为离散化系统状态噪声向量和量测噪声向量；

K时刻的状态一步预测值X_k,k-1为：

X_k,k-1＝φ_k,k-1X_k-1

滤波增益K_k为：

k时刻状态估计值X_k为：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

一步预测均方误差P_k,k-1为：

P_k，k-1＝φ_k，k-1P_k-1，k-1φ_k，k-1 ^T+Q_k-1

估计均方误差协方差矩阵P_k,k为：

式中，P_k-1,k-1表示k-1时刻的均方误差，Q_k-1表示系统噪声协方差矩阵，R_k表示量测噪声协方差矩阵，I表示单位矩阵；

通过以静止区间的速度作为量测值，利用卡尔曼滤波估计出位置、速度和姿态的状态误差后，对位置、速度和姿态信息进行补偿得到更为精确的定位信息。

本发明的有益效果：通过分析足底压力与步态相位内在联系，从加速度、陀螺仪和足底受力等方面入手，设定多个静止区间阈值与判定条件，提高了检测静止区间的准确性，有助于高动态下的静止区间的检测。利用静止区间内MIMU的速度值作为量测值并通过卡尔曼滤波对速度、位置和姿态误差的修正，可以提高行人导航的定位精度。本发明方法将足底压力传感器应用于零速修正中，有效的提高了零速区间检测的精度，符合了人体运动生物力学并提高了行人导航的定位精度。

附图说明

图1是本发明所述足底压力传感器安装位置图；

图中：A为后脚跟足底压力传感器安装位置；B为前脚掌足底村力传感器安装位置；

图2是本发明所述MIMU安装位置图；

图中：C为MIMU安装位置；

图3是本发明所述步态相位图；

图中：D为支撑期E为摆动期；

图4是本发明所述卡尔曼滤波误差修正图。

具体实施方式

为了实现高动态下的零速修正，本发明提供了一种基于足底压力检测零速修正方法。下面结合附图对发明的技术方案进行详细的说明：

(1)在人体足底的前脚掌和后脚跟安装压力传感器，实时采集运动时前脚掌与后脚跟的压力值，将MIMU固定在人体脚部的踝关节处，采集运动过程中的加速度和角速度信息；

在导航开始之前，将足底压力传感器与MIMU安装在准确位置上，根据人体行走时脚掌所受压力区间主要为前脚掌和后脚跟两个部位的特点，本发明分别在人体足底前脚掌和后脚跟处各安装一个足底压力传感器，安装方法如图1所示，其中B处为前脚掌安装位置，A处为后脚跟安装位置。图2所示c处为本发明中MIMU的具体安装位置，在人体脚踝上方安装MIMU以此减小脚面运动对MIMU的影响。

(2)根据人体步态相位的分析，结合每个离散时刻足底压力值、加速度值、角速度值及噪声特性设定判定为静止区间的上、下阈值；

结合行走过程中的步态相位与足底受力分析设定足压的判断阈值，其中步态相位分为支撑相位和摆动相位(如图3所示)，图中D为支撑相位，E为摆动相位，支撑相位又分为支撑前期、支撑中期和支撑后期；由步态相位与足底受力情况内在联系，可知行人处于静止区间时其支撑脚所受的力为人体总质量，结合图3以右脚为研究对象，当处于静止区间时，为单脚支撑期即支撑前期，则右脚承受人体总质量。根据人体体重和单位窗口内的足底平均压力值设定阈值F_V；根据理想情况下静止区间内脚部的合加速度大小为G(重力加速度)且方向垂直地面向下这一特点，设定加速度幅值的判断阈值为[G_V1,G_V2]；根据单位窗口内平均加速度幅值与噪声特性设定加速度幅值标准差的判断阈值为G_V3；根据单位窗口内角速度幅值与噪声特性设定角速度幅值的判断阈值为ω_V。

(3)根据足底压力变化与步态变化的内在联系，结合足底压力传感器、加速度计和陀螺仪设定若干个判定脚掌全部落地即静止区间的条件，并根据这些条件所进行的与操作，最终判定出静止区间。

首先，设定检测静止区间的4个判断条件分别为C₁、C₂、C₃和C₄。当行人处于静止区间内，其脚掌完全贴地，前后掌均受到地面反作用力且支撑脚所受力为人体自重，因此，设定判断条件C₁为

F₁＝f₁＞0&f₂＞0

F₂＝f₁+f₂

式中，f₁为前脚掌所受力，f₂为后脚跟所受力，F₁表示前脚掌与后脚跟均受力时为真，否则为假，F₂为前后掌总受力。判断条件C₂为通过加速度模值进行判定，如下

式中，a_kx、a_ky、a_kz分别为加速度沿三轴方向上的分量，|a_k|为加速度模值。判断条件C₃根据加速度矢量的幅值方差进行判定

是k时刻的平均加速度幅值，s为平滑窗口长度，a_j为加速度采样点，则k时刻加速度的幅值方差为

式中，δ(a_k)为k时刻加速度的幅值标准差，G_V3为判定阈值。利用角速度幅值设定判断条件C₄，角速度幅值表示为

ω_kx、ω_ky、ω_kz分别表示角速度在三轴方向上的分量，|ω_k|为角速度模值，则判断条件C₄表达式为

对C₁、C₂、C₃和C₄条件进行与操作，最终的静止区间的判定条件为：C＝C₁&C₂&C₃&C₄，当四个条件都满足时，C记录为1，表示检测到静止区间，否则记录为0，表示脚部处于运动状态。

(4)将导航实验过程中采集到的足压数据、加速度数据以及角速度数据通过所述步骤(3)中提出的4个判断条件C₁、C₂、C₃和C₄对静止区间进行检测。

在检测出的静止区间内触发卡尔曼滤波器，将静止区间的运动速度视为零，则MIMU在静止区间输出的速度值即为MIMU的误差漂移量，以该时刻MIMU所测得的速度值作为量测量通过卡尔曼滤波器对其他误差参数进行估计并修正，其系统框图如图4所示。

卡尔曼滤波离散化后的状态方程和量测方程分别为：

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化系统观测矩阵；φ_k,k-1为离散化状态转移矩阵；H_k为离散化系统量测矩阵；W_k-1和V_k分别为离散化系统状态噪声向量和量测噪声向量；

K时刻的状态一步预测值X_k,k-1为：

X_k,k-1＝φ_k,k-1X_k-1

滤波增益K_k为：

k时刻状态估计值X_k为：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

一步预测均方误差P_k,k-1为：

P_k，k-1＝φ_k，k-1P_k-1，k-1φ_k，k-1 ^T+Q_k-1

估计均方误差协方差矩阵P_k，k为：

式中，P_k-1，k-1表示k-1时刻的均方误差，Q_k-1表示系统噪声协方差矩阵，R_k表示量测噪声协方差矩阵，I表示单位矩阵；

通过以静止区间的速度作为量测值，利用卡尔曼滤波估计出位置、速度和姿态的状态误差后，对位置、速度和姿态信息进行补偿得到更为精确的定位信息。

上述给出了本发明的一个具体实施方案，本发明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

上面结合附图对本发明的实施方式作了说明，但本发明并不局限于上述实施方式，在本领域的技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (2)

1.一种行人导航中基于足底压力检测的零速修正方法，其特征在于：包括以下步骤：

1)在人体足底的前脚掌和后脚跟安装压力传感器，实时采集运动时前脚掌与后脚跟的压力值，将微型惯性测量单元即MIMU固定在人体脚部的踝关节上方，采集运动过程中的加速度和角速度信息，同时减少人体行走状态对MIMU的影响；

2)根据人体步态相位的分析，结合每个离散时刻足底压力值、加速度值、角速度值以及噪声特性设定静止区间的上、下阈值；

所述步骤2)中的步态相位为行走时的支撑相位和摆动相位，由步态相位与足底受力情况内在联系的分析，可知行人处于静止区间时其支撑脚所受的力为人体总质量，根据人体体重和单位窗口内的足底平均压力值设定阈值F_V；根据理想情况下静止区间内脚部的合加速度大小为G即重力加速度，且方向垂直地面向下这一特点，设定加速度幅值的判断阈值为[G_V1,G_V2]；根据单位窗口内平均加速度幅值与噪声特性设定加速度幅值标准差的判断阈值为G_V3；根据单位窗口内角速度幅值与噪声特性设定角速度幅值的判断阈值为ω_V；

3)根据足底压力变化与步态变化的内在联系，结合足底压力传感器、加速度计和陀螺仪分别设定判定脚掌全部落地即静止区间的条件，并根据这些条件所进行的与操作，最终判定出静止区间；

所述步骤3)中，设定检测静止区间的4个判断条件分别为C₁、C₂、C₃和C₄；当行人处于静止区间内，脚掌完全贴地，前后掌均受到地面反作用力且支撑脚所受力为人体自重，因此，设定判断条件C₁为

F₁＝f₁＞0&f₂＞0

F₂＝f₁+f₂

式中，f₁为前脚掌所受力，f₂为后脚跟所受力，F₁表示前脚掌与后脚跟均受力时为真，否则为假，F₂为前后掌总受力；判断条件C₂为通过加速度模值进行判定，如下

式中，a_kx、a_ky、a_kz分别为加速度沿三轴方向上的分量，|a_k|为加速度模值；判断条件C₃根据加速度矢量的幅值方差进行判定

是k时刻的平均加速度幅值，s为平滑窗口长度，a_j为加速度采样点，则k时刻加速度的幅值方差为

式中，δ(a_k)为k时刻加速度的幅值标准差，G_V3为加速度幅值标准差的判断阈值；利用角速度幅值设定判断条件C₄，角速度幅值表示为

ω_kx、ω_ky、ω_kz分别表示角速度在三轴方向上的分量，|ω_k|为角速度模值，则判断条件C₄表达式为

对C₁、C₂、C₃和C₄条件进行与操作，最终的静止区间的判定条件为：C＝C₁&C₂&C₃&C₄，当四个条件都满足时，C记录为1，表示检测到静止区间，否则记录为0，表示脚部处于运动状态；

4)零速修正在静止区间内触发卡尔曼滤波器，在检测出的静止区间内人体脚部的运动速度视为零，以此时MIMU所输出的速度值作为卡尔曼滤波器的量测量，利用卡尔曼滤波估计更多的误差参数，从而修正基于MIMU的惯性行人导航系统中的速度误差、位置误差和姿态误差。

2.根据权利要求1所述的一种行人导航中基于足底压力检测的零速修正方法，其特征在于：所述步骤4)中，利用步骤2)、3)中的静态区间检测方法检测出高动态下行人导航中的静态区间，将此时刻的运动速度视为零，MIMU在静止区间输出的速度值即为MIMU的误差漂移量，以该时刻MIMU所测得的速度值作为量测量通过卡尔曼滤波对其他误差参数进行估计并修正；

卡尔曼滤波离散化后的状态方程和量测方程分别为：

式中，X_k、X_k-1分别表示k时刻、k-1时刻的状态估计；Z_k为离散化系统观测矩阵；φ_k,k-1为离散化状态转移矩阵；H_k为离散化系统量测矩阵；W_k-1和V_k分别为离散化系统状态噪声向量和量测噪声向量；

K时刻的状态一步预测值X_k,k-1为：

X_k,k-1＝φ_k,k-1X_k-1

滤波增益K_k为：

k时刻状态估计值X_k为：

X_k＝X_k,k-1+K_k(Z_k-H_kX_k,k-1)

一步预测均方误差P_k,k-1为：

P_k，k-1＝φ_k，k-1P_k-1，k-1φ_k，k-1 ^T+Q_k-1

估计均方误差协方差矩阵P_k,k为：

式中，P_k-1,k-1表示k-1时刻的均方误差，Q_k-1表示系统噪声协方差矩阵，R_k表示量测噪声协方差矩阵，I表示单位矩阵；

通过以静止区间的速度作为量测值，利用卡尔曼滤波估计出位置、速度和姿态的状态误差后，对位置、速度和姿态信息进行补偿得到更为精确的定位信息。