[发明专利]一种基于改进互补滤波的人体运动姿态解算方法

说明书

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

技术领域

本发明属于微机电惯性测量单元的动作捕捉领域，具体涉及一种基于改进互补滤波的人体运动姿态解算方法。

背景技术

基于微机电惯性测量单元(MIMU)的姿态解算系统由于其自主性强，受外界环境限制或干扰较小，性价比高，体型小巧的特点，被广泛应用于飞行器、机器人、人体动作捕捉等多个领域。惯性测量单元由三轴陀螺仪和三轴加速度计组成，可用于测量载体的角速度和加速度。由于陀螺仪存在瞬时漂移，在使用角速度积分获得载体姿态的过程中，系统误差会随时间累积，所解算的姿态存在低频噪声，长时间工作很可能造成发散。而利用载体坐标系下重力加速度分量解算姿态角只基于瞬时加速度数据，其误差不随时间积累，所解算的姿态存在高频噪声。基于互补滤波的姿态解算方法可通过PI控制器对陀螺仪所估计姿态进行高通滤波，对加速度计所估计姿态进行低通滤波，从而提高姿态解算精度，使系统可以长时间工作。该类算法的计算量小，解算效果较好。但传统互补滤波法所输入进PI控制器的误差量没有明确物理含义，导致算法受参数影响较大，鲁棒性较弱。

发明内容

有鉴于此，本发明提供了一种基于改进互补滤波的人体运动姿态解算方法，能够高效地补偿角速率误差，提高解算精度。

实现本发明的技术方案如下：

一种基于改进互补滤波的人体运动姿态解算方法，包括以下步骤：

步骤1：分别建立陀螺仪和加速度计的量测模型，将微机电惯性测量单元(MIMU)固定在人体上，采集人体运动的加速度和角速率；

步骤2：利用MIMU所采集的角速率y_G,t求解姿态四元数微分方程，获得人体先验姿态

步骤3：将人体先验姿态代入加速度计的量测模型计算当前时刻加速度估计值利用当前时刻加速度y_A,t与所求得的加速度估计值计算出姿态误差角速率e_t；

步骤4：判断此时人体运动状态，如果处于静止状态，则直接使用姿态误差角速率e_t对角速率y_G,t进行补偿；如果处于运动状态，则将姿态误差角速率e_t经PI控制器的比例运算和积分运算后获得补偿信息，再对角速率y_G,t进行补偿；

步骤5：利用补偿后的角速率更新姿态四元数微分方程，获得人体后验姿态

进一步地，

其中，T为MIMU中陀螺仪的采样周期，和分别为向量的三个元素。

进一步地，步骤四中，利用y_A,t与之差的幅值判断当前人体是否处于静止状态。

有益效果：

1、本发明提出了一种基于改进互补滤波的人体运动姿态解算方法，它通过加速度数据对陀螺仪数据进行补偿，利用补偿后的角速率对姿态四元数进行更新，可用于全姿态解算。

2、考虑到当前时刻之前的解算误差和当前时刻的陀螺仪噪声所引入误差，本发明定义了一种姿态误差角速率，并通过建立加速度计和陀螺仪量测模型，推导出姿态误差角速率的计算公式。