一种基于模糊控制的MEMS多传感器数据融合处理方法技术

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

【技术实现步骤摘要】
一种基于模糊控制的MEMS多传感器数据融合处理方法


[0001]本专利技术属于导航
，具体涉及一种基于模糊控制的MEMS多传感器数据融合处理方法。

技术介绍

[0002]MEMS(Micro
‑
Electro
‑
Mechanical Systems，微机电系统)器件因具有体积小、成本低、测量范围大、可靠性高等优点，作为先进的导航、制导构成部件被广泛应用于汽车工业、生物医学工程、航空航天、精密仪器、移动通信、国防科技等领域。由三轴MEMS陀螺仪，三轴MEMS加速度计、三轴MEMS磁传感器、单片机以及相应的外围电路构成的九轴磁罗盘，可广泛应用于载体姿态测量、人体姿态感知等领域。
[0003]然而，由三轴陀螺仪测得的三轴角速度经过积分运算得到的姿态角虽然短时精度较高，但是由于MEMS陀螺仪精度低、漂移大等缺点，误差会随着时间的增长而累积，而利用三轴MEMS加速度计与三轴MEMS磁传感器测得的载体姿态角虽然不会随着时间的增长而累积，但当载体运动时，会受运动加速度的影响，加速度计的信号将不能正确反映载体的姿态信息。因此，需要设计一种多传感器数据融合方法，将三轴陀螺仪、三轴加速度计与三轴磁传感器的数据进行融合，将彼此的优缺点进行互补，建立一种基于模糊控制的MEMS多传感器数据融合处理方法，计算出准确的姿态角。

技术实现思路

[0004]有鉴于此，本专利技术提供了一种基于模糊控制的MEMS多传感器数据融合处理方法，通过自适应卡尔曼滤波实现三轴加速度计、三轴磁传感器、三轴陀螺仪的数据融合，解算得到高精度姿态角。
[0005]本专利技术是通过下述技术方案实现的：
[0006]一种基于模糊控制的MEMS多传感器数据融合处理方法，包括如下步骤：
[0007]步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；
[0008]步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；
[0009]步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；
[0010]步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。
[0011]进一步的，所述步骤一中自适应卡尔曼滤波的系统状态方程，是基于姿态四元数与旋转矢量之间的关系推导转动四元数微分方程，然后解微分方程得到的。
[0012]进一步的，所述步骤一中自适应卡尔曼滤波的系统量测方程，是根据三轴加速度和三轴磁场数据在载体坐标系下与地理坐标系下的坐标变换关系推导得到的。
[0013]进一步的，所述步骤二中的系统状态初值的求解方法具体为：
[0014]S1:根据准静态条件下重力加速度在载体坐标系与地理坐标系下的坐标变换关系计算得到俯仰角以及横滚角；
[0015]S2:利用俯仰角与横滚角以及测得的三轴磁场数据计算得到地磁场的水平分量，再用反三角函数计算得到方位角；
[0016]S3:通过姿态角与四元数之间的转换关系计算得到初始状态下的姿态角四元数。
[0017]进一步的，所述步骤三中的自适应卡尔曼滤波流程具体实现步骤如下：
[0018]S1：在得到系统状态初值后，状态更新得到一步状态预测值以及状态预测协方差值；
[0019]S2：根据量测输入以及模糊控制得到的量测协方差矩阵进行量测更新，计算系统卡尔曼滤波增益，更新系统估计值及误差协方差；
[0020]S3：将状态量估计值以及误差协方差的估计值作为下一次运算的初值，根据量测输入实时循环估计载体姿态四元数。
[0021]进一步的，基于模糊控制的量测噪声协方差的更新方法具体为：
[0022]S2.1：计算系统残差的理论方差与实际方差，将两个方差矩阵迹的比值作为模糊控制的输入；
[0023]S2.2：构建模糊控制规则以及输入输出隶属度函数，根据模糊控制的输出实时调整系统量测噪声矩阵的大小。
[0024]进一步的，步骤四中的载体姿态角的求解，是根据系统状态量的估计值中的姿态四元数归一化之后，根据四元数与姿态角的转换关系计算得到的。
[0025]有益效果：
[0026]本专利技术能够通过卡尔曼滤波数据融合实现高精度姿态角输出，最终输出的姿态角不受运动加速度及外界磁干扰的影响，且误差不会随着时间的增长而累积。
附图说明
[0027]图1为系统卡尔曼滤波流程图；
[0028]图2为模糊控制输入隶属度函数图；
[0029]图3为模糊控制输出隶属度函数图。
具体实施方式
[0030]下面结合附图，对本专利技术进行详细描述。
[0031]本专利技术基于模糊控制的MEMS多传感器数据融合处理方法，包括以下步骤：
[0032]步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。
[0033]以下具体介绍各个步骤：
[0034](1)建立自适应卡尔曼滤波系统的状态方程及量测方程：
[0035]由于使用欧拉角方法计算姿态角存在奇点问题。本专利技术从工程实用角度出发，采
用四元数方法求载体的姿态角，四元数法是利用一种超复数的乘法性质来等效描述坐标系转动的方法，转动四元数Q的定义为：
[0036]Q＝q0+q1i+q2j+q3k(1)
[0037]其中，q0～q3为四元数的数值，i、j、k分别为对应的四元数虚数标志，坐标系的瞬时转动可由如下的转动四元数的微分方程表示：
[0038][0039]式中为载体坐标系下的三轴角速率输出值即三轴陀螺仪的输出值，表示为四元数乘法。将上式按照四元数乘法展开计算可以得：
[0040][0041]重力加速度在地理坐标系下定义为g
e
，在载体坐标系下定义为g
b
，同样的，地理坐标系各坐标轴向量与载体坐标系各坐标轴向量间的转换关系可由如下四元数乘法表示：
[0042][0043]其中，Q*表示Q的共轭，将式(4)左右两边同时左乘Q可得：
[0044][0045]将式(5)用四元数乘法展开可表示成：
[0046][0047]其中，三维向量A＝[A
x
A
y
A
z
]，由此可以得到四元数Q与量测向量g
b
、g
e
之间的线性关系式：
[0048]h
g
·
Q＝04×1(7)
[0049]其中
[0050]同样的可以得到地磁向量与四元数之间的线性本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于模糊控制的MEMS多传感器数据融合处理方法，其特征在于，所述数据融合处理方法步骤如下：步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。2.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤一中自适应卡尔曼滤波的系统状态方程，是基于姿态四元数与旋转矢量之间的关系推导转动四元数微分方程，然后解微分方程得到的。3.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤一中自适应卡尔曼滤波的系统量测方程，是根据三轴加速度和三轴磁场数据在载体坐标系下与地理坐标系下的坐标变换关系推导得到的。4.如权利要求1所述的MEMS多传感器数据融合处理方法，其特征在于，所述步骤二中的系统状态初值的求解方法具体为：S1:根据准静态条件下重力加速度在载体坐标系与地理坐标系下的坐标变换关系计算得到俯仰角以及横滚角；S2:利用俯仰角与横滚...

【专利技术属性】
技术研发人员：伍东凌，吕冰，邓超凡，窦柯，孟诚，
申请(专利权)人：宜昌测试技术研究所，
类型：发明
国别省市：