CN110017837A - 一种姿态抗磁干扰的组合导航方法 - Google Patents

Abstract

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

Description

一种姿态抗磁干扰的组合导航方法

技术领域

本发明属于一种环境中出现磁场干扰引起磁场畸变情况下的姿态抗干扰组合导航联邦卡尔曼滤波算法，具体为一种姿态抗磁干扰的组合导航方法。

背景技术

对于中小型无人机而言，降低导航系统中传感器的成本，提高导航系统的精度和稳定性是组合导航系统设计中需要考虑的基本问题之一。通常由三轴MEMS陀螺仪、加速度计和磁力计组成的MARG传感器，并广泛应用于无人机的姿态估计。其中由MEMS陀螺仪和加速度计组成惯性测量单元(IMU)，用于测量无人机运动的角速度和加速度。同时还用加速度计感知重力场；用磁力计感知地磁场。并通过设计卡尔曼滤波器或自适应卡尔曼滤波器来进行无人机的姿态估计。然而，在卡尔曼滤波器中用加速度计和磁力计测量重力场和地磁场的过程中分别易受机体振动和磁场畸变的影响。因此，有必要开发姿态抗干扰尤其是磁场畸变干扰的组合导航算法。

发明内容

无人机导航系统中由MARG传感器组成的姿态测量系统极易受磁场畸变的影响，此外，由无人机倾角补偿后的磁力计输出计算得到的偏航角，其误差不但有些大，而且在磁场畸变干扰的情况下偏航角的误差出现大幅变化。且将磁强计作为卡尔曼滤波器的量测传感器之一时，磁场的畸变将影响整个姿态估计。因此，为了解决上述技术问题，本发明提供一种姿态抗磁干扰的组合导航方法，磁场畸变不会通过磁力计对俯仰和横滚的姿态估计产生影响，且减小对偏航角估计的影响。

本发明是这样实现的，一种姿态抗磁干扰的组合导航方法，用子滤波器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的估计四元数转换得到的欧拉角构成的矢量，如下

与现有技术相比，本发明的优点在于：设计了一种组合导航联邦卡尔曼滤波器，利用重力场、地磁场和太阳矢量同时辅助微机电捷联惯导系统(MEMS-SINS)进行无人机的姿态估计，并采用联邦滤波器的无重置模式，在子滤波器中利用无人机倾角补偿后的磁力计输出得到的偏航角来校正子滤波器中量测更新的姿态四元数；并设计了一种欧拉角信息融合方法。因此，环境中的动态磁场畸变基本不会通过磁力计对俯仰和滚转角的估计产生影响；且偏航角的估计误差相对于用磁力计矫正量测更新姿态四元数的子滤波器的估计误差也减小一倍左右。

附图说明

图1是本发明提供的一种联邦滤波器；

图2是本实施过程数值仿真中由MEMS-SINS估计得到的偏航、俯仰和横滚姿态误差。

图3是本实施过程数值仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的偏航角估计误差。

图4是本实施过程数值仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的俯仰角估计误差。

图5是本实施过程数值仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的横滚角估计误差。

图6是在无磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的偏航角估计误差。

图7是在无磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的俯仰角估计误差。

图8是在无磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的横滚角估计误差。

图9是在磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的偏航角估计误差。

图10是在磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的俯仰角估计误差。

图11是在磁场干扰情况下本实施过程半实物仿真子滤波器1、子滤波器2、主滤波器中姿态四元数融合更新和欧拉角融合更新的横滚角估计误差。

具体实施方式

以下结合图1-11和具体实施过程对本发明做进一步详细说明，图1为本发明提供的一种联邦滤波器：

数值仿真：给定数值仿真的步长为Δt＝10^-3s，仿真时间为500s。给定单位太阳矢量、单位重力矢量和单位地磁矢量分别为g_n＝[0 0 1]^T和h_n＝[10 0]^T，并假定磁偏角和磁倾角都为0。太阳矢量和地磁矢量的随机白噪声漂移分别为0.03和0.003。陀螺仪的随机白噪声漂移和一阶马尔科夫过程漂移分别为0.002rad/s和0.001rad/s，并取马尔科夫随机过程的相关时间为7200/s。加速度计的随机白噪声漂移和一阶马尔科夫过程漂移分别为0.8mg和2mg，并取马尔科夫随机过程的相关时间为3600/s。取GPS的位置和速度漂移分别为2m和0.3m/s。并给定无人机动态模型的运动包括加速、转弯、爬升等运动状态。并在仿真的第300s，在给定单位地磁矢量的基础上加磁场干扰如下式：

得到如图2-5所示数值仿真结果。

半实物仿真：在数值仿真的基础上，用从pixhawk飞控中读取的已校准的MARG传感器数据和太阳矢量模型一起进行算法的仿真验证。并通过地面站调整无人机水平静止在地面，且偏航角调整为零；同时给定数值仿真中无人机动态模型也水平静止且偏航角为零。在不给磁力计加外部磁场干扰的情况下，得到如图6-8所示数值仿真结果；在给磁力计加外部磁场干扰的情况下，得到如图9-11所示半实物仿真结果。

Claims (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的估计四元数转换得到的欧拉角构成的矢量，如下