[发明专利]一种基于EKF的无人机组合导航方法

说明书

本发明公开了一种基于EKF的无人机组合导航方法，该方法设计的组合导航系统可实时输出高精度的无人机姿态、速度以及位置信息；该方法可通过MATLAB直接生成可编程利用的代码，减少复杂的公式推导过程，并且可提高CPU 60％的运行效率；该方法可实时对量测信息进行一致性检测，防止误差较大的量测信息对滤波器造成的不利的影响；该方法可实时对滤波器的协方差矩阵进行正定性检测，防止滤波器发散，及时对滤波器进行调整。

技术领域

本发明属于无人机技术领域，特别涉及一种基于EKF的无人机组合导航方法。

背景技术

目前，无人机技术的发展突飞猛进，基于无人机的应用领域也越来越广泛。在军事侦查、战场监视、火灾探测、环境与交通监测方面都得到了广泛的应用。无人机在执行任务期间周围环境复杂多变，为了顺利完成任务，优秀的组合导航方法至关重要。目前无人机常用的导航方法包括采用互补滤波的AHRS方法与基于误差方程的组合导航方法，但是基于互补滤波的AHRS方法不能满足无人机大机动情况下的导航需求，输出的姿态精度不能满足要求，这是互补滤波方法本身存在的缺陷，虽然有一些适用于机动的AHRS 方法，但是效果总是不尽人意，基于误差方程的组合导航方法能够满足无人机对导航系统的要求，但是需要建立复杂的导航系统误差方程，而且无人机使用的传感器类型很多，包括：GPS、电子罗盘、三轴陀螺仪、三轴加速度计、气压高度计、超声波测距仪、单/双目相机等，因此该过程过于繁琐，并且在使用过程中需要定期对误差进行反馈重置处理。

发明内容

本发明的目的在于提供一种基于EKF的无人机组合导航方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种基于EKF的无人机组合导航方法，包括以下步骤：

步骤1，选取组合导航系统的状态向量X_k，构建状态向量的预测方程，计算状态预测的雅克比矩阵，此矩阵为状态转移矩阵Φ_k/k-1；

步骤2，根据EKF滤波过程计算状态向量误差协方差矩阵预测以及滤波器增益矩阵K_k；

步骤3，根据组合导航系统中的传感器选取量测并计算量测的预测值然后计算量测预测值的雅克比矩阵，此矩阵为量测矩阵H_k，完成组合导航系统的模型建立；

步骤4，将各个矩阵的表达式按字符串形式存储至文件，至此组合导航系统的可编程利用代码完成；

步骤5，在滤波器使用时加入量测一致性检查与协方差矩阵正定性检测。

进一步的，步骤1具体为：

选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量，其中为q₀为无人机姿态四元数实部，q₁、q₂、q₃为无人机姿态四元数虚部，v_n为无人机北向速度，v_e为无人机东向速度，v_d为无人机地向速度，p_n为无人机北向位置，p_e为无人机东向位置， p_d为无人机地向位置，ε_x为x轴陀螺零偏，ε_y为y轴陀螺零偏，ε_z为z轴陀螺零偏，为x轴加速度计零偏，为y轴加速度计零偏，为z轴加速度计零偏；

系统的状态方程为：

其中：X_k-1—k-1时刻状态向量；Φ_k/k-1—状态转移矩阵；时刻状态向量的预测。

进一步的，无人机姿态四元数的状态预测包括：