一种基于脚部和腿部微惯性测量单元的行人定位方法

1.本发明属于行人惯性导航领域，具体涉及一种基于脚部和腿部微惯性测量单元的行人定位方法。


背景技术：

2.近年来，医疗界大力推进以便携式智能设备为核心的阿尔兹海默症康复训练方法，目的是实现对阿尔兹海默症患者的位姿状态进行实时监测，并对患者治疗效果与病情发展情况进行合理评估。提升设备佩戴者位姿信息的测量精确度，以采取合适的方法治疗阿尔兹海默症患者，对保障我国老年群体的健康生活有重要意义。
3.随着行人惯性导航系统等便携式智能设备的不断发展，其在应急救援、军事、人体运动、步态分析和康复等领域得到了广泛应用。然而，传统行人惯性导航系统的位置、速度误差发散速度快，特别是在使用低成本，高噪声的磁力计和惯性测量单元的情况下，往往很难获得较高精度的定位信息。为了获得高精度的定位信息，提高行人惯性导航系统的测量精度是当前研究的热点，而如何在运动状态复杂的行走过程中及时修正惯性导航系统的测量误差是解决该问题的关键。
4.专利号为cn111307148a的发明中提及一种基于惯性网络的行人定位方法，基于行人行走过程中，其腰部和足部具有不同的运动特征，将惯性传感器分别安装于人体的腰部和足部分别敏感其运行信息，整合利用人体不同部位的运动特征，利用行人行走过程中腰部和足部之间特有的角度和距离约束关系，基于不等式约束卡尔曼滤波方法，抑制行人航迹推算方法中的航向角和距离测量误差，从而实现更精确的行人定位。但是该发明没有涉及腿部和脚部的步态区分，以及在此基础上的定位方法。
5.专利号为cn112066980a的发明中提及一种基于人体四节点运动约束的行人导航定位方法，该发明解决了变速/剧烈/快速运动下零速状态缺失时，导航系统累积误差难以抑制的问题，具有实时性好，运算量小的优势，提高了零速检测精度。但不涉及行人正常行走时时的惯性导航系统的定位精度提升问题。
6.专利号为cn112362057a的发明中提及一种基于零速修正与姿态自观测的惯性行人导航算法，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。同样的，该发明没有涉及腿部和脚部的步态区分，以及在此基础上的定位方法。


技术实现要素：

7.解决的技术问题：本发明针对单个惯性测量单元精度不足，提供了一种基于多个组合微惯性测量单元信息融合的行人定位方法，本发明所提出的行人定位方法适用于医疗领域的实时步态分析，提高了设备佩戴者的定位精度，能够获得较为准确的佩戴者实时定位信息，推动了医疗领域相关疾病诊断的智能化、精准化。
8.技术方案：一种基于脚部和腿部微惯性测量单元的行人定位方法，所述行人定位方法包括以下步骤：s1，在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；根据相应的运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断；s2，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程；s3，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果。
9.进一步地，步骤s1中，所述相应的运动学特性是指：摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作；当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。
10.进一步地，步骤s1中，采用阈值法进行步态相位判断的过程包括以下步骤：对惯性测量单元信号进行包括平滑、滤波和归一化在内的预处理；采用滑动窗口对预处理后的数据进行分割，对单个窗口提取运动学特征；结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
11.进一步地，步骤s2中，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：结合动态三点几何约束推导得到位置约束方程：；其中，表示传感器i坐标系，n坐标系为导航坐标系，其原点表示为o；为从i指向j的向量，向量是在n坐标系下定义的；为n坐标系中的第i个向量，为传感器坐标系下的第i个向量，表示从传感器坐标系到n坐标系的旋转矩阵；和分别为三阶识别矩阵和零矩阵；根据站立时的零速校正公式：；得到站立时的速度校正方程为：
；其中为n坐标系中第i个传感器中的速度；根据摆动前后阶段的运动特征公式：；得到摆动前后阶段的速度校正方程为：。
12.进一步地，步骤s3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程。
13.进一步地，所述信息融合系统的离散状态空间表示为：；；其中，为k时刻的状态向量，为k时刻系统输入，为k时刻观察向量，和分别为k时刻过程噪声和测量噪声，为k时刻状态转移矩阵，为k时刻观测矩阵；选择状态向量为：；其中，和分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：；
其中，t为采样周期，为k时刻下坐标系中点的加速度的投影，为重力加速度；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：站立、摆动前和摆动后阶段的观测方程为：； ； ；摆动阶段的观测方程为： 。
14.进一步地，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。
15.有益效果：本发明将步态从两个相位重新定义为四个相位，并采用多阈值法进行步态相位判断，有效提高了步态判断的精确性，避免因步态相位判断模糊影响定位测量的精度；本发明提出的行人定位方法通过动态三点几何约束构建位置约束方程，通过零速更新方法与相关运动学特征构建速度约束方程，能够对误差进行实时修正，将误差限制在一定范围内，有效减小了惯性测量单元的测量误差，提高了定位精度；并且能够结合互补滤波器与卡尔曼滤波器进行信息融合，通过不同滤波器之间的频域互补特性去除噪声信号，从而进一步抑制了测量误差，提高了定位的精确度。
附图说明
16.图1为本发明提供的基于脚部和腿部微惯性测量单元的行人定位方法流程图；图2 为基于互补滤波器与卡尔曼滤波器的信息融合方法融合框架图；图3 为本发明提供的一种基于脚部和腿部微惯性测量单元的行人定位方法的相关硬件结构图；图4 为脚尖-脚跟-小腿微惯性测量单元的二维行人轨迹导航定位结果图；图5 为脚尖-脚跟-小腿微惯性测量单元的三维行人轨迹导航定位结果图；图6为脚尖-脚跟-小腿微惯性测量单元的安装位置示意图。
具体实施方式
17.下面的实施例可使本专业技术人员更全面地理解本发明，但不以任何方式限制本发明。
18.如图1所示，本发明提出了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，包括运动步态阶段划分与步态相位判断，基于动态几何约束和科里奥利理论以构建步态四个连续阶段的速度和位置约束，基于互补滤波器与卡尔曼滤波器的信息融合方法获得各运动阶段的高精度观测方程。
19.相应的，本发明提供的一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法硬件结构图如图3所示，包括惯性测量单元、主机、信号融合模块，各模块之间采用can总线进行连接，以确保通信的稳定性，其中：三个惯性测量单元由三个分别被安装在设备佩戴者脚尖、脚跟和小腿处的微惯性测量单元组成，传感器网络由三个测量单元和一台主机组成，惯性测量单元的型号为低成本的icm20948，测量单元和主机的cpu分别为stm32f4和imx6ull。信号融合模块由三种不同的传感器对输入信号进行采样，形成了互补滤波器，再通过卡尔曼滤波器输出各运动状态的观测方程。其中加速度计、陀螺仪和磁力计的采样频率可以分别设置为100hz、100hz和50hz。
20.根据各运动状态的观测方程获得行人导航定位结果。图4为二维显示界面下，行人轨迹的导航定位结果；图5为三维显示界面下，行人轨迹的导航定位结果；图6表明了3个微惯性测量单元在脚尖、脚跟、小腿处的具体安装位置。
21.该行人定位方法的具体流程如下：步骤一、运动步态阶段划分与步态相位判断在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，传感器布局如图4。依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。根据以上运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断，即结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
22.为更准确地提取步态切换时的运动与加速度信息，对惯性测量单元信号进行平滑、滤波、归一化等预处理。为控制数据误差、保证数据的实时性，使用尽可能小的、平滑的滤波窗进行处理，然后采用滑动窗口对数据进行分割，对单个窗口提取运动学特征。
23.步骤二、基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束为了减小各运动状态下的位姿测量误差，根据3个惯性测量单元的空间位置信息构建位置约束方程。根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，从而实现周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程。
24.动态三点几何约束条件，是通过对分别安装在脚尖、脚跟、小腿三处的微惯性测量单元进行空间位置建模，以运动状态下人骨骼相对固定的位置关系构成三点处的几何位置约束，形成动态三点几何约束条件。
25.基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是根据脚在站立、摆动前和摆动后阶段接触地面，此时脚掌的速度为零，因此可以使用科里奥利定理。科里奥利定理的方程形式为：；其中和分别为向量在k坐标系和m坐标系中的速度。为k坐标系对m坐标系的相对角速度，可以利用系统的科里奥利定理的方程，结合动态三点几何约束推导得到的位置约束方程，推断各运动阶段的速度校正模型。
26.根据动态三点几何约束公式：；；；位置约束方程为：；其中表示传感器i坐标系，黑点表示传感器坐标系原点，n坐标系为导航坐标系，其原点表示为o；为从i指向j的向量，向量是在n坐标系下定义的。设为n坐标系中的第i个向量，为传感器坐标系下的第i个向量，表示从传感器坐标系到n坐标系的旋转矩阵；和分别为三阶识别矩阵和零矩阵。
27.根据站立时的零速校正公式：；站立时的速度校正方程为：；其中为n坐标系中第i个传感器中的速度。
28.根据摆动前后阶段的运动特征公式：；摆动前后阶段的速度校正方程为：。
29.步骤三、基于互补滤波器与卡尔曼滤波器的信息融合方法基于互补滤波器与卡尔曼滤波器的信息融合方法采用的融合框架图如图2所示。将修正后的传感器值通过互补滤波器，以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程即可得到各运动状态修正后的定位结果。
30.用于实现信息融合方法的信息融合系统被分为两个模块。第一个模块为基于互补滤波器的分散式姿态估计，第二个模块为集中处理速度和位置估计值的线性卡尔曼滤波器。在互补滤波器构成的第一模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值。第二模块中，卡尔曼滤波器根据第一模块的最优估计值输出各运动阶段的观测方程，由各运动阶段的观测方程即可获得经过误差修正的高精度导航系统定位结果。
31.该信息融合系统为线性高斯系统，对于线性高斯系统，离散状态空间可以表示为：；；其中，为k时刻的状态向量，为系统输入，为观察向量，和分别为过程噪声和测量噪声，为状态转移矩阵，为观测矩阵。选择状态向量为：；其中，和分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：；其中，t为采样周期，为k时刻下坐标系中点的加速度的投影，g为重力加速度。根据理论测量方法并结合系统的状态转移方程，可以得到各运动阶段的观测方程。
32.站立、摆动前和摆动后阶段的观测方程为： ； ； ；摆动阶段的观测方程为： 。
33.本发明公开了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，该方法包括将步态划分为四个阶段，采用多阈值法对步态相位进行判断，再针对每个步态阶段构建精确的运动学模型，建立基于科里奥利理论的速度校正模型以抑制速度漂移，适应不同的步行速度。然后采用动态三点几何约束以增强系统的定位精度和鲁棒性，并设计了基于互补滤波器的分布式姿态估计。最后将姿态估计值通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计。其中，将步态划分为四个阶段，采用多阈值法对步态相位进行判断，是在脚部和小腿假定为两个刚性段的基础上，将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段的运动学特征是以脚尖为支点的摆动动作，摆动后阶段的运动学特征是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处；当系统处于摆动后阶段时，脚跟处的加速度则小于脚尖处。根据这一运动学特性以及加速度和角速度的统计学特性，可以采用阈值法进行步态相位判断；基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是通过建立科里奥利旋转坐标系，对四个连续阶段的运动偏移量进行合理推算，得到速度与位置约束方程。再应用坐标系下的速度与位置约束方程，针对每个步态阶
段构建精确的运动学模型，并建立基于科里奥利理论的速度校正模型以抑制速度漂移；基于互补滤波器的分散式姿态估计，是通过加速度计、陀螺仪等不同特性的传感器对惯性测量单元进行采样。在姿态角估计的过程中，短时间内采用陀螺仪得到的角度作为最优估计值，定时对加速度采样得到的角度取平均值来校正陀螺仪得到的角度，以取长补短的方式消除测量过程中的误差；通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计，是通过最小方差估计法，将信号中的误差减少到最小；所述系统包括步态运动学建模模块、基于科里奥利理论的速度校正模块、信息融合模块，各模块之间采用有线或无线传输方式。本发明所提出的基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法可应用于应急救援、军事、人体运动、步态分析和康复等领域，能够有效提高行人惯性导航系统的定位精度。