[发明专利]一种基于地磁场自适应修正的航姿系统姿态解算方法

说明书

本发明提出一种基于地磁场自适应修正的航姿系统姿态解算方法，通过分别计算状态转移矩阵、量测矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵和自适应矩阵，实现采用自适应扩展卡尔曼滤波算法进行航姿系统姿态解算；本发明有效解决了载体姿态的累积误差较大的问题，同时提高了姿态解算精度。

技术领域

本发明涉及一种基于地球重力场与地磁场自适应修正的姿态解算方法，属于定位导航技术领域。

背景技术

MEMS传感器(陀螺仪，加速度计，磁力计)在移动设备领域使用越来越普及，具有成本低、集成度高以及体积小等优点，但是在进行姿态解算时，却存在输出噪声过大、零点漂移无法完全消除、角速率输出易受干扰等问题。因此，通常情况下，需要采用多个传感器通过信息融合算法实现高精度的姿态解算。航姿系统(AHRS)传感器在姿态解算过程中，一般采用基于四元数的互补滤波算法和扩展卡尔曼滤波算法来解决上述问题，但目前在基于四元数的互补滤波算法和扩展卡尔曼滤波算法中，对于环境中的软磁干扰问题都是直接利用世界地磁模型进行修正，忽略了实际中可能会出现的磁异常对姿态解算的影响。

发明内容

针对现有技术存在的：(1)当仅仅使用陀螺仪与加速度数据时，解算的姿态角存在累积误差；(2)引入磁力计数据时，由于环境中的磁干扰会对磁测量产生影响，出现航向姿态输出噪声过大、姿态解算精度降低等问题。本发明提出一种基于地磁场自适应修正的航姿系统姿态解算方法，在引入地磁场信息后，修正载体航向姿态的累计误差，保证解算姿态的精度。

本发明的技术方案为：

所述一种基于地磁场自适应修正的航姿系统姿态解算方法，其特征在于：采用自适应扩展卡尔曼滤波算法进行航姿系统姿态解算，其中

状态方程为：

X_k＝Φ_k/k-1αX_k-1+W_k-1

量测方程为：

Z_k＝H_kX_k+V_k

其中X_k为t_k时刻的状态向量，Z_k为t_k时刻的量测向量，Φ_k/k-1为t_k-1时刻至t_k时刻的状态转移矩阵，H_k为量测矩阵，V_k为量测噪声，W_k-1为系统噪声，系统噪声协方差矩阵为 Q_k-1，量测噪声协方差矩阵为R_k；α为自适应矩阵；

采用以下步骤计算状态转移矩阵、量测矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵和自适应矩阵：

状态转移矩阵采用公式

计算得到，其中I为单位矩阵，Δθ根据公式确定，ΔΘ根据公式

得到，其中为航姿系统中传感器输出的载体坐标系B下的载体角速度信息；

量测矩阵采用公式

计算得到，其中

a为在载体坐标系B中测量的重力加速度矢量，g为在全局坐标系G中测量的重力加速度矢量，并采用零标量的四元数方式表示矢量g与a；

系统噪声协方差矩阵采用公式