[发明专利]一种基于模糊控制的MEMS多传感器数据融合处理方法

说明书

本发明涉及一种基于模糊控制的MEMS多传感器数据融合处理方法，其步骤包括：建立自适应卡尔曼滤波系统的状态方程及量测方程；利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。本发明能够通过卡尔曼滤波数据融合实现高精度姿态角输出，最终输出的姿态角不受运动加速度及外界磁干扰的影响，且误差不会随着时间的增长而累积。

技术领域

本发明属于导航技术领域，具体涉及一种基于模糊控制的MEMS多传感器数据融合处理方法。

背景技术

MEMS(Micro-Electro-Mechanical Systems，微机电系统)器件因具有体积小、成本低、测量范围大、可靠性高等优点，作为先进的导航、制导构成部件被广泛应用于汽车工业、生物医学工程、航空航天、精密仪器、移动通信、国防科技等领域。由三轴MEMS陀螺仪，三轴MEMS加速度计、三轴MEMS磁传感器、单片机以及相应的外围电路构成的九轴磁罗盘，可广泛应用于载体姿态测量、人体姿态感知等领域。

然而，由三轴陀螺仪测得的三轴角速度经过积分运算得到的姿态角虽然短时精度较高，但是由于MEMS陀螺仪精度低、漂移大等缺点，误差会随着时间的增长而累积，而利用三轴MEMS加速度计与三轴MEMS磁传感器测得的载体姿态角虽然不会随着时间的增长而累积，但当载体运动时，会受运动加速度的影响，加速度计的信号将不能正确反映载体的姿态信息。因此，需要设计一种多传感器数据融合方法，将三轴陀螺仪、三轴加速度计与三轴磁传感器的数据进行融合，将彼此的优缺点进行互补，建立一种基于模糊控制的MEMS多传感器数据融合处理方法，计算出准确的姿态角。

发明内容

有鉴于此，本发明提供了一种基于模糊控制的MEMS多传感器数据融合处理方法，通过自适应卡尔曼滤波实现三轴加速度计、三轴磁传感器、三轴陀螺仪的数据融合，解算得到高精度姿态角。

本发明是通过下述技术方案实现的：

一种基于模糊控制的MEMS多传感器数据融合处理方法，包括如下步骤：

步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；

步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；

步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；

步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。

进一步的，所述步骤一中自适应卡尔曼滤波的系统状态方程，是基于姿态四元数与旋转矢量之间的关系推导转动四元数微分方程，然后解微分方程得到的。

进一步的，所述步骤一中自适应卡尔曼滤波的系统量测方程，是根据三轴加速度和三轴磁场数据在载体坐标系下与地理坐标系下的坐标变换关系推导得到的。

进一步的，所述步骤二中的系统状态初值的求解方法具体为：

S1:根据准静态条件下重力加速度在载体坐标系与地理坐标系下的坐标变换关系计算得到俯仰角以及横滚角；

S2:利用俯仰角与横滚角以及测得的三轴磁场数据计算得到地磁场的水平分量，再用反三角函数计算得到方位角；

S3:通过姿态角与四元数之间的转换关系计算得到初始状态下的姿态角四元数。

进一步的，所述步骤三中的自适应卡尔曼滤波流程具体实现步骤如下：

S1：在得到系统状态初值后，状态更新得到一步状态预测值以及状态预测协方差值；