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

权利要求书

1.一种用于航姿系统的自适应姿态估计方法，其特征在于，所述方法的实现步骤包括：

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

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

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

2.根据权利要求1所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，所述基于四元数的低阶EKF姿态估计算法，具体为采用三轴加速度计，三轴陀螺仪、磁力计，应用低阶卡尔曼滤波器实现各传感器数据融合，解算无人机姿态与航向信息。

3.根据权利要求2所述的一种用于航姿系统的自适应姿态估计方法，其特征在于，所述的基于四元数的低阶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时刻后验估计状态，其中：

其中，表示角速率矢量矩阵：

式中：ω_x表示绕横滚轴角速率，ω_y表示绕俯仰轴角速率，ω_z表示绕航向轴角速率。