CN115655265A - 一种基于惯性传感的行人导航室内定位方法 - Google Patents

一种基于惯性传感的行人导航室内定位方法 Download PDF

Info

Publication number
CN115655265A
CN115655265A CN202211164932.0A CN202211164932A CN115655265A CN 115655265 A CN115655265 A CN 115655265A CN 202211164932 A CN202211164932 A CN 202211164932A CN 115655265 A CN115655265 A CN 115655265A
Authority
CN
China
Prior art keywords
coordinate system
sensor
data
state
navigation
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.)
Pending
Application number
CN202211164932.0A
Other languages
English (en)
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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202211164932.0A priority Critical patent/CN115655265A/zh
Publication of CN115655265A publication Critical patent/CN115655265A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)

Abstract

本发明公开了一种基于惯性传感的行人导航室内定位方法，其步骤如下：(1)利用固定于脚踝部位的惯性传感节点采集人体在正常行走过程中的惯性数据，其中每个传感节点由一个三轴加速度计，一个三轴陀螺仪和一个三轴磁力计组成；(2)对传感器采集的数据进行预处理和误差校准；(3)对得到的传感节点的数据进行滑动窗口分割和运动强度检测，判断步行过程中的静止相分布；(4)对传感器9轴数据进行融合，计算步行过程中的行进方向；(5)通过步骤3中判断的静止相，采用零速更新方法对步行过程中的行进轨迹进行计算，得到三维的空间轨迹。

Description

一种基于惯性传感的行人导航室内定位方法
技术领域
本发明属于导航检测领域，尤其涉及一种基于惯性传感的行人导航室内定位方法。
技术背景
步态是人体行为过程中最基本的运动，它能够反映一个人的运动习惯，健康状况等因素，在医疗、体育、康复、人类学、宇航、工业等方面均有重要的科学意义及应用价值。通过步态分析，可以获取人体在各体态和运动下的生理、病理的力学和数学参数，进行人体各部位和机能检测，所检测的数据经进一步分析计算后可获得人体各部位(特别是关节)的受力状态、以及机械功、代谢能量消耗的情况。在临床医学(包括骨科、神经科)中，通过步态分析策略术前的各种人体运动曲线、数据及分析结果，可对关节疾患的程度进行测定，并在此基础上对手术方案进行最优化拟定；对术后数据的测量及分析结果则是评定治疗效果的最精确的客观定量指标。另外可以指导患者术后的行走训练，是设计智能化假肢的参考。此外在体育科学领域，通过对运动员在体育训练中的数据进行记录和分析，能够给教练员制定正确的训练计划提供参考。此外步态分析还广泛应用于工业测量与控制，人机工程等领域，是目前的一个研究热点。
随着时代的发展，目前的行人导航技术主要是通过GPS来实现的，如手机导航，车载导航等。但该类技术容易受到环境因素的影响，且精度一般较低。随着无线传感器网络、RFID、ZigBee、蓝牙等技术的发展，基于惯性测量单元(Inertial Measurement Unit,IMU)的行人室内导航技术逐步兴起，其对外界环境的适应性较强，且精度一般高于GPS系统，主要与算法的精度有关。
步态分析的早期，大多采用肉眼观察和手绘的方法来了解和记录步态信息，随着传感技术，通讯技术，计算能力的发展，相继出现了基于视频传感器、压力传感器、雷达传感器、超声传感器以及惯性传感器等现代化的步态分析测量技术。这些技术各有特色，均可用来进行步态的分析，但是检测精度和适应范围各有不同。基于雷达和超声等技术的步态分析方法，一般适应性较差，需要特定的环境场合；基于视频传感器的步态分析系统，其需要事先在测量部位布置光学标记，并采用高速高精的摄像头来捕捉特定光点的位置，其精度较高，但是成本一般比较昂贵，且易受光照，遮挡等环境的影响；基于压力传感器的步态分析方法主要是将压力传感器阵列制成鞋垫状并内置于鞋底夹层中，在人体行走过程中，通过压力传感器来感知脚底局部压力大小的变化，一般用来划分步态的时相，但具体到步长，步速等就无法适应了；随着微电子机械系统(Micro Electro Mechanical Systems,MEMS)技术的不断发展，采用微型IMU传感器的步态分析方法逐步涌现。通过惯性IMU传感器的步态分析方法相比于视频，压力分析技术具有许多优点，其不受光照，遮挡等时空因素的干扰，此外其成本远低于其他它方法，且能够精确的反映出三维空间内中整个步态的运动过程，穿戴方便，便携性好。
目前行人室内导航算法主要分为两种，分别为行人航迹推算(Pedestrian DeadReckoning,PDR)和零速更新ZUPT算法。PDR主要包括航向估计、步频估计、步长估计等，但其精度一般较低；ZUPT是一种根据人体步态规律通过惯性导航技术(Inertial NavigationSystem,INS)来对行人位置进行精确定位的方法，其精度一般取决于零速区间的判断准确度。
针对目前ZUPT存在的零速区间的判断问题，目前的方法主要可以分为三大类：峰值检测，过零检测和平区检测。但是测量值的峰值一般与步速密切相关，而过零点易受测量值波动的影响，这会导致基于峰值点和过零点的检测方法容易造成关键步态事件的误检测和漏检测。平区检测一般是检测信号波动不大甚至静止的状态，是目前常用的检测步态静止相的方法，但由于信号波动的随意性较大，且受环境和器件本身的影响，平区检测过程中设置的检测阈值一般不能适应所有情况，当环境或者器件改变时，设置固定的阈值易造成静止相的误检测。
此外，在对行人步态分析的过程中，位姿的计算是实现精确定位的关键部分，在目前的研究中，常用的惯性传感器定位方法主要有互补滤波，粒子滤波，卡尔曼滤波等，如公开号为201310520233.X的专利公开了一种基于双MEMS-IMU的行人卡尔曼滤波零速修正算法，来对步态过程中的方向进行融合计算；如申请号为201710493018.3的专利公开了一种基于粒子滤波的行人室内定位方法，采用粒子滤波融合技术来对行人运动过程中方位进行融合计算；如申请号为201410247629.6的专利公开了一种基于互补滤波的数据融合方法来对载体的姿态进行计算。虽然目前许多的学者进行了相关的研究，但是大多还处于实验理论阶段，且目前大多算法都存在数据漂移，适应性不强等特点。在实现本发明的过程中，发明人发现现有的行人导航室内定位技术中至少存在适用性不强，算法精度不够等缺陷。
发明内容
发明目的：本发明主要解决现有的基于惯导的室内行人导航技术存在的由于静止相识别失准造成的运动信号积分累计误差问题，以及在位姿计算过程中存在的因为滤波参数固定而造成的高速情况下姿态失准问题，提出了一种基于惯性传感的行人导航室内定位方法，并根据运动强度的变化，提出了一种基于运动强度分级的扩展卡尔曼滤波融合算法，从而提高人体步行过程中下肢运动信息解算的精度和可靠性。
技术方案：为了实现本发明的目的，所采用的技术方案如下：一种基于惯性传感的行人导航室内定位方法，该方法包括如下步骤：
(1)基于惯性传感器的运动姿态数据采集，获取正常行走时脚踝部位的惯性数据；
(2)采用帧频谱检测器(FSD)对静止相和摆动相进行划分；
(3)对惯性传感器采集到的加速度、角速度和磁力计数据进行降噪和误差校准；
(4)对多传感器数据进行融合，利用加速度计和磁力计的数据完成姿态的初始标定，随后根据时相的检测结果，对扩展卡尔曼滤波器设置不同的滤波参数，并使用扩展卡尔曼滤波对9轴数据进行融合，计算出整个步态过程的朝向信息；
(5)零速更新，首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，通过积分计算出速度矢量，随后根据时相的判断结果，采用零速更新技术对速度矢量进行校正，最后对修正后的速度矢量进行积分，得到整个步态过程的位移矢量。
进一步的，步骤(2)的方法如下：
在步态过程中，将检测器输出ν(n)与预定义的阈值进行比较，如式(1)所示，用标签“0”和“1”来标记每一段的运动强度，小于阈值标记为“0”，代表静止相，大于阈值标记为“1”，代表摆动相；
Figure BDA0003860907420000031
式中，Nfft＝512是快速傅立叶变换的分辨率，N(δ)是δ频带下的平均噪声频谱幅度，S(δ,n)是在δ频带下输入信号在第n帧的频谱，ρ是预定义的阈值。
进一步的，步骤3中的惯性传感器数据的降噪和校准过程如下：使用巴特沃兹低通滤波器，滤除惯性传感器中加速度计的高频的信号，低通滤波器的频率为5Hz；使用巴特沃兹高通滤波器滤除惯性传感器中陀螺仪的低频信号，高通滤波器的频率为0.001Hz，并通过加权平均的方法去除陀螺仪的零漂；采用椭球拟合的方法对惯性传感器中磁力计的磁场数据进行拟合修正。
进一步的，步骤(4)中的多传感器的数据融合具体的过程如下：
步骤4.1)坐标系的定义
(1)传感器坐标系(SCS)：惯性传感器自身规定的坐标系；
(2)体坐标系(BCS)：本实施实例中为脚部的坐标系；
(3)导航坐标系(GCS)：X轴指向正北，Y轴垂直赤道指向东方，Z轴垂直指向地面；
步骤4.2)根据定义的坐标系，进行初始姿态的标定：
人体以直立方式面朝正北站立，使体坐标系BCS和导航坐标系GCS重合，并保持该状态若干秒，记录该状态下惯性传感器中磁力计和加速度计的数据，通过公式(2)-(8)计算求得坐标系BCS到GCS的初始对准关系，即初始状态下，传感器和导航坐标系之间用翻滚角，俯仰角和航向角表示的旋转关系：
Figure BDA0003860907420000041
Figure BDA0003860907420000042
Figure BDA0003860907420000043
Figure BDA0003860907420000044
Figure BDA0003860907420000045
其中，g表示重力加速度，
Figure BDA0003860907420000046
表示惯性传感器中加速度计校准后得到的三轴加速度，
Figure BDA0003860907420000047
表示惯性传感器中磁力计校准后的三轴磁场强度，φin，θin，
Figure BDA0003860907420000048
分别表示通过加速度数据和磁场强度解算出的翻滚角，俯仰角和航向角；式(7)为欧拉角转四元数的公式，
Figure BDA0003860907420000049
表示初始状态下的传感器坐标系SCS到导航坐标系GCS之间的旋转矩阵；
Figure BDA00038609074200000410
其中，c和s表示cos和sin函数，初始时刻，坐标系GCS与BCS近似重合，所以从SCS到BCS与SCS到GCS之间的初始对准关系
Figure BDA00038609074200000411
Figure BDA00038609074200000412
表示初始状态下的传感器坐标系SCS到BCS体坐标系之间的旋转矩阵。传感器始终固定在肢体的同一部位，用
Figure BDA00038609074200000413
给
Figure BDA00038609074200000414
赋初值：
Figure BDA00038609074200000415
Figure BDA00038609074200000416
表示传感器坐标系SCS到BCS之间旋转矩阵，后续根据人体步态的变化，
Figure BDA00038609074200000417
数值会发生更新，因为
Figure BDA00038609074200000418
*表示复共轭，求出BCS到GCS之间的初始对准关系，即式(8)，从而计算出人体骨骼在导航坐标系下的初始姿态
Figure BDA00038609074200000419
Figure BDA00038609074200000420
步骤4.3)扩展卡尔曼滤波器的参数设定：
初始化状态变量参数x(0)＝[qint ωbias]，其中，初始四元数qint由初始姿态标定计算得出，陀螺仪的零点漂移ωbias在传感器静止状态下加权平均求出，根据对信号运动强度的检测，设置两套参数以分别对应于静止相和摆动相两种情况；
步骤4.4)使用扩展卡尔曼滤波进行数据融合，计算步态过程的朝向信息：
步骤4.4.1)时间更新：在先验的基础上预测状态变化，并用状态矩阵F表示状态变化；
定义状态变量：
x(t)＝[q(t) ωbias(t)] (9)
系统的状态方程为：
Figure BDA0003860907420000051
其中，q(t)表示t时刻的姿态四元数，q＝[q1,q2,q3,q4]，ωbias(t)为陀螺仪的零点漂移，δt表示积分时间，该时间使用采样周期，I为单位矩阵，wω(t-1)表示t-1时刻角速度偏置的过程噪声，wq(t-1)表示t-1时刻四元数的过程噪声，
Figure BDA0003860907420000052
此时，f(x(t-1),t-1)的Jacobi矩阵F为：
Figure BDA0003860907420000053
其中，ωx、ωy、ωz分别为惯性传感器中陀螺仪在x、y、z三轴上的角速度数据，
Figure BDA0003860907420000054
Figure BDA0003860907420000055
分别为陀螺仪在x、y、z三轴上的零点偏移，q1、q2、q3、q4分别为四元数q(t)的四个分量；
已知Jacobi矩阵F，由状态转移函数可得状态下一步的预测值
Figure BDA0003860907420000056
四元数可以通过下式得到：
Figure BDA0003860907420000061
对q(t)归一化，即可得到单位四元数，从而随时间变化来更新状态变量；
步骤4.4.2)量测更新：
首先将加速度值归一化，得到系统观测值zt，即：
Figure BDA0003860907420000062
其中，ax、ay和az是加速度计在x、y、z轴上的分量，v(t)是白噪声，h(x(t),t)表示离散情况下，t时刻所对应的状态转移矩阵，对应公式(14)；
Figure BDA0003860907420000063
随后，通过对h(x(t),t)计算Jacobi矩阵，可得系统的量测矩阵为：
Figure BDA0003860907420000064
由量测函数可得量测向量的一步预测值：
Figure BDA0003860907420000065
并计算状态增益矩阵K(t)：
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1 (16)
其中，P(t,t-1)是t时刻的先验估计
Figure BDA0003860907420000066
的协方差，是滤波过程的中间计算结果，R(t)为测量噪声协方差，通过滤波器的观测得出；
通过公式(17)求得t时刻状态向量估计值，从而获得当前时刻步态过程的朝向信息
Figure BDA0003860907420000067
Figure BDA0003860907420000068
进一步的，步骤5中，完成对步行轨迹的估计：
步骤5.1)首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，得到导航坐标系下的加速度矢量，在计算线性速度过程中，使用公式(18)去除重力加速度g的干扰：
Figure BDA0003860907420000071
式中，
Figure BDA0003860907420000072
表示通过扩展卡尔曼滤波计算出的传感器相对于导航系下的旋转四元数，*表示矩阵共轭，aG(t)表示在导航系下的线性加速度，aS(t)表示在传感器坐标系下的线性加速度，g表示重力分量，完成后对加速度进行一次积分计算出速度矢量；
步骤5.2)根据对静止相和摆动相的判断结果，采用零速更新技术对步骤5.1中求出的速度矢量进行修正，如标签为静止相，则将此过程中的速度归零；
步骤5.3)最后对步骤5.2中修正后的速度矢量进行一次积分，计算整个步行过程的位移矢量，计算由式(19)、(20)表示：
Figure BDA0003860907420000073
Figure BDA0003860907420000074
式中，vG(0)和dG(0)分别表示在零时刻的速度和位移矢量，vG(t)和dG(t)分别表示在t时刻的速度和位移矢量。
本发明中多传感器的数据融合过程，首先通过加速度和磁场强度信号，获取人体的初始姿态，初始姿态包含初始静止站立状态的四元数，根据初始的四元数获得传感器坐标系与导航坐标系之间的偏差量，并采用所述的偏差对姿态四元数进行补偿，从而获得初始的方位信息。其次根据运动强度检测的结果设置不同的滤波系数，并采用扩展卡尔曼滤波器进行方向估计，其特征在于本发明中滤波融合过程区别于传统的滤波器，发明中通过感测的运动强度来改变过程和测量噪声的值达到改善滤波器的性能的效果。
本发明中的零速修正过程，首先根据扩展卡尔曼滤波融合计算得到的姿态四元数，通过矩阵相乘将传感器坐标系下的加速度转化到导航坐标系下，并进行一次积分，计算速度矢量。随后根据运动强度检测器检测的运动强度，将人体步态过程的运动信息划分为两个状态，分别为支持状态和摆动状态，分别用“1”和“0”来表示。在状态“1”过程中，将速度归零，进行零速修正过程。修正完成后，对修正后的速度矢量进行一次积分，得到位移矢量。
附图说明
此处附图对本发明进一步阐释，并构成说明书一部分，用来一起解释本发明，且不构成对本发明的限制。在附图中：
图1为本发明的基于运动强度检测的行人导航分析方法示意图；
图2为本发明中所涉及坐标系的示意图。
具体实施方式
下面结合附图对本发明的实施实例进行说明，另注意，此处描述的优选实施实例仅仅用于解释本发明，并不限定。
根据本发明的实施实例，提供了一种基于惯性传感的行人导航室内定位方法，该方法流程图如附图1所示，该方法包括如下步骤：
(1)基于惯性传感器的运动姿态数据采集，获取正常行走时脚踝部位的惯性数据；
(2)采用帧频谱检测器(FSD)对静止相和摆动相进行划分；
(3)对惯性传感器采集到的加速度、角速度和磁力计数据进行降噪和误差校准；
(4)对多传感器数据进行融合，利用加速度计和磁力计的数据完成姿态的初始标定，随后根据时相的检测结果，对扩展卡尔曼滤波器设置不同的滤波参数，并使用扩展卡尔曼滤波对9轴数据进行融合，计算出整个步态过程的朝向信息；
(5)零速更新，首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，通过积分计算出速度矢量，随后根据时相的判断结果，采用零速更新技术对速度矢量进行校正，最后对修正后的速度矢量进行积分，得到整个步态过程的位移矢量。
本实施实例中的传感节点主要由9轴惯性传感器和运算核心STM32F103C8T6以及安信可的LoRa系列模块(Ra-01)和数据存储SD卡组成。穿戴完传感点后，上位机通过USBdongle给传感节点发送数据采集的请求，传感节点接收指令后采集数据，并将其实时写入SD卡。当上位机发送停止采集命令后，传感节点停止采集。随后通过读卡器将SD卡的数据进行读取操作。
本实施实例中，运动强度检测器检测时相过程(步骤2)如下：
在步态过程中，静止相的运动强度一般远低于摆动相，从而可以通过能量强度的检测来划分时相。在步态过程中，将检测器输出ν(n)与预定义的阈值进行比较，如式(1)所示，用标签“0”和“1”来标记每一段的运动强度，小于阈值标记为“0”，代表静止相，大于阈值标记为“1”，代表摆动相；
Figure BDA0003860907420000091
式中，Nfft＝512是快速傅立叶变换的分辨率，N(δ)是δ频带下的平均噪声频谱幅度，S(δ,n)是在δ频带下输入信号在第n帧的频谱，ρ是预定义的阈值。
本实施实例中，惯性传感器数据的降噪和校准过程(步骤3)如下：使用巴特沃兹低通滤波器(5Hz)，滤除惯性传感器中加速度计的高频的信号；使用巴特沃兹高通滤波器(0.001Hz)，滤除惯性传感器中陀螺仪的低频信号，并通过加权平均的方法去除陀螺仪的零漂；采用椭球拟合的方法，对惯性传感器中磁力计的磁场数据进行拟合修正。
多传感器的数据融合具体的实施过程(步骤4)如下：
步骤4.1)坐标系的定义
本实施实例中，定义的坐标系统如图2所示：
步骤4.1)坐标系的定义
(1)传感器坐标系(SCS)：惯性传感器自身规定的坐标系；
(2)体坐标系(BCS)：本实施实例中为脚部的坐标系；
(3)导航坐标系(GCS)：X轴指向正北，Y轴垂直赤道指向东方，Z轴垂直指向地面；
步骤4.2)根据定义的坐标系，进行初始姿态的标定：
人体以直立方式面朝正北站立，使体坐标系BCS和导航坐标系GCS重合，并保持该状态若干秒，记录该状态下惯性传感器中磁力计和加速度计的数据，通过公式(2)-(8)计算求得坐标系BCS到GCS的初始对准关系，即初始状态下，传感器和导航坐标系之间用翻滚角，俯仰角和航向角表示的旋转关系：
Figure BDA0003860907420000092
Figure BDA0003860907420000093
Figure BDA0003860907420000094
Figure BDA0003860907420000095
Figure BDA0003860907420000096
其中，g表示重力加速度，
Figure BDA0003860907420000097
表示惯性传感器中加速度计校准后得到的三轴加速度，
Figure BDA0003860907420000098
表示惯性传感器中磁力计校准后的三轴磁场强度，φin，θin，
Figure BDA0003860907420000099
分别表示通过加速度数据和磁场强度解算出的翻滚角，俯仰角和航向角；式(7)为欧拉角转四元数的公式，
Figure BDA0003860907420000101
表示初始状态下的传感器坐标系SCS到导航坐标系GCS之间的旋转矩阵；
Figure BDA0003860907420000102
其中，c和s表示cos和sin函数，初始时刻，坐标系GCS与BCS近似重合，所以从SCS到BCS与SCS到GCS之间的初始对准关系
Figure BDA0003860907420000103
Figure BDA0003860907420000104
表示初始状态下的传感器坐标系SCS到BCS体坐标系之间的旋转矩阵。传感器始终固定在肢体的同一部位，用
Figure BDA0003860907420000105
给
Figure BDA0003860907420000106
赋初值：
Figure BDA0003860907420000107
Figure BDA0003860907420000108
表示传感器坐标系SCS到BCS之间旋转矩阵，后续根据人体步态的变化，
Figure BDA0003860907420000109
数值会发生更新，因为
Figure BDA00038609074200001010
*表示复共轭，求出BCS到GCS之间的初始对准关系，即式(8)，从而计算出人体骨骼在导航坐标系下的初始姿态
Figure BDA00038609074200001011
Figure BDA00038609074200001012
步骤4.3)扩展卡尔曼滤波器的参数设定：
初始化状态变量参数x(0)＝[qint ωbias]，其中，初始四元数qint由初始姿态标定计算得出，陀螺仪的零点漂移ωbias在传感器静止状态下加权平均求出，根据对信号运动强度的检测，设置两套参数以分别对应于静止相和摆动相两种情况；
步骤4.4)使用扩展卡尔曼滤波进行数据融合，计算步态过程的朝向信息：
步骤4.4.1)时间更新：在先验的基础上预测状态变化，并用状态矩阵F表示状态变化；
定义状态变量：
x(t)＝[q(t) ωbias(t)] (9)
系统的状态方程为：
Figure BDA00038609074200001013
其中，q(t)表示t时刻的姿态四元数，q＝[q1,q2,q3,q4]，ωbias(t)为陀螺仪的零点漂移，δt表示积分时间，该时间使用采样周期，I为单位矩阵，wω(t-1)表示t-1时刻角速度偏置的过程噪声，wq(t-1)表示t-1时刻四元数的过程噪声，
Figure BDA0003860907420000111
此时，f(x(t-1),t-1)的Jacobi矩阵F为：
Figure BDA0003860907420000112
其中，ωx、ωy、ωz分别为惯性传感器中陀螺仪在x、y、z三轴上的角速度数据，
Figure BDA0003860907420000113
Figure BDA0003860907420000114
分别为陀螺仪在x、y、z三轴上的零点偏移，q1、q2、q3、q4分别为四元数q(t)的四个分量；
已知Jacobi矩阵F，由状态转移函数可得状态下一步的预测值
Figure BDA0003860907420000115
四元数可以通过下式得到：
Figure BDA0003860907420000116
对q(t)归一化，即可得到单位四元数，从而随时间变化来更新状态变量；
步骤4.4.2)量测更新：
首先将加速度值归一化，得到系统观测值zt，即：
Figure BDA0003860907420000117
其中，ax、ay和az是加速度计在x、y、z轴上的分量，v(t)是白噪声，h(x(t),t)表示离散情况下，t时刻所对应的状态转移矩阵，对应公式(14)；
Figure BDA0003860907420000118
随后，通过对h(x(t),t)计算Jacobi矩阵，可得系统的量测矩阵为：
Figure BDA0003860907420000121
由量测函数可得量测向量的一步预测值：
Figure BDA0003860907420000122
并计算状态增益矩阵K(t)：
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1 (16)
其中，P(t,t-1)是t时刻的先验估计
Figure BDA0003860907420000123
的协方差，是滤波过程的中间计算结果，R(t)为测量噪声协方差，通过滤波器的观测得出；
通过公式(17)求得t时刻状态向量估计值，从而获得当前时刻步态过程的朝向信息
Figure BDA0003860907420000124
Figure BDA0003860907420000125
零速更新(步骤5)完成对步行轨迹的估计：
步骤5.1)首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，得到导航坐标系下的加速度矢量，在计算线性速度过程中，使用公式(18)去除重力加速度g的干扰：
Figure BDA0003860907420000126
式中，
Figure BDA0003860907420000127
表示通过扩展卡尔曼滤波计算出的传感器相对于导航系下的旋转四元数，*表示矩阵共轭，aG(t)表示在导航系下的线性加速度，aS(t)表示在传感器坐标系下的线性加速度，g表示重力分量，完成后对加速度进行一次积分计算出速度矢量；
步骤5.2)根据对静止相和摆动相的判断结果，采用零速更新技术对步骤5.1中求出的速度矢量进行修正，如标签为静止相，则将此过程中的速度归零；
步骤5.3)最后对步骤5.2中修正后的速度矢量进行一次积分，计算整个步行过程的位移矢量，计算由式(19)、(20)表示：
Figure BDA0003860907420000128
Figure BDA0003860907420000129
式中，vG(0)和dG(0)分别表示在零时刻的速度和位移矢量，vG(t)和dG(t)分别表示在t时刻的速度和位移矢量。
本实施实例阐述了一种基于运动强度检测的行人导航方法与系统算法原理及其实现过程，该系统采用MEMS惯性传感器采集行人在步行状态下脚部的步态运动信息，通过所提出的运动强度检测器来对步态运动过程中的静止相和摆动相进行划分，并在此基础上，利用划分的时相，设置不同的滤波参数，采用基于运动强度分级的扩展卡尔曼滤波器对运动信息进行融合，估计出步态过程中的方向信息，最后利用零速修正方法对线性速度进行修正，并计算出最终的位移信息。该实施实例相比于其它系统，具有精度高，实时性，便携性强等特点。另外此类系统应用前景比较广泛，目前已知的领域就有医疗康复，体育训练，机器假肢，航空航天等领域，因此，具有较大的经济和学术参考价值。
最后应说明的是，以上所述为本发明的较佳实施例而已，但本发明不应该局限于该实施例和附图所公开的内容。所以凡是不脱离本发明所公开的精神下完成的等效或修改，都落入本发明保护的范围。

Claims (5)

1.一种基于惯性传感的行人导航室内定位方法，其特征在于，该方法包括如下步骤：
(1)基于惯性传感器的运动姿态数据采集，获取正常行走时脚踝部位的惯性数据；
(2)采用帧频谱检测器(FSD)对静止相和摆动相进行划分；
(3)对惯性传感器采集到的加速度、角速度和磁力计数据进行降噪和误差校准；
(4)对多传感器数据进行融合，利用加速度计和磁力计的数据完成姿态的初始标定，随后根据时相的检测结果，对扩展卡尔曼滤波器设置不同的滤波参数，并使用扩展卡尔曼滤波对9轴数据进行融合，计算出整个步态过程的朝向信息；
(5)零速更新，首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，通过积分计算出速度矢量，随后根据时相的判断结果，采用零速更新技术对速度矢量进行校正，最后对修正后的速度矢量进行积分，得到整个步态过程的位移矢量。
2.根据权利要求1所述的一种基于惯性传感的行人导航室内定位方法，其特征在于，步骤(2)的方法如下：在步态过程中，将检测器输出ν(n)与预定义的阈值进行比较，如式(1)所示，用标签“0”和“1”来标记每一段的运动强度，小于阈值标记为“0”，代表静止相，大于阈值标记为“1”，代表摆动相；
Figure FDA0003860907410000011
式中，Nfft＝512是快速傅立叶变换的分辨率，N(δ)是δ频带下的平均噪声频谱幅度，S(δ,n)是在δ频带下输入信号在第n帧的频谱，ρ是预定义的阈值。
3.根据权利要求1或2所述的一种基于惯性传感的行人导航室内定位方法，其特征在于，步骤3中的惯性传感器数据的降噪和校准过程如下：使用巴特沃兹低通滤波器，滤除惯性传感器中加速度计的高频的信号，低通滤波器的频率为5Hz；使用巴特沃兹高通滤波器滤除惯性传感器中陀螺仪的低频信号，高通滤波器的频率为0.001Hz，并通过加权平均的方法去除陀螺仪的零漂；采用椭球拟合的方法对惯性传感器中磁力计的磁场数据进行拟合修正。
4.根据权利要求1所述的一种基于惯性传感的行人导航室内定位方法，其特征在于，步骤(4)中的多传感器的数据融合具体的过程如下：
步骤4.1)坐标系的定义
(1)传感器坐标系(SCS)：惯性传感器自身规定的坐标系；
(2)体坐标系(BCS)：定义为脚部的坐标系；
(3)导航坐标系(GCS)：X轴指向正北，Y轴垂直赤道指向东方，Z轴垂直指向地面；
步骤4.2)根据定义的坐标系，进行初始姿态的标定：
人体以直立方式面朝正北站立，使体坐标系BCS和导航坐标系GCS重合，并保持该状态若干秒，记录该状态下惯性传感器中磁力计和加速度计的数据，通过公式(2)-(8)计算求得坐标系BCS到GCS的初始对准关系，即初始状态下，传感器和导航坐标系之间用翻滚角，俯仰角和航向角表示的旋转关系：
Figure FDA0003860907410000021
Figure FDA0003860907410000022
Figure FDA0003860907410000023
Figure FDA0003860907410000024
Figure FDA0003860907410000025
其中，g表示重力加速度，
Figure FDA0003860907410000026
表示惯性传感器中加速度计校准后得到的三轴加速度，
Figure FDA0003860907410000027
表示惯性传感器中磁力计校准后的三轴磁场强度，φin，θin，
Figure FDA0003860907410000028
分别表示通过加速度数据和磁场强度解算出的翻滚角，俯仰角和航向角；式(7)为欧拉角转四元数的公式，
Figure FDA0003860907410000029
表示初始状态下的传感器坐标系SCS到导航坐标系GCS之间的旋转矩阵；
Figure FDA00038609074100000210
其中，c和s表示cos和sin函数，初始时刻，坐标系GCS与BCS近似重合，所以从SCS到BCS与SCS到GCS之间的初始对准关系
Figure FDA00038609074100000211
Figure FDA00038609074100000212
表示初始状态下的传感器坐标系SCS到BCS体坐标系之间的旋转矩阵，传感器始终固定在肢体的同一部位，用
Figure FDA00038609074100000213
给
Figure FDA00038609074100000214
赋初值：
Figure FDA00038609074100000215
Figure FDA00038609074100000216
表示传感器坐标系SCS到BCS之间旋转矩阵，后续根据人体步态的变化，
Figure FDA00038609074100000217
数值会发生更新，因为
Figure FDA00038609074100000218
*表示复共轭，求出BCS到GCS之间的初始对准关系，即式(8)，从而计算出人体骨骼在导航坐标系下的初始姿态
Figure FDA00038609074100000219
Figure FDA00038609074100000220
步骤4.3)扩展卡尔曼滤波器的参数设定：
初始化状态变量参数x(0)＝[qint ωbias]，其中，初始四元数qint由初始姿态标定计算得出，陀螺仪的零点漂移ωbias在传感器静止状态下加权平均求出，根据对信号运动强度的检测，设置两套参数以分别对应于静止相和摆动相两种情况；
步骤4.4)使用扩展卡尔曼滤波进行数据融合，计算步态过程的朝向信息：
步骤4.4.1)时间更新：在先验的基础上预测状态变化，并用状态矩阵F表示状态变化；
定义状态变量：
x(t)＝[q(t) ωbias(t)] (9)
系统的状态方程为：
Figure FDA0003860907410000031
其中，q(t)表示t时刻的姿态四元数，q＝[q1,q2,q3,q4]，ωbias(t)为陀螺仪的零点漂移，δt表示积分时间，该时间使用采样周期，I为单位矩阵，wω(t-1)表示t-1时刻角速度偏置的过程噪声，wq(t-1)表示t-1时刻四元数的过程噪声，
Figure FDA0003860907410000032
此时，f(x(t-1),t-1)的Jacobi矩阵F为：
Figure FDA0003860907410000033
其中，ωx、ωy、ωz分别为惯性传感器中陀螺仪在x、y、z三轴上的角速度数据，
Figure FDA0003860907410000034
Figure FDA0003860907410000035
分别为陀螺仪在x、y、z三轴上的零点偏移，q1、q2、q3、q4分别为四元数q(t)的四个分量；
已知Jacobi矩阵F，由状态转移函数可得状态下一步的预测值
Figure FDA0003860907410000036
四元数可以通过下式得到：
Figure FDA0003860907410000041
对q(t)归一化，即可得到单位四元数，从而随时间变化来更新状态变量；
步骤4.4.2)量测更新：
首先将加速度值归一化，得到系统观测值zt，即：
Figure FDA0003860907410000042
其中，ax、ay和az是加速度计在x、y、z轴上的分量，v(t)是白噪声，h(x(t),t)表示离散情况下，t时刻所对应的状态转移矩阵，对应公式(14)；
Figure FDA0003860907410000043
随后，通过对h(x(t),t)计算Jacobi矩阵，可得系统的量测矩阵为：
Figure FDA0003860907410000044
由量测函数可得量测向量的一步预测值：
Figure FDA0003860907410000045
并计算状态增益矩阵K(t)：
K(t)＝P(t,t-1)HT(t)[H(t)P(t,t-1)HT(t)+R(t)]-1 (16)
其中，P(t,t-1)是t时刻的先验估计
Figure FDA0003860907410000046
的协方差，其是滤波过程的中间计算结果，R(t)为测量噪声协方差，通过滤波器的观测得出；
通过公式(17)求得t时刻状态向量估计值，从而获得当前时刻步态过程的朝向信息
Figure FDA0003860907410000047
Figure FDA0003860907410000048
5.根据权利要求1所述的一种基于惯性传感的行人导航室内定位方法，其特征在于，步骤5中，完成对步行轨迹的估计：
步骤5.1)首先根据计算出的朝向信息，将传感器下的加速度数据转换到导航系下，得到导航坐标系下的加速度矢量，在计算线性速度过程中，使用公式(18)去除重力加速度g的干扰：
Figure FDA0003860907410000051
式中，
Figure FDA0003860907410000052
表示通过扩展卡尔曼滤波计算出的传感器相对于导航系下的旋转四元数，*表示矩阵共轭，aG(t)表示在导航系下的线性加速度，aS(t)表示在传感器坐标系下的线性加速度，g表示重力分量，完成后对加速度进行一次积分计算出速度矢量；
步骤5.2)根据对静止相和摆动相的判断结果，采用零速更新技术对步骤5.1中求出的速度矢量进行修正，如果标签为静止相，则将此过程中的速度归零；
步骤5.3)最后对步骤5.2中修正后的速度矢量进行一次积分，计算整个步行过程的位移矢量，计算由式(19)、(20)表示：
Figure FDA0003860907410000053
Figure FDA0003860907410000054
式中，vG(0)和dG(0)分别表示在零时刻的速度和位移矢量，vG(t)和dG(t)分别表示在t时刻的速度和位移矢量。
CN202211164932.0A 2022-09-23 2022-09-23 一种基于惯性传感的行人导航室内定位方法 Pending CN115655265A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211164932.0A CN115655265A (zh) 2022-09-23 2022-09-23 一种基于惯性传感的行人导航室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211164932.0A CN115655265A (zh) 2022-09-23 2022-09-23 一种基于惯性传感的行人导航室内定位方法

Publications (1)

Publication Number Publication Date
CN115655265A true CN115655265A (zh) 2023-01-31

Family

ID=84986360

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211164932.0A Pending CN115655265A (zh) 2022-09-23 2022-09-23 一种基于惯性传感的行人导航室内定位方法

Country Status (1)

Country Link
CN (1) CN115655265A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117390347A (zh) * 2023-12-06 2024-01-12 无锡车联天下信息技术有限公司 一种基于运动估计漂移优化的汽车航向角解算方法
CN117577339A (zh) * 2024-01-17 2024-02-20 徐州医科大学 基于微惯性导航的下肢力线精准建模与定位方法和系统

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117390347A (zh) * 2023-12-06 2024-01-12 无锡车联天下信息技术有限公司 一种基于运动估计漂移优化的汽车航向角解算方法
CN117577339A (zh) * 2024-01-17 2024-02-20 徐州医科大学 基于微惯性导航的下肢力线精准建模与定位方法和系统
CN117577339B (zh) * 2024-01-17 2024-03-15 徐州医科大学 基于微惯性导航的下肢力线精准建模与定位方法和系统

Similar Documents

Publication Publication Date Title
CN104757976B (zh) 一种基于多传感器融合的人体步态分析方法和系统
CN106500695B (zh) 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法
CN109579853B (zh) 基于bp神经网络的惯性导航室内定位方法
CN115655265A (zh) 一种基于惯性传感的行人导航室内定位方法
CN108836346A (zh) 一种基于惯性传感器的人体步态分析方法和系统
Yan et al. Ronin: Robust neural inertial navigation in the wild: Benchmark, evaluations, and new methods
KR101751760B1 (ko) 하지 관절 각도를 이용한 보행 인자 추정 방법
CN104406586B (zh) 基于惯性传感器的行人导航装置和方法
CN109827577A (zh) 基于运动状态检测的高精度惯性导航定位算法
JP2010534316A (ja) 対象物の動きを捕捉するシステム及び方法
CN108478222B (zh) 一种基于神经网络的关节角误差补偿实验装置及方法
Van Nguyen et al. Real-time human foot motion localization algorithm with dynamic speed
Tjhai et al. Step-size estimation using fusion of multiple wearable inertial sensors
CN114469073B (zh) 基于可穿戴传感器的步态分析与异常检测方法
CN104457741A (zh) 一种基于蚁群算法误差修正的人体手臂运动跟踪方法
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
WO2023108315A1 (zh) 基于步可穿戴传感器的步态分析与异常检测方法
CN113229806A (zh) 可穿戴人体步态检测及导航系统及其运行方法
Castañeda et al. Knee joint angle monitoring system based on inertial measurement units for human gait analysis
Qiu et al. Heterogeneous data fusion for three-dimensional gait analysis using wearable MARG sensors
Chen et al. Pedestrian positioning with physical activity classification for indoors
Tjhai et al. Using a mobile range-camera motion capture system to evaluate the performance of integration of multiple low-cost wearable sensors and gait kinematics for pedestrian navigation in realistic environments
CN114469078B (zh) 一种基于光惯融合的人体运动检测方法
CN105232053A (zh) 一种人体踝关节跖屈相位检测系统及方法
Tinh et al. Image-based gramian angular field processing for pedestrian stride-length estimation using convolutional neural network

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