CN111207734A

CN111207734A - 一种基于ekf的无人机组合导航方法

Info

Publication number: CN111207734A
Application number: CN202010048409.6A
Authority: CN
Inventors: 陈朋印; 李定涌; 闫永驰; 陈柯柯; 马一鸣; 曾幼涵
Original assignee: Xi'an Innno Aviation Technology Co ltd
Current assignee: Xi'an Innno Aviation Technology Co ltd
Priority date: 2020-01-16
Filing date: 2020-01-16
Publication date: 2020-05-29
Anticipated expiration: 2040-01-16
Also published as: CN111207734B

Abstract

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

Description

一种基于EKF的无人机组合导航方法

技术领域

本发明属于无人机技术领域，特别涉及一种基于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—状态转移矩阵；

时刻状态向量的预测。

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

其中：

时刻无人机姿态四元数预测；q_k-1—k-1时刻无人机姿态四元数；Δq—k-1至k时刻无人机姿态四元数变化量；

—四元数相乘；

k-1至k时刻无人机姿态四元数变化量计算公式如下：

其中：Δθ_x—k-1至k时刻无人机x轴角增量；Δθ_y—k-1至k时刻无人机y轴角增量；Δθ_z—k-1至k时刻无人机z轴角增量；k-1至k时刻无人机各轴角增量计算公式如下：

Δθ_x＝(ω_x-ε_x)·Δt

Δθ_y＝(ω_y-εy)·Δt

Δθ_z＝(ω_z-ε_z)·Δt

其中：ω_x—无人机x轴陀螺输出的角速度；ε_x—无人机x轴陀螺零偏；ω_y——无人机y轴陀螺输出的角速度；ε_y—无人机y轴陀螺零偏；ω_z—无人机z轴陀螺输出的角速度；ε_z—无人机z轴陀螺零偏；Δt—k-1至k时刻时间间隔。

进一步的，无人机速度预测：

其中：

—无人机k时刻速度预测；v_k-1—无人机k-1时刻速度；g₀—无人机飞行地点当地重力加速度；

—载体坐标系至导航坐标系的坐标转换矩阵；Δv—k-1至k时刻速度增量；

载体坐标系至导航坐标系的坐标转换矩阵的计算公式如下：

k-1至k时刻速度增量的计算公式如下：

其中：a_x—无人机x轴加速度计输出的加速度；

—无人机x轴加速度计零偏；a_y—无人机y轴加速度计输出的加速度；

—无人机y轴加速度计零偏；a_z—无人机z 轴加速度计输出的加速度；

—无人机z轴加速度计零偏。

进一步的，无人机的位置预测：

其中：

——k时刻无人机位置预测；p_k-1——k-1时刻无人机位置；v_k-1——k-1时刻无人机速度；

无人机陀螺零偏与加速度计零偏的预测：

其中：

——k时刻无人机陀螺零偏；ε_k-1——k-1时刻无人机陀螺零偏；

——k时刻无人机加速度计零偏；

——k-1时刻无人机加速度计零偏。

进一步的，步骤1中，状态转移矩阵Φ_k/k-1的计算：

设k时刻无人机的状态向量为

k-1时刻无人机的状态向量为

计算k时刻状态向量与k-1时刻状态向量雅克比矩阵，该矩阵通过调用MATLAB中的jacobian函数计算，即为k-1时刻至k时刻的状态转移矩阵Φ_k/k-1。

进一步的，步骤3中，量测矩阵的计算：

首先介绍姿态量测矩阵的计算

假设某系统可提供无人机的姿态角[γ θ ψ]^T，γ为无人机滚转角，θ为无人机俯仰角，ψ为无人机航向角，无人机姿态角的预测值如下：

其中：

—无人机滚转角量测预测值；

—无人机俯仰角量测预测值；

—无人机航向角量测预测值；q₀—

中的实部；[q₁ q₂ q₃]—

中的虚部；

计算姿态量测预测值与状态向量的雅克比矩阵得到姿态量测矩阵H_att；

速度与位置的量测矩阵的计算：

假设某系统可提供无人机的速度[v_n v_e v_d]^T与位置[p_n p_e p_d]^T，无人机的速度与位置预测值如下：

其中：

—无人机速度量测预测值；

—无人机速度状态预测值；

—无人机位置量测预测值；

—无人机位置状态预测值；

分别计算速度量测预测值与位置量测预测值与状态向量的雅克比矩阵得到速度的量测矩阵H_v与位置的量测矩阵H_p。

进一步的，EKF滤波实施过程如下：

状态预测

状态误差协方差矩阵预测

滤波器增益

状态误差协方差矩阵更新

状态更新

其中：

P_k-1—k-1时刻EKF滤波状态误差协方差矩阵；

—k时刻EKF滤波状态误差协方差矩阵预测值；Q_k-1—k-1时刻系统噪声矩阵；R_k—k时刻量测噪声矩阵；K_k—k时刻滤波器增益矩阵；P_k—k时刻EKF滤波状态误差协方差矩阵。

进一步的，量测进行一致性检测：

假设北向速度量测为v_n，北向速度量测预测值为

将北向速度量测值与北向速度量测预测值做差记为

假设量测噪声阵R_k中对应的北向速度量测噪声为

误差协方差矩阵

中对应的北向速度误差为

量测一致性检测公式如下：

其中k为系数，认为量测噪声阵R_k与误差协方差矩阵

中对应的量测误差是在1σ下计算的，而且

与

均为误差的平方，若要使一致性检测在3σ下通过，k一般取9，具体值可根据提供量测数据的传感器进行设置；

其它量测的一致性检测与北向速度的一致性检测相同，在进行EKF滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行滤波器协方差矩阵正定性检测，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，不进行滤波器协方差矩阵正定性检测。

进一步的，滤波器协方差矩阵正定性检测：

设

在进行滤波器协方差矩阵更新时，需要保证P_k为正定矩阵，根据EKF滤波第4步公式可知需要保证

的每个对角线元素分别大于KHP对应对角线元素，滤波器协方差矩阵正定性检测公式如下：

进行EKF滤波时，只有量测通过一致性检测，滤波器协方差矩阵通过正定性检测才利用该量测对滤波器状态进行修正，只要有一个检测未通过，均不可利用该量测对滤波器状态进行修正，只进行状态预测与状态误差协方差矩阵预测。

与现有技术相比，本发明有以下技术效果：

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

解决了现有技术中计算量大、鲁棒性差以及模型建立过程复杂繁琐的问题。

本发明计算量小，实时性高。

本发明可直接输出无人机的姿态、速度与位置，不需要建立复杂的误差模型。

本发明设计过程简单，模型建立与代码生成全部由MATLAB自动完成。

本发明可实时对量测信息进行一致性检测，防止误差较大的量测对滤波器造成影响。

本发明可实时对滤波器协方差矩阵进行正定性检测，防止滤波器发散，并及时对滤波器进行调整。

附图说明

图1是基于EKF的无人机组合导航方法功能框图；

图2是序贯滤波流程框图。

具体实施方式

以下结合附图对本发明进一步说明：

本发明提出的一种基于EKF的无人机组合导航方法的具体实施方式如下：

第一步选取组合导航系统的状态向量(X_k)，构建状态向量的预测方程，在MATLAB中计算状态预测的雅克比矩阵，此矩阵为状态转移矩阵(Φ_k/k-1)，至此，状态向量的预测过程完成。

第二步根据EKF滤波过程计算状态向量误差协方差矩阵预测

以及滤波器增益矩阵(K_k)。

第三步根据组合导航系统中的传感器选取量测并计算量测的预测值

然后计算量测预测值的雅克比矩阵，此矩阵为量测矩阵(H_k)，至此组合导航系统的模型建立完成。

第四步将各个矩阵的表达式按字符串形式存储至文件，至此组合导航系统的可编程利用代码完成，该代码可直接在程序中使用。

第五步在滤波器使用时加入量测一致性检查与协方差矩阵正定性检测，至此，基于 EKF的组合导航系统设计完成。

下面结合附图对本发明提出的一种基于EKF的无人机组合导航方法的原理进行详细说明。

选取无人机的姿态四元数、速度、位置、陀螺零偏与加速度计零偏为状态向量，即：

为x轴加速度计零偏，

为y轴加速度计零偏，

为z轴加速度计零偏。系统的状态方程为

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

——k时刻状态向量的预测；

下面介绍状态向量的预测与Φ_k/k-1的计算过程：

首先介绍无人机姿态四元数的状态预测

其中：

——k时刻无人机姿态四元数预测；q_k-1——k-1时刻无人机姿态四元数；Δq——k-1至k时刻无人机姿态四元数变化量；

——四元数相乘；

k-1至k时刻无人机姿态四元数变化量计算公式如下：

其中：Δθ_x——k-1至k时刻无人机x轴角增量；Δθ_y——k-1至k时刻无人机y轴角增量；Δθ_z——k-1至k时刻无人机z轴角增量；

k-1至k时刻无人机各轴角增量计算公式如下：

Δθ_x＝(ω_x-ε_x)·Δt

Δθ_y＝(ω_y-εy)·Δt

Δθ_z＝(ω_z-ε_z)·Δt

其中：ω_x——无人机x轴陀螺输出的角速度；ε_x——无人机x轴陀螺零偏；ω_y——无人机y轴陀螺输出的角速度；ε_y——无人机y轴陀螺零偏；ω_z——无人机z轴陀螺输出的角速度；ε_z——无人机z轴陀螺零偏；Δt——k-1至k时刻时间间隔；

至此无人机姿态四元数预测完成，下面介绍无人机速度预测

其中：

——无人机k时刻速度预测；v_k-1——无人机k-1时刻速度；g₀——无人机飞行地点当地重力加速度；

——载体坐标系至导航坐标系的坐标转换矩阵；Δv——k-1至k时刻速度增量；

载体坐标系至导航坐标系的坐标转换矩阵的计算公式如下：

k-1至k时刻速度增量的计算公式如下：

其中：a_x——无人机x轴加速度计输出的加速度；

——无人机x轴加速度计零偏；a_y——无人机y轴加速度计输出的加速度；

——无人机y轴加速度计零偏；a_z——无人机z轴加速度计输出的加速度；

——无人机z轴加速度计零偏；

至此，无人机的速度预测完成，下面介绍无人机的位置预测

其中：

——k时刻无人机位置预测；p_k-1——k-1时刻无人机位置；v_k-1—— k-1时刻无人机速度；

至此，无人机位置预测完成，下面介绍无人机陀螺零偏与加速度计零偏的预测

其中：

——k时刻无人机加速度计零偏；

——k-1时刻无人机加速度计零偏；

至此无人机的状态向量的预测全部完成，下面介绍状态转移矩阵Φ_k/k-1的计算方法，设k时刻无人机的状态向量为

k-1时刻无人机的状态向量为

k-1时刻至k时刻状态向量的预测公式上面已介绍，因此只需计算k时刻状态向量与k-1时刻状态向量雅克比矩阵(该矩阵可通过调用MATLAB中的jacobian函数计算)，即为k-1时刻至k时刻的状态转移矩阵Φ_k/k-1，至此已完成状态向量的预测过程与状态转移矩阵Φ_k/k-1的计算。下面介绍量测矩阵的计算，首先介绍姿态量测矩阵的计算

其中：

——无人机滚转角量测预测值；

——无人机俯仰角量测预测值；

——无人机航向角量测预测值；q₀——

中的实部；[q₁ q₂ q₃]——

中的虚部；

只需计算姿态量测预测值与状态向量的雅克比矩阵即可计算出姿态量测矩阵H_att。至此已完成姿态量测矩阵的计算，下面介绍速度与位置的量测矩阵的计算。

其中：

——无人机速度量测预测值；

——无人机速度状态预测值；

——无人机位置量测预测值；

——无人机位置状态预测值；

只需分别计算速度量测预测值与位置量测预测值与状态向量的雅克比矩阵即可计算出速度的量测矩阵H_v与位置的量测矩阵H_p。至此已完成速度量测矩阵与位置量测矩阵的计算，下面只需根据EKF滤波方程计算误差协方差矩阵与增益矩阵的表达式并存储到文件中即可，EKF滤波实施过程如下：

状态预测

状态误差协方差矩阵预测

滤波器增益

状态误差协方差矩阵更新

状态更新

其中：P_k-1——k-1时刻EKF滤波状态误差协方差矩阵；

——k时刻EKF滤波状态误差协方差矩阵预测值；Q_k-1——k-1时刻系统噪声矩阵；R_k——k时刻量测噪声矩阵；K_k——k时刻滤波器增益矩阵；P_k——k时刻EKF滤波状态误差协方差矩阵；

为了增加系统的鲁棒性，避免误差较大的量测对滤波器造成不利的影响，需要实时对量测进行一致性检测，为了实现对每个量测分别进行一致性检测，需要将上述EKF滤波形式设计为序贯滤波的形式，序贯滤波的实施流程图如图2所示，下面介绍对量测进行一致性检测的方法。

假设北向速度量测为v_n，北向速度量测预测值为

将北向速度量测值与北向速度量测预测值做差记为

假设量测噪声阵(R_k)中对应的北向速度量测噪声为

误差协方差矩阵

中对应的北向速度误差为

量测一致性检测公式如下：

其中k为系数，一般认为量测噪声阵(R_k)与误差协方差矩阵

中对应的量测误差是在1σ下计算的，而且

与

均为误差的平方，若要使一致性检测在3σ下通过，k一般取9，具体值可根据提供量测数据的传感器进行设置。

其它量测的一致性检测与北向速度的一致性检测相同，在进行EKF滤波时分别对每个量测进行一致性检测，一致性检测通过后再进行滤波器协方差矩阵正定性检测，即EKF第4步，假如该量测没有通过一致性检测，则只进行状态预测与状态误差协方差矩阵预测，不进行滤波器协方差矩阵正定性检测，即EKF滤波的第1步与第2步，为了减小CPU 的计算量，一致性检测步骤一般EKF滤波第2步与第3步之间，假如一致性检测未通过则不需计算滤波器增益，可大大减少CPU的计算量。

至此量测一致性检测已完成，下面介绍对滤波器协方差矩阵正定性检测，设

在进行滤波器协方差矩阵更新时，即EKF滤波第4步，需要保证P_k为正定矩阵，根据EKF滤波第4步公式可知需要保证

的每个对角线元素分别大于 KHP对应对角线元素，滤波器协方差矩阵正定性检测公式如下：

进行EKF滤波时，只有量测通过一致性检测，滤波器协方差矩阵通过正定性检测才可利用该量测对滤波器状态进行修正，只要有一个检测未通过，均不可利用该量测对滤波器状态进行修正，只进行状态预测与状态误差协方差矩阵预测，即EKF滤波的第1步与第2步。