CN114440883B - 一种基于脚部和腿部微惯性测量单元的行人定位方法 - Google Patents
一种基于脚部和腿部微惯性测量单元的行人定位方法 Download PDFInfo
- 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
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 62
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000033001 locomotion Effects 0.000 claims abstract description 54
- 230000005021 gait Effects 0.000 claims abstract description 46
- 230000000295 complement effect Effects 0.000 claims abstract description 28
- 210000002414 leg Anatomy 0.000 claims abstract description 14
- 230000036544 posture Effects 0.000 claims abstract description 4
- 230000001133 acceleration Effects 0.000 claims description 24
- 210000002683 foot Anatomy 0.000 claims description 19
- 238000012937 correction Methods 0.000 claims description 17
- 239000011159 matrix material Substances 0.000 claims description 16
- 230000004927 fusion Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 14
- 238000001914 filtration Methods 0.000 claims description 10
- 230000007704 transition Effects 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 230000009471 action Effects 0.000 claims description 6
- 238000009499 grossing Methods 0.000 claims description 3
- 210000003141 lower extremity Anatomy 0.000 claims description 3
- 238000000691 measurement method Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012935 Averaging Methods 0.000 claims description 2
- 238000009795 derivation Methods 0.000 claims description 2
- 238000007781 pre-processing Methods 0.000 claims description 2
- 210000000544 articulatio talocruralis Anatomy 0.000 claims 8
- 210000001699 lower leg Anatomy 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 6
- 210000003371 toe Anatomy 0.000 description 6
- 238000007500 overflow downdraw method Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 208000024827 Alzheimer disease Diseases 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 210000000988 bone and bone Anatomy 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 201000010099 disease Diseases 0.000 description 1
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
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个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：
结合动态三点几何约束推导得到位置约束方程：
其中，
表示传感器i坐标系，n坐标系为导航坐标系，其原点表示为O；
为从i指向j的向量，向量是在n坐标系下定义的；
为n坐标系中的第i个向量，
为
传感器坐标系下的第i个向量，
表示从
传感器坐标系到n坐标系的旋转矩阵；
和
分别为三阶识别矩阵和零矩阵；
根据站立时的零速校正公式：
得到站立时的速度校正方程为：
其中
为n坐标系中第i个传感器中的速度；
根据摆动前后阶段的运动特征公式：
得到摆动前后阶段的速度校正方程为：
进一步地，步骤S3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：
构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量
，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程。
进一步地，所述信息融合系统的离散状态空间表示为：
其中，
为k时刻的状态向量，
为k时刻系统输入，
为k时刻观察向量，
和
分别为k时刻过程噪声和测量噪声，
为k时刻状态转移矩阵，
为k时刻观测矩阵；选择状态向量为：
其中，
和
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
其中，T为采样周期，
为k时刻下
坐标系中
点的加速度的投影，
为重力加速度；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：
站立、摆动前和摆动后阶段的观测方程为：
摆动阶段的观测方程为：
进一步地，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。
有益效果：
本发明将步态从两个相位重新定义为四个相位，并采用多阈值法进行步态相位判断，有效提高了步态判断的精确性，避免因步态相位判断模糊影响定位测量的精度；本发明提出的行人定位方法通过动态三点几何约束构建位置约束方程，通过零速更新方法与相关运动学特征构建速度约束方程，能够对误差进行实时修正，将误差限制在一定范围内，有效减小了惯性测量单元的测量误差，提高了定位精度；并且能够结合互补滤波器与卡尔曼滤波器进行信息融合，通过不同滤波器之间的频域互补特性去除噪声信号，从而进一步抑制了测量误差，提高了定位的精确度。
附图说明
图1为本发明提供的基于脚部和腿部微惯性测量单元的行人定位方法流程图；
图2 为基于互补滤波器与卡尔曼滤波器的信息融合方法融合框架图；
图3 为本发明提供的一种基于脚部和腿部微惯性测量单元的行人定位方法的相关硬件结构图；
图4 为脚尖-脚跟-小腿微惯性测量单元的二维行人轨迹导航定位结果图；
图5 为脚尖-脚跟-小腿微惯性测量单元的三维行人轨迹导航定位结果图；
图6为脚尖-脚跟-小腿微惯性测量单元的安装位置示意图。
具体实施方式
下面的实施例可使本专业技术人员更全面地理解本发明，但不以任何方式限制本发明。
如图1所示，本发明提出了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，包括运动步态阶段划分与步态相位判断，基于动态几何约束和科里奥利理论以构建步态四个连续阶段的速度和位置约束，基于互补滤波器与卡尔曼滤波器的信息融合方法获得各运动阶段的高精度观测方程。
相应的，本发明提供的一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法硬件结构图如图3所示，包括惯性测量单元、主机、信号融合模块，各模块之间采用CAN总线进行连接，以确保通信的稳定性，其中：三个惯性测量单元由三个分别被安装在设备佩戴者脚尖、脚跟和小腿处的微惯性测量单元组成，传感器网络由三个测量单元和一台主机组成，惯性测量单元的型号为低成本的ICM20948，测量单元和主机的CPU分别为STM32F4和IMX6ULL。信号融合模块由三种不同的传感器对输入信号进行采样，形成了互补滤波器，再通过卡尔曼滤波器输出各运动状态的观测方程。其中加速度计、陀螺仪和磁力计的采样频率可以分别设置为100HZ、100HZ和50HZ。
根据各运动状态的观测方程获得行人导航定位结果。图4为二维显示界面下，行人轨迹的导航定位结果；图5为三维显示界面下，行人轨迹的导航定位结果；图6表明了3个微惯性测量单元在脚尖、脚跟、小腿处的具体安装位置。
该行人定位方法的具体流程如下：
步骤一、运动步态阶段划分与步态相位判断
在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，传感器布局如图4。依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。根据以上运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断，即结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
为更准确地提取步态切换时的运动与加速度信息，对惯性测量单元信号进行平滑、滤波、归一化等预处理。为控制数据误差、保证数据的实时性，使用尽可能小的、平滑的滤波窗进行处理，然后采用滑动窗口对数据进行分割，对单个窗口提取运动学特征。
步骤二、基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束
为了减小各运动状态下的位姿测量误差，根据3个惯性测量单元的空间位置信息构建位置约束方程。根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，从而实现周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程。
动态三点几何约束条件，是通过对分别安装在脚尖、脚跟、小腿三处的微惯性测量单元进行空间位置建模，以运动状态下人骨骼相对固定的位置关系构成三点处的几何位置约束，形成动态三点几何约束条件。
基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是根据脚在站立、摆动前和摆动后阶段接触地面，此时脚掌的速度为零，因此可以使用科里奥利定理。科里奥利定理的方程形式为：
其中
和
分别为向量
在k坐标系和m坐标系中的速度。
为k坐标系对m坐标系的相对角速度，可以利用系统的科里奥利定理的方程，结合动态三点几何约束推导得到的位置约束方程，推断各运动阶段的速度校正模型。
根据动态三点几何约束公式：
位置约束方程为：
其中
表示传感器i坐标系，黑点表示传感器坐标系原点，n坐标系为导航坐标系，其原点表示为O；
为从i指向j的向量，向量是在n坐标系下定义的。设
为n坐标系中的第i个向量，
为
传感器坐标系下的第i个向量，
表示从
传感器坐标系到n坐标系的旋转矩阵；
和
分别为三阶识别矩阵和零矩阵。
根据站立时的零速校正公式：
站立时的速度校正方程为：
其中
为n坐标系中第i个传感器中的速度。
根据摆动前后阶段的运动特征公式：
摆动前后阶段的速度校正方程为：
步骤三、基于互补滤波器与卡尔曼滤波器的信息融合方法
基于互补滤波器与卡尔曼滤波器的信息融合方法采用的融合框架图如图2所示。将修正后的传感器值通过互补滤波器，以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程即可得到各运动状态修正后的定位结果。
用于实现信息融合方法的信息融合系统被分为两个模块。第一个模块为基于互补滤波器的分散式姿态估计，第二个模块为集中处理速度和位置估计值的线性卡尔曼滤波器。在互补滤波器构成的第一模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量
，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值。第二模块中，卡尔曼滤波器根据第一模块的最优估计值输出各运动阶段的观测方程，由各运动阶段的观测方程即可获得经过误差修正的高精度导航系统定位结果。
该信息融合系统为线性高斯系统，对于线性高斯系统，离散状态空间可以表示为：
其中，
为k时刻的状态向量，
为系统输入，
为观察向量，
和
分别为过程噪声和测量噪声，
为状态转移矩阵，
为观测矩阵。选择状态向量为：
其中，
和
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
其中，T为采样周期，
为k时刻下
坐标系中
点的加速度的投影，g为重力加速度。根据理论测量方法并结合系统的状态转移方程，可以得到各运动阶段的观测方程。
站立、摆动前和摆动后阶段的观测方程为：
摆动阶段的观测方程为：
本发明公开了一种基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法，该方法包括将步态划分为四个阶段，采用多阈值法对步态相位进行判断，再针对每个步态阶段构建精确的运动学模型，建立基于科里奥利理论的速度校正模型以抑制速度漂移，适应不同的步行速度。然后采用动态三点几何约束以增强系统的定位精度和鲁棒性，并设计了基于互补滤波器的分布式姿态估计。最后将姿态估计值通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计。其中，将步态划分为四个阶段，采用多阈值法对步态相位进行判断，是在脚部和小腿假定为两个刚性段的基础上，将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段，以精确描述步态特性。摆动前阶段的运动学特征是以脚尖为支点的摆动动作，摆动后阶段的运动学特征是以脚跟为中心的旋转动作。当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处；当系统处于摆动后阶段时，脚跟处的加速度则小于脚尖处。根据这一运动学特性以及加速度和角速度的统计学特性，可以采用阈值法进行步态相位判断；基于动态几何约束和科里奥利理论以构建四个连续步态阶段的速度和位置约束，是通过建立科里奥利旋转坐标系，对四个连续阶段的运动偏移量进行合理推算，得到速度与位置约束方程。再应用坐标系下的速度与位置约束方程，针对每个步态阶段构建精确的运动学模型，并建立基于科里奥利理论的速度校正模型以抑制速度漂移；基于互补滤波器的分散式姿态估计，是通过加速度计、陀螺仪等不同特性的传感器对惯性测量单元进行采样。在姿态角估计的过程中，短时间内采用陀螺仪得到的角度作为最优估计值，定时对加速度采样得到的角度取平均值来校正陀螺仪得到的角度，以取长补短的方式消除测量过程中的误差；通过卡尔曼滤波器获得各运动状态的观测方程，实现最优定位估计，是通过最小方差估计法，将信号中的误差减少到最小；所述系统包括步态运动学建模模块、基于科里奥利理论的速度校正模块、信息融合模块，各模块之间采用有线或无线传输方式。本发明所提出的基于脚尖-脚跟-小腿微惯性测量单元的行人定位方法可应用于应急救援、军事、人体运动、步态分析和康复等领域，能够有效提高行人惯性导航系统的定位精度。
Claims (4)
1.一种基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述行人定位方法包括以下步骤：
S1，在设备佩戴者的脚尖、脚跟和小腿处分别安装3个惯性测量单元，依据脚面与小腿的姿态以及传感器速度将步态周期划分为四个阶段：站立阶段、摆动前阶段、摆动阶段和摆动后阶段；根据相应的运动学特性以及加速度和角速度的统计特性，采用阈值法进行步态相位判断；
S2，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束；根据人体运动学相关特征，在站立阶段、摆动前阶段、摆动后阶段，脚的速度为零，将零赋值给各传感器，周期性地修正速度真实值与传感器测量值之间的误差，构建各运动状态下的速度约束方程；
S3，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果；
步骤S2中，基于动态几何约束和科里奥利理论，根据3个惯性测量单元的空间位置信息构建四个连续步态阶段的速度和位置约束的过程包括以下步骤：
定义3个惯性测量单元分别为传感器0、传感器1和传感器2；结合动态三点几何约束推导得到位置约束方程：
其中，i为传感器的序号，si表示传感器i所在的坐标系，n坐标系为导航坐标系，其原点表示为O，A代表踝关节；rij为从i指向j的向量，向量是在n坐标系下定义的；
表示由传感器0所在的坐标系中踝关节A位置指向传感器0所在坐标系原点的向量，
表示由传感器1所在的坐标系中踝关节A位置指向传感器1所在坐标系原点的向量，
表示由传感器2所在的坐标系中踝关节A位置指向传感器2所在坐标系原点的向量，
表示传感器0所在的坐标系原点指向导航坐标系原点的向量，
表示传感器1所在的坐标系原点指向导航坐标系原点的向量，
表示传感器2所在的坐标系原点指向导航坐标系原点的向量，
表示从si传感器坐标系到n坐标系的旋转矩阵；I3和O3分别为三阶识别矩阵和零矩阵；
根据站立时的零速校正公式：
得到站立时的速度校正方程为：
其中,
为向量rAs2在导航坐标系n中的速度,
为n坐标系对s2坐标系的相对角速度,
表示传感器2所在的坐标系中踝关节位置指向导航坐标系原点的向量，O3*1表示该行元素都为0；
为n坐标系中第i个传感器中的速度；
根据摆动前后阶段的运动特征公式：
其中rs0A为传感器0所在的坐标系原点指向踝关节A的向量，
为向量rs0A相对于踝关节A的速度,
为向量rAs2在导航坐标系n中的速度,ωAn为踝关节A相对于导航坐标系的相对角速度,
为n坐标系对s0坐标系的相对角速度，
为n坐标系对s1坐标系的相对角速度，到摆动前后阶段的速度校正方程为：
步骤S3中，将修正后的传感器值通过互补滤波器以消除噪声的干扰，再通过卡尔曼滤波器获得各运动状态下的观测方程，由各运动状态下的高精度观测方程得到各运动状态修正后的定位结果的过程包括以下步骤：
构建信息融合系统，信息融合系统包括基于互补滤波器的分散式姿态估计模块和用于集中处理速度和位置估计值的线性卡尔曼滤波器；在分散式姿态估计模块中，通过陀螺仪、加速度计与磁力计对三个惯性测量单元进行数据采样，分别输出角速度、加速度和磁场矢量xoptimal，根据不同传感器的特性，经过互补滤波算法解算测量的最优估计值；线性卡尔曼滤波器根据分散式姿态估计模块的最优估计值输出各运动阶段的观测方程；
所述信息融合系统的离散状态空间表示为：
xk＝Ak-1xk-1+uk+wk；
yk＝Ckxk+nk；
其中，xk为k时刻的状态向量，uk为k时刻系统输入，yk为k时刻观察向量，wk和nk分别为k时刻过程噪声和测量噪声，Ak为k时刻状态转移矩阵，Ck为k时刻观测矩阵；选择状态向量为：
其中，
和
分别为第i个传感器在n坐标系中k时刻的速度和位置，上述参量满足系统状态转移公式：
其中，T为采样周期，
为k时刻下si坐标系中si点的加速度的投影，g为重力加速度，
表示从bi传感器坐标系到n坐标系的第k个旋转矩阵，Ck,1表示k时刻的第1个观测方程，Ck,2表示k时刻的第2个观测方程；根据理论测量方法并结合系统的状态转移方程，得到各运动阶段的观测方程：
站立、摆动前和摆动后阶段的观测方程为：
摆动阶段的观测方程为：
2.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤S1中，所述相应的运动学特性是指：
摆动前阶段是以脚尖为支点的摆动动作，摆动后阶段是以脚跟为中心的旋转动作；当系统处于摆动前阶段时，脚跟处的加速度大于脚尖处，摆动后阶段则相反。
3.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，步骤S1中，采用阈值法进行步态相位判断的过程包括以下步骤：
对惯性测量单元信号进行包括平滑、滤波和归一化在内的预处理；
采用滑动窗口对预处理后的数据进行分割，对单个窗口提取运动学特征；
结合测量的加速度信息，判断下肢各部分运动的先行趋势；结合角度阈值判断，对步态切换时刻点进行精确定位。
4.根据权利要求1所述的基于脚部和腿部微惯性测量单元的行人定位方法，其特征在于，所述基于互补滤波器的分散式姿态估计模块根据加速度计、陀螺仪和磁力计这三个不同传感器的互补特性，采用互补滤波算法对传感器的数据进行姿态角的求解；其中，在预设时长范围内选择陀螺仪值作为姿态角的最优估计，在预设时长范围外对加速度计和磁力计测量值取平均修正陀螺仪测量的姿态角。
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 (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115363545A (zh) * | 2022-09-01 | 2022-11-22 | 北京雪扬科技有限公司 | 一种基于可穿戴设备的血脂监测方法 |
| CN117577339B (zh) * | 2024-01-17 | 2024-03-15 | 徐州医科大学 | 基于微惯性导航的下肢力线精准建模与定位方法和系统 |
Family Cites Families (11)
| 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 | 北京信息科技大学 | 一种基于腿部姿态信息的行人运动检测方法 |
| JP2019514448A (ja) * | 2016-03-31 | 2019-06-06 | ゾール メディカル コーポレイションＺＯＬＬ Ｍｅｄｉｃａｌ Ｃｏｒｐｏｒａｔｉｏｎ | 患者の動きを追跡するシステムおよび方法 |
| 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 | 苏州自如医疗器械有限公司 | 一种基于多传感器融合的行走步态无线检测系统 |
-
2022
- 2022-04-08 CN CN202210367129.0A patent/CN114440883B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114440883A (zh) | 2022-05-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104757976B (zh) | 一种基于多传感器融合的人体步态分析方法和系统 | |
| CN107080540B (zh) | 用于分析人的步态和姿势平衡的系统和方法 | |
| CN108836346A (zh) | 一种基于惯性传感器的人体步态分析方法和系统 | |
| CN114440883B (zh) | 一种基于脚部和腿部微惯性测量单元的行人定位方法 | |
| CN102323854B (zh) | 人体运动捕获装置 | |
| Laudanski et al. | A concurrent comparison of inertia sensor-based walking speed estimation methods | |
| 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 | |
| Joukov et al. | Online tracking of the lower body joint angles using IMUs for gait rehabilitation | |
| CN111895997B (zh) | 一种无需标准矫正姿势的基于惯性传感器的人体动作采集方法 | |
| CN114469073B (zh) | 基于可穿戴传感器的步态分析与异常检测方法 | |
| WO2023108315A1 (zh) | 基于步可穿戴传感器的步态分析与异常检测方法 | |
| CN108592907A (zh) | 一种基于双向滤波平滑技术的准实时步进式行人导航方法 | |
| Wang et al. | CanoeSense: Monitoring canoe sprint motion using wearable sensors | |
| Joukov et al. | Rhythmic EKF for pose estimation during gait | |
| Daponte et al. | Experimental comparison of orientation estimation algorithms in motion tracking for rehabilitation | |
| Sun et al. | An artificial neural network framework for lower limb motion signal estimation with foot-mounted inertial sensors | |
| CN114271812A (zh) | 基于惯性传感器的三维步态分析系统及方法 | |
| Alam et al. | Gait speed estimation using Kalman Filtering on inertial measurement unit data | |
| CN116206358A (zh) | 一种基于vio系统的下肢外骨骼运动模式预测方法及系统 | |
| CN105232053B (zh) | 一种人体踝关节跖屈相位检测系统及方法 | |
| US20240065581A1 (en) | Gait measurement system, gait measurement method, and program recording medium | |
| Refai et al. | Portable gait lab: Zero moment point for minimal sensing of gait | |
| Liang et al. | Accurate estimation of gait altitude using one wearable IMU sensor |
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 |