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

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

Info

Publication number
CN114440883B
CN114440883B CN202210367129.0A CN202210367129A CN114440883B CN 114440883 B CN114440883 B CN 114440883B CN 202210367129 A CN202210367129 A CN 202210367129A CN 114440883 B CN114440883 B CN 114440883B
Authority
CN
China
Prior art keywords
coordinate system
sensor
vector
swing
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210367129.0A
Other languages
English (en)
Other versions
CN114440883A (zh
Inventor
宋治儒
钱伟行
万游
周学成
方铭宇
刘童
沈皓轩
洪越
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Normal University
Original Assignee
Nanjing Normal University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Normal University filed Critical Nanjing Normal University
Priority to CN202210367129.0A priority Critical patent/CN114440883B/zh
Publication of CN114440883A publication Critical patent/CN114440883A/zh
Application granted granted Critical
Publication of CN114440883B publication Critical patent/CN114440883B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/103—Detecting, measuring or recording devices for testing the shape, pattern, colour, size or movement of the body or parts thereof, for diagnostic purposes
    • A61B5/11—Measuring movement of the entire body or parts thereof, e.g. head or hand tremor, mobility of a limb
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/103—Detecting, measuring or recording devices for testing the shape, pattern, colour, size or movement of the body or parts thereof, for diagnostic purposes
    • A61B5/11—Measuring movement of the entire body or parts thereof, e.g. head or hand tremor, mobility of a limb
    • A61B5/112—Gait analysis
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Pathology (AREA)
  • Medical Informatics (AREA)
  • Dentistry (AREA)
  • Biophysics (AREA)
  • Physiology (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Oral & Maxillofacial Surgery (AREA)
  • Molecular Biology (AREA)
  • Surgery (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Navigation (AREA)
  • Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)

Abstract

本发明公开了一种基于脚部和腿部微惯性测量单元的行人定位方法，包括：依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；采用阈值法进行步态相位判断；构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，构建各运动状态下的速度约束方程；将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果。本发明提高了设备佩戴者的定位精度，能够获得较为准确的佩戴者实时定位信息。

Description

一种基于脚部和腿部微惯性测量单元的行人定位方法
技术领域
本发明属于行人惯性导航领域，具体涉及一种基于脚部和腿部微惯性测量单元的行人定位方法。
背景技术
近年来，医疗界大力推进以便携式智能设备为核心的阿尔兹海默症康复训练方法，目的是实现对阿尔兹海默症患者的位姿状态进行实时监测，并对患者治疗效果与病情发展情况进行合理评估。提升设备佩戴者位姿信息的测量精确度，以采取合适的方法治疗阿尔兹海默症患者，对保障我国老年群体的健康生活有重要意义。
随着行人惯性导航系统等便携式智能设备的不断发展，其在应急救援、军事、人体运动、步态分析和康复等领域得到了广泛应用。然而，传统行人惯性导航系统的位置、速度误差发散速度快，特别是在使用低成本，高噪声的磁力计和惯性测量单元的情况下，往往很难获得较高精度的定位信息。为了获得高精度的定位信息，提高行人惯性导航系统的测量精度是当前研究的热点，而如何在运动状态复杂的行走过程中及时修正惯性导航系统的测量误差是解决该问题的关键。
专利号为CN111307148A的发明中提及一种基于惯性网络的行人定位方法，基于行人行走过程中，其腰部和足部具有不同的运动特征，将惯性传感器分别安装于人体的腰部和足部分别敏感其运行信息，整合利用人体不同部位的运动特征，利用行人行走过程中腰部和足部之间特有的角度和距离约束关系，基于不等式约束卡尔曼滤波方法，抑制行人航迹推算方法中的航向角和距离测量误差，从而实现更精确的行人定位。但是该发明没有涉及腿部和脚部的步态区分，以及在此基础上的定位方法。
专利号为CN112066980A的发明中提及一种基于人体四节点运动约束的行人导航定位方法，该发明解决了变速/剧烈/快速运动下零速状态缺失时，导航系统累积误差难以抑制的问题，具有实时性好，运算量小的优势，提高了零速检测精度。但不涉及行人正常行走时时的惯性导航系统的定位精度提升问题。
专利号为CN112362057A的发明中提及一种基于零速修正与姿态自观测的惯性行人导航算法，对采集的数据同时进行零速检测，检测出行走时的零速区间，设计卡尔曼滤波器，在零速区间内运用零速修正与姿态自观测算法估计出导航姿态、位置以及速度误差，并对惯导解算结果进行误差修正，从而实现对行人的精确定位与导航。同样的，该发明没有涉及腿部和脚部的步态区分，以及在此基础上的定位方法。
发明内容
解决的技术问题：本发明针对单个惯性测量单元精度不足，提供了一种基于多个组合微惯性测量单元信息融合的行人定位方法，本发明所提出的行人定位方法适用于医疗领域的实时步态分析，提高了设备佩戴者的定位精度，能够获得较为准确的佩戴者实时定位信息，推动了医疗领域相关疾病诊断的智能化、精准化。
技术方案：
一种基于脚部和腿部微惯性测量单元的行人定位方法，所述行人定位方法包括以下步骤：
S1，在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；根据相应的运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断；
S2，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程；
S3，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果。
进一步地，步骤S1中，所述相应的运动学特性是指：
摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作；当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。
进一步地，步骤S1中，采用阈值法进行步态相位判断的过程包括以下步骤：
对惯性测量单元信号进行包括平滑、滤波和归一化在内的预处理；
采用滑动窗口对预处理后的数据进行分割，对单个窗口提取运动学特征；
结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
进一步地，步骤S2中，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：
结合动态三点几何约束推导得到位置约束方程：
Figure DEST_PATH_IMAGE002
；
其中，
Figure DEST_PATH_IMAGE004
表示传感器i坐标系，n坐标系为导航坐标系，其原点表示为O；
Figure DEST_PATH_IMAGE006
为从i指向j的向量，向量是在n坐标系下定义的；
Figure DEST_PATH_IMAGE008
为n坐标系中的第i个向量，
Figure DEST_PATH_IMAGE010
为
Figure DEST_PATH_IMAGE012
传感器坐标系下的第i个向量，
Figure DEST_PATH_IMAGE014
表示从
Figure 878994DEST_PATH_IMAGE012
传感器坐标系到n坐标系的旋转矩阵；
Figure DEST_PATH_IMAGE016
和
Figure DEST_PATH_IMAGE018
分别为三阶识别矩阵和零矩阵；
根据站立时的零速校正公式：
Figure DEST_PATH_IMAGE020
；
得到站立时的速度校正方程为：
Figure DEST_PATH_IMAGE022
；
其中
Figure DEST_PATH_IMAGE024
为n坐标系中第i个传感器中的速度；
根据摆动前后阶段的运动特征公式：
Figure DEST_PATH_IMAGE026
；
得到摆动前后阶段的速度校正方程为：
Figure DEST_PATH_IMAGE028
。
进一步地，步骤S3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：
构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量
Figure DEST_PATH_IMAGE030
，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程。
进一步地，所述信息融合系统的离散状态空间表示为：
Figure DEST_PATH_IMAGE032
；
Figure DEST_PATH_IMAGE034
；
其中，
Figure DEST_PATH_IMAGE036
为k时刻的状态向量，
Figure DEST_PATH_IMAGE038
为k时刻系统输入，
Figure DEST_PATH_IMAGE040
为k时刻观察向量，
Figure DEST_PATH_IMAGE042
和
Figure DEST_PATH_IMAGE044
分别为k时刻过程噪声和测量噪声，
Figure DEST_PATH_IMAGE046
为k时刻状态转移矩阵，
Figure DEST_PATH_IMAGE048
为k时刻观测矩阵；选择状态向量为：
Figure DEST_PATH_IMAGE050
；
其中，
Figure DEST_PATH_IMAGE052
和
Figure DEST_PATH_IMAGE054
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
Figure DEST_PATH_IMAGE056
；
其中，T为采样周期，
Figure DEST_PATH_IMAGE058
为k时刻下
Figure 40549DEST_PATH_IMAGE012
坐标系中
Figure 33913DEST_PATH_IMAGE012
点的加速度的投影，
Figure DEST_PATH_IMAGE060
为重力加速度；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：
站立、摆动前和摆动后阶段的观测方程为：
Figure DEST_PATH_IMAGE062
；
Figure DEST_PATH_IMAGE064
+
Figure DEST_PATH_IMAGE066
；
Figure DEST_PATH_IMAGE068
+
Figure 663608DEST_PATH_IMAGE066
；
摆动阶段的观测方程为：
Figure DEST_PATH_IMAGE070
+
Figure 400620DEST_PATH_IMAGE066
。
进一步地，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。
有益效果：
本发明将步态从两个相位重新定义为四个相位，并采用多阈值法进行步态相位判断，有效提高了步态判断的精确性，避免因步态相位判断模糊影响定位测量的精度；本发明提出的行人定位方法通过动态三点几何约束构建位置约束方程，通过零速更新方法与相关运动学特征构建速度约束方程，能够对误差进行实时修正，将误差限制在一定范围内，有效减小了惯性测量单元的测量误差，提高了定位精度；并且能够结合互补滤波器与卡尔曼滤波器进行信息融合，通过不同滤波器之间的频域互补特性去除噪声信号，从而进一步抑制了测量误差，提高了定位的精确度。
附图说明
图1为本发明提供的基于脚部和腿部微惯性测量单元的行人定位方法流程图；
图2 为基于互补滤波器与卡尔曼滤波器的信息融合方法融合框架图；
图3 为本发明提供的一种基于脚部和腿部微惯性测量单元的行人定位方法的相关硬件结构图；
图4 为脚尖-脚跟-小腿微惯性测量单元的二维行人轨迹导航定位结果图；
图5 为脚尖-脚跟-小腿微惯性测量单元的三维行人轨迹导航定位结果图；
图6为脚尖-脚跟-小腿微惯性测量单元的安装位置示意图。
具体实施方式
下面的实施例可使本专业技术人员更全面地理解本发明，但不以任何方式限制本发明。
如图1所示，本发明提出了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，包括运动步态阶段划分与步态相位判断，基于动态几何约束和科里奥利理论以构建步态四个连续阶段的速度和位置约束，基于互补滤波器与卡尔曼滤波器的信息融合方法获得各运动阶段的高精度观测方程。
相应的，本发明提供的一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法硬件结构图如图3所示，包括惯性测量单元、主机、信号融合模块，各模块之间采用CAN总线进行连接，以确保通信的稳定性，其中：三个惯性测量单元由三个分别被安装在设备佩戴者脚尖、脚跟和小腿处的微惯性测量单元组成，传感器网络由三个测量单元和一台主机组成，惯性测量单元的型号为低成本的ICM20948，测量单元和主机的CPU分别为STM32F4和IMX6ULL。信号融合模块由三种不同的传感器对输入信号进行采样，形成了互补滤波器，再通过卡尔曼滤波器输出各运动状态的观测方程。其中加速度计、陀螺仪和磁力计的采样频率可以分别设置为100HZ、100HZ和50HZ。
根据各运动状态的观测方程获得行人导航定位结果。图4为二维显示界面下，行人轨迹的导航定位结果；图5为三维显示界面下，行人轨迹的导航定位结果；图6表明了3个微惯性测量单元在脚尖、脚跟、小腿处的具体安装位置。
该行人定位方法的具体流程如下：
步骤一、运动步态阶段划分与步态相位判断
在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，传感器布局如图4。依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。根据以上运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断，即结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
为更准确地提取步态切换时的运动与加速度信息，对惯性测量单元信号进行平滑、滤波、归一化等预处理。为控制数据误差、保证数据的实时性，使用尽可能小的、平滑的滤波窗进行处理，然后采用滑动窗口对数据进行分割，对单个窗口提取运动学特征。
步骤二、基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束
为了减小各运动状态下的位姿测量误差，根据3个惯性测量单元的空间位置信息构建位置约束方程。根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，从而实现周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程。
动态三点几何约束条件，是通过对分别安装在脚尖、脚跟、小腿三处的微惯性测量单元进行空间位置建模，以运动状态下人骨骼相对固定的位置关系构成三点处的几何位置约束，形成动态三点几何约束条件。
基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是根据脚在站立、摆动前和摆动后阶段接触地面，此时脚掌的速度为零，因此可以使用科里奥利定理。科里奥利定理的方程形式为：
Figure DEST_PATH_IMAGE072
；
其中
Figure DEST_PATH_IMAGE074
和
Figure DEST_PATH_IMAGE076
分别为向量
Figure DEST_PATH_IMAGE078
在k坐标系和m坐标系中的速度。
Figure DEST_PATH_IMAGE080
为k坐标系对m坐标系的相对角速度，可以利用系统的科里奥利定理的方程，结合动态三点几何约束推导得到的位置约束方程，推断各运动阶段的速度校正模型。
根据动态三点几何约束公式：
Figure DEST_PATH_IMAGE082
；
Figure DEST_PATH_IMAGE084
；
Figure DEST_PATH_IMAGE086
；
位置约束方程为：
Figure DEST_PATH_IMAGE088
；
其中
Figure DEST_PATH_IMAGE090
表示传感器i坐标系，黑点表示传感器坐标系原点，n坐标系为导航坐标系，其原点表示为O；
Figure 173273DEST_PATH_IMAGE078
为从i指向j的向量，向量是在n坐标系下定义的。设
Figure DEST_PATH_IMAGE092
为n坐标系中的第i个向量，
Figure DEST_PATH_IMAGE094
为
Figure DEST_PATH_IMAGE096
传感器坐标系下的第i个向量，
Figure DEST_PATH_IMAGE098
表示从
Figure 665434DEST_PATH_IMAGE096
传感器坐标系到n坐标系的旋转矩阵；
Figure DEST_PATH_IMAGE100
和
Figure DEST_PATH_IMAGE102
分别为三阶识别矩阵和零矩阵。
根据站立时的零速校正公式：
Figure DEST_PATH_IMAGE104
；
站立时的速度校正方程为：
Figure DEST_PATH_IMAGE106
；
其中
Figure DEST_PATH_IMAGE108
为n坐标系中第i个传感器中的速度。
根据摆动前后阶段的运动特征公式：
Figure DEST_PATH_IMAGE110
；
摆动前后阶段的速度校正方程为：
Figure DEST_PATH_IMAGE112
。
步骤三、基于互补滤波器与卡尔曼滤波器的信息融合方法
基于互补滤波器与卡尔曼滤波器的信息融合方法采用的融合框架图如图2所示。将修正后的传感器值通过互补滤波器，以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程即可得到各运动状态修正后的定位结果。
用于实现信息融合方法的信息融合系统被分为两个模块。第一个模块为基于互补滤波器的分散式姿态估计，第二个模块为集中处理速度和位置估计值的线性卡尔曼滤波器。在互补滤波器构成的第一模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量
Figure DEST_PATH_IMAGE114
，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值。第二模块中，卡尔曼滤波器根据第一模块的最优估计值输出各运动阶段的观测方程，由各运动阶段的观测方程即可获得经过误差修正的高精度导航系统定位结果。
该信息融合系统为线性高斯系统，对于线性高斯系统，离散状态空间可以表示为：
Figure DEST_PATH_IMAGE116
；
Figure DEST_PATH_IMAGE118
；
其中，
Figure 94010DEST_PATH_IMAGE036
为k时刻的状态向量，
Figure 369134DEST_PATH_IMAGE038
为系统输入，
Figure 74922DEST_PATH_IMAGE040
为观察向量，
Figure 410088DEST_PATH_IMAGE042
和
Figure 139010DEST_PATH_IMAGE044
分别为过程噪声和测量噪声，
Figure 529409DEST_PATH_IMAGE046
为状态转移矩阵，
Figure 27386DEST_PATH_IMAGE048
为观测矩阵。选择状态向量为：
Figure DEST_PATH_IMAGE120
；
其中，
Figure DEST_PATH_IMAGE122
和
Figure DEST_PATH_IMAGE124
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
Figure DEST_PATH_IMAGE126
；
其中，T为采样周期，
Figure DEST_PATH_IMAGE128
为k时刻下
Figure 674399DEST_PATH_IMAGE096
坐标系中
Figure 625038DEST_PATH_IMAGE096
点的加速度的投影，g为重力加速度。根据理论测量方法并结合系统的状态转移方程，可以得到各运动阶段的观测方程。
站立、摆动前和摆动后阶段的观测方程为：
Figure DEST_PATH_IMAGE130
+
Figure 507543DEST_PATH_IMAGE044
；
Figure DEST_PATH_IMAGE132
+
Figure 922344DEST_PATH_IMAGE044
；
Figure DEST_PATH_IMAGE134
+
Figure 864892DEST_PATH_IMAGE044
；
摆动阶段的观测方程为：
Figure DEST_PATH_IMAGE136
+
Figure 620271DEST_PATH_IMAGE044
。
本发明公开了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，该方法包括将步态划分为四个阶段，采用多阈值法对步态相位进行判断，再针对每个步态阶段构建精确的运动学模型，建立基于科里奥利理论的速度校正模型以抑制速度漂移，适应不同的步行速度。然后采用动态三点几何约束以增强系统的定位精度和鲁棒性，并设计了基于互补滤波器的分布式姿态估计。最后将姿态估计值通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计。其中，将步态划分为四个阶段，采用多阈值法对步态相位进行判断，是在脚部和小腿假定为两个刚性段的基础上，将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段的运动学特征是以脚尖为支点的摆动动作，摆动后阶段的运动学特征是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处；当系统处于摆动后阶段时，脚跟处的加速度则小于脚尖处。根据这一运动学特性以及加速度和角速度的统计学特性，可以采用阈值法进行步态相位判断；基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是通过建立科里奥利旋转坐标系，对四个连续阶段的运动偏移量进行合理推算，得到速度与位置约束方程。再应用坐标系下的速度与位置约束方程，针对每个步态阶段构建精确的运动学模型，并建立基于科里奥利理论的速度校正模型以抑制速度漂移；基于互补滤波器的分散式姿态估计，是通过加速度计、陀螺仪等不同特性的传感器对惯性测量单元进行采样。在姿态角估计的过程中，短时间内采用陀螺仪得到的角度作为最优估计值，定时对加速度采样得到的角度取平均值来校正陀螺仪得到的角度，以取长补短的方式消除测量过程中的误差；通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计，是通过最小方差估计法，将信号中的误差减少到最小；所述系统包括步态运动学建模模块、基于科里奥利理论的速度校正模块、信息融合模块，各模块之间采用有线或无线传输方式。本发明所提出的基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法可应用于应急救援、军事、人体运动、步态分析和康复等领域，能够有效提高行人惯性导航系统的定位精度。

Claims (4)

1.一种基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述行人定位方法包括以下步骤：
S1，在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；根据相应的运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断；
S2，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程；
S3，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果；
步骤S2中，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：
定义3个惯性测量单元分别为传感器0、传感器1和传感器2；结合动态三点几何约束推导得到位置约束方程：
Figure FDA0003640439520000011
其中，i为传感器的序号，si表示传感器i所在的坐标系，n坐标系为导航坐标系，其原点表示为O，A代表踝关节；rij为从i指向j的向量，向量是在n坐标系下定义的；
Figure FDA0003640439520000012
表示由传感器0所在的坐标系中踝关节A位置指向传感器0所在坐标系原点的向量，
Figure FDA0003640439520000013
表示由传感器1所在的坐标系中踝关节A位置指向传感器1所在坐标系原点的向量，
Figure FDA0003640439520000014
表示由传感器2所在的坐标系中踝关节A位置指向传感器2所在坐标系原点的向量，
Figure FDA0003640439520000015
表示传感器0所在的坐标系原点指向导航坐标系原点的向量，
Figure FDA0003640439520000016
表示传感器1所在的坐标系原点指向导航坐标系原点的向量，
Figure FDA0003640439520000017
表示传感器2所在的坐标系原点指向导航坐标系原点的向量，
Figure FDA0003640439520000018
表示从si传感器坐标系到n坐标系的旋转矩阵；I3和O3分别为三阶识别矩阵和零矩阵；
根据站立时的零速校正公式：
Figure FDA0003640439520000021
得到站立时的速度校正方程为：
Figure FDA0003640439520000022
其中,
Figure FDA0003640439520000023
为向量rAs2在导航坐标系n中的速度,
Figure FDA0003640439520000024
为n坐标系对s2坐标系的相对角速度,
Figure FDA0003640439520000025
表示传感器2所在的坐标系中踝关节位置指向导航坐标系原点的向量，O3*1表示该行元素都为0；
Figure FDA0003640439520000026
为n坐标系中第i个传感器中的速度；
根据摆动前后阶段的运动特征公式：
Figure FDA0003640439520000027
其中rs0A为传感器0所在的坐标系原点指向踝关节A的向量，
Figure FDA0003640439520000028
为向量rs0A相对于踝关节A的速度,
Figure FDA0003640439520000029
为向量rAs2在导航坐标系n中的速度,ωAn为踝关节A相对于导航坐标系的相对角速度,
Figure FDA00036404395200000210
为n坐标系对s0坐标系的相对角速度，
Figure FDA00036404395200000211
为n坐标系对s1坐标系的相对角速度，到摆动前后阶段的速度校正方程为：
Figure FDA00036404395200000212
步骤S3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：
构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量xoptimal，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程；
所述信息融合系统的离散状态空间表示为：
xk＝Ak-1xk-1+uk+wk；
yk＝Ckxk+nk；
其中，xk为k时刻的状态向量，uk为k时刻系统输入，yk为k时刻观察向量，wk和nk分别为k时刻过程噪声和测量噪声，Ak为k时刻状态转移矩阵，Ck为k时刻观测矩阵；选择状态向量为：
Figure FDA0003640439520000031
其中，
Figure FDA0003640439520000032
和
Figure FDA0003640439520000033
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
Figure FDA0003640439520000034
其中，T为采样周期，
Figure FDA0003640439520000035
为k时刻下si坐标系中si点的加速度的投影，g为重力加速度，
Figure FDA0003640439520000036
表示从bi传感器坐标系到n坐标系的第k个旋转矩阵，Ck,1表示k时刻的第1个观测方程，Ck,2表示k时刻的第2个观测方程；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：
站立、摆动前和摆动后阶段的观测方程为：
Figure FDA0003640439520000037
Figure FDA0003640439520000038
Figure FDA0003640439520000041
摆动阶段的观测方程为：
Figure FDA0003640439520000042
2.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤S1中，所述相应的运动学特性是指：
摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作；当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。
3.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤S1中，采用阈值法进行步态相位判断的过程包括以下步骤：
对惯性测量单元信号进行包括平滑、滤波和归一化在内的预处理；
采用滑动窗口对预处理后的数据进行分割，对单个窗口提取运动学特征；
结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
4.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。
CN202210367129.0A 2022-04-08 2022-04-08 一种基于脚部和腿部微惯性测量单元的行人定位方法 Active CN114440883B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210367129.0A CN114440883B (zh) 2022-04-08 2022-04-08 一种基于脚部和腿部微惯性测量单元的行人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210367129.0A CN114440883B (zh) 2022-04-08 2022-04-08 一种基于脚部和腿部微惯性测量单元的行人定位方法

Publications (2)

Publication Number Publication Date
CN114440883A CN114440883A (zh) 2022-05-06
CN114440883B true CN114440883B (zh) 2022-06-17

Family

ID=81359331

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210367129.0A Active CN114440883B (zh) 2022-04-08 2022-04-08 一种基于脚部和腿部微惯性测量单元的行人定位方法

Country Status (1)

Country Link
CN (1) CN114440883B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117577339B (zh) * 2024-01-17 2024-03-15 徐州医科大学 基于微惯性导航的下肢力线精准建模与定位方法和系统

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6594617B2 (en) * 2000-08-18 2003-07-15 Applanix Corporation Pedometer navigator system
DE102011079187A1 (de) * 2011-07-14 2013-01-17 Bayerische Motoren Werke Aktiengesellschaft Verfahren und Vorrichtung zur Schritterkennung eines Fußgängers für ein tragbares Endgerät
CN103759730B (zh) * 2014-01-16 2016-06-29 南京师范大学 一种基于导航信息双向融合的行人与智能移动载体的协同导航系统及其导航方法
CN103968827B (zh) * 2014-04-09 2017-11-28 北京信息科技大学 一种可穿戴式人体步态检测的自主定位方法
CN104613963B (zh) * 2015-01-23 2017-10-10 南京师范大学 基于人体运动学模型的行人导航系统与导航定位方法
CN104887237B (zh) * 2015-04-14 2017-11-14 南昌大学 一种基于人体运动模式监测的行人导航方法
CN104864884B (zh) * 2015-05-12 2018-03-06 上海大学 一种行人步幅测量系统
CN104913772B (zh) * 2015-06-05 2018-10-26 北京信息科技大学 一种基于腿部姿态信息的行人运动检测方法
CN108882892A (zh) * 2016-03-31 2018-11-23 Zoll医疗公司 跟踪患者运动的系统和方法
DE102017124173B4 (de) * 2017-06-22 2024-01-25 Deutsches Zentrum für Luft- und Raumfahrt e.V. Drift-Reduzierung bei der Orientierungseinschätzung mittels INS durch Anwendung biometrischer Modelle
CN110420029A (zh) * 2019-08-03 2019-11-08 苏州自如医疗器械有限公司 一种基于多传感器融合的行走步态无线检测系统

Also Published As

Publication number Publication date
CN114440883A (zh) 2022-05-06

Similar Documents

Publication Publication Date Title
CN108836346A (zh) 一种基于惯性传感器的人体步态分析方法和系统
CN104757976B (zh) 一种基于多传感器融合的人体步态分析方法和系统
CN107080540B (zh) 用于分析人的步态和姿势平衡的系统和方法
CN106705968B (zh) 基于姿态识别和步长模型的室内惯性导航算法
Mannini et al. Walking speed estimation using foot-mounted inertial sensors: Comparing machine learning and strap-down integration methods
Laudanski et al. A concurrent comparison of inertia sensor-based walking speed estimation methods
WO2014043757A1 (en) Stride detection
Wang et al. Estimate spatial-temporal parameters of human gait using inertial sensors
Tumkur et al. Modeling human walking for step detection and stride determination by 3-axis accelerometer readings in pedometer
Chen et al. Extracting spatio-temporal information from inertial body sensor networks for gait speed estimation
WO2018132999A1 (zh) 一种用于可穿戴式设备的人体步长测量方法及其测量设备
CN114440883B (zh) 一种基于脚部和腿部微惯性测量单元的行人定位方法
CN114469073B (zh) 基于可穿戴传感器的步态分析与异常检测方法
Hindle et al. Inertial-based human motion capture: A technical summary of current processing methodologies for spatiotemporal and kinematic measures
CN108836344A (zh) 步长步频估算方法和装置及步态检测仪
CN111895997B (zh) 一种无需标准矫正姿势的基于惯性传感器的人体动作采集方法
Ladetto et al. Human walking analysis assisted by DGPS
WO2023108315A1 (zh) 基于步可穿戴传感器的步态分析与异常检测方法
CN114271812A (zh) 基于惯性传感器的三维步态分析系统及方法
Daponte et al. Experimental comparison of orientation estimation algorithms in motion tracking for rehabilitation
Joukov et al. Rhythmic EKF for pose estimation during gait
Alam et al. Gait speed estimation using Kalman Filtering on inertial measurement unit data
Sun et al. An artificial neural network framework for lower limb motion signal estimation with foot-mounted inertial sensors
CN105232053B (zh) 一种人体踝关节跖屈相位检测系统及方法
US20240065581A1 (en) Gait measurement system, gait measurement method, and program recording medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant