复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法技术

本发明专利技术公开了复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，包括：获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；判断是否需要进行滤波；若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；若不需要进行滤波，直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；分别用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；根据载体当前运动姿态进行数据融合，得到最优方位角。本发明专利技术提供的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，使MEMS电子罗盘在磁扰动和动态条件下，自适应完成准确的方位角解算，提高定向精度；算法结构简单，收敛速度快。

A method for calculating the dynamic azimuth of MEMS electronic compass in complex magnetic disturbance scene

【技术实现步骤摘要】
复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法
本申请涉及电子罗盘
，尤其涉及复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法。
技术介绍
近年来微米纳米技术快速发展，由MEMS器件所组成的管道系统在探测、机械、交通、军事等各个领域都被广泛应用，特别在交通领域中使用MEMS电子罗盘和MEMS陀螺仪等器件进行定姿定向已成为发展趋势。在载体运动时，MEMS电子罗盘中的磁力计容易受到外接磁扰动的影响，将导致MEMS电子罗盘计算的方位角完全失真；MEMS电子罗盘中的加速度计在遇到突发的加减速时，并会产生大量的额外加速度，从而计算载体姿态的精度下降，进而影响方位角的精度；MEMS电子罗盘中的陀螺仪误差会随时间累计；并且，在动态方位角的计算中，无法使用由起始位置校准的误差模型推算出来的椭圆拟合算法和12位置校准算法进行解算。
技术实现思路
本专利技术的目的是要提供一种复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，可以解决上述现有技术问题中的一个或者多个。根据本专利技术的一个方面，提供一种复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，包括以下步骤：获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；判断是否需要进行滤波；若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；根据载体当前运动姿态进行数据融合，得到最优方位角。在一些实施方式中，多冗余传感器由两组MEMS传感器正向贴合构成，分别为第一传感器与第二传感器，第一传感器包括第一陀螺仪、第一磁力计和第一加速度计，第二传感器包括第二陀螺仪、第二磁力计和第二加速度计，且第一传感器与第二传感器关系如下所示：式中，其中G1xyz和G2xyz分别为第一陀螺仪和第二陀螺仪三轴输出值，A1xyz和A2xyz分别为第一加速度计和第二加速度计三轴输出值，M1xyz和M2xyz分别为第一磁力计和第二磁力计三轴输出值。在一些实施方式中，判断是否需要进行滤波，包括以下步骤：获取磁力计的输出值计算所有时刻的磁场总量，划分时间区间，并以每个时间区间的前n个时刻的磁场总量的平均值作为基准，基准的磁场总量表达式如下：式中，MFn是第n时刻的磁场总量，MFstd代表基准的磁场总量；计算每一个时间区间中磁场总量的最大值MFreg_max和最小值MFreg_min，并计算下列公式：MFreg_var＝MFreg_max-MFreg_min式中，MFreg_var代表每个区间的磁场波动数值，代表每个区磁场环境与基准磁场环境之间的差异；将所求MFreg_var和与预先设置的阈值相比较，若所求MFreg_var和均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。在一些实施方式中，若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值包括：确定状态量如下：式中，和分别是第一陀螺仪和第二陀螺仪三轴角速度状态值，和分别第一陀螺仪和第二陀螺仪的三轴加速度状态值，和是第一磁力计和第二磁力计的状态值。确定状态方程如下：其中，计算式如下：计算式如下：计算式如下：计算式如下：计算式如下：计算式如下：式中，ηgxyz,ηwxyz,ηmxyz分别为三轴角速度状态误差、三轴角加速度状态误差、三轴磁力计状态误差，Δt为数据采样周期；MM1和MM2分别是第一磁力计和第二磁力计的三轴的旋转矩阵，MM1和MM2的表达式分别如下：ωxyz是第一陀螺仪和第二陀螺仪提供的旋转矩阵，表达式如下：用每一个状态量对状态方程进行偏微分，获得状态转移矩阵A：确定观测量如下：Zn＝[G1xyzG2xyzM1xyzM1xyz]确定观测方程如下：式中，分别为第一陀螺仪和第二陀螺仪观测噪声误差，分别为第一磁力计和第二磁力计观测值噪声误差，H为观测转移矩阵，H的表达式如下：卡尔曼滤波基本算法编排，该算法流程如下：状态量更新方程：x(k|k-1)＝A×x(k-1|k-1)+B×u(k)均方误差方程：P(k|k-1)＝A×P(k-1|k-1)×A′+Q滤波增益矩阵：K(k)＝P(k|k-1)×H′×(H×P(k|k-1)×H′+R)-1k时刻状态估值计算方程：x(k|k)＝x(k|k-1)+K(k)×(Z(k)-H×x(k|k-1))估计均方误差方程：P(k|k)＝(I-K(k)×H)*P(k|k-1)式中，状态误差协方差Q设置为0，以上公式不断循环运算，得到加速计、磁力计和陀螺仪的输出值的最优估值。在一些实施方式中，倾角补偿算法具体包括以下步骤：利用加速度计输出值计算载体的俯仰角和横滚角，公式如下：式中，Ax、Ay和Az分别为第一加速度计和第二加速度计测量值的平均值，α为俯仰角，β为横滚角；将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：式中，Mbx、Mby和Mbz分别为第一磁力计与第二磁力计测量值的平均值；简化得到：Mnx＝Mbxcosα+Mbysinαsinβ-MbzsinαcosβMny＝Mbycosβ+Mbzcosβ式中，Mnx为水平面上X轴的投影，Mny为水平面上Y轴的投影；计算方位角，公式如下：在一些实施方式中，磁场比例角补偿算法具体包括以下步骤：计算载体坐标系的X轴与Y轴切割磁力线的角度，公式如下：式中，Mbx、Mby和Mbz分别为第一磁力计与第二磁力计测量值的平均值，θ为磁场X轴切割磁力线的角度，为为磁场Y轴切割磁力线的角度；将磁力计的测量值进行转化，求得磁力计输出值在水平面上X轴和Y轴的投影，转化公式如下：简化得到：式中，M′nx为水平面上X轴的投影，M′ny为水平面上Y轴的投影；计算方位角，公式如下：在一些实施方式中，陀螺仪Z轴积分算法具体包括以下步骤：利用陀螺仪Z轴的角速度积分解算，公式如下：式中，为k时刻陀螺仪Z轴积分解算出来的方位角，为k-1时刻陀螺仪Z轴积分解算出来的方位角，其中k≥1。在一些实施方式中，根据载体当前运动姿态进行数据融合，得到最优方位角具体包括本文档来自技高网...

【技术保护点】
1.复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，包括以下步骤：/n获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；/n判断是否需要进行滤波；/n若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；/n若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；/n分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；/n根据载体当前运动姿态进行数据融合，得到最优方位角。/n

【技术特征摘要】
1.复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，包括以下步骤：
获取多冗余传感器中加速度计、磁力计和陀螺仪的输出值；
判断是否需要进行滤波；
若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值；
若不需要进行滤波，则直接输出多冗余传感器中加速度计、磁力计和陀螺仪的输出值；
分别使用倾角补偿算法、磁场比例角补偿算法和陀螺仪Z轴积分算法解算方位角；
根据载体当前运动姿态进行数据融合，得到最优方位角。


2.根据权利要求1所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述多冗余传感器由两组MEMS传感器正向贴合构成，分别为第一传感器与第二传感器，所述第一传感器包括第一陀螺仪、第一磁力计和第一加速度计，所述第二传感器包括第二陀螺仪、第二磁力计和第二加速度计，且所述第一传感器与第二传感器关系如下所示：



式中，其中G1xyz和G2xyz分别为第一陀螺仪和第二陀螺仪三轴输出值，A1xyz和A2xyz分别为第一加速度计和第二加速度计三轴输出值，M1xyz和M2xyz分别为第一磁力计和第二磁力计三轴输出值。


3.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述判断是否需要进行滤波，包括以下步骤：
获取磁力计的输出值计算所有时刻的磁场总量，划分时间区间，并以每个时间区间的前n个时刻的磁场总量的平均值作为基准，基准的磁场总量表达式如下：



式中，MFn是第n时刻的磁场总量，MFstd代表基准的磁场总量；
计算每一个时间区间中磁场总量的最大值MFreg_max和最小值MFreg_min，并计算下列公式：
MFreg_var＝MFreg_max-MFreg_min



式中，MFreg_var代表每个区间的磁场波动数值，代表每个区磁场环境与基准磁场环境之间的差异；
将所求MFreg_var和与预先设置的阈值相比较，若所求MFreg_var和均在所设阈值范围内，则判断不需要进行滤波；否则，则需要进行滤波。


4.根据权利要求2所述的复杂磁扰动场景MEMS电子罗盘的动态方位角解算方法，其特征在于，所述若需要进行滤波，构建扩展卡尔曼滤波器，输出加速度计、磁力计和陀螺仪的输出值的最优估值包括：
确定状态量如下：



式中，和分别是第一陀螺仪和第二陀螺仪三轴角速度状态值，和分别第一陀螺仪和第二陀螺仪的三轴加速度状态值，和是第一磁力计和第二磁力计的状态值；
确定状态方程如下：



其中，计算式如下：




计算式如下：




计算式如下：




计算式如下：




计算式如下：




计算式如下：



式中，ηgxyz,ηwxyz,ηmxyz分别为三轴角速度状态误差、三轴角加速度状态误差、三轴磁力计状态误差，Δt为数据采样周期；
MM1和MM2分别是第一磁力计和第二磁力计的三轴的旋转矩阵，MM1和MM2的表达式分别如下：






ωxyz是第一陀螺仪和第二陀螺仪提供的旋转矩阵，表达式如下：



用每一个状态量对状态方程进行偏微分，获得状态转移矩阵A：






确定观测量如下：

【专利技术属性】
技术研发人员：叶景成，时广轶，王春波，徐开明，吴志刚，金玉丰，
申请(专利权)人：无锡北微传感科技有限公司，北京大学深圳研究生院，
类型：发明
国别省市：江苏;32