[发明专利]一种姿态抗磁干扰的组合导航方法

摘要

本发明公开了一种姿态抗磁干扰的组合导航方法。设计了一种联邦滤波器，利用重力场、地磁场和由多偏振敏感器测得的太阳矢量同时辅助捷联惯导系统进行无人机的姿态估计。且为防止子滤波器间交叉污染，联邦滤波器工作在无重置模式。在该算法中利用倾角补偿磁力计输出计算得到的偏航角来校正子滤波器1量测更新的姿态四元数；为防止姿态信息融合时四元数估计均方误差阵对估计结果的影响，设计了一种基于欧拉角信息融合方法。因此，在环境中存在动态磁场畸变时，由本发明设计的滤波算法估计得到的俯仰角和滚转角不会受到影响；且融合后的偏航角的估计误差相对于子滤波器1的估计误差相对减小。

主权项

1.一种姿态抗磁干扰的组合导航方法，其特征在于，用子滤波器1对无人机的位置、速度和姿态进行估计，则子滤波器1中的无人机状态模型为：其中，状态量x＝[q^T p^T v^T]^T为无人机的姿态四元数、位置和速度，a_n∈R³为由加速度计量测得到的无人机运动加速度，系统矩阵A和输入矩阵B满足：其中，Ω满足：其中，和为由陀螺仪量测到的角速度；用子滤波器2对无人机的姿态进行估计，则子滤波器2中的无人机状态模型为：将子滤波器1和子滤波器2量测更新后的姿态信息在主滤波器中进行欧拉角信息转换和融合更新，算法具体步骤如下：(1)时间更新：采用四阶龙格库塔法对子滤波器1的状态方程进行离散化：其中，为k‑1时刻状态的最优估计，为在k‑1时刻对k时刻状态的一步预测，T_h为采样周期，且在子滤波器2中采用相同的方法对状态方程进行离散化；子滤波器1估计均方误差阵的时间更新为：其中，P_1,k‑1为k‑1时刻的估计均方误差阵，P_1,k/k‑1为k‑1时刻对k时刻估计均方误差阵的预测，R_u为输入a_n的噪声方差阵，Q为系统过程噪声方差阵，且在子滤波器2中采用相同的方法进行估计均方误差阵的时间更新；(2)量测更新：其中，R_i为子滤波器i的量测噪声方差阵；H_i,k为子滤波器i的观测矩阵，其中子滤波器1观测重力矢量，子滤波器2观测太阳矢量和重力矢量；Z_i,k为子滤波器i中传感器的量测矢量，如下：其中，a_b为加速度计量测量；b_a为加速度计的零偏；p^G和v^G为由GPS得到的位置和速度；s_b为由多偏振敏感器量测到的太阳矢量；(3)子滤波器1量测更新四元数中偏航角分量的矫正：计算倾角补偿后的磁力计输出：m_c＝C_YXm_b,其中，C_YX满足：θ_k/k‑1和γ_k/k‑1为由子滤波器1四元数状态量的一步预测转换得到的横滚和俯仰姿态角，由倾角补偿后的磁力计输出得到偏航角：其中，δ_k为当地磁偏角，由磁力计得到的偏航角与由子滤波器1估计四元数转换得到的偏航角的差为：构建无人机当前姿态下的旋转四元数：则子滤波器1姿态四元数的矫正更新为：其中，为子滤波器1状态估计量中四元数部分；代表四元数的乘法运算；(4)欧拉角信息转换：构建欧拉角估计均方误差阵其中，和为子滤波器i中由估计四元数转换得到的偏航、俯仰和横滚姿态角的估计均方误差，其构成分别为(5)信息融合：其中，是由子滤波器i的估计四元数转换得到的欧拉角构成的矢量，如下