[发明专利]一种用于航姿系统的自适应姿态估计方法及其平滑切换算法

说明书

本发明公布了一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，由基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法构成自适应姿态平滑切换。该算法根据GPS状态实现低阶与高阶的切换，包括以下步骤：在GPS状态良好的情况下，采用高阶组合导航融合算法估计姿态信息；在GPS信号受到干扰时，停止高阶组合导航算法并切换为低阶姿态估计算法；当GPS信号恢复后，将低阶姿态估计状态作为高阶滤波算法的初值，实现姿态融合算法的平滑切换。针对该算法设计基于ARM架构的航姿系统，并通过仿真与试验验证设计算法的可靠性，在切换过程中姿态角误差小于0.5°，收敛时间小于0.2s，有效地提高了有、无GPS定位状态下无人机的姿态估计精度。

技术领域

本发明涉及一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，适用于航空导航技术领域。

技术背景

随着微机电技术的发展，三轴MEMS陀螺仪、加速度计、磁力计成为最主要的惯性测量元件。但现有的MEMS器件均存在某些缺陷：MEMS陀螺仪存在漂移，单独使用陀螺仪估计姿态存在累积误差；MEMS加速度计受线性加速度和震动影响较大，单独利用加速度计估计姿态角，会出现较大误差；MEMS磁力计易受周围环境所产生的磁场干扰，从而影响航向角的输出。由于此类误差是随机的，无法预先消除，因此不能单独通过上述方法获取准确的姿态角，必须通过合理的数据融合方法来补偿各类误差带来的影响，才能获得稳定可靠的姿态信息。

发明内容

发明目的

本发明的目的在于为了提高系统在GPS信号丢失情况下姿态估计的准确性，确保无人机在丢失GPS信号器件仍能够稳定飞行。

技术方案

为了实现上述目的，本发明采用以下技术方案：一种用于航姿系统的自适应姿态估计方法，所述方法的实现步骤包括：

步骤1，在GPS状态良好的情况下，基于四元数的低阶EKF姿态估计算法和基于GPS速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；

步骤2，在GPS信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；

步骤3，当GPS信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换。

进一步的，所述基于四元数的低阶EKF姿态估计算法，具体为采用三轴加速度计，三轴陀螺仪、磁力计，应用低阶卡尔曼滤波器实现各传感器数据融合，解算无人机姿态与航向信息。

更进一步的，所述的基于四元数的低阶EKF姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量X为姿态四元数：

X＝[q₀ q₁ q₂ q₃]^T (1)

其中，q₀、q₁、q₂、q₃为姿态四元数；

根据卡尔曼滤波器原理，定义离散系统的状态方程如下：

X_k＝A_kX_k-1+ω_k-1 (2)

其中，X_k、X_k-1分别表示第k、k-时刻的系统状态量，A_k为从k-1时刻到k时刻的状态转移矩阵，ω_k-1表示k-1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为：

表示系统k时刻先验估计状态，表示系统k-1时刻后验估计状态，其中：