[发明专利]一种在地磁异常情况下应用AHRS测量姿态的方法

摘要

本发明涉及一种在地磁异常情况下应用AHRS测量姿态的方法，其通过对磁力计输出值进行修正，用以提高当物体处于磁异常的地域环境下的姿态角输出精度。当物体处于地磁异常的情况时，通过对方向向量的微分方程的推导和利用，求解磁力计输出值在一段连续时刻之间的关系；再利用无迹卡尔曼滤波，通过上一时段磁力计的输出值连续校准下一时刻磁力计的输出值；最终，利用校准后的磁力计输出值同陀螺仪、加速度计的输出值进行数据融合，以求解出姿态角。本发明方法可依据磁力计连续时刻的输出关系，修正磁力计在磁异常环境下的输出值，其计算量较低，在海底勘探仪器的姿态测量方面，能有效的提高当物体处于磁异常的地域环境下的姿态角输出精度。

主权项

一种在地磁异常情况下应用AHRS测量姿态的方法，其特征在于，包括以下步骤：A、基于无迹卡尔曼滤波算法的磁力计异常矫正：A1、磁异常的判断：通过对三轴磁力计的输出值求出总磁通量和磁倾角，当其偏离当地磁通量和磁倾角时则此时刻磁力计处于磁异常状态；A2、为卡尔曼滤波选定状态量其含义为方位角偏离值和磁力计异常值；A3、利用状态量的运动关系推导出状态方程，并求出状态矩阵A和状态估计值Xt；A4、通过对方向向量开微分方程的改进，在其原来旋转矩阵的基础上，推导出连续时刻磁力计输出的关系，进而推导出测量方程，并求解出测量矩阵Z和测量估计值zt；A5、基于上述状态方程和测量方程，通过无迹卡尔曼滤波算法对磁力计输出的异常值进行修正；A6、通过迭代算法用上一时段磁力计的输出值连续校准下一时刻磁力计的输出值使每一个异常值可以多次参加卡尔曼滤波过程，尽可能的消除极端异常值对整个过程的影响；B、求解方位角方法：B1、根据四元数的微分方程推算出姿态运动学方程作为卡尔曼滤波状态方程，代入磁力计修正值和加速度计值求出方位角测量值；B2、通过扩展卡尔曼滤波融合AHRS数据求出姿态角。