CN111189442B - 基于cepf的无人机多源导航信息状态预测方法 - Google Patents

Abstract

本发明公开了基于CEPF的无人机多源导航信息状态预测方法，旨在解决解决无人机组合导航系统中多源信息融合面临的模型不确定性和偏差问题。本发明首先建立INS组合导航系统的直接法状态方程和量测方程，然后对组合导航系统方程进行离散化，最后利用CEPF算法降低测量信息中的偏差对组合导航系统的不利影响；CEPF首先使用“consider”方法将偏差的统计信息合并到状态估计方程中，Consider卡尔曼滤波完成对目标的滤波估计处理。本发明的有益技术效果在于：有效地降低了偏差和模型不确定性的影响，提高了无人机组合导航的精度。

Description

基于CEPF的无人机多源导航信息状态预测方法

技术领域

本发明涉及一种无人机多源信息融合导航系统，具体涉及一种基于Consider扩展卡尔曼滤波(Consider Extended Kalman Particle Filter,CEPF)的无人机多源导航信息融合方法。

背景技术

无人机具有机动性能强、可垂直起降和定点悬停等优点，在军事和民用领域应用前景广泛。随着应用范围的发展，无人机执行飞行任务面临的问题越来越困难，且具有较高的动态性和较大的不确定性。无人机导航系统可以为无人机提供自身的运动参数、物理环境参数等信息，而精准的导航信息是无人机执行飞行任务的首要条件。由于惯性导航系统(Inertial Navigation System，INS)具有自主性强、隐蔽性好、信息更加全面和宽频带等特有优点，无人机导航系统选用INS作为组合导航系统的主导航系统，并引入GPS、磁力计、光流、气压高度计辅助导航方式，通过它们的观测信息与主导航系统的信息进行数据的融合处理，进而修正惯导的累积误差，提高导航的精度。但是无人机组合导航系统中往往存在模型不确定和系统偏差等问题，当采用PF对无人机组合导航系统中的多源信息进行融合时，这些模型的不确定性和系统偏差将会极大地降低导航信息获取的精度，甚至会导致发散。

发明内容

本发明要解决的技术问题是提供一种基于CEPF的无人机多源导航信息状态预测方法，以解决无人机组合导航系统中多源信息融合面临的模型不确定性和偏差的技术问题。

Consider扩展卡尔曼滤波(Consider Extended Kalman Filter,CEKF)的核心思想是将非线性系统展开成泰勒级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用Consider卡尔曼滤波完成对目标的滤波估计处理，因此，多源信息融合算法是实现无人机能够搭载组合导航系统的核心技术。

为解决上述技术问题，本发明采用如下技术方案：

设计一种基于CEPF的无人机多源导航信息状态预测方法，包括如下步骤：

S1：获取无人机组合导航系统中不同时刻相关的导航参数，带入无人机的状态方程和量测方程；所述量测方程包括偏差p；

S2：利用初始时刻导航参数，获取无人机状态一步预测、方差一步预测、一步预测的协方差、状态估计误差方差阵及滤波增益矩阵一步预测；

S3：利用步骤S2中无人机状态一步预测、方差一步预测、一步预测的协方差、状态估计误差方差阵及滤波增益矩阵一步预测；根据卡尔曼滤波算法，获得下一时刻的无人机状态更新；

S4：更新粒子集合的状态估计值和量测值、每个粒子的权重；

S5：对粒子集合进行复制和淘汰；获得重采样之后的状态估计粒子集合；

S6：对上述重采样之后获得的状态估计粒子集合求均值；

S7：循环迭代S1-S6，即可得到无人机组合导航系统的此时刻的参数。

优选的，在步骤S1中，所述导航参数包括：

GPS测得的载体位置及载体位置的东方向分量、北方向分量和天方向分量，东方向、北方向、天方向的GPS位置偏差，东方向、北方向、天方向的GPS位置量测值的随机误差；

气压高度计测得的载体高度、高度偏差、测值的随机误差；

光流速度矢量，光流速度的偏差，光流速度量测值的随机误差；

磁力计测定的地磁场矢量，东方向、北方向、天方向的磁力计，东方向、北方向、天方向的磁力计量测值的随机误差；

加速度计测定的东方向分量、北方向分量和天方向分量线加速度，加速度计常值漂移；

无人机的横滚角、俯仰角、航向角；

陀螺仪在IMU中机体坐标系下输出的三个轴向的瞬时旋转角速度，陀螺仪常值漂移。

优选的，在步骤S1中，所述量测方程为：

z＝h(x)+p+v_b

其中，v_b为量测噪声，其表达式为：v_b＝[v_me,v_mn,v_mu,v_re,v_rn,v_ru,v_vx,v_vy,v_h]^T

式中，h(x)＝[m_b r v_flow h_baro]^T，m_b＝[m_bx m_by m_bz]为地理系下的地磁场矢量；

r＝[r_ge r_gn r_gu]为GPS测得的载体位置；

r_ge、r_gn和r_gu是GPS测得载体位置的东方向分量、北方向分量和天方向分量；

v_flow＝[v_{flow_x} v_{flow_y}]为光流速度矢量，v_{flow_x}和v_{flow_y}分别是光流传感器测得的载体光流速度x方向分量和y方向分量；

h_baro为气压高度计测得的载体高度；p＝[p_me,p_mn,p_mu,p_re,p_rn,p_ru,p_vx,p_vy,p_h]^T为偏差，p_me、p_mn和p_mu分别是东北天方向的磁力计偏差，p_re、p_rn和p_ru分别是东北天方向的GPS位置偏差，p_vx和p_vy是光流速度的偏差，p_h为气压高度计的高度偏差；

v_b为量测噪声，v_me、v_mn和v_mu分别为在导航坐标系东、北、天方向磁力计量测值的随机误差；v_re，v_rn和v_ru分别为在导航坐标系东、北、天GPS位置量测值的随机误差；

v_vx和v_vy是光流速度量测值的随机误差；v_h为气压高度计量测值的随机误差。

优选的，在步骤S1中，所述状态方程预先要经过离散化得到离散状态方程：

x_k＝F(x_k-1,p)+w_b,k-1

所述离散量测方程为：

z_k＝h(x_k,p)+v_b,k；

其中，x_k是状态向量；z_k是量测向量；F非线性状态方程函数；h是非线性量测方程函数；w_b,k-1和v_b,k分别为过程噪声和量测噪声。

进一步，将建立的离散状态方程和量测方程进行线性处理得到线性化离散状态方程和线性化离散状态方程系数矩阵，线性化离散量测方程和线性化离散量测方程系数矩阵。

进一步，所述线性化离散状态方程是采用线性化一阶Taylor级数展开方法，在状态估计

和偏差均值/>

附近展开，并进行整理，可以得到，其表达式：

所述线性化离散状态方程系数矩阵Φ_k/k-1和Ψ_k/k-1为：

所述线性化离散量测方程为：

所述线性化离散量测方程系数矩阵H_k和N_k为：

优选的，在步骤S2中，获取状态估计集合点为：

状态估计误差方差集合点为：

初始化状态估计误差与偏差的互协方差集合点：

进一步，在步骤S3中，根据第k-1步的状态估计值

状态估计误差方差阵/>

状态估计误差与偏差的互协方差为/>

则第k步的状态一步预测为：

第k步线性化后的离散状态方程系数矩阵

和/>

为：

第k步的系统量测方程为：

线性化后的系数矩阵H_k和N_k为：

第k步状态估计误差方差阵一步预测为：

第k步状态估计误差与不确定参数的互协方差为：

进一步，在步骤S4中，所述计算滤波增益矩阵为：

其中，

计算状态更新和计算协方差更新

状态估计误差方差阵为：

状态估计误差与偏差的互协方差为：

更新粒子集合的状态估计值和量测值

归一化得：

进一步，在步骤S5中，所述重采样算法是根据归一化权值

大小，对粒子集合/>

进行复制和淘汰，获得重采样之后的状态估计粒子集合/>

与现有技术相比，本发明的有益技术效果在于：

1.本发明通过将粒子滤波方法引入到CEKF算法中，提出了一种能消除系统偏差的CEPF算法；CEPF算法采用将偏差的统计特性(方差)引入到滤波过程，不直接估计该偏差，但无人机实际的状态预测的精度会受偏差的影响，导致状态的估计精度下降，甚至导致状态预测的发散致使就是无人机会失控；本发明将偏差直接引入量测方程中，对无人机的状态预测更接近无人机的真实飞行场景；同时，本发明采用一种基于序列的粒子滤波器进行重要抽样，具体是在状态X的方差范围内选取粒子点，增加了粒子取值，增加状态估计值的精确度。

2.本发明与PF算法相比，PF算法由于受到系统偏差的影响，导航误差出现了偏差，偏离了零点，且受到模型偏差的影响导致滤波过程的瞬时误差较大，而采用CEPF算法的位置误差与PF算法的位置误差相比数值较小且更具稳定性，CEPF算法鲁棒性明显优于PF算法，即存在较大的外部干扰或者系统偏差的情况下CEPF能较好的保证状态估计的精度，使信息预测结果更接近于真实状态。

3.本发明因模型存在不确定性及飞行状态突变和存在偏差情况下，在状态方程和量测方程的基础上，结合CEPF滤波计算出来增益K，然后通过一步预测和量测增益K求得下一步的预测值，有效降低了偏差的干扰，降低了偏差不确定性的影响；本发明无人机导航信息预测方法效果明显优于PF预测方法，有效地降低了偏差和模型不确定性的影响，提高了无人机组合导航的精度。

附图说明

图1为CEPF和PF算法的位置误差比较图；

图2为CEPF和PF算法的姿态角误差比较图；

图3为CEPF和PF算法的速度误差比较图。

具体实施方式

下面结合附图和实施例来说明本发明的具体实施方式，但以下实施例只是用来详细说明本发明，并不以任何方式限制本发明的范围。

实施例1：基于CEPF的无人机多源导航信息状态预测方法，包括以下步骤：

步骤一、建立无人机组合导航系统的动力学方程

式中，r＝[r_e r_n r_u]^T为无人机东、北、天方向的位置向量，v＝[v_e v_n v_u]^T为东、北、天方向的速度向量，

为从机体坐标系到导航坐标系的转换矩阵，/>

为IMU中机体坐标系下的由加速度计输出的三个轴向的线加速度，b_a为加速度计的常值漂移，g为地球的重力加速度在导航坐标系下的投影。Ω＝[γ θ ψ]^T，其中γ、θ、ψ分别为无人机的横滚角、俯仰角、航向角，/>

为IMU中机体坐标系下的由陀螺输出的三个轴向的瞬时旋转角速度，b_w为陀螺的常值漂移。K为姿态运动学矩阵

选取状态向量为x＝[r v Ω]^T，则无人机组合导航系统的动力学方程(1)可简化为

式中，f(x)为系统的非线性连续状态转移函数，w_b为零均值的高斯白噪声。

步骤二、无人机组合导航系统的量测方程

本发明采用无人机组合导航系统的多源信息融合，并以恒定采样时间间隔测量无人机的位置和速度，建立相应的导航系统的量测方程为：

z＝h(x)+p+v_b (4)

r＝[r_ge r_gn r_gu]为GPS测得的载体位置，r_ge、r_gn和r_gu是GPS测得载体位置的东方向分量、北方向分量和天方向分量；v_flow＝[v_{flow_x} v_{flow_y}]为光流速度矢量，v_{flow_x}和v_{flow_y}分别是光流传感器测得的载体光流速度x方向分量和y方向分量；h_baro为气压高度计测得的载体高度；p＝[p_me,p_mn,p_mu,p_re,p_rn,p_ru,p_vx,p_vy,p_h]^T为偏差，p_me、p_mn和p_mu分别是东、北、天方向的磁力计偏差，p_re、p_rn和p_ru分别是东、北、天方向的GPS位置偏差，p_vx和p_vy是光流速度的偏差，p_h为气压高度计的高度偏差；v_b为量测噪声

v_b＝[v_me,v_mn,v_mu,v_re,v_rn,v_ru,v_vx,v_vy,v_h]^T (5)

其中，v_me、v_mn和v_mu分别为在导航坐标系东、北、天方向磁力计量测值的随机误差；v_re，v_rn和v_ru分别为在导航坐标系东、北、天GPS位置量测值的随机误差；v_vx和v_vy是光流速度量测值的随机误差；v_h为气压高度计量测值的随机误差。并且建立的量测方程本身已经是时间离散的，故无需离散化处理。

量测方程中采用磁传感器的地磁矢量表达式为：

式中，mⁿ为地理系下的地磁场矢量。光流速度矢量表达式：

式中，f为焦距，w＝[w_x w_y w_z]为角运动速度。假设无人机平稳飞行状态，即横滚角与俯仰角都较小，则光流传感器至地面的高度h＝p_u。导航坐标系至机体坐标系速度转换矩阵为

其中，T_x和T_y分别是x方向和y方向上机载光流传感器测量的光流像素点的线运动速度分量。

步骤三：离散化上述动力学方程和量测方程

对无人机组合导航系统的动力学方程(3)进行离散化处理，以获得离散的动力学方程：

x_k＝x_k-1+f(x_k-1)·t (9)

式中，t对应于构建量测方程步骤的采样时间间隔，x_k-1是T_k-1时刻的状态向量，x_k是T_k时刻的状态向量。

基于所述无人机组合导航系统中带有的偏差建立非线性离散状态方程和量测方程：

x_k＝F(x_k-1,p)+w_b,k-1 (10)

z_k＝h(x_k,p)+v_b,k (11)

其中，x_k是状态向量；z_k是量测向量；F非线性状态方程函数；h是非线性量测方程函数；p称为偏差，具有方差P_pp，且满足：

式中，

为偏差均值；w_b,k-1和v_k分别为过程噪声和量测噪声，且二者是相互独立的零均值高斯白噪声，二者仍满足以下条件：

其中，δ_kj为Kroneckerδ函数，当k＝j时，δ_kj＝1；当k≠j时，δ_kj＝0；通过统计学方法获得GPS/INS导航系统模型的系统噪声方差阵Q_k和量测噪声方差阵R_k，Q_k为状态方程中w_b,k-1的方差，R_k为量测方程中v_b,k的方差；

状态估计误差与偏差的互协方差为：

其中，

为状态估计误差，C_k的其初始值为/>

对上述非线性状态方程(10)采用线性化一阶Taylor级数展开方法进行线性化。利用一阶Taylor级数，围绕状态估计

和偏差均值/>

非线性状态方程(10)和非线性量测方程(11)进行泰勒展开，线性化后并进行整理，可以得到：

其中，线性化以后的系数矩阵Φ_k/k-1和Ψ_k/k-1为：

同样，对非线性量测方程(11)在先验状态估计

和偏差均值/>

附近进行展开线性化，可得：

其中，线性化以后的系数矩阵H_k和N_k为：

步骤四、对离散后的状态方程和量测方程进行处理，输出无人机组合导航系统位置、速度及姿态；

1.初始化与该无人机组合导航系统匹配的状态模型

其中，初始状态

初始载体所在位置r₀＝[0 0 5m]；初始的载体速度v₀＝[0 0 0]；初始的载体姿态角Ω＝[0 0 π/2]；初始状态估计误差方差矩阵/>

为初始位置方差阵，/>

为初始速度方差阵，/>

为初始陀螺仪常值漂移方差阵。P₀、Q_k和R_k均不相关；

2.初始的状态估计值粒子集合点：

式中，m为状态估计的粒子点数。

初始化状态估计误差方差粒子集合点：

初始化状态估计误差与偏差的互协方差粒子集合点：

3.计算状态粒子集合的一步预测和方差的一步预测

设第k-1步的状态估计值、状态估计误差方差阵分别为

和/>

第k-1步状态估计误差与偏差的互协方差为/>

则第k步的状态一步预测为：

由方程(16)和(17)，可得线性化后的系数矩阵

和/>

为：

第k步的系统量测方程为：

由方程(19)和(20)，可得线性化离散量测方程系数矩阵H_k和N_k为：

状态估计误差方差阵一步预测为：

状态估计误差与偏差的互协方差为

4.计算滤波增益矩阵

其中，

5.计算状态更新和计算协方差更新

状态更新

状态估计误差方差阵为：

状态估计误差与偏差的互协方差为：

6.更新粒子集合的状态估计值和量测值

7.计算每个粒子的权重

归一化得：

8.选择阶段(重采样)

利用重采样算法，根据归一化权值

大小，对粒子集合/>

进行复制和淘汰。获得重采样之后的状态估计粒子集合/>

9.获得CEPF算法的第k步状态估计值

对重新获得的状态估计粒子集求均值：

以上9步循环迭代，得到无人机组合导航系统的位置、速度和姿态。

进一步的在所述的无人机组合导航系统中，采样时间间隔t＝1[s]，在观测次数为

N＝50、总的采样时间为T＝3600s统计获得

本发明通过将粒子滤波方法引入到CEKF算法中，提出了一种能消除系统偏差的CEPF算法。CEPF算法采用将偏差的统计特性(方差)引入到滤波过程不直接估计该偏差；同时，采用一种基于序列的粒子滤波器进行重要抽样。图1～3分别为位置、姿态角和速度的误差比较图，如图所示，采用本发明的基于CEPF的无人机多源导航信息状态预测方法时，位置、姿态角和速度的误差均在零值附近，误差较小；相比而言，采用PF算法的误差较大，性能较差。总之，本发明基于CEPF处理方法可以有效地减轻导航参数采样偏差负面影响，并提高组合导航系统的导航精度。

上面结合附图和实施例对本发明作了详细的说明，但是，所属技术领域的技术人员能够理解，在不脱离本发明宗旨的前提下，还可以对上述实施例中的各个具体参数进行变更，形成多个具体的实施例，均为本发明的常见变化范围，在此不再一一详述。

Claims (10)

1.一种基于CEPF的无人机多源导航信息状态预测方法，其中，CEPF指ConsiderExtended Particle Filter，意为Consider扩展粒子滤波，其特征在于，包括如下步骤：

S1：获取无人机组合导航系统中不同时刻相关的导航参数，包括无人机组合导航系统的位置、速度和姿态，带入无人机的状态方程和量测方程；所述量测方程包括偏差p；

S2：利用初始时刻导航参数，获取无人机状态一步预测、方差一步预测、一步预测的协方差、状态估计误差方差阵及滤波增益矩阵一步预测；

S3：利用步骤S2中无人机状态一步预测、方差一步预测、一步预测的协方差、状态估计误差方差阵及滤波增益矩阵一步预测；根据卡尔曼滤波算法，获得下一时刻的无人机状态更新；

S4：更新粒子集合的状态估计值和量测值、每个粒子的权重；

S5：对粒子集合进行复制和淘汰；获得重采样之后的状态估计粒子集合；

S6：对上述重采样之后获得的状态估计粒子集合求均值；

S7：循环迭代S1-S6，即可得到无人机组合导航系统的预测时刻的位置、速度和姿态。

2.根据权利要求1所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S1中，所述导航参数包括：

GPS测得的载体位置及载体位置的东方向分量、北方向分量和天方向分量，东方向、北方向、天方向的GPS位置偏差，东方向、北方向、天方向的GPS位置量测值的随机误差；

气压高度计测得的载体高度、高度偏差、量测值的随机误差；

光流速度矢量，光流速度的偏差，光流速度量测值的随机误差；

磁力计测定的地磁场矢量，东方向、北方向、天方向的磁力计偏差，东方向、北方向、天方向的磁力计量测值的随机误差；

加速度计测定的东方向分量、北方向分量和天方向分量线加速度，加速度计常值漂移；

无人机的横滚角、俯仰角、航向角；

陀螺仪在IMU中机体坐标系下输出的三个轴向的瞬时旋转角速度，陀螺仪常值漂移。

3.根据权利要求1所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S1中，所述量测方程为：

z＝h(x)+p+v_b

其中，v_b为量测噪声，其表达式为：v_b＝[v_me,v_mn,v_mu,v_re,v_rn,v_ru,v_vx,v_vy,v_h]^T

式中，h(x)＝[m_b r v_flow h_baro]^T，m_b＝[m_bx m_by m_bz]为地理系下的地磁场矢量；

r＝[r_ge r_gn r_gu]为GPS测得的载体位置；

r_ge、r_gn和r_gu是GPS测得载体位置的东方向分量、北方向分量和天方向分量；

v_flow＝[v_{flow_x} v_{flow_y}]为光流速度矢量，v_{flow_x}和v_{flow_y}分别是光流传感器测得的载体光流速度x方向分量和y方向分量；

h_baro为气压高度计测得的载体高度；p＝[p_me,p_mn,p_mu,p_re,p_rn,p_ru,p_vx,p_vy,p_h]^T为偏差，p_me、p_mn和p_mu分别是东北天方向的磁力计偏差，p_re、p_rn和p_ru分别是东北天方向的GPS位置偏差，p_vx和p_vy是光流速度的偏差，p_h为气压高度计的高度偏差；

v_b为量测噪声，v_me、v_mn和v_mu分别为在导航坐标系东、北、天方向磁力计量测值的随机误差；v_re，v_rn和v_ru分别为在导航坐标系东、北、天GPS位置量测值的随机误差；

v_vx和v_vy是光流速度量测值的随机误差；v_h为气压高度计量测值的随机误差。

4.根据权利要求1所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S1中，所述状态方程预先要经过离散化得到离散状态方程：

x_k＝F(x_k-1,p)+w_b,k-1

以及离散量测方程：

z_k＝h(x_k,p)+v_b,k；

其中，x_k是状态向量；z_k是量测向量；F非线性状态方程函数；h是非线性量测方程函数；w_b,k-1和v_b,k分别为过程噪声和量测噪声。

5.根据权利要求4所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：将建立的离散状态方程和量测方程进行线性处理得到线性化离散状态方程和线性化离散状态方程系数矩阵，线性化离散量测方程和线性化离散量测方程系数矩阵。

6.根据权利要求5所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：所述线性化离散状态方程是采用线性化一阶Taylor级数展开方法，在状态估计

和偏差均值p附近展开，并进行整理，可以得到，其表达式：

w_k-1为过程噪声；

所述线性化离散状态方程系数矩阵Φ_k/k-1和Ψ_k/k-1为：

所述线性化离散量测方程为：

v_k为量测噪声；

所述线性化离散量测方程系数矩阵H_k和N_k为：

7.根据权利要求6所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S2中，获取状态估计集合点为：

状态估计误差方差集合点为：

初始化状态估计误差与偏差的互协方差集合点：

以上各式中，初始状态为

初始状态估计误差方差矩阵为/>

C₀为初始状态估计误差与偏差的互协方差；j＝1,2,...,m为状态估计的粒子点数。

8.根据权利要求6所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S3中，根据第k-1步的状态估计值

状态估计误差方差阵/>

状态估计误差与偏差的互协方差为/>

则第k步的状态一步预测为：

第k步线性化后的离散状态方程系数矩阵

和/>

为：

第k步的系统量测方程为：

线性化后的系数矩阵H_k和N_k为：

第k步状态估计误差方差阵一步预测为：

第k步状态估计误差与不确定参数的互协方差为：

以上各式中，j＝1,2,…,m为状态估计的粒子点数，T表示转置，f表示对应法则，Q_k-1表示k-1时刻的噪声方差阵，P_pp表示偏差p的方差。

9.根据权利要求8所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S4中，

所述计算滤波增益矩阵为：

其中，

计算状态更新和计算协方差更新

状态估计误差方差阵为：

状态估计误差与偏差的互协方差为：

更新粒子集合的状态估计值和量测值

归一化得：

。

10.根据权利要求9所述的基于CEPF的无人机多源导航信息状态预测方法，其特征在于：在步骤S5中，所述重采样是根据归一化权值

大小，对粒子集合/>

进行复制和淘汰，获得重采样之后的状态估计粒子集合/>

Images