[发明专利]基于自适应EKF算法的小型无人机MARG航姿估计方法

说明书

本发明涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法，属于组合导航技术领域。本发明中，首先，将磁力计和加速度计量测更新分开进行，增加了磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明还提出了针对外部加速度的自适应滤波算法，为了提高估计精度，不直接判断三轴加速度模值，而是对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。

技术领域

本发明属于组合导航技术领域，具体涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法。

背景技术

小型无人机由于其体积和成本限制，通常使用低成本的MEMS惯性器件作为其导航系统的主要传感器，结合磁力计和GPS卫星定位系统进行组合导航。在飞行中遇到卫星数不足或在室内飞行的情况下，需要切换到航姿模式(AHRS)提供姿态信息。常用的AHRS姿态算法有互补滤波法、梯度下降法和卡尔曼滤波等，都是基于MARG(magnetic,angular rate,and gravity)传感器测量模型进行姿态估计。前两种算法实现起来比较容易，无需针对系统进行专门的建模，在大部分的情况下可以满足飞行要求，但遇到大的机动飞行或磁干扰等情况，会导致姿态角精度下降、误差收敛速度慢等情况，使飞机发生振荡、自旋等现象，影响飞行安全。卡尔曼滤波相对前两种方法更为复杂，需要对系统进行建模，但可以对不同传感器的量测噪声进行调节，同时还能对传感器的误差进行估计。飞机的导航系统方程通常都是非线性的，扩展卡尔曼滤波(Extended Kalman Filter，EKF)可将系统方程和量测方程作泰勒级数展开，略去高阶项，近似为线性系统，再做卡尔曼滤波估计。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何设计一种小型无人机MARG航姿估计方法，提高姿态估计精度。

(二)技术方案

为了解决上述技术问题，本发明提供了一种基于自适应EKF算法的小型无人机MARG航姿估计方法，包括以下步骤：

步骤一、采集陀螺仪测量值加速度计测量值和磁力计的测量值

步骤二、更新载体姿态四元数；

姿态四元数的递推计算公式为：

式中，表示四元数乘法，q_k为当前时刻k的姿态四元数，k＝0,1,2，q_new为[t,t_k+1]时间段内载体姿态变化四元数；

式中，Δθ_x,Δθ_y,Δθ_z是[t,t_k+1]时间段内x,y,z陀螺的角增量输出，Δθ是[t,t_k+1]时间段内的角增量向量，

步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；

由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：

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

步骤四、选取所述姿态四元数的扩展卡尔曼滤波系统状态向量为:

X＝[q₀ q₁ q₂ q₃ bw_x bw_y bw_z]