CN109674480B - 一种基于改进互补滤波的人体运动姿态解算方法 - Google Patents

一种基于改进互补滤波的人体运动姿态解算方法 Download PDF

Info

Publication number
CN109674480B
CN109674480B CN201910107785.5A CN201910107785A CN109674480B CN 109674480 B CN109674480 B CN 109674480B CN 201910107785 A CN201910107785 A CN 201910107785A CN 109674480 B CN109674480 B CN 109674480B
Authority
CN
China
Prior art keywords
human body
angular rate
attitude
motion
acceleration
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
CN201910107785.5A
Other languages
English (en)
Other versions
CN109674480A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201910107785.5A priority Critical patent/CN109674480B/zh
Publication of CN109674480A publication Critical patent/CN109674480A/zh
Application granted granted Critical
Publication of CN109674480B publication Critical patent/CN109674480B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/1118—Determining activity level
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B5/00—Measuring for diagnostic purposes; Identification of persons
    • A61B5/68—Arrangements of detecting, measuring or recording means, e.g. sensors, in relation to patient
    • A61B5/6801—Arrangements of detecting, measuring or recording means, e.g. sensors, in relation to patient specially adapted to be attached to or worn on the body surface
    • A61B5/6802—Sensor mounted on worn items

Landscapes

  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Medical Informatics (AREA)
  • Biophysics (AREA)
  • Pathology (AREA)
  • Engineering & Computer Science (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Physics & Mathematics (AREA)
  • Molecular Biology (AREA)
  • Surgery (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Physiology (AREA)
  • Dentistry (AREA)
  • Oral & Maxillofacial Surgery (AREA)
  • Gyroscopes (AREA)

Abstract

本发明公开一种基于改进互补滤波的人体运动姿态解算方法，1：分别建立陀螺仪和加速度计的量测模型，将MIMU固定在人体上，采集人体运动的加速度和角速率；2：利用MIMU所采集的角速率求解姿态四元数微分方程，获得人体先验姿态；3：将人体先验姿态代入加速度计的量测模型计算当前时刻加速度估计值，利用当前时刻加速度与所求得的加速度估计值计算出姿态误差角速率；4：判断此时人体运动状态，若静止，则直接使用姿态误差角速率对角速率进行补偿；若运动，则将姿态误差角速率经PI控制器处理后获得补偿信息，再对角速率进行补偿；5：利用补偿后的角速率更新姿态四元数微分方程，获得人体后验姿态，本发明能够高效地补偿角速率误差，提高解算精度。

Description

一种基于改进互补滤波的人体运动姿态解算方法
技术领域
本发明属于微机电惯性测量单元的动作捕捉领域，具体涉及一种基于改进互补滤波的人体运动姿态解算方法。
背景技术
基于微机电惯性测量单元(MIMU)的姿态解算系统由于其自主性强，受外界环境限制或干扰较小，性价比高，体型小巧的特点，被广泛应用于飞行器、机器人、人体动作捕捉等多个领域。惯性测量单元由三轴陀螺仪和三轴加速度计组成，可用于测量载体的角速度和加速度。由于陀螺仪存在瞬时漂移，在使用角速度积分获得载体姿态的过程中，系统误差会随时间累积，所解算的姿态存在低频噪声，长时间工作很可能造成发散。而利用载体坐标系下重力加速度分量解算姿态角只基于瞬时加速度数据，其误差不随时间积累，所解算的姿态存在高频噪声。基于互补滤波的姿态解算方法可通过PI控制器对陀螺仪所估计姿态进行高通滤波，对加速度计所估计姿态进行低通滤波，从而提高姿态解算精度，使系统可以长时间工作。该类算法的计算量小，解算效果较好。但传统互补滤波法所输入进PI控制器的误差量没有明确物理含义，导致算法受参数影响较大，鲁棒性较弱。
发明内容
有鉴于此，本发明提供了一种基于改进互补滤波的人体运动姿态解算方法，能够高效地补偿角速率误差，提高解算精度。
实现本发明的技术方案如下：
一种基于改进互补滤波的人体运动姿态解算方法，包括以下步骤：
步骤1：分别建立陀螺仪和加速度计的量测模型，将微机电惯性测量单元(MIMU)固定在人体上，采集人体运动的加速度和角速率；
步骤2：利用MIMU所采集的角速率yG,t求解姿态四元数微分方程，获得人体先验姿态
Figure BDA0001967061820000021
步骤3：将人体先验姿态
Figure BDA0001967061820000022
代入加速度计的量测模型计算当前时刻加速度估计值
Figure BDA0001967061820000023
利用当前时刻加速度yA,t与所求得的加速度估计值
Figure BDA0001967061820000024
计算出姿态误差角速率et；
步骤4：判断此时人体运动状态，如果处于静止状态，则直接使用姿态误差角速率et对角速率yG,t进行补偿；如果处于运动状态，则将姿态误差角速率et经PI控制器的比例运算和积分运算后获得补偿信息，再对角速率yG,t进行补偿；
步骤5：利用补偿后的角速率
Figure BDA0001967061820000025
更新姿态四元数微分方程，获得人体后验姿态
Figure BDA0001967061820000026
进一步地，
Figure BDA0001967061820000027
其中，T为MIMU中陀螺仪的采样周期，
Figure BDA0001967061820000028
Figure BDA0001967061820000029
和
Figure BDA00019670618200000210
分别为
Figure BDA00019670618200000211
向量的三个元素。
进一步地，步骤四中，利用yA,t与
Figure BDA00019670618200000212
之差的幅值判断当前人体是否处于静止状态。
有益效果：
1、本发明提出了一种基于改进互补滤波的人体运动姿态解算方法，它通过加速度数据对陀螺仪数据进行补偿，利用补偿后的角速率对姿态四元数进行更新，可用于全姿态解算。
2、考虑到当前时刻之前的解算误差和当前时刻的陀螺仪噪声所引入误差，本发明定义了一种姿态误差角速率，并通过建立加速度计和陀螺仪量测模型，推导出姿态误差角速率的计算公式。
3、针对运动加速度引入误差问题，本发明设计了一种有效的判断人体是否处于静止或匀速运动状态的方法，当人体处于静态时，算法通过姿态误差角速率直接补偿系统此前解算误差和当前陀螺仪噪声，当人体处于动态时，算法将姿态误差角速率经PI控制器处理后补偿陀螺仪数据。
4、本发明可以很好地均衡人体处于不同运动状态时滤波器的解算效果，有效提高系统的鲁棒性，且相比于传统互补滤波法，本发明可以更快更好地补偿角速率误差，以提高解算精度。
附图说明
图1为本发明方法流程图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本发明针对传统互补滤波法补偿量没有明确物理含义，鲁棒性较弱的缺点，提出了一种基于改进互补滤波的人体运动姿态解算方法。它利用加速度计数据获得先验姿态的估计误差角速率，根据当前人体运动状态，选择将其经PI控制器处理后或直接对陀螺仪数据进行补偿，最后用补偿后的角速率更新人体姿态，如图1所示，本发明的具体步骤包括：
步骤1：分别建立陀螺仪和加速度计的量测模型，将微机电惯性测量单元(MIMU)固定在人体上，采集人体运动的加速度和角速率；
将MIMU固定于人体。设载体坐标系Obxbybzb与MIMU固连，其原点Ob位于MIMU重心，xb轴沿MIMU横轴指向右侧，zb轴沿MIMU竖轴指向上方，yb轴与xb、zb轴构成右手直角坐标系。设导航坐标系Onxnynzn与初始时刻的载体坐标系重合。MIMU采集获得人体运动时加速度和角速率信息，从而进行后续的人体姿态解算。
设t时刻加速度计数据为yA,t，建立加速度计量测模型：
Figure BDA0001967061820000041
其中，bA表示加速度计常值零偏，可以通过数据预处理修正；vA,t表示加速度计噪声，为高斯白噪声；at表示t时刻的人体运动加速度；g为导航坐标系下所测得的当地重力加速度；qt代表t时刻真实姿态四元数；
Figure BDA0001967061820000042
为从导航坐标系到载体坐标系的旋转矩阵：
Figure BDA0001967061820000043
因此
Figure BDA0001967061820000044
表示t时刻在载体坐标系下所观测的当地重力加速度。
建立陀螺仪量测模型：
yG,t＝ωt+bG+vG,t (3)
其中，yG,t为t时刻陀螺仪数据；ωt表示t时刻人体旋转角速度；bG表示陀螺仪常值零偏；vG,t表示陀螺仪噪声，为高斯白噪声。
步骤2：利用MIMU所采集的角速率yG,t求解姿态四元数微分方程，获得人体先验姿态
Figure BDA0001967061820000045
利用MIMU中的陀螺仪采集得到角速率yG,t，使用姿态四元数表示人体姿态，利用离散化的姿态四元数微分方程，对人体先验姿态进行更新：
Figure BDA0001967061820000046
其中，
Figure BDA0001967061820000047
为t-1时刻人体后验姿态四元数，
Figure BDA0001967061820000048
为t时刻人体先验姿态四元数，q(0)为设定的初始姿态，T为陀螺仪采样周期，yG,t为t时刻陀螺仪数据，I为四阶单位阵。Ω(·)为角速率ω的反对称矩阵，如下式所示：
Figure BDA0001967061820000051
步骤3：将人体先验姿态
Figure BDA0001967061820000052
代入加速度计的量测模型计算当前时刻加速度估计值
Figure BDA0001967061820000053
利用当前时刻加速度yA,t与所求得的加速度估计值
Figure BDA0001967061820000054
计算出姿态误差角速率et；
由于本发明应用场景是人体运动捕捉，传感器模块固定在人体上，而人体的运动加速度随时间随机改变，因此重力加速度部分主导了加速度计数据的低频响应，t时刻加速计数据yA,t近似于：
Figure BDA0001967061820000055
而t时刻加速度估计值可通过下式计算：
Figure BDA0001967061820000056
由于在步骤2中基于陀螺仪数据yG,t更新的先验姿态四元数
Figure BDA0001967061820000057
包含了t时刻以前的姿态解算误差与t时刻陀螺仪漂移引入的误差，通过对陀螺仪数据进行补偿可以对以上误差进行修正。
令Rt为t时刻由n系变换到b系的真实旋转矩阵，
Figure BDA0001967061820000058
为t时刻先验估计旋转矩阵，
Figure BDA0001967061820000059
为t时刻后验估计旋转矩阵。
设
Figure BDA00019670618200000510
其中，Rp,t-1为t时刻之前的解算误差的旋转矩阵，θp,t-1代表t时刻之前的解算误差角，可看作小角。R(·)为忽略高阶无穷小量的旋转矩阵公式，即：
Figure BDA00019670618200000511
设bG＝bA＝[0 0 0]T，忽略运动加速度at及加速度计噪声vA,t，则公式(1)和公式(3)可简化为：
Figure BDA0001967061820000061
公式(7)可写作：
Figure BDA0001967061820000062
由公式(8)，公式(10)及姿态矩阵微分方程可得：
Figure BDA0001967061820000063
令姿态误差角速率et为：
et＝-vG,t+θp,t-1/T (13)
将公式(13)代入公式(12)可得：
Figure BDA0001967061820000064
根据公式(10)，公式(11)及公式(14)可得：
Figure BDA0001967061820000065
令
Figure BDA0001967061820000066
展开公式(15)然后求逆可得：
Figure BDA0001967061820000067
步骤4：判断此时人体运动状态，如果处于静止状态，则直接使用姿态误差角速率et对角速率yG,t进行补偿；如果处于运动状态，则将姿态误差角速率et经PI控制器的比例运算和积分运算后获得补偿信息，再对角速率yG,t进行补偿；
利用yA,t与
Figure BDA0001967061820000068
之差的幅值判断当前人体是否处于静止状态。如果静止，则不包含运动加速度的干扰，可以使用et直接对t时刻陀螺仪数据进行补偿，设补偿后的陀螺仪数据为
Figure BDA0001967061820000069
补偿公式如下：
Figure BDA00019670618200000610
如果人体处于非静止状态，则将et通过PI控制器后对陀螺仪数据进行补偿，其公式如下：
Figure BDA0001967061820000071
Figure BDA0001967061820000072
其中，kp为比例因子，ki为积分因子。
当ei过大时，很有可能是由于当前人体运动加速度较大导致的，此时计算获得的补偿值ei可信度较低，因此不将其用于积分补偿，所以本发明中通过对ei取欧几里得范数‖ei‖，并设置判断阈值k，用以去除掉过大的ei。
步骤5：利用补偿后的角速率
Figure BDA0001967061820000073
更新姿态四元数微分方程，获得人体后验姿态
Figure BDA0001967061820000074
利用补偿后的角速率更新人体姿态，获得人体后验姿态：
Figure BDA0001967061820000075
其中，
Figure BDA0001967061820000076
为四元数乘法。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于改进互补滤波的人体运动姿态解算方法，其特征在于，包括以下步骤：
步骤1：分别建立陀螺仪和加速度计的量测模型，将MIMU固定在人体上，采集人体运动的加速度和角速率；
步骤2：利用MIMU所采集的角速率yG，t求解姿态四元数微分方程，获得人体先验姿态
Figure FDA0002636532690000011
步骤3：将人体先验姿态
Figure FDA0002636532690000012
代入加速度计的量测模型计算当前时刻加速度估计值
Figure FDA0002636532690000013
利用当前时刻加速度yA，t与所求得的加速度估计值
Figure FDA0002636532690000014
计算出姿态误差角速率et；
Figure FDA0002636532690000015
其中，T为MIMU中陀螺仪的采样周期，
Figure FDA0002636532690000016
步骤4：判断此时人体运动状态，如果处于静止状态，则直接使用姿态误差角速率et对角速率yG，t进行补偿；如果处于运动状态，则将姿态误差角速率et经PI控制器的比例运算和积分运算后获得补偿信息，再对角速率yG，t进行补偿；
步骤5：利用补偿后的角速率
Figure FDA0002636532690000017
更新姿态四元数微分方程，获得人体后验姿态
Figure FDA0002636532690000018
2.如权利要求1所述的一种基于改进互补滤波的人体运动姿态解算方法，其特征在于，步骤4 中，利用yA，t与
Figure FDA0002636532690000019
之差的幅值判断当前人体是否处于静止状态。
CN201910107785.5A 2019-02-02 2019-02-02 一种基于改进互补滤波的人体运动姿态解算方法 Active CN109674480B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910107785.5A CN109674480B (zh) 2019-02-02 2019-02-02 一种基于改进互补滤波的人体运动姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910107785.5A CN109674480B (zh) 2019-02-02 2019-02-02 一种基于改进互补滤波的人体运动姿态解算方法

Publications (2)

Publication Number Publication Date
CN109674480A CN109674480A (zh) 2019-04-26
CN109674480B true CN109674480B (zh) 2020-10-30

Family

ID=66195579

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910107785.5A Active CN109674480B (zh) 2019-02-02 2019-02-02 一种基于改进互补滤波的人体运动姿态解算方法

Country Status (1)

Country Link
CN (1) CN109674480B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110101388B (zh) * 2019-05-17 2022-02-18 南京东奇智能制造研究院有限公司 一种基于mimu的便携脊柱测量仪及方法
CN112773338A (zh) * 2021-01-29 2021-05-11 中山大学 一种监测人体生理信息的牙套及头部运动数据采集方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110191083A1 (en) * 2010-02-04 2011-08-04 Bruce William Adams System and Method for Measuring and Reporting the Relative Functions of Dental Anatomical Structures
KR101147133B1 (ko) * 2011-02-17 2012-05-25 (주)엔티렉스 각도측정을 위한 상보필터 및 그 방법
CN106937871A (zh) * 2016-01-05 2017-07-11 袁囡囡 智能运动状态检测系统
CN106249745B (zh) * 2016-07-07 2019-02-19 苏州大学 四轴无人机的控制方法
CN106482734A (zh) * 2016-09-28 2017-03-08 湖南优象科技有限公司 一种用于imu多传感器数据融合的滤波方法
CN107063262A (zh) * 2017-04-07 2017-08-18 武汉理工大学 一种用于无人机姿态解算的互补滤波方法
CN107830861A (zh) * 2017-12-07 2018-03-23 智灵飞（北京）科技有限公司 基于自适应增益互补滤波运动物体姿态测量方法及装置
CN108398128B (zh) * 2018-01-22 2021-08-24 北京大学深圳研究生院 一种姿态角的融合解算方法和装置
CN108670263A (zh) * 2018-05-18 2018-10-19 哈尔滨理工大学 一种基于mpu-6050的睡眠姿态判定方法
CN108939512B (zh) * 2018-07-23 2020-05-19 大连理工大学 一种基于穿戴式传感器的游泳姿态测量方法

Also Published As

Publication number Publication date
CN109674480A (zh) 2019-04-26

Similar Documents

Publication Publication Date Title
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
WO2017063388A1 (zh) 一种惯导装置初始对准方法
JP4876204B2 (ja) 小型姿勢センサ
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN109612471B (zh) 一种基于多传感器融合的运动体姿态解算方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN109682377A (zh) 一种基于动态步长梯度下降的姿态估计方法
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
JP2012173190A (ja) 測位システム、測位方法
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN112066985B (zh) 一种组合导航系统初始化方法、装置、介质及电子设备
CN113155129B (zh) 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN113670314B (zh) 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN109674480B (zh) 一种基于改进互补滤波的人体运动姿态解算方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN106709222A (zh) 基于单目视觉的imu漂移补偿方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
EP3227634A1 (en) Method and system for estimating relative angle between headings
CN109506674B (zh) 一种加速度的校正方法及装置

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