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

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


背景技术：

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


技术实现要素：

4.有鉴于此，本发明提供了一种基于模糊控制的mems多传感器数据融合处理方法，通过自适应卡尔曼滤波实现三轴加速度计、三轴磁传感器、三轴陀螺仪的数据融合，解算得到高精度姿态角。
5.本发明是通过下述技术方案实现的：
6.一种基于模糊控制的mems多传感器数据融合处理方法，包括如下步骤：
7.步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；
8.步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；
9.步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；
10.步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。
11.进一步的，所述步骤一中自适应卡尔曼滤波的系统状态方程，是基于姿态四元数与旋转矢量之间的关系推导转动四元数微分方程，然后解微分方程得到的。
12.进一步的，所述步骤一中自适应卡尔曼滤波的系统量测方程，是根据三轴加速度和三轴磁场数据在载体坐标系下与地理坐标系下的坐标变换关系推导得到的。
13.进一步的，所述步骤二中的系统状态初值的求解方法具体为：
14.s1:根据准静态条件下重力加速度在载体坐标系与地理坐标系下的坐标变换关系计算得到俯仰角以及横滚角；
15.s2:利用俯仰角与横滚角以及测得的三轴磁场数据计算得到地磁场的水平分量，再用反三角函数计算得到方位角；
16.s3:通过姿态角与四元数之间的转换关系计算得到初始状态下的姿态角四元数。
17.进一步的，所述步骤三中的自适应卡尔曼滤波流程具体实现步骤如下：
18.s1：在得到系统状态初值后，状态更新得到一步状态预测值以及状态预测协方差值；
19.s2：根据量测输入以及模糊控制得到的量测协方差矩阵进行量测更新，计算系统卡尔曼滤波增益，更新系统估计值及误差协方差；
20.s3：将状态量估计值以及误差协方差的估计值作为下一次运算的初值，根据量测输入实时循环估计载体姿态四元数。
21.进一步的，基于模糊控制的量测噪声协方差的更新方法具体为：
22.s2.1：计算系统残差的理论方差与实际方差，将两个方差矩阵迹的比值作为模糊控制的输入；
23.s2.2：构建模糊控制规则以及输入输出隶属度函数，根据模糊控制的输出实时调整系统量测噪声矩阵的大小。
24.进一步的，步骤四中的载体姿态角的求解，是根据系统状态量的估计值中的姿态四元数归一化之后，根据四元数与姿态角的转换关系计算得到的。
25.有益效果：
26.本发明能够通过卡尔曼滤波数据融合实现高精度姿态角输出，最终输出的姿态角不受运动加速度及外界磁干扰的影响，且误差不会随着时间的增长而累积。
附图说明
27.图1为系统卡尔曼滤波流程图；
28.图2为模糊控制输入隶属度函数图；
29.图3为模糊控制输出隶属度函数图。
具体实施方式
30.下面结合附图，对本发明进行详细描述。
31.本发明基于模糊控制的mems多传感器数据融合处理方法，包括以下步骤：
32.步骤一：建立自适应卡尔曼滤波系统的状态方程及量测方程；步骤二：利用三轴磁传感器数据以及三轴加速度计数据计算系统状态初始值及初始方差；步骤三：启动系统自适应卡尔曼滤波递推解散过程，在卡尔曼滤波过程中，利用模糊控制对系统的量测噪声矩阵进行实时调整，解算得到高精度的实时姿态四元数的输出估计值；步骤四：利用卡尔曼滤波实时解算得到的姿态四元数求解载体姿态角。
33.以下具体介绍各个步骤：
34.(1)建立自适应卡尔曼滤波系统的状态方程及量测方程：
35.由于使用欧拉角方法计算姿态角存在奇点问题。本发明从工程实用角度出发，采
用四元数方法求载体的姿态角，四元数法是利用一种超复数的乘法性质来等效描述坐标系转动的方法，转动四元数q的定义为：
36.q＝q0 q1i q2j q3k(1)
37.其中，q0～q3为四元数的数值，i、j、k分别为对应的四元数虚数标志，坐标系的瞬时转动可由如下的转动四元数的微分方程表示：
[0038][0039]
式中为载体坐标系下的三轴角速率输出值即三轴陀螺仪的输出值，表示为四元数乘法。将上式按照四元数乘法展开计算可以得：
[0040][0041]
重力加速度在地理坐标系下定义为ge，在载体坐标系下定义为gb，同样的，地理坐标系各坐标轴向量与载体坐标系各坐标轴向量间的转换关系可由如下四元数乘法表示：
[0042][0043]
其中，q*表示q的共轭，将式(4)左右两边同时左乘q可得：
[0044][0045]
将式(5)用四元数乘法展开可表示成：
[0046][0047]
其中，三维向量a＝[a
xayaz
]，由此可以得到四元数q与量测向量gb、ge之间的线性关系式：
[0048]
hg·
q＝04×1(7)
[0049]
其中
[0050]
同样的可以得到地磁向量与四元数之间的线性关系式。由此以重力和地磁向量作为观测量可以得到线性化的量测方程如下。
[0051][0052]
其中，地磁场在地理坐标系下定义为me，在载体坐标系下定义为mb，以状态四元数与陀螺仪零偏误差[ω
gx
ω
gy
ω
gz
]为状态量：
[0053]
x＝[q0q1q2q3ω
gx
ω
gy
ω
gz
]
t
(9)
[0054]
求解式(3)中的微分方程可以得到系统状态方程如公式(10)所示。
[0055]
建立对应的系统状态方程与量测方程：
[0056]
xk＝φ
k,k-1
x
k-1
w(10)
[0057]
zk＝hxk v(11)
[0058]
式中系统状态变量为xk，系统状态转移矩阵为系统量测矩阵zk＝08×1为的8
×
1零矩阵，w和v分别为过程激励高斯白噪声和量测高斯白噪声，方差分别为系统状态噪声方差qa和系统量测噪声方差r。
[0059]
(2)计算系统状态初始值及初始方差：
[0060]
卡尔曼滤波流程如附图1所示，具体实现方式如下：
[0061]
(a)给定系统的状态初始值及初始方差
[0062]
系统状态量初值由三轴加速度计及三轴磁传感器计算姿态角变换成四元数得到，具体方法如下：
[0063]
在准静态条件下，重力加速度在地理坐标系中的水平方向上没有投影分量，加速度计只在z轴方向上有测量值输出，测量值为g。则有：
[0064][0065]
其中，为重力加速度在载体坐标系下的三轴输出，为导航坐标系到载体坐标系的矩阵，将(12)式展开可得：
[0066][0067]
根据公式(13)可计算俯仰β、横滚角γ：
[0068][0069][0070]
载体坐标系与地理坐标系是不重合的。此时，地球磁场在各个轴的分量会产生叠加，三轴磁传感器测得的地球磁场在载体坐标系三个轴上的分量，应先求出它们在水平面内的投影，才能求解航向角α。
[0071][0072]
α＝arctan2(m
x
′
,my′
)(17)
[0073]
其中，[m
xmymz
]
t
为地磁场在载体坐标系下的三轴输出，[m
x
′my
′
]
t
为他们在水平面上的投影，根据姿态角与四元数之间的转换关系得到：
[0074][0075]
x0＝[q0q1q2q3000]，p0＝10^(-5)*i7，i7为7
×
7的单位矩阵。
[0076]
(3)计算高精度实时姿态四元数的输出估计值：
[0077]
令k＝1，启动卡尔曼滤波流程如下：
[0078]
(b)一步状态预测，x
k/k-1
为k-1到k时刻的估计值，带x
k/k-1
表示估计值。
[0079][0080]
(c)计算状态预测协方差p
k/k-1
，p
k-1
为k-1时刻的协方差值，qa为状态噪声矩阵。
[0081][0082]
(d)计算卡尔曼滤波增益，rk为量测噪声矩阵。
[0083][0084]
(e)根据观测量更新估计值
[0085][0086]
(f)误差协方差更新，kk为卡尔曼滤波增益矩阵
[0087]
pk＝(i-k
khk
)p
k/k-1
(23)
[0088]
(g)其中k为滤波更新次数，令k＝k 1，重复上述步骤(b)～(f)即可得到实时的载体姿态四元数的估计值即状态估计的前四项。
[0089]
卡尔曼滤波需要系统状态噪声和观测噪声的先验信息，其中系统状态噪声可以通过对系统的大量反复试验来得到，且减少系统状态噪声的实时估计，可以降低系统的计算复杂度，因此，系统状态噪声方差qa采用固定值。而量测噪声不仅仅来源于mems传感器的误差和噪声，还受到外部环境以及载体运动状态等情况影响。因此量测噪声是比较容易变化的，有太多的不确定性，难以建模。因此，需要用合适的算法对量测噪声方差r进行实时的自适应估计，这样才能保证滤波器的精度，防止滤波器发散。
[0090]
又被称为残差向量，它反映了滤波模型的准确性。自适应滤波的基本原理是通过监测残差的量测方差与理论方差的比值来对量测方差进行实时估计。当模型准确时，比值应在1附近；当观测噪声发生改变时，则其比值偏离1，需要对观测噪声进行修正。
[0091]
残差向量的量测方差：
[0092][0093]
由于噪声干扰，常使得量测方差波动较大，因此需要对其进行数据平滑操作。目前采用的方法经常是对一段时间内的量测方差求算数平均值。但由于观测噪声为时变噪声，对新数据敏感，可对其取较大权值；而陈旧的数据则应该逐渐遗忘，权值也相应地较小。因此，在计算残差向量的量测方差时引入了遗忘因子dk，计算得到的残差向量方差的实际值为：
[0094][0095]
残差的理论方差定义为：
[0096][0097]
定义q＝tr(ck)/tr(sk)作为模糊控制模块的输入来监测观测噪声，其中tr(
·
)为取矩阵的迹。
[0098]
由q的定义可以看到，可以通过改变rk来调整q，当模型准确时，q在1附近；如果量测噪声增大，则ck增大进而q增大，此时应增大rk使q回到1附近；如果量测噪声减小，则应减小rk使q回到1附近。rk的控制通过aqq来实现。aqq大于1表示增大rk，aqq小于1表示减小rk，aqq等于1表示保持rk不变。定义模糊子集equal1表示在1附近，more1表示基本大于1，less1表示基本小于1。模糊控制规则如下：
[0099]
[0100]
输入q和输出aqq的隶属度函数分别如图2和图3所示，根据模糊控制模块输出的aqq可对系统量测噪声矩阵进行如下调整：
[0101]rk
＝aqq*r
k-1
(28)
[0102]
(4)求解载体姿态角：
[0103]
通过四元数到姿态角的转化公式可以得到实时的载体姿态角数据，由四元数到姿态角之间的转换公式如式(29)。
[0104][0105]
其中，[q0q1q2q3]k＝qk。
[0106]
由此，可以利用三轴加速度数据、三轴角速度数据以及三轴磁场数据通过自适应卡尔曼滤波数据融合得到载体的姿态角数据。本发明能够通过卡尔曼滤波数据融合实现高精度姿态角输出，最终输出的姿态角不受运动加速度及外界磁干扰的影响，且误差不会随着时间的增长而累积。