[发明专利]一种基于四元数和卡尔曼滤波的人体姿态解算方法

说明书

技术领域

本发明属于姿态检测技术领域，特别是一种基于四元数和卡尔曼滤波的人体姿态解算方法。

背景技术

人体姿态解算的方法主要有余弦矩阵法、欧拉角法和四元数法，余弦矩阵法计算量较大、解算较为复杂，欧拉角法虽然可以更加直观地表示姿态角，但是在姿态解算中存在奇点问题，而四元数法不但计算相对余弦矩阵法简单，而且避免了欧拉角法存在的奇点问题。

传统的姿态解算大都基于加速度计和陀螺仪两种惯性传感器，陀螺仪通过积分求得姿态角，在短时间内可以提供准确的姿态信息，但长期精度较差。而加速度计的测量误差不随时间积累，长期性能较好，但是短期内精度较差，动态响应较慢。现有的较多解算方法缺少对含有噪声的数据进行滤波，部分算法采用了互补滤波算法，但是互补滤波算法过于简单并且参数的修正较为繁琐，因此达不到良好的滤波效果。同时，也没有对数据进行有效的融合，由此导致姿态解算的结果精度较低，稳定性和可靠性不足。除此之外，惯性传感器选择上的不足同样影响姿态解算的精度。

发明内容

本发明的目的在于提供一种基于四元数和卡尔曼滤波的人体姿态解算方法，以提高人体姿态检测的精度和响应速度，使人体姿态检测系统具有更好的实时性和稳定性。

本发明的技术解决方案为：一种基于四元数和卡尔曼滤波的人体姿态解算方法，包括以下步骤：

步骤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所述加速度计、陀螺仪和磁强计分别采集加速度、角速度和磁感应强度信号，根据采集到的姿态数据对人体姿态检测系统进行初始对准，求得初始的姿态角即俯仰角、翻滚角和航向角，并将初始姿态角转化为初始化四元数，具体如下：

三轴加速度计测量的是重力加速度在载体坐标系上的投影，假设加速度计的测量值f_b为：

其中，为加速度计的测量值在载体坐标系X、Y、Z轴方向的投影；

重力加速度在导航坐标系上的投影f_n为：

f_n＝[0 0 g]^T

由于加速度计的输出与航向角无关，所以姿态矩阵中的航向角为零，将此时的姿态矩阵代入得：

则载体的俯仰角θ和滚转角γ分别为：

假设磁强计的测量值B_b沿着载体坐标系的三个坐标轴分解得到如下分量：

其中，为磁强计的测量值在载体坐标系X、Y、Z轴方向的投影；

当载体坐标系与导航坐标系重合时为：

其中，为磁强计的测量值在导航坐标系X、Y、Z轴方向的投影；

使载体先绕载体坐标系Y轴旋转当前俯仰角值，再绕X轴旋转当前滚转角值，从而使当前载体与水平面平行，根据坐标变换理论有：

由上式推出地磁场水平分量的表达式为：