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

权利要求书

1.一种基于模糊控制的MEMS多传感器数据融合处理方法，其特征在于，所述数据融合处理方法步骤如下：

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

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

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

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

2.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤一中自适应卡尔曼滤波的系统状态方程，是基于姿态四元数与旋转矢量之间的关系推导转动四元数微分方程，然后解微分方程得到的。

3.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤一中自适应卡尔曼滤波的系统量测方程，是根据三轴加速度和三轴磁场数据在载体坐标系下与地理坐标系下的坐标变换关系推导得到的。

4.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤二中的系统状态初值的求解方法具体为：

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

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

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

5.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤三中的自适应卡尔曼滤波流程具体实现步骤如下：

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

S2：根据量测输入以及模糊控制得到的量测协方差矩阵进行量测更新，计算系统卡尔曼滤波增益，更新系统估计值及误差协方差；

S3：将状态量估计值以及误差协方差的估计值作为下一次运算的初值，根据量测输入实时循环估计载体姿态四元数。

6.如权利要求5所述的MEMS多传感器数据融合处理方法，其特征在于，基于模糊控制的量测噪声协方差的更新方法具体为：

S2.1：计算系统残差的理论方差与实际方差，将两个方差矩阵迹的比值作为模糊控制的输入；

S2.2：构建模糊控制规则以及输入输出隶属度函数，根据模糊控制的输出实时调整系统量测噪声矩阵的大小。

7.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤四中的载体姿态角的求解，是根据系统状态量的估计值中的姿态四元数归一化之后，根据四元数与姿态角的转换关系计算得到的。