CN110243377B - 一种基于分层式结构的集群飞行器协同导航方法 - Google Patents

Abstract

本发明公开了一种基于分层式结构的集群飞行器协同导航方法，在不同机载定位精度的飞行器集群飞行时，根据定位精度的差异，将飞行器分为高精度层和低精度层两种类型，通过机载传感器获取高精度层飞行器和低精度层的相互距离、相对速度、相对位置以及相对的方位角信息，建立低精度层飞行器的状态方程和所有高精度层飞行器的相对速度量测方程和相对距离量测方程，在此基础上，根据状态方程和量测方程通过卡尔曼滤波程序依次对所有低精度层飞行器的机载定位信息进行修正。与未采用该飞行器协同导航方法的低精度层飞行器相比，本发明在高精度层飞行器数量越多的情况下，对低精度层飞行器的机载定位信息修正作用越好，适合实际使用。

Description

一种基于分层式结构的集群飞行器协同导航方法

技术领域

本发明涉及一种基于分层式结构的集群飞行器协同导航方法，属于定位与导航技术领域。

背景技术

近年来，无人机集群协同技术日益受到国内外的关注。无人机集群具有工作范围大、可靠性高、可同时执行多重任务、整体效率高等优点，可用于灾害勘测和救援等领域。无人机集群技术，即多架无人机为适应任务要求而进行的某种队形排列和任务分配的组织模式，它既包括集群飞行的队形产生、保持和变化，也包括集群飞行任务的规划和组织，是未来无人机飞行技术发展的一个重要趋势。

飞行器集群飞行中存在着不同用途、不同类型的飞行器协同飞行情况，但是对于不同类型和性能的飞行器组成编队飞行阵列时，由于不同飞行器的导航性能差距较大，使得各飞行器无法同时准确到达既定阵列位置，因而会影响飞行器的整体编队效果。

发明内容

本发明所要解决的技术问题是：提供一种基于分层式结构的集群飞行器协同导航方法，根据定位精度将飞行器分成高精度层和低精度层，利用高精度层飞行器对低精度层飞行器进行机载定位信息修正，使参与集群飞行的飞行器定位精度得到提高。

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

一种基于分层式结构的集群飞行器协同导航方法，包括如下步骤：

步骤1，确定所有参与集群飞行的飞行器数量N，N≥3，获取所有飞行器的经度、纬度和高度信息，以及所有飞行器自身机载导航设备定位精度，设定定位精度区分界限，将参与集群飞行的飞行器按照其定位精度分为高精度层飞行器和低精度层飞行器；

步骤2，根据步骤1的分类，确定低精度层飞行器和高精度层飞行器各自的数量N₁和N₂，获取低精度层飞行器i和每一个高精度层飞行器的位置信息以及相互距离，获取低精度层飞行器i相对于每一个高精度层飞行器的相对速度和相对角度，i＝1,2,3,…,N₁；

步骤3，设定低精度层飞行器i的状态量，根据状态量建立低精度层飞行器i的状态方程，并进行离散化；

步骤4，根据步骤2获取的相对速度和相对角度，建立低精度层飞行器i和所有高精度层飞行器的相对速度量测方程；

步骤5，根据步骤2获取的位置信息和相互距离，建立低精度层飞行器i和所有高精度层飞行器的相对距离量测方程；

步骤6，重复步骤4和步骤5，依次建立每个低精度层飞行器和所有高精度层飞行器的相对速度量测方程、相对距离量测方程，将状态方程和量测方程经过卡尔曼滤波后，输出低精度层飞行器i机载导航设备经度、纬度和高度信息修正值进行修正；

步骤7，判断所有低精度层飞行器机载导航设备位置信息是否得到修正，是则导航结束，否则重复步骤3-步骤6。

作为本发明的一种优选方案，所述步骤3的具体过程为：

步骤31，设定低精度层飞行器i的状态量，状态量表达式为：

其中，

分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角；

分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量；δL、δλ、δh分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的纬度误差量、经度误差量、高度误差量；ε_bx、ε_by、ε_bz分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误差；ε_rx、ε_ry、ε_rz分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量；Δ_x、Δ_y、Δ_z分别为低精度层飞行器i的机载惯性导航系统的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏，上标^T表示矩阵转置；

步骤32，根据步骤31设定的状态量，建立低精度层飞行器i的状态方程为：

其中，

为状态量X的导数，

F_N为系统阵，

为姿态角转移矩阵，T_gx、T_gy、T_gz分别为低精度层飞行器i机载陀螺仪X、Y、Z轴方向相关时间，T_ax、T_ay、T_az分别为低精度层飞行器i机载加速度计X、Y、Z轴方向相关时间，G为误差系数矩阵，

I为单位矩阵，W为白噪声随机误差矢量；

步骤33，将步骤32的状态方程进行离散化，离散化后的方程为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中，X_k、X_k-1分别为系统在t_k、t_k-1时刻的状态量，Φ_k/k-1为t_k-1到t_k时刻的系统状态转移矩阵，Γ_k-1为t_k-1到t_k时刻的系统噪声误差驱动矩阵，W_k-1为t_k-1时刻的系统噪声矢量。

作为本发明的一种优选方案，所述步骤4的具体过程为：

步骤41，根据低精度层飞行器i相对于每一个高精度层飞行器的相对速度v_ij，建立低精度层飞行器i和所有高精度层飞行器的相对速度方程，为：

其中，j为高精度层飞行器编号，

为低精度层飞行器i和高精度层飞行器j相对速度标准值，δv_ij为相对速度的误差值；

步骤42，根据低精度层飞行器i相对于每一个高精度层飞行器的相对角度，建立低精度层飞行器i和所有高精度层飞行器的相对角度方程，为：

其中，a_ij、b_ij分别为低精度层飞行器i和高精度层飞行器j相对俯仰角测量值、相对偏航角测量值，

分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值，

分别为相对俯仰角、相对偏航角的测量误差值；

步骤43，将相对速度v_ij沿机体坐标系进行分解，得到：

其中，v_ijx、v_ijy、v_ijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解；

步骤44，将步骤41和步骤42建立的相对速度方程和相对角度方程代入步骤43中，并化简得到方程：

其中，

为相对速度标准值在机体坐标系上的投影，且

步骤45，将步骤44得到的方程转换为地理坐标下的方程，从而得到依据传感器测量数据建立的相对速度方程，为：

其中，

，

为姿态角转移矩阵，ψ_i、θ_i、γ_i为低精度层飞行器i的航向角、俯仰角、横滚角；

步骤46，根据低精度层飞行器i和高精度层飞行器j自身机载导航设备，获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度

北向速度

天向速度

步骤47，计算低精度层飞行器i和高精度层飞行器j东向、北向、天向相对速度

计算表达式如下：

步骤48，根据步骤47计算的低精度层飞行器i和高精度层飞行器j的相对速度，建立依据飞行器自身机载导航设备数据的相对速度方程，为：

其中，

分别为低精度层飞行器i和高精度层飞行器j相对速度标准值，

为低精度层飞行器i在地理坐标系东向、北向、天向上的位置误差；

步骤49，根据步骤45依据传感器测量数据建立的相对速度方程以及步骤48依据飞行器自身机载导航设备数据建立的相对速度方程，建立低精度层飞行器i的相对速度量测方程，为：

其中，H_ijV(t)＝[0_3×3 diag[-1 -1 -1] 0_3×12]，

X(t)为状态量，上标^T表示矩阵转置。

作为本发明的一种优选方案，所述步骤5的具体过程为：

步骤51，根据低精度层飞行器i和高精度层飞行器j之间的相互距离d_ij，建立相对距离方程：

其中，

为低精度层飞行器i和高精度层飞行器j相对距离标准值，δd_ij为距离误差值；

步骤52，将相互距离d_ij沿机体坐标系进行分解，得到：

其中，d_ijx、d_ijy、d_ijz分别为相互距离d_ij在机体坐标系X轴、Y轴、Z轴方向上的相对距离；

步骤53，将低精度层飞行器i和所有高精度层飞行器的相对角度方程代入步骤52得到的方程中，并进行化简，得到：

其中，

为相对距离标准值在机体坐标系上的投影，且

分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值，

分别为相对俯仰角、相对偏航角的测量误差值；

步骤54，将步骤53得到的方程转换为地理坐标下的方程，从而得到依据传感器测量数据建立的相对距离方程，为：

其中，

为姿态角转移矩阵；

步骤55，根据高精度层飞行器的经度λ_j、纬度L_j和高度h_j位置信息和低精度层飞行器的经度λ_i、纬度L_i和高度h_i位置信息，建立依据飞行器自身机载导航设备数据的相对距离方程，为：

其中，R为地球半径，δλ_i、δL_i、δh_i分别为低精度层飞行器i的机载惯性导航经度、纬度、高度误差，

分别为相对距离标准值在东、北、天方向上的分量，

分别为依据机载导航设备信息计算的相对距离在东、北、天方向上的分量；

步骤56，根据步骤54依据传感器测量数据建立的相对距离方程以及步骤55依据飞行器自身机载导航设备数据建立的相对距离方程，建立低精度层飞行器i的相对距离量测方程，为：

其中，

X(t)为状态量。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明考虑到参与集群飞行的飞行器定位精度之间存在较大差异，根据定位精度将飞行器分成高精度层和低精度层，利用高精度层飞行器对低精度层飞行器进行机载定位信息修正，使参与集群飞行的飞行器定位精度得到提高。

2、本发明在高精度层飞行器数量越多的情况下，对低精度层飞行器的机载定位信息修正作用越好，适合实际使用。

附图说明

图1是本发明方法的原理流程示意图。

图2是仿真设计一架低精度层飞行器、两架高精度层集群飞行器的飞行航迹。

图3是纯惯导的低精度层飞行器定位误差变化曲线。

图4是纯惯导的低精度层飞行器速度误差变化曲线。

图5是采用本发明协同导航方法后的定位误差变化曲线。

图6是采用本发明协同导航方法后的速度误差变化曲线。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明是一种基于分层式结构的集群飞行器协同导航方法，应用于不同定位精度的飞行器集群飞行器的情况，根据飞行器的定位精度，将其划分成了高精度层和低精度层，并测量了低精度层飞行器和高精度层飞行器间的相对距离，相对速度，以及高精度层飞行器和低精度层飞行器间的方位角信息，建立了低精度层飞行器的状态方程和量测方程，最后根据状态方程和量测方程通过卡尔曼滤波程序依次修正了所有低精度层飞行器的机载导航设备的定位精度。本发明能够利用飞行器集群飞行中其他高精度层飞行器的信息来修正低精度层飞行器的机载导航设备定位精度，适合实际使用。

如图1所示，为一种基于分层式结构的集群飞行器协同导航方法的原理流程示意图，包括以下步骤：

步骤(1)，获取所有参与集群飞行器的飞行器数量，获取所有飞行器的经度、纬度和高度位置信息，根据飞行器所携带的机载导航信息和定位精度，将参与集群的飞行器分为两层，高精度层飞行器和低精度层飞行器；

步骤(2)，根据步骤(1)的分类结果，获取高精度层和低精度层飞行器的数量，获取低精度层飞行器和每一个高精度层飞行器的相对距离信息和相对速度信息以及相对俯仰角和相对偏航角信息；

步骤(3)，选取低精度层飞行器的导航状态量，建立低精度层飞行器的导航状态方程；

步骤(4)，根据步骤(2)获取的高精度层飞行器相对速度信息和角度信息，建立低精度层飞行器的相对速度量测方程；

步骤(5)，根据步骤(2)获取的高精度层飞行器相对距离和角度信息，建立低精度层飞行器相对距离量测方程；

步骤(6)，重复上述步骤(4)、步骤(5)，依次建立低精度飞行器和所有高精度层飞行器的相对速度量测方程和相对距离量测方程并利用卡尔曼滤波方程修正低精度层飞行器；

步骤(7)，判断所有低精度层飞行器机载惯性导航位置信息是否得到修正，是则导航结束，否则重复上述步骤(3)至步骤(6)。

步骤(1)包括如下具体步骤：

步骤(1-1)，确定所有参与集群飞行器的飞行器数量N，若N＜3，则算法无法使用，应继续等待，直至N≥3；

步骤(1-2)，获取所有参与集群飞行的飞行器的经度λ、纬度L和高度h位置信息和所有集群飞行器机载设备定位精度E；

步骤(1-3)，设定定位精度区分界限τ，根据步骤(1-2)获取的所有飞行器的定位精度E，将参与集群飞行的飞行器划分为高精度层飞行器(E≥τ)和低精度层飞行器(E＜τ)。

步骤(2)包括如下具体步骤：

步骤(2-1)，根据步骤(1)对集群飞行器进行分类的结果，确定低精度层飞行器数量N₁，高精度层飞行器数量N₂；

步骤(2-2)，对所有高精度层飞行器和低精度层飞行器分别进行编号，i(i＝1,2,3,…,N₁)代表低精度层飞行器编号，j(j＝1,2,3,…,N₂)代表高精度层飞行器的编号；

步骤(2-3)，利用机载传感器获取低精度层飞行器i和高精度层飞行器j之间的相互距离信息d_ij、相对速度信息v_ij；

步骤(2-4)，利用机载传感器获取低精度层飞行器i和高精度层飞行器j之间的相对俯仰角和相对偏航角，用a_ij、b_ij代表俯仰角和偏航角，其中i,j分别代表低精度层飞行器和高精度层飞行器的编号。

步骤(3)包括如下具体步骤：

步骤(3-1)，首先选取低精度层飞行器编号i＝1的飞行器进行状态方程建立；

步骤(3-2)，获取低精度层飞行器i的航向角ψ_i，俯仰角θ_i和横滚角γ_i；

步骤(3-3)，对编号为i的低精度层飞行器建立状态矢量X，其中状态矢量X表达式如下：

其中，

分别为编号为i的低精度层飞行器机载惯性导航系统中误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角；

分别为低精度层飞行器i的机载惯性导航系统中的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量；δL、δλ、δh分别表示低精度层飞行器i的机载惯性导航系统中误差状态量中的纬度误差量、经度误差量和高度误差量；ε_bx、ε_by、ε_bz分别为低精度层飞行器i的机载惯性导航系统中的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误；ε_rx、ε_ry、ε_rz分别为低精度层飞行器i的机载惯性导航系统中的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量；Δ_x、Δ_y、Δ_z分别为低精度层飞行器i的机载惯性导航系统中的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏，^T为矩阵转置；

步骤(3-4)，根据步骤(3-3)设定的状态量，建立低精度层飞行器i的状态方程，方程如下所示：

其中，

为状态矢量X的导数，

F_N为由地球自转角速度，低精度层飞行器i经度、纬度和高度，地球子午圈曲率半径，地球卯酉圈曲率半径以及低精度层飞行器i地理坐标系在东向、北向、天向速度，9个基本导航参数组成的系统阵，

其中，T_gx、T_gy、T_gz为低精度层飞行器i机载陀螺仪X轴、Y轴、Z轴方向相关时间，T_ax、T_ay、T_az为低精度层飞行器i机载加速度计X轴、Y轴、Z轴方向相关时间，W为白噪声随机误差矢量，误差系数矩阵

步骤(3-5)，将上述步骤(3-4)的低精度层飞行器i的状态方程进行离散化，离散化的方程如下所示：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中，Φ_k/k-1为t_k-1时刻到t_k时刻的系统状态转移矩阵，

Γ_k-1为t_k-1时刻到t_k时刻的系统噪声误差驱动矩阵，

上述Φ_k/k-1，Γ_k-1计算公式中，n为离散点数，T为迭代周期，W_k-1为t_k-1时刻的系统噪声矢量，X_k-1、X_k为系统在t_k-1时刻和t_k时刻的系统状态矢量。

步骤(4)包括如下具体步骤：

步骤(4-1)，取高精度层飞行器编号j＝1的飞行器；

步骤(4-2)，根据步骤(2-3)获取的低精度层飞行器i和所有高精度层飞行器j相对速度v_ij，建立低精度层飞行器i和所有高精度层飞行器的相对速度方程，方程如下所示：

其中，j为高精度层飞行器编号，

为低精度层飞行器i和高精度层飞行器j相对速度标准值，δv_ij为相对速度的误差值；

步骤(4-3)，根据步骤(2-4)获取的低精度层飞行器和高精度层飞行器间的相对俯仰角a_ij和相对偏航角b_ij，建立俯仰角和偏航角测量值和标准值间的方程，如下所示：

其中，

为低精度层飞行器i和高精度层飞行器j相对俯仰角真值和相对偏航角真值，

分别为相对俯仰角和相对偏航角的测量误差值；

步骤(4-4)，根据步骤(4-2)传感器获取的相对速度v_ij以及步骤(4-3)获取的相对俯仰角a_ij和偏航角b_ij，将相对速度v_ij沿着机体坐标系进行分解，得到方程如下所示：

其中，v_ijx、v_ijy、v_ijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解；

步骤(4-5)，根据步骤(4-2)和步骤(4-3)建立的相对速度方程和相对角度方程，将其代入到步骤(4-4)的方程中，得到如下方程：

因为

都是角度误差，且数值较小，所以

且设定小量乘积为0，相对速度误差和角度误差乘积为0。上述公式化简为：

继续化简得到：

其中，

为相对速度真值在机体坐标系上的投影，

步骤(4-6)，根据步骤(4-5)得到的化简后的相对速度方程，因该方程是建立在机体坐标系下，所以需要将该坐标系方程转换为地理坐标系下，步骤(4-5)方程变换如下所示：

其中，各参数计算公式如下所示：

其中，

为姿态角转移矩阵，根据步骤(3-2)，ψ_i、θ_i、γ_i为低精度层飞行器i的航向角、俯仰角和横滚角；

步骤(4-7)，上述步骤(4-6)建立的是传感器测量的相对速度方程，接下来需要建立低精度层飞行器i和高精度层飞行器j自身机载导航设备信息相对速度方程，首先获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度

北向速度

天向速度

步骤(4-8)，计算低精度层飞行器i和高精度层飞行器j相对速度

计算表达式如下：

步骤(4-9)，根据步骤(4-8)计算的低精度层飞行器i和高精度层飞行器j的相对速度，建立低精度层飞行器i和高精度层飞行器j自身机载导航设备信息相对速度方程如下所示：

其中，

为低精度层飞行器i和高精度层飞行器j相对速度真值，

为低精度层飞行器i在地理坐标系东向、北向和天向上的位置误差；

步骤(4-10)，根据步骤(4-6)依据传感器测量数据建立的相对速度方程以及步骤(4-9)飞行器自身机载导航数据建立的相对速度方程建立低精度层飞行器i的量测方程，方程如下所示：

其中，H_ijV(t)＝[0_3×3 diag[-1 -1 -1] 0_3×12]，

为矩阵转置。

步骤(5)包括如下具体步骤：

步骤(5-1)，根据步骤(2-3)获取低精度层飞行器i和高精度层飞行器j之间的相互距离信息d_ij，建立相对距离方程：

其中，

为低精度层飞行器i和高精度层飞行器j相对距离真值，δd_ij为距离误差值；

步骤(5-2)，根据步骤(2-4)获取的低精度层飞行器和高精度层飞行器间的相对俯仰角a_ij和相对偏航角b_ij将相对距离沿机体坐标系进行分解，得到相对距离在机体坐标系X轴、Y轴、Z轴方向上的相对距离d_ijx、d_ijy、d_ijz，方程如下所示：

步骤(5-3)，根据步骤(4-3)建立的俯仰角和偏航角测量值和标准值间的方程，以及步骤(5-1)的相对距离方程，对步骤(5-2)方程进行变量替换：

对该方程进行化简，因为

都是角度误差，且数值较小，所以

且设定小量乘积为0，相对速度误差和角度误差乘积为0。上述公式化简为：

继续化简得到：

其中，

为相对速度真值在机体坐标系上的投影，

步骤(5-4)，根据步骤(4-6)将机体系转化为地理系，对步骤(5-3)的方程化简为地理系，如下所示：

其中

的计算方式和步骤(4-6)相同；

步骤(5-5)，根据步骤(1-2)获取的高精度层飞行器的经度λ_j、纬度L_j和高度h_j位置信息和低精度层飞行器的经度λ_i、纬度L_i和高度h_i位置信息，计算低精度层飞行器i和高精度层飞行器j相对距离，计算表达式如下：

其中，R为地球半径，δλ_i、δL_i、δh_i分别为低精度层飞行器i的机载惯性导航经度，纬度和高度误差，

为相对距离真值在东北天方向上的分量，

分别为依据机载导航信息计算的相对距离在东北天方向上的分量；

步骤(5-6)，根据步骤(5-4)依据传感器测量数据建立的相对距离方程以及步骤(5-5)飞行器自身机载导航数据建立的相对距离方程建立的低精度层飞行器i的量测方程，方程如下所示：

其中，

步骤(6)包括如下具体步骤：

步骤(6-1)，上述步骤已经建立了低精度层飞行器i和高精度层飞行器j之间的相对速度量测方程和相对距离量测方程；

步骤(6-2)，判断j和N₂的大小，如果j＜N₂,则j＝j+1，则跳转执行步骤(4-2)，如果j≥N₂，则执行步骤(6-3)；

步骤(6-3)，根据步骤(6-2)，已经通过循环建立了低精度层飞行器i和所有高精度层飞行器(j＝1,2,3,…,N₂)之间的相对速度量测方程

和相对位置量测方程

如下所示：

其中，H_ijV(t)＝[0_3×3 diag[-1 -1 -1] 0_3×12]，

为矩阵转置，

步骤(6-4)，联立步骤(6-3)的所有相对速度方程和相对位置方程，方程如下所示：

步骤(6-5)，根据步骤(3-5)和步骤(6-4)建立的状态方程和量测方程，经过卡尔曼滤波程序后，输出低精度层飞行器i机载惯性导航系统经度、纬度和高度位置信息修正值(λ′,L′,h′)，λ′为经度修正值，L′为纬度修正值，h′为高度修正值。

步骤(7)包括如下具体步骤：

判断i和N₁的大小，如果i＜N₁，则i＝i+1，且跳转执行步骤(3-2)，如果i≥N₁，则所有低精度层飞行器机载定位信息得到修正，则导航结束。

下面以一实施例来对本发明进行说明。仿真设计1架低精度层飞行器，两架高精度层飞行器集群飞行，低精度层飞行器仅装有惯导设备，高精度层飞行器装有GNSS导航设备，通过对比低精度层飞行器在使用本发明的协同导航算法后位置和速度误差，如图2所示，为仿真设计一架低精度层飞行器，两架高精度层飞行器集群飞行航迹。图3、图4为在没有使用协同导航算法前的纯惯导的低精度层飞行器位置、速度误差变化曲线，该曲线误差较大，处于发散状态。图5、图6为采用本发明协同导航方法后的定位、速度误差变化曲线。通过对比位置误差和速度误差曲线，可以看出本发明设计的协同导航算法能有效提高低精度层飞行器的定位精度和速度精度。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (4)

1.一种基于分层式结构的集群飞行器协同导航方法，其特征在于，包括如下步骤：

步骤1，确定所有参与集群飞行的飞行器数量N，N≥3，获取所有飞行器的经度、纬度和高度信息，以及所有飞行器自身机载导航设备定位精度，设定定位精度区分界限，将参与集群飞行的飞行器按照其定位精度分为高精度层飞行器和低精度层飞行器；

步骤2，根据步骤1的分类，确定低精度层飞行器和高精度层飞行器各自的数量N₁和N₂，获取低精度层飞行器i和每一个高精度层飞行器的位置信息以及相互距离，获取低精度层飞行器i相对于每一个高精度层飞行器的相对速度和相对角度，i＝1,2,3,…,N₁；

步骤3，设定低精度层飞行器i的状态量，根据状态量建立低精度层飞行器i的状态方程，并进行离散化；

步骤4，根据步骤2获取的相对速度和相对角度，建立低精度层飞行器i和所有高精度层飞行器的相对速度量测方程；

步骤5，根据步骤2获取的位置信息和相互距离，建立低精度层飞行器i和所有高精度层飞行器的相对距离量测方程；

步骤6，重复步骤4和步骤5，依次建立每个低精度层飞行器和所有高精度层飞行器的相对速度量测方程、相对距离量测方程，将状态方程和量测方程经过卡尔曼滤波后，输出低精度层飞行器i机载导航设备经度、纬度和高度信息修正值进行修正；

步骤7，判断所有低精度层飞行器机载导航设备位置信息是否得到修正，是则导航结束，否则重复步骤3-步骤6。

2.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法，其特征在于，所述步骤3的具体过程为：

步骤31，设定低精度层飞行器i的状态量，状态量表达式为：

其中，

分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的东向平台误差角、北向平台误差角、天向平台误差角；

分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的东向速度误差量、北向速度误差量、天向速度误差量；δL、δλ、δh分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的纬度误差量、经度误差量、高度误差量；ε_bx、ε_by、ε_bz分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的X轴、Y轴、Z轴方向上的陀螺仪常值漂移误差；ε_rx、ε_ry、ε_rz分别为低精度层飞行器i的机载惯性导航系统的误差状态量中的X轴、Y轴、Z轴方向上的陀螺一阶马尔可夫漂移误差状态量；Δ_x、Δ_y、Δ_z分别为低精度层飞行器i的机载惯性导航系统的误差状态量中X轴、Y轴、Z轴方向上的加速度计零偏，上标T表示矩阵转置；

步骤32，根据步骤31设定的状态量，建立低精度层飞行器i的状态方程为：

其中，

为状态量X的导数，

F_N为系统阵，

为姿态角转移矩阵，T_gx、T_gy、T_gz分别为低精度层飞行器i机载陀螺仪X、Y、Z轴方向相关时间，T_ax、T_ay、T_az分别为低精度层飞行器i机载加速度计X、Y、Z轴方向相关时间，G为误差系数矩阵，

I为单位矩阵，W为白噪声随机误差矢量；

步骤33，将步骤32的状态方程进行离散化，离散化后的方程为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中，X_k、X_k-1分别为系统在t_k、t_k-1时刻的状态量，Φ_k/k-1为t_k-1到t_k时刻的系统状态转移矩阵，Γ_k-1为t_k-1到t_k时刻的系统噪声误差驱动矩阵，W_k-1为t_k-1时刻的系统噪声矢量。

3.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法，其特征在于，所述步骤4的具体过程为：

步骤41，根据低精度层飞行器i相对于每一个高精度层飞行器的相对速度v_ij，建立低精度层飞行器i和所有高精度层飞行器的相对速度方程，为：

其中，j为高精度层飞行器编号，

为低精度层飞行器i和高精度层飞行器j相对速度标准值，δv_ij为相对速度的误差值；

步骤42，根据低精度层飞行器i相对于每一个高精度层飞行器的相对角度，建立低精度层飞行器i和所有高精度层飞行器的相对角度方程，为：

其中，a_ij、b_ij分别为低精度层飞行器i和高精度层飞行器j相对俯仰角测量值、相对偏航角测量值，

分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值，

分别为相对俯仰角、相对偏航角的测量误差值；

步骤43，将相对速度v_ij沿机体坐标系进行分解，得到：

其中，v_ijx、v_ijy、v_ijz为低精度层飞行器i和高精度层飞行器j之间相对速度在机体坐标系X轴、Y轴、Z轴方向上的分解；

步骤44，将步骤41和步骤42建立的相对速度方程和相对角度方程代入步骤43中，并化简得到方程：

其中，

为相对速度标准值在机体坐标系上的投影，且

步骤45，将步骤44得到的方程转换为地理坐标下的方程，从而得到依据传感器测量数据建立的相对速度方程，为：

其中，

为姿态角转移矩阵，ψ_i、θ_i、γ_i为低精度层飞行器i的航向角、俯仰角、横滚角；

步骤46，根据低精度层飞行器i和高精度层飞行器j自身机载导航设备，获取低精度层飞行器i和高精度层飞行器j在地理坐标系下东向速度

北向速度

天向速度

步骤47，计算低精度层飞行器i和高精度层飞行器j东向、北向、天向相对速度

计算表达式如下：

步骤48，根据步骤47计算的低精度层飞行器i和高精度层飞行器j的相对速度，建立依据飞行器自身机载导航设备数据的相对速度方程，为：

其中，

分别为低精度层飞行器i和高精度层飞行器j相对速度标准值，

为低精度层飞行器i在地理坐标系东向、北向、天向上的位置误差；

步骤49，根据步骤45依据传感器测量数据建立的相对速度方程以及步骤48依据飞行器自身机载导航设备数据建立的相对速度方程，建立低精度层飞行器i的相对速度量测方程，为：

其中，H_ijV(t)＝[0_3×3 diag[-1 -1 -1] 0_3×12]，

X(t)为状态量，上标T表示矩阵转置。

4.根据权利要求1所述基于分层式结构的集群飞行器协同导航方法，其特征在于，所述步骤5的具体过程为：

步骤51，根据低精度层飞行器i和高精度层飞行器j之间的相互距离d_ij，建立相对距离方程：

其中，

为低精度层飞行器i和高精度层飞行器j相对距离标准值，δd_ij为距离误差值；

步骤52，将相互距离d_ij沿机体坐标系进行分解，得到：

其中，d_ijx、d_ijy、d_ijz分别为相互距离d_ij在机体坐标系X轴、Y轴、Z轴方向上的相对距离，a_ij、b_ij分别为低精度层飞行器i和高精度层飞行器j相对俯仰角测量值、相对偏航角测量值；

步骤53，将低精度层飞行器i和所有高精度层飞行器的相对角度方程代入步骤52得到的方程中，并进行化简，得到：

其中，

为相对距离标准值在机体坐标系上的投影，且

分别为低精度层飞行器i和高精度层飞行器j相对俯仰角标准值、相对偏航角标准值，

分别为相对俯仰角、相对偏航角的测量误差值；

步骤54，将步骤53得到的方程转换为地理坐标下的方程，从而得到依据传感器测量数据建立的相对距离方程，为：

其中，

为姿态角转移矩阵；

步骤55，根据高精度层飞行器的经度λ_j、纬度L_j和高度h_j位置信息和低精度层飞行器的经度λ_i、纬度L_i和高度h_i位置信息，建立依据飞行器自身机载导航设备数据的相对距离方程，为：

其中，R为地球半径，δλ_i、δL_i、δh_i分别为低精度层飞行器i的机载惯性导航经度、纬度、高度误差，

分别为相对距离标准值在东、北、天方向上的分量，

分别为依据机载导航设备信息计算的相对距离在东、北、天方向上的分量；

步骤56，根据步骤54依据传感器测量数据建立的相对距离方程以及步骤55依据飞行器自身机载导航设备数据建立的相对距离方程，建立低精度层飞行器i的相对距离量测方程，为：

其中，

X(t)为状态量。

Images