[发明专利]一种多MEMS传感器组合测姿方法

权利要求书

1.一种多MEMS传感器组合测姿方法，其特征在于，包括以下步骤：

(1)在载体坐标系的三个轴上，将四组MEMS传感器两两对角安装在载体坐标系的各轴上，相位差为180°；

(2)采集四组MEMS传感器的测量数据与四元数估计数据的向量积作为传感器系统误差参数；

(3)将传感器系统误差参数作为模糊控制器和PI控制器的输入参数，将控制器的输出参数作为互补滤波器的输入参数，以修正陀螺仪的角速度输出；

(4)根据修正后的陀螺仪角速度数据，通过四元数微分方程解算出姿态角数据；

所述步骤(1)的具体过程为：

首先采用十二组MEMS传感器均匀分布在载体坐标系的各轴上，即每一轴上应用四组MEMS传感器，将四组MEMS传感器两两对角安装，为了方便计算将四组MEMS传感器分为a，b组每一组由两组传感器对角安装，设置俯仰角为θ，横滚角为γ以及航向角为载体坐标系为n系，参考坐标系为s系，x_sy_sz_s为参考坐标系北东地方向，初始载体和参考坐标系的三轴相重合，参考坐标系经过旋转后与载体坐标系的关系为：载体坐标系等于方向余弦矩阵与参考坐标系的乘积，欧拉角是一个坐标系到另一个坐标系的变换，可以通过不同坐标轴的3次连续转动来实现，用欧拉角的形式可以表示方向余弦矩阵

所述步骤(2)的具体过程为：

先计算a组MEMS传感器所测得的数据和参考坐标系下加速度和磁场强度数据通过坐标变换矩阵，得到俯仰角θ₁，横滚角γ₁以及航向角b组和a组的方法一样，同理求得b组的俯仰角θ₂，横滚角γ₂以及航向角利用均值法得到整个系统的俯仰角θ，横滚角γ以及航向角由于欧拉角存在奇点问题，所以将解算出欧拉角初始数据表示为四元数的初始值，即可得四元数矩阵四元数用q₀，q₁，q₂，q₃表示成一个具有4个参数的复数形式，q₀为其实部，q₁，q₂，q₃为其虚部，利用向量积法算出俯仰角和横滚角的误差e_θγ以及航向角误差e_θγ为四元数矩阵第三列数据与加速度传感器测量数据做向量积，为四元数矩阵与地理坐标系下的地磁测量向量的乘积再与磁力计输出数据的单位向量的乘积，得到每组传感器的系统误差求得第一组至第四组传感器的系统误差e；

所述步骤(3)中将四组MEMS传感器系统误差e分别作为两组模糊控制器和两组PI控制器的输入参数，将两组模糊控制器和两组PI控制器的输出参数u作为互补滤波器的输入参数，以修正陀螺仪的角速度输出；

所述步骤(4)的具体过程为：

通过两组模糊控制器与两组PI控制器调节，互补滤波修正后得到的角速度采用四阶龙格库塔算法得到四元数微分方程的解，进行归一化处理后，即单位化，得到新的四元数矩阵进而转换成姿态角，四元数转化姿态角俯仰角θ＝-arcsin2(q₁q₃-q₀q₂)，横滚角航向角

2.根据权利要求1所述的一种多MEMS传感器组合测姿方法，其特征在于，所述步骤(3)中，利用互补滤波原理修正陀螺仪漂移误差的具体方法包括以下步骤：

(3-1)将两组模糊控制器都是选用二维模糊控制器，其的输入误差A，误差变化率B以及输出C论域为[-6，6]，[-6，6]，[-3，3]；模糊集均为{NB,NM,NS,ZO,PS,PM,PB}，模糊推理语句选用“If A and B then C”蕴含关系为A∧B→C；根据关系矩阵R得出输出量，关系矩阵R为A和B向量积的列转置与C合成；选用三角形隶属函数trimf(x,[a,b,c])；选择Mamdani模糊系统进行推理；反模糊化选用面积重心法centroid，两组PI控制器相应参数设置为P为2，I为0.02；

(3-2)将模糊控制器构造好以及PI控制器相应参数设置好后，将四组MEMS传感器的系统误差分别作为两组模糊控制器和两组PI控制器的输入量，即将第一组传感器和第二组传感器的系统误差e¹和e²作为两组模糊控制器的输入量，第三组传感器和第四组传感器的系统误差e³和e⁴作为两组PI控制器的输入量；

(3-3)将两组模糊控制器和两组PI控制器的输出参数u₁，u₂，u₃，u₄，作为互补滤波的输入参数，得到四组修正后的角速度通过均值法得到载体的角速度