[发明专利]一种附有运动加速度补偿的MARG姿态计算方法

说明书

本发明公开了本发明提出一种附有运动加速度补偿的MARG姿态计算方法，属于导航领域。该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。

技术领域

本发明属于导航领域，具体是附有滑动递推窗口运动加速度补偿的重磁矢量对快速解析的MARG姿态估计方法。

背景技术

近几十年来，随着微电子机械(Micro-Electro-Mechanical System,MEMS)技术的发展，MEMS惯性传感器被广泛应用于工业领域和科学研究，用以提供位置姿态信息。由三轴陀螺仪和三轴加速度计构成的MEMS惯性传感器，搭配三轴磁传感器的组合模块，被称为MARG(magnetic,angular rate and gravity,MARG)传感器。为了最佳地融合MARG传感器数据，发展了众多数据融合方法，主要分为两大类。其中一类是加速度计和陀螺仪互补滤波的方法，基于加速度计和陀螺仪输出信息在频域具有互补特性。偏航角可通过磁传感器进行进一步修正。互补滤波具有计算时间短，易于在硬件平台应用等特点，然而其只能提供次优估计。为获得更加准确的姿态，另一类方法——线性卡尔曼滤波(Kalman filter,KF)被用于数据融合，其可获得最小均方差意义下的最优估计。此外，针对非线性模型，发展了各类非线性滤波器如扩展KF、无迹KF和粒子KF，用以估计载体位置与姿态。然而，MARG传感器姿态估计易受非重力加速度及磁场干扰的影响。这些干扰将导致系统不再满足滤波假设模型，引起滤波估计有偏甚至发散。为解决这些问题，需建立加速度计与磁力计检测准则，进行异常数据检测。

发明内容

针对上述问题，本发明提出一种附有运动加速度补偿的MARG姿态计算方法；该方法首先利用陀螺仪进行姿态一步预测，然后利用一步预测值对加速度计与磁力计进行检测，最后针对不同检测结果执行相应的数据融合算法。通过使用本方法进行MARG传感器姿态估计，有效对载体运动加速度与磁场干扰进行检测；针对运动载体的动态特征，引入滑动窗口数据拟合运动加速度变化规律并进行相应的模型补偿；推导重磁(加速度和磁感应强度测量值)矢量对的在Wahba问题下的解析解从而加快姿态求解速度，显著提高MARG姿态估计效率。

本发明的技术方案为：一种附有运动加速度补偿的MARG姿态计算方法，具体包括以下步骤：

步骤1：利用陀螺仪进行一步预测；

步骤2：利用一步预测值，对加速度计与磁力计进行检测，通过滑动窗口多项式拟合，对运动加速度进行补偿；

步骤3：针对不同的检测结果执行相应的融合算法：

无磁场干扰，无运动加速度(或运动加速度已补偿)：转至步骤4；

存在磁干扰，无运动加速度(或运动加速度已补偿)：转步骤5；

无磁干扰，存在运动加速度且运动加速度未补偿，转步骤6；

存在磁干扰，存在运动加速度且运动加速度未补偿：将陀螺推算的一步预测结果作为姿态估计值，等待下一时刻的传感器数据。

步骤4：建立加速度和磁场强度的矢量对方程，通过快速线性四元数估计方法求解获得量测信息，基于卡尔曼滤波进行姿态估计。

步骤5：利用加速度计数据建立量测模型，基于非线性滤波实现状态估计，并对磁干扰进行估计。

步骤6：利用磁力计数据建立量测模型，基于非线性滤波实现状态估计。