CN107478223A - 一种基于四元数和卡尔曼滤波的人体姿态解算方法 - Google Patents

一种基于四元数和卡尔曼滤波的人体姿态解算方法 Download PDF

Info

Publication number
CN107478223A
CN107478223A CN201610405814.2A CN201610405814A CN107478223A CN 107478223 A CN107478223 A CN 107478223A CN 201610405814 A CN201610405814 A CN 201610405814A CN 107478223 A CN107478223 A CN 107478223A
Authority
CN
China
Prior art keywords
mtd
msub
mrow
mtr
msubsup
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
CN201610405814.2A
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.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
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 University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN201610405814.2A priority Critical patent/CN107478223A/zh
Publication of CN107478223A publication Critical patent/CN107478223A/zh
Pending legal-status Critical Current

Links

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/18—Stabilised platforms, e.g. by gyroscope
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth

Abstract

本发明公开了一种基于四元数和卡尔曼滤波的人体姿态解算方法，步骤如下：确定载体坐标系和导航坐标系，并确定姿态矩阵；加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，然后对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数；根据四元数微分方程进行系统建模，以初始化四元数为测量值输入，采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，即更新后的四元数；将更新后的四元数进行归一化处理，作为最终的姿态信息，并更新姿态矩阵，得到更新后的姿态角。本发明有效地提高了人体姿态检测的精度，响应速度快，具有良好的稳定性和实时性，具有广阔的应用前景。

Description

一种基于四元数和卡尔曼滤波的人体姿态解算方法
技术领域
本发明属于姿态检测技术领域，特别是一种基于四元数和卡尔曼滤波的人体姿态解算方法。
背景技术
人体姿态解算的方法主要有余弦矩阵法、欧拉角法和四元数法，余弦矩阵法计算量较大、解算较为复杂，欧拉角法虽然可以更加直观地表示姿态角，但是在姿态解算中存在奇点问题，而四元数法不但计算相对余弦矩阵法简单，而且避免了欧拉角法存在的奇点问题。
传统的姿态解算大都基于加速度计和陀螺仪两种惯性传感器，陀螺仪通过积分求得姿态角，在短时间内可以提供准确的姿态信息，但长期精度较差。而加速度计的测量误差不随时间积累，长期性能较好，但是短期内精度较差，动态响应较慢。现有的较多解算方法缺少对含有噪声的数据进行滤波，部分算法采用了互补滤波算法，但是互补滤波算法过于简单并且参数的修正较为繁琐，因此达不到良好的滤波效果。同时，也没有对数据进行有效的融合，由此导致姿态解算的结果精度较低，稳定性和可靠性不足。除此之外，惯性传感器选择上的不足同样影响姿态解算的精度。
发明内容
本发明的目的在于提供一种基于四元数和卡尔曼滤波的人体姿态解算方法，以提高人体姿态检测的精度和响应速度，使人体姿态检测系统具有更好的实时性和稳定性。
本发明的技术解决方案为：一种基于四元数和卡尔曼滤波的人体姿态解算方法，包括以下步骤：
步骤1，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；
步骤2，加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数；
步骤3，根据四元数微分方程进行系统建模，以初始化四元数为测量值输入，采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，即更新后的四元数；
步骤4，将更新后的四元数进行归一化处理，作为最终的姿态信息，并以归一化处理后的四元数更新姿态矩阵，得到更新后的姿态角。
进一步地，步骤1所述的载体坐标系即b系Y轴指向前进方向，X轴水平向右，Z轴垂直向下；导航坐标系即n系坐标轴沿北、东、地方向；姿态角由载体坐标系到导航坐标系之间的相对角位置关系来表示，用欧拉角来对人体姿态进行描述，欧拉角包括俯仰角θ、横滚角γ和偏航角ψ，俯仰角是坐标系沿Y轴旋转的角度，旋转范围为-90°～+90°；横滚角是坐标系绕X轴旋转的角度，旋转范围为-180°～+180°，偏航角是坐标系绕Z轴旋转的角度，旋转范围为0°～360°；
两个坐标系之间的相互转换由姿态矩阵来表示，姿态矩阵也称作捷联矩阵T，由载体坐标系到导航坐标系的姿态矩阵表示为：
由载体坐标系与导航坐标系之间的转换关系得：
因此，由捷联矩阵T与姿态矩阵的矩阵关系求得的三个姿态角γT、θT、ψT计算如下：
进一步地，步骤2所述加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数，具体如下：
三轴加速度计测量的是重力加速度在载体坐标系上的投影，假设加速度计的测量值fb为：
其中，为加速度计的测量值在载体坐标系X、Y、Z轴方向的投影；
重力加速度在导航坐标系上的投影fn为：
fn＝[0 0 g]T
由于加速度计的输出与航向角无关，所以姿态矩阵中的航向角为零，将此时的姿态矩阵代入得：
则载体的俯仰角θ和滚转角γ分别为：
假设磁强计的测量值Bb沿着载体坐标系的三个坐标轴分解得到如下分量：
其中，为磁强计的测量值在载体坐标系X、Y、Z轴方向的投影；
当载体坐标系与导航坐标系重合时为：
其中，为磁强计的测量值在导航坐标系X、Y、Z轴方向的投影；
使载体先绕载体坐标系Y轴旋转当前俯仰角值，再绕X轴旋转当前滚转角值，从而使当前载体与水平面平行，根据坐标变换理论有：
由上式推出地磁场水平分量的表达式为：
则航向角公式为：
由初始姿态角求得初始四元数q0、q1、q2、q3，求解公式如下：
进一步地，步骤3所述根据四元数微分方程进行系统建模，其中四元数微分方程如下：
其中，ω1、ω2、ω3分别为陀螺仪三轴的角速度输出；
采用四阶龙格-库塔法对四元数微分方程进行求解，求出更新后的四元数；
在每个周期的姿态更新之后对四元数进行归一化处理，处理之后得到用四元数表示的姿态矩阵
进一步地，步骤3所述采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，过程如下：
(1)设置四元数误差协方差矩阵测量噪声协方差矩阵R，过程噪声协方差矩阵Q、量测矩阵H的初始值：
(2)根据四元数微分方程对系统进行系统建模，四元数微分方程转换为状态一步预测方程：的形式，所以选择四元数微分方程进行系统建模，四元数作为状态变量，系统模型如下：
其中，为k时刻的预测状态变量，为k-1时刻的状态变量，I为一个4乘4的单位矩阵，Δt为k-1时刻到k时刻的时间差，下标k表示k时刻，k-1表示k-1时刻，k+1表示k+1时刻；
由系统模型推出系统状态矩阵A：
(3)根据初始值以及系统状态矩阵A求取预测状态变量和预测误差协方差矩阵
(4)根据误差协方差矩阵、测量噪声协方差矩阵和量测矩阵计算卡尔曼增益Kk：
(5)系统初始对准求得的初始四元数作为观测值zk，根据测量值来更新估计值并更新误差协方差矩阵：
估计值即为经过卡尔曼滤波数据融合算法后的人体姿态最终输出值。
本发明与现有技术相比，其显著优点在于：(1)采用四元数法对人体姿态进行解算，避免了复杂的计算，并且没有欧拉角法的奇点问题，可以实现全姿态表达；(2)采用了加速度计、陀螺仪和磁强计三种MEMS惯性传感器，对三种惯性传感器采集的姿态数据进行有效的数据融合，大大提高了人体姿态解算的精度和动态性能；(3)卡尔曼滤波可以把含噪声的数据进行处理之后得出相对真值，数据融合基于卡尔曼滤波算法进行，有效提高了解算结果的精度。
附图说明
图1是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的系统结构框图。
图2是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的流程图。
图3是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的载体坐标系。
图4是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的导航坐标系。
图5是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的卡尔曼滤波工作原理框图。
图6是本发明基于四元数和卡尔曼滤波的人体姿态解算方法的数据融合算法框图。
具体实施方式
以下结合附图及具体实施方式对本发明做进一步详细说明。
本发明提供了一种基于四元数和卡尔曼滤波的人体姿态解算方法，整个解算过程都用四元数对人体姿态进行表示，解算过程中需要用卡尔曼滤波算法对加速度计、陀螺仪和磁强计采集的数据进行滤波，并将姿态数据融合处理，以求得最终的人体姿态数据。
结合图1、图2，本发明基于四元数和卡尔曼滤波的人体姿态解算方法，具体实现方法如下：
步骤1，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；
要进行人体姿态的获取首先要确定载体坐标系和导航坐标系，载体坐标系即b系如图3所示，Y轴指向前进方向，X轴水平向右，Z轴垂直向下。导航坐标系即n系如图4所示，坐标轴沿北、东、地方向，也就是通常所说的NED系。
姿态角由载体坐标系到导航坐标系之间的相对角位置关系来表示，欧拉角是比较直观的姿态角表示方式，用欧拉角来对人体姿态进行描述，欧拉角包括俯仰角θ、横滚角γ和偏航角ψ，俯仰角是坐标系沿y轴旋转的角度，旋转范围为-90°～+90°；横滚角是坐标系绕x轴旋转的角度，旋转范围为-180°～+180°，偏航角是坐标系绕z轴旋转的角度，旋转范围为0°～360°；
空间直角坐标系的原点保持不动，绕着某一个轴旋转而构成新的坐标系，这个旋转过程就叫做坐标旋转。在旧坐标系中的坐标与在旋转后新坐标系中的坐标有一定的转换关系，这种关系可以用转换矩阵来表示。这种转换关系可以用转换矩阵来表示，表示方法如下：
绕坐标系Z轴旋转ψ角，转换矩阵C1为：
绕坐标轴Y轴旋转θ角，转换矩阵C2为：
绕坐标轴X轴旋转γ角，转换矩阵C3为：
两个坐标系之间的相互转换可以由姿态矩阵来表示，姿态矩阵也可称作捷联矩阵(T)。那么由载体坐标系到导航坐标系的姿态矩阵可以表示为：
由载体坐标系与导航坐标系之间的转换关系可知：
由公式对比可得姿态角与姿态矩阵的元素之间的关系，三个姿态角可计算如下：
θT＝arcsin(-T31) (3)
其中，γT、θT、ψT是由捷联矩阵T与姿态矩阵的矩阵关系求得的三个姿态角。
由此，载体坐标系和导航坐标系都已确定，并且根据已确定的坐标系进行公式推导求得姿态矩阵，以上公式在步骤4中求得四元数更新值后，用来求取更新后的姿态角。
步骤2，加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数。
初始对准也就是确定系统的初始值，通过对采集的人体姿态数据进行解算求得初始的俯仰角、翻滚角和航向角，并根据姿态角求得初始四元数，初始对准过程中用到了加速度计和磁强计两种传感器采集的姿态数据。
三轴加速度计测量的是重力加速度在载体坐标系上的投影，假设加速度计的测量值fb为：
其中，为加速度计的测量值在载体坐标系x、y、z轴方向的投影；
重力加速度在导航坐标系上的投影fn为：
fn＝[0 0 g]T
由于加速度计的输出与航向角无关，所以姿态矩阵中的航向角为零，将此时的姿态矩阵代入得：
则载体的俯仰角θ和滚转角γ分别为：
当载体的姿态角不为零时，根据地磁场在水平方向上的分量始终指向地磁北极这一理论基础，以及通过加速度计计算出的俯仰角和横滚角，可以推出偏航角的公式。
假设磁强计的测量值Bb沿着载体坐标系的三个坐标轴分解得到如下分量：
其中，为磁强计的测量值在载体坐标系X、Y、Z轴方向的投影；
当载体坐标系与导航坐标系重合时为：
其中，为磁强计的测量值在导航坐标系X、Y、Z轴方向的投影；
在此我们使载体坐标系先绕Y轴使载体旋转当前俯仰角值，再绕X轴旋转当前滚转角值，从而使当前载体与水平面平行，根据坐标变换理论有：
由上式可推出地磁场水平分量的表达式为：
则航向角公式为：
由初始姿态角可求得初始四元数q0、q1、q2、q3，求解公式如下：
步骤3，根据四元数微分方程进行系统建模，以初始化四元数为测量值输入，采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，即更新后的四元数；
四元数是一种姿态表达方式，四元数法相对方向余弦法计算更简单，而相对欧拉角法则有效避免了奇点的问题。四元数法通过对四元数微分方程进行求解来进行姿态的求解，四元数微分方程如下，其中，ω1，ω2，ω3为陀螺仪三轴的角速度输出：
求解上述方程的方法有毕卡法、泰勒展开法和四阶龙格--库塔法，考虑到算法的速度和精度等问题，在此采用四阶龙格--库塔法对四元数微分方程进行求解，求出更新后的四元数。
在每个周期的姿态更新之后都要对四元数进行归一化处理，处理之后可得到用四元数表示的姿态矩阵
结合图5、图6，将三种姿态数据进行数据融合，根据加速度计和磁强计采集的数据对系统初始对准，求得的初始四元数与陀螺仪采集的数据进行基于卡尔曼滤波的数据融合。卡尔曼滤波算法以初始化四元数为测量值输入，对姿态数据进行数据融合输出估计值，即更新后的四元数，过程如下：
(1)设置四元数误差协方差矩阵测量噪声协方差矩阵R，过程噪声协方差矩阵Q、量测矩阵H的初始值：
(2)根据四元数微分方程对系统进行系统建模，四元数微分方程转换为状态一步预测方程：的形式，所以选择四元数微分方程进行系统建模，四元数作为状态变量，系统模型如下：
其中，为k时刻的预测状态变量，为k-1时刻的状态变量，I为一个4乘4的单位矩阵，Δt为k-1时刻到k时刻的时间差，下标k表示k时刻，k-1表示k-1时刻，k+1表示k+1时刻。
由系统模型推出系统状态矩阵A：
(3)根据初始值以及系统状态矩阵A求取预测状态变量和预测误差协方差矩阵
(4)根据误差协方差矩阵、测量噪声协方差矩阵和量测矩阵计算卡尔曼增益Kk：
(5)系统初始对准求得的初始四元数作为观测值zk，根据测量值来更新估计值并更新误差协方差矩阵：
估计值即为经过卡尔曼滤波数据融合算法后的人体姿态最终输出值。
数据融合的核心思想是将加速度计和磁强计求取的初始化四元数与陀螺仪采集的角速度进行融合，使三种惯性传感器优缺点互补，以得到更加精确的姿态信息。
步骤4，将更新后的四元数进行归一化处理，作为最终的姿态信息，并以归一化处理后的四元数更新姿态矩阵，得到更新后的姿态角，具体如下：
将更新后的四元数进行归一化处理，作为最终的姿态信息：
以归一化处理后的四元数更新姿态矩阵：
并根据公式更新姿态角，以便直观地观察姿态信息：
本发明提出的一种基于四元数和卡尔曼滤波的人体姿态解算方法，有效地提高了人体姿态检测的精度，该方法响应速度快，具有良好的稳定性和实时性，不仅可以适应复杂的检测环境，而且能有效地提高人体姿态检测系统的可靠性和姿态跟踪性能，具有广阔的应用前景。

Claims (5)

1.一种基于四元数和卡尔曼滤波的人体姿态解算方法，其特征在于，包括以下步骤：
步骤1，确定载体坐标系和导航坐标系，根据已确定的坐标系确定姿态矩阵；
步骤2，加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数；
步骤3，根据四元数微分方程进行系统建模，以初始化四元数为测量值输入，采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，即更新后的四元数；
步骤4，将更新后的四元数进行归一化处理，作为最终的姿态信息，并以归一化处理后的四元数更新姿态矩阵，得到更新后的姿态角。
2.根据权利要求1所述的基于四元数和卡尔曼滤波的人体姿态解算方法，其特征在于，步骤1所述的载体坐标系即b系Y轴指向前进方向，X轴水平向右，Z轴垂直向下；导航坐标系即n系坐标轴沿北、东、地方向；姿态角由载体坐标系到导航坐标系之间的相对角位置关系来表示，用欧拉角来对人体姿态进行描述，欧拉角包括俯仰角θ、横滚角γ和偏航角ψ，俯仰角是坐标系沿Y轴旋转的角度，旋转范围为-90°～+90°；横滚角是坐标系绕X轴旋转的角度，旋转范围为-180°～+180°，偏航角是坐标系绕Z轴旋转的角度，旋转范围为0°～360°；
两个坐标系之间的相互转换由姿态矩阵来表示，姿态矩阵也称作捷联矩阵T，由载体坐标系到导航坐标系的姿态矩阵表示为：
<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mi>T</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>T</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>11</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>T</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
由载体坐标系与导航坐标系之间的转换关系得：
<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
因此，由捷联矩阵T与姿态矩阵的矩阵关系求得的三个姿态角γT、θT、ψT计算如下：
θT＝arcsin(-T31)
3.根据权利要求1所述的基于四元数和卡尔曼滤波的人体姿态解算方法，其特征在于，步骤2所述加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数，具体如下：
三轴加速度计测量的是重力加速度在载体坐标系上的投影，假设加速度计的测量值fb为：
<mrow> <msub> <mi>f</mi> <mi>b</mi> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>x</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，为加速度计的测量值在载体坐标系X、Y、Z轴方向的投影；
重力加速度在导航坐标系上的投影fn为：
fn＝[0 0 g]T
由于加速度计的输出与航向角无关，所以姿态矩阵中的航向角为零，将此时的姿态矩阵代入得：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mi>g</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>x</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>y</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>f</mi> <mi>b</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> </mrow>
则载体的俯仰角θ和滚转角γ分别为：
<mrow> <mi>&amp;theta;</mi> <mo>=</mo> <mo>-</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <msubsup> <mi>f</mi> <mi>b</mi> <mi>x</mi> </msubsup> <mi>g</mi> </mfrac> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>&amp;gamma;</mi> <mo>=</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <msubsup> <mi>f</mi> <mi>b</mi> <mi>y</mi> </msubsup> <msubsup> <mi>f</mi> <mi>b</mi> <mi>z</mi> </msubsup> </mfrac> <mo>)</mo> </mrow> </mrow>
假设磁强计的测量值Bb沿着载体坐标系的三个坐标轴分解得到如下分量：
<mrow> <msub> <mi>B</mi> <mi>b</mi> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>x</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，为磁强计的测量值在载体坐标系X、Y、Z轴方向的投影；
当载体坐标系与导航坐标系重合时为：
<mrow> <msub> <mi>B</mi> <mi>n</mi> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>X</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，为磁强计的测量值在导航坐标系X、Y、Z轴方向的投影；
使载体先绕载体坐标系Y轴旋转当前俯仰角值，再绕X轴旋转当前滚转角值，从而使当前载体与水平面平行，根据坐标变换理论有：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>X</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Y</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>x</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>y</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msubsup> <mi>B</mi> <mi>b</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> </mrow>
由上式推出地磁场水平分量的表达式为：
<mrow> <msubsup> <mi>B</mi> <mi>n</mi> <mi>X</mi> </msubsup> <mo>=</mo> <msubsup> <mi>B</mi> <mi>b</mi> <mi>x</mi> </msubsup> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;theta;</mi> <mo>+</mo> <msubsup> <mi>B</mi> <mi>b</mi> <mi>y</mi> </msubsup> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;gamma;</mi> <mo>+</mo> <msubsup> <mi>B</mi> <mi>b</mi> <mi>z</mi> </msubsup> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;gamma;</mi> </mrow>
<mrow> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Y</mi> </msubsup> <mo>=</mo> <msubsup> <mi>B</mi> <mi>b</mi> <mi>y</mi> </msubsup> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;gamma;</mi> <mo>-</mo> <msubsup> <mi>B</mi> <mi>b</mi> <mi>z</mi> </msubsup> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow>
则航向角公式为：
<mrow> <mi>&amp;psi;</mi> <mo>=</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <msubsup> <mi>B</mi> <mi>n</mi> <mi>Y</mi> </msubsup> <msubsup> <mi>B</mi> <mi>n</mi> <mi>X</mi> </msubsup> </mfrac> <mo>)</mo> </mrow> </mrow>
由初始姿态角求得初始四元数q0、q1、q2、q3，求解公式如下：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> <mo>+</mo> <mi>sin</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <mi>cos</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> <mo>+</mo> <mi>sin</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <mi>sin</mi> <mfrac> <mi>&amp;gamma;</mi> <mn>2</mn> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;theta;</mi> <mn>2</mn> </mfrac> <mi>cos</mi> <mfrac> <mi>&amp;psi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
4.根据权利要求1所述的基于四元数和卡尔曼滤波的人体姿态解算方法，其特征在于，步骤3所述根据四元数微分方程进行系统建模，其中四元数微分方程如下：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中，ω1、ω2、ω3分别为陀螺仪三轴的角速度输出；
采用四阶龙格-库塔法对四元数微分方程进行求解，求出更新后的四元数；
在每个周期的姿态更新之后对四元数进行归一化处理，处理之后得到用四元数表示的姿态矩阵
<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>1</mn> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
5.根据权利要求1所述的基于四元数和卡尔曼滤波的人体姿态解算方法，其特征在于，步骤3所述采用卡尔曼滤波算法对姿态数据进行数据融合输出估计值，过程如下：
(1)设置四元数误差协方差矩阵测量噪声协方差矩阵R，过程噪声协方差矩阵Q、量测矩阵H的初始值：
<mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mn>0</mn> <mo>-</mo> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msubsup> <mi>P</mi> <mn>0</mn> <mo>-</mo> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>R</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>10</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>10</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>10</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>10</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <mi>H</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>Q</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0.0001</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0.0001</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0.0001</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0.0001</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
(2)根据四元数微分方程对系统进行系统建模，四元数微分方程转换为状态一步预测方程：的形式，所以选择四元数微分方程进行系统建模，四元数作为状态变量，系统模型如下：
<mrow> <msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mo>&amp;lsqb;</mo> <mi>I</mi> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>&amp;CenterDot;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>&amp;rsqb;</mo> <msub> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>k</mi> </msub> </mrow>
其中，为k时刻的预测状态变量，为k-1时刻的状态变量，I为一个4乘4的单位矩阵，Δt为k-1时刻到k时刻的时间差，下标k表示k时刻，k-1表示k-1时刻，k+1表示k+1时刻；
由系统模型推出系统状态矩阵A：
<mrow> <mi>A</mi> <mo>=</mo> <mi>I</mi> <mo>+</mo> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>&amp;CenterDot;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
(3)根据初始值以及系统状态矩阵A求取预测状态变量和预测误差协方差矩阵
<mrow> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <mi>A</mi> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
<mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>=</mo> <msub> <mi>AP</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msup> <mi>A</mi> <mi>T</mi> </msup> <mo>+</mo> <mi>Q</mi> </mrow>
(4)根据误差协方差矩阵、测量噪声协方差矩阵和量测矩阵计算卡尔曼增益Kk：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mo>-</mo> </msubsup> <msup> <mi>H</mi> <mi>T</mi> </msup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>HP</mi> <mi>k</mi> <mo>-</mo> </msubsup> <msup> <mi>H</mi> <mi>T</mi> </msup> <mo>+</mo> <mi>R</mi> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
(5)系统初始对准求得的初始四元数作为观测值zk，根据测量值来更新估计值并更新误差协方差矩阵：
<mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>H</mi> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mi>k</mi> <mo>-</mo> </msubsup> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mo>-</mo> </msubsup> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msubsup> <mi>HP</mi> <mi>k</mi> <mo>-</mo> </msubsup> </mrow>
估计值即为经过卡尔曼滤波数据融合算法后的人体姿态最终输出值。
CN201610405814.2A 2016-06-08 2016-06-08 一种基于四元数和卡尔曼滤波的人体姿态解算方法 Pending CN107478223A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610405814.2A CN107478223A (zh) 2016-06-08 2016-06-08 一种基于四元数和卡尔曼滤波的人体姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610405814.2A CN107478223A (zh) 2016-06-08 2016-06-08 一种基于四元数和卡尔曼滤波的人体姿态解算方法

Publications (1)

Publication Number Publication Date
CN107478223A true CN107478223A (zh) 2017-12-15

Family

ID=60594399

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610405814.2A Pending CN107478223A (zh) 2016-06-08 2016-06-08 一种基于四元数和卡尔曼滤波的人体姿态解算方法

Country Status (1)

Country Link
CN (1) CN107478223A (zh)

Cited By (73)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108458714A (zh) * 2018-01-11 2018-08-28 山东大学 一种姿态检测系统中不含重力加速度的欧拉角求解方法
CN108507527A (zh) * 2018-03-07 2018-09-07 北京电子工程总体研究所 一种摆动喷管姿态角解算方法
CN108519090A (zh) * 2018-03-27 2018-09-11 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN108731676A (zh) * 2018-05-04 2018-11-02 北京摩高科技有限公司 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN108827290A (zh) * 2018-06-15 2018-11-16 安徽中科智链信息科技有限公司 一种人体运动状态反演装置及方法
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN108871319A (zh) * 2018-04-26 2018-11-23 李志� 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN109029435A (zh) * 2018-06-22 2018-12-18 常州大学 提高惯性-地磁组合动态定姿精度的方法
CN109374001A (zh) * 2018-11-20 2019-02-22 中国电子科技集团公司第五十四研究所 一种结合行人运动场景约束的方位角校准算法
CN109459028A (zh) * 2018-11-22 2019-03-12 东南大学 一种基于梯度下降的自适应步长估计方法
CN109550219A (zh) * 2018-11-30 2019-04-02 歌尔科技有限公司 一种运动信息的确定方法、系统及移动设备
CN109584521A (zh) * 2018-10-15 2019-04-05 西安理工大学 一种基于Notch传感器的不良坐姿监测方法
CN109579836A (zh) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN109582197A (zh) * 2018-11-30 2019-04-05 北京小米移动软件有限公司 屏幕控制方法、装置及存储介质
CN109709576A (zh) * 2018-12-20 2019-05-03 安徽优思天成智能科技有限公司 一种用于废气激光雷达的姿态估计方法
CN109974714A (zh) * 2019-04-29 2019-07-05 南京航空航天大学 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110141240A (zh) * 2019-06-05 2019-08-20 河南理工大学 一种基于磁/惯性器件的脊柱测量仪
CN110160530A (zh) * 2019-06-18 2019-08-23 北京电子工程总体研究所 一种基于四元数的航天器姿态滤波方法
CN110189359A (zh) * 2019-05-20 2019-08-30 仇永生 一种基于分类学习的快速智能跟踪方法和装置
CN110207687A (zh) * 2018-04-25 2019-09-06 腾讯科技（深圳）有限公司 物体姿态测量方法、测量装置、电子设备及存储介质
CN110279420A (zh) * 2019-07-18 2019-09-27 郑州轻工业学院 基于极限学习机的便携式跌倒检测装置及检测方法
CN110285810A (zh) * 2019-06-13 2019-09-27 兖矿集团有限公司 一种基于惯性导航数据的采煤机自主定位方法及装置
CN110398256A (zh) * 2019-06-19 2019-11-01 北京摩高科技有限公司 一种人体单一姿态的初始校正方法
CN110595434A (zh) * 2019-09-10 2019-12-20 兰州交通大学 基于mems传感器的四元数融合姿态估计方法
CN110779554A (zh) * 2019-12-05 2020-02-11 上海钛米机器人科技有限公司 一种机械臂、基于imu的初始位姿的校准系统与方法
CN110877335A (zh) * 2019-09-27 2020-03-13 华南理工大学 一种基于混合滤波器自适应无标记机械臂轨迹跟踪方法
CN110954102A (zh) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111076722A (zh) * 2019-11-18 2020-04-28 广州南方卫星导航仪器有限公司 基于自适应的四元数的姿态估计方法及装置
CN111189473A (zh) * 2020-01-08 2020-05-22 湖北三江航天红峰控制有限公司 一种基于磁传感器和加表的航姿系统陀螺误差补偿方法
CN111288993A (zh) * 2018-12-10 2020-06-16 北京京东尚科信息技术有限公司 导航处理方法、装置、导航设备及存储介质
CN111325123A (zh) * 2020-02-10 2020-06-23 北京格灵深瞳信息技术有限公司 面部角度表示和标注方法、装置及计算机存储介质
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111590572A (zh) * 2020-05-15 2020-08-28 深圳国信泰富科技有限公司 一种机器人姿态更新方法及系统
CN111654634A (zh) * 2020-06-24 2020-09-11 杭州海康威视数字技术股份有限公司 确定摄像机中机芯组件和云台组件倾斜的方法、摄像机
CN111649745A (zh) * 2020-05-18 2020-09-11 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质
CN111700624A (zh) * 2020-07-27 2020-09-25 中国科学院合肥物质科学研究院 一种智能手环检测运动姿态的模式识别方法及系统
CN111707268A (zh) * 2020-06-19 2020-09-25 中国人民解放军国防科技大学 基于双欧法和四元数混合编排的无人机导航方法和系统
CN111803903A (zh) * 2019-04-10 2020-10-23 深圳先进技术研究院 一种健身动作识别方法、系统及电子设备
CN111964676A (zh) * 2020-08-26 2020-11-20 三一机器人科技有限公司 航姿解算方法、装置、电子设备及计算机可读存储介质
WO2020232727A1 (zh) * 2019-05-17 2020-11-26 南京东奇智能制造研究院有限公司 一种基于mimu的便携脊柱测量仪及方法
CN112084458A (zh) * 2020-08-07 2020-12-15 深圳市瑞立视多媒体科技有限公司 刚体姿态追踪方法及其装置、设备、存储介质
CN112577484A (zh) * 2019-09-29 2021-03-30 北京信息科技大学 一种小型气象探测设备应用的遥测装置
CN112603295A (zh) * 2020-12-15 2021-04-06 深圳先进技术研究院 一种基于可穿戴传感器的康复评估方法和系统
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112710309A (zh) * 2020-12-08 2021-04-27 中国石油大学胜利学院 航姿参数估计方法
CN112729222A (zh) * 2020-12-14 2021-04-30 北京航空航天大学 一种桩挖转杆位置的实时测量方法
CN112729279A (zh) * 2020-11-17 2021-04-30 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种基于cckf的水下无人潜器姿态测量方法
CN112729348A (zh) * 2021-01-10 2021-04-30 河南理工大学 一种用于imu系统的姿态自适应校正方法
CN112859138A (zh) * 2019-11-28 2021-05-28 中移物联网有限公司 一种姿态测量方法、装置及电子设备
CN112902828A (zh) * 2021-01-19 2021-06-04 陕西福音假肢有限责任公司 一种角度计算方法
CN112928688A (zh) * 2020-12-03 2021-06-08 福建和盛高科技产业有限公司 一种安装在架空线路上设备的保护方法
CN113063416A (zh) * 2021-02-05 2021-07-02 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113114105A (zh) * 2021-03-10 2021-07-13 上海工程技术大学 一种光伏电池组件输出特性动态测量方法
CN113126642A (zh) * 2021-04-26 2021-07-16 广东华南计算技术研究所 基于多mems惯性传感器的偏航角测量系统、方法和飞控系统
CN113296139A (zh) * 2021-05-27 2021-08-24 桂林电子科技大学 一种自适应图像光流与rtk融合测姿方法
CN113405553A (zh) * 2020-11-30 2021-09-17 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN113639744A (zh) * 2021-07-07 2021-11-12 武汉工程大学 一种用于仿生机器鱼的导航定位方法和系统
CN113793360A (zh) * 2021-08-31 2021-12-14 大连理工大学 基于惯性传感技术的三维人体重构方法
CN113830220A (zh) * 2021-11-04 2021-12-24 江苏科技大学 基于信息融合的电动车助力控制方法
CN114166216A (zh) * 2021-11-25 2022-03-11 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114459479A (zh) * 2022-02-21 2022-05-10 北京航天嘉诚精密科技发展有限公司 旋转载体姿态、位置测量装置与方法
CN111780749B (zh) * 2020-05-26 2022-06-03 北京航天控制仪器研究所 一种变轨机动飞机全姿态惯性导航的姿态控制方法
CN114608517A (zh) * 2022-03-14 2022-06-10 东南大学 一种应用于农用无人机植保作业的姿态解算方法
CN114964224A (zh) * 2022-04-19 2022-08-30 北京自动化控制设备研究所 一种微惯导系统误差自主抑制方法
CN114952804A (zh) * 2022-07-13 2022-08-30 山东中科先进技术有限公司 一种外骨骼一体化助力装置及方法
CN115031785A (zh) * 2022-06-21 2022-09-09 浙江大学 一种基于多传感器融合技术的软土勘测方法
CN115063945A (zh) * 2022-06-20 2022-09-16 浙江科技学院 基于姿态融合解算的跌倒检测报警方法及系统
CN115560756A (zh) * 2022-08-26 2023-01-03 北京开拓航宇导控科技有限公司 一种发射坐标系下微型自寻的导弹捷联导航方法
CN115855038A (zh) * 2022-11-22 2023-03-28 哈尔滨工程大学 一种短时高精度姿态保持方法
CN116058829A (zh) * 2022-12-26 2023-05-05 青岛大学 基于imu的实时显示人体下肢姿态的系统
CN116182839A (zh) * 2023-04-27 2023-05-30 北京李龚导航科技有限公司 一种飞行器姿态的确定方法、装置、电子设备及存储介质
CN117128956A (zh) * 2023-08-30 2023-11-28 中国海洋大学 一种基于角速度转换的动态倾角方法及应用该方法的设备

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN106342284B (zh) * 2008-08-18 2011-11-23 西北工业大学 一种飞行载体姿态确定方法
CN103170979A (zh) * 2013-02-06 2013-06-26 华南理工大学 基于惯性测量仪的在线机器人参数辨识方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106342284B (zh) * 2008-08-18 2011-11-23 西北工业大学 一种飞行载体姿态确定方法
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN103170979A (zh) * 2013-02-06 2013-06-26 华南理工大学 基于惯性测量仪的在线机器人参数辨识方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
XIAO-CHEN LAI,等: "Research on Posture Recognition Based on Kalman Filter and Quaternion", 《2012 INTERNATIONAL CONFERENCE ON SYSTEM SCIENCE AND ENGINEERING》 *
张志福: "基于MEMS传感器的人体姿态跟踪系统研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
李经纬: "基于微型惯性传感器腿部康复动作捕捉系统研究", 《万方硕士学位论文》 *
陈伟: "基于四元数和卡尔曼滤波的姿态角估计算法研究与应用", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (108)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108458714A (zh) * 2018-01-11 2018-08-28 山东大学 一种姿态检测系统中不含重力加速度的欧拉角求解方法
CN108507527A (zh) * 2018-03-07 2018-09-07 北京电子工程总体研究所 一种摆动喷管姿态角解算方法
CN108507527B (zh) * 2018-03-07 2020-12-22 北京电子工程总体研究所 一种摆动喷管姿态角解算方法
CN108519090B (zh) * 2018-03-27 2021-08-20 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN108519090A (zh) * 2018-03-27 2018-09-11 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN108827301A (zh) * 2018-04-16 2018-11-16 南京航空航天大学 一种改进误差四元数卡尔曼滤波机器人姿态解算方法
CN110207687A (zh) * 2018-04-25 2019-09-06 腾讯科技（深圳）有限公司 物体姿态测量方法、测量装置、电子设备及存储介质
CN110207687B (zh) * 2018-04-25 2022-08-23 腾讯科技（深圳）有限公司 物体姿态测量方法、测量装置、电子设备及存储介质
CN108871319A (zh) * 2018-04-26 2018-11-23 李志� 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN108731676A (zh) * 2018-05-04 2018-11-02 北京摩高科技有限公司 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN108731676B (zh) * 2018-05-04 2021-06-15 北京摩高科技有限公司 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN108827290A (zh) * 2018-06-15 2018-11-16 安徽中科智链信息科技有限公司 一种人体运动状态反演装置及方法
CN109029435B (zh) * 2018-06-22 2021-11-02 常州大学 提高惯性-地磁组合动态定姿精度的方法
CN109029435A (zh) * 2018-06-22 2018-12-18 常州大学 提高惯性-地磁组合动态定姿精度的方法
CN109584521A (zh) * 2018-10-15 2019-04-05 西安理工大学 一种基于Notch传感器的不良坐姿监测方法
CN109374001A (zh) * 2018-11-20 2019-02-22 中国电子科技集团公司第五十四研究所 一种结合行人运动场景约束的方位角校准算法
CN109579836B (zh) * 2018-11-21 2022-09-06 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN109579836A (zh) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN109459028A (zh) * 2018-11-22 2019-03-12 东南大学 一种基于梯度下降的自适应步长估计方法
CN109582197A (zh) * 2018-11-30 2019-04-05 北京小米移动软件有限公司 屏幕控制方法、装置及存储介质
CN109550219A (zh) * 2018-11-30 2019-04-02 歌尔科技有限公司 一种运动信息的确定方法、系统及移动设备
CN111288993B (zh) * 2018-12-10 2023-12-05 北京京东尚科信息技术有限公司 导航处理方法、装置、导航设备及存储介质
CN111288993A (zh) * 2018-12-10 2020-06-16 北京京东尚科信息技术有限公司 导航处理方法、装置、导航设备及存储介质
CN109709576A (zh) * 2018-12-20 2019-05-03 安徽优思天成智能科技有限公司 一种用于废气激光雷达的姿态估计方法
CN109709576B (zh) * 2018-12-20 2022-05-17 安徽优思天成智能科技有限公司 一种用于废气激光雷达的姿态估计方法
CN111803903A (zh) * 2019-04-10 2020-10-23 深圳先进技术研究院 一种健身动作识别方法、系统及电子设备
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110017837B (zh) * 2019-04-26 2022-11-25 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN109974714A (zh) * 2019-04-29 2019-07-05 南京航空航天大学 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
WO2020232727A1 (zh) * 2019-05-17 2020-11-26 南京东奇智能制造研究院有限公司 一种基于mimu的便携脊柱测量仪及方法
CN110189359B (zh) * 2019-05-20 2021-08-27 仇永生 一种基于分类学习的快速智能跟踪方法和装置
CN110189359A (zh) * 2019-05-20 2019-08-30 仇永生 一种基于分类学习的快速智能跟踪方法和装置
CN110141240A (zh) * 2019-06-05 2019-08-20 河南理工大学 一种基于磁/惯性器件的脊柱测量仪
CN110285810A (zh) * 2019-06-13 2019-09-27 兖矿集团有限公司 一种基于惯性导航数据的采煤机自主定位方法及装置
CN110160530A (zh) * 2019-06-18 2019-08-23 北京电子工程总体研究所 一种基于四元数的航天器姿态滤波方法
CN110398256B (zh) * 2019-06-19 2021-12-03 北京摩高科技有限公司 一种人体单一姿态的初始校正方法
CN110398256A (zh) * 2019-06-19 2019-11-01 北京摩高科技有限公司 一种人体单一姿态的初始校正方法
CN110279420A (zh) * 2019-07-18 2019-09-27 郑州轻工业学院 基于极限学习机的便携式跌倒检测装置及检测方法
CN110595434A (zh) * 2019-09-10 2019-12-20 兰州交通大学 基于mems传感器的四元数融合姿态估计方法
CN110877335A (zh) * 2019-09-27 2020-03-13 华南理工大学 一种基于混合滤波器自适应无标记机械臂轨迹跟踪方法
CN112577484A (zh) * 2019-09-29 2021-03-30 北京信息科技大学 一种小型气象探测设备应用的遥测装置
CN111076722A (zh) * 2019-11-18 2020-04-28 广州南方卫星导航仪器有限公司 基于自适应的四元数的姿态估计方法及装置
CN111076722B (zh) * 2019-11-18 2022-07-19 广州南方卫星导航仪器有限公司 基于自适应的四元数的姿态估计方法及装置
CN112859138B (zh) * 2019-11-28 2023-09-05 中移物联网有限公司 一种姿态测量方法、装置及电子设备
CN112859138A (zh) * 2019-11-28 2021-05-28 中移物联网有限公司 一种姿态测量方法、装置及电子设备
CN110779554A (zh) * 2019-12-05 2020-02-11 上海钛米机器人科技有限公司 一种机械臂、基于imu的初始位姿的校准系统与方法
CN110954102A (zh) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110954102B (zh) * 2019-12-18 2022-01-07 无锡北微传感科技有限公司 用于机器人定位的磁力计辅助惯性导航系统及方法
CN111189473A (zh) * 2020-01-08 2020-05-22 湖北三江航天红峰控制有限公司 一种基于磁传感器和加表的航姿系统陀螺误差补偿方法
CN111325123A (zh) * 2020-02-10 2020-06-23 北京格灵深瞳信息技术有限公司 面部角度表示和标注方法、装置及计算机存储介质
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111590572B (zh) * 2020-05-15 2021-05-04 深圳国信泰富科技有限公司 一种机器人姿态更新方法及系统
CN111590572A (zh) * 2020-05-15 2020-08-28 深圳国信泰富科技有限公司 一种机器人姿态更新方法及系统
CN111649745B (zh) * 2020-05-18 2022-04-05 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质
CN111649745A (zh) * 2020-05-18 2020-09-11 北京三快在线科技有限公司 电子设备的姿态估计方法、装置、电子设备和存储介质
CN111780749B (zh) * 2020-05-26 2022-06-03 北京航天控制仪器研究所 一种变轨机动飞机全姿态惯性导航的姿态控制方法
CN111707268A (zh) * 2020-06-19 2020-09-25 中国人民解放军国防科技大学 基于双欧法和四元数混合编排的无人机导航方法和系统
CN111654634B (zh) * 2020-06-24 2022-02-08 杭州海康威视数字技术股份有限公司 确定摄像机中机芯组件和云台组件倾斜的方法、摄像机
CN111654634A (zh) * 2020-06-24 2020-09-11 杭州海康威视数字技术股份有限公司 确定摄像机中机芯组件和云台组件倾斜的方法、摄像机
CN111700624B (zh) * 2020-07-27 2024-03-12 中国科学院合肥物质科学研究院 一种智能手环检测运动姿态的模式识别方法及系统
CN111700624A (zh) * 2020-07-27 2020-09-25 中国科学院合肥物质科学研究院 一种智能手环检测运动姿态的模式识别方法及系统
CN112084458A (zh) * 2020-08-07 2020-12-15 深圳市瑞立视多媒体科技有限公司 刚体姿态追踪方法及其装置、设备、存储介质
CN111964676A (zh) * 2020-08-26 2020-11-20 三一机器人科技有限公司 航姿解算方法、装置、电子设备及计算机可读存储介质
CN112729279A (zh) * 2020-11-17 2021-04-30 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种基于cckf的水下无人潜器姿态测量方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN113405553A (zh) * 2020-11-30 2021-09-17 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN113405553B (zh) * 2020-11-30 2023-05-26 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN112928688A (zh) * 2020-12-03 2021-06-08 福建和盛高科技产业有限公司 一种安装在架空线路上设备的保护方法
CN112710309A (zh) * 2020-12-08 2021-04-27 中国石油大学胜利学院 航姿参数估计方法
CN112729222A (zh) * 2020-12-14 2021-04-30 北京航空航天大学 一种桩挖转杆位置的实时测量方法
CN112603295B (zh) * 2020-12-15 2022-11-08 深圳先进技术研究院 一种基于可穿戴传感器的康复评估方法和系统
CN112603295A (zh) * 2020-12-15 2021-04-06 深圳先进技术研究院 一种基于可穿戴传感器的康复评估方法和系统
CN112729348A (zh) * 2021-01-10 2021-04-30 河南理工大学 一种用于imu系统的姿态自适应校正方法
CN112729348B (zh) * 2021-01-10 2023-11-28 河南理工大学 一种用于imu系统的姿态自适应校正方法
CN112902828B (zh) * 2021-01-19 2023-09-08 陕西福音假肢有限责任公司 一种角度计算方法
CN112902828A (zh) * 2021-01-19 2021-06-04 陕西福音假肢有限责任公司 一种角度计算方法
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113063416A (zh) * 2021-02-05 2021-07-02 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113114105B (zh) * 2021-03-10 2022-08-09 上海工程技术大学 一种光伏电池组件输出特性动态测量方法
CN113114105A (zh) * 2021-03-10 2021-07-13 上海工程技术大学 一种光伏电池组件输出特性动态测量方法
CN113126642A (zh) * 2021-04-26 2021-07-16 广东华南计算技术研究所 基于多mems惯性传感器的偏航角测量系统、方法和飞控系统
CN113296139A (zh) * 2021-05-27 2021-08-24 桂林电子科技大学 一种自适应图像光流与rtk融合测姿方法
CN113296139B (zh) * 2021-05-27 2022-05-03 桂林电子科技大学 一种自适应图像光流与rtk融合测姿方法
CN113639744B (zh) * 2021-07-07 2023-10-20 武汉工程大学 一种用于仿生机器鱼的导航定位方法和系统
CN113639744A (zh) * 2021-07-07 2021-11-12 武汉工程大学 一种用于仿生机器鱼的导航定位方法和系统
CN113793360B (zh) * 2021-08-31 2024-02-06 大连理工大学 基于惯性传感技术的三维人体重构方法
CN113793360A (zh) * 2021-08-31 2021-12-14 大连理工大学 基于惯性传感技术的三维人体重构方法
CN113830220B (zh) * 2021-11-04 2022-09-13 浙江欧飞电动车有限公司 基于信息融合的电动车助力控制方法
CN113830220A (zh) * 2021-11-04 2021-12-24 江苏科技大学 基于信息融合的电动车助力控制方法
CN114166216A (zh) * 2021-11-25 2022-03-11 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114166216B (zh) * 2021-11-25 2023-07-21 哈尔滨工程大学 一种aru正置与倒置安装的水平姿态测量方法
CN114459479A (zh) * 2022-02-21 2022-05-10 北京航天嘉诚精密科技发展有限公司 旋转载体姿态、位置测量装置与方法
CN114608517A (zh) * 2022-03-14 2022-06-10 东南大学 一种应用于农用无人机植保作业的姿态解算方法
CN114608517B (zh) * 2022-03-14 2023-11-10 东南大学 一种应用于农用无人机植保作业的姿态解算方法
CN114964224B (zh) * 2022-04-19 2023-11-03 北京自动化控制设备研究所 一种微惯导系统误差自主抑制方法
CN114964224A (zh) * 2022-04-19 2022-08-30 北京自动化控制设备研究所 一种微惯导系统误差自主抑制方法
CN115063945B (zh) * 2022-06-20 2023-12-29 浙江科技学院 基于姿态融合解算的跌倒检测报警方法及系统
CN115063945A (zh) * 2022-06-20 2022-09-16 浙江科技学院 基于姿态融合解算的跌倒检测报警方法及系统
CN115031785A (zh) * 2022-06-21 2022-09-09 浙江大学 一种基于多传感器融合技术的软土勘测方法
CN114952804A (zh) * 2022-07-13 2022-08-30 山东中科先进技术有限公司 一种外骨骼一体化助力装置及方法
CN115560756A (zh) * 2022-08-26 2023-01-03 北京开拓航宇导控科技有限公司 一种发射坐标系下微型自寻的导弹捷联导航方法
CN115855038B (zh) * 2022-11-22 2024-01-09 哈尔滨工程大学 一种短时高精度姿态保持方法
CN115855038A (zh) * 2022-11-22 2023-03-28 哈尔滨工程大学 一种短时高精度姿态保持方法
CN116058829A (zh) * 2022-12-26 2023-05-05 青岛大学 基于imu的实时显示人体下肢姿态的系统
CN116182839A (zh) * 2023-04-27 2023-05-30 北京李龚导航科技有限公司 一种飞行器姿态的确定方法、装置、电子设备及存储介质
CN117128956A (zh) * 2023-08-30 2023-11-28 中国海洋大学 一种基于角速度转换的动态倾角方法及应用该方法的设备
CN117128956B (zh) * 2023-08-30 2024-03-26 中国海洋大学 基于角速度转换的动态倾角获取方法及应用该方法的设备

Similar Documents

Publication Publication Date Title
CN107478223A (zh) 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN104898681B (zh) 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
CN108225308A (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN104635251B (zh) 一种ins/gps组合定位定姿新方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN106767785B (zh) 一种双回路无人机的导航方法及装置
CN103776446B (zh) 一种基于双mems-imu的行人自主导航解算算法
CN106979780B (zh) 一种无人车实时姿态测量方法
CN106679649A (zh) 一种手部运动追踪系统及追踪方法
CN106780699A (zh) 一种基于sins/gps和里程计辅助的视觉slam方法
CN110398257A (zh) Gps辅助的sins系统快速动基座初始对准方法
CN106153073B (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN106643715A (zh) 一种基于bp神经网络改善的室内惯性导航方法
CN102937450B (zh) 一种基于陀螺测量信息的相对姿态确定方法
CN103940442A (zh) 一种采用加速收敛算法的定位方法及装置
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN107255474B (zh) 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN103512584A (zh) 导航姿态信息输出方法、装置及捷联航姿参考系统
CN108132053B (zh) 一种行人轨迹构建方法、系统及惯性测量装置
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN106052716A (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
RJ01 Rejection of invention patent application after publication

Application publication date: 20171215

RJ01 Rejection of invention patent application after publication