CN109813311B - 一种无人机编队协同导航方法 - Google Patents

Abstract

本发明公开了一种无人机编队协同导航方法。在无人机编队中，长机无人机节点搭载惯性导航***、GPS接收机，通过机载惯导/GPS融合滤波对姿态、速度、位置进行估计，得到长机无人机节点高精度的姿态、速度和位置导航信息，其余无人机节点搭载惯性导航***、视觉导航设备、光流传感器，通过视觉导航设备获得与长机的相对距离、俯仰角、方位角数学解算出相邻与长机无人机节点的绝对位置信息，光流传感器解算得到无人机的速度信息，通过卡尔曼滤波对机载的惯导误差进行修正。此外，编队中距离长机较远的无人机节点通过视觉导航设备解算得到与其它无人机节点的相对距离，并据此解算出绝对位置信息。

Description

一种无人机编队协同导航方法

技术领域

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

背景技术

无人机***是一种无人驾驶，是能够自主控制或者遥控的空中飞行机器人***，可执行对地、对海、对空的作战任务，具有可回收，可重复使用，高效等特点。随着任务需求的多样性和复杂性，单架无人机执行任务的能力有限，生存能力受到极大的挑战。而多架无人机之间的协同可以实现信息的互享和行动协调，实现单架无人机的任务扩展，提高任务执行的成功率。导航***为无人机提供其飞行控制***所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

现阶段惯导/GPS组合导航***广泛应用为无人机提供导航服务。无人机编队中受限于成本、载荷等因素，不可能让编队中所有的无人机都搭载高精度的导航传感器，因此主从式的导航方式成为现阶段研究无人机编队协同导航的主流，长机搭载高精度的导航传感器通过滤波融合得到长机高精度的导航信息，在编队导航数据链及其他测距传感器的辅助下，通过编队协同方式完成对编队中其他无人机节点的高精度定位。然而现有的多无人机协同导航方法的精度并不理想。

发明内容

为了解决上述背景技术提出的技术问题，本发明提出了一种无人机编队协同导航方法。

为了实现上述技术目的，本发明的技术方案为：

一种无人机编队协同导航方法，无人机编队包含4架无人机节点：无人机节点1～4，包括以下步骤：

(1)建立机体坐标系和导航坐标系；

(2)在无人机节点1上搭载惯性导航***和GPS接收机，周期读取k时刻无人机节点1的机载传感器信息，包括速度信息、位置信息，陀螺仪信息和加速度计信息；

(3)预测k时刻无人机节点i的姿态四元数、速度和位置信息，其中i＝1,2,3,4；

(4)利用卡尔曼滤波器，对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正，得到无人机节点1的高精度导航信息；

(5)在无人机节点i上搭载惯性导航***、视觉导航设备和光流传感器，周期性读取k时刻无人机节点i的机载传感器信息，包括相对于无人机节点1的距离信息、俯仰角和方位角，以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息，其中i＝2,3,4；

(6)根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息；

(7)利用卡尔曼滤波器，根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息；

(8)根据无人机节点1～3的位置信息推导出无人机节点4的绝对位置信息；

(9)利用卡尔曼滤波器，将无人机节点4的位置信息和速度信息作为量测信息，对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点4的高精度导航信息。

进一步地，在步骤(1)中，机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向，导航坐标系的X、Y、Z轴分别为东向、北向、天向。

进一步地，在步骤(3)中，采用下式预测姿态四元数：

其中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；

通过下式计算：

其中

通过下式计算：

其中，

为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，

为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；i＝1,2,3,4；

采用下式预测速度信息：

其中，

为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；

为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值；

为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；

为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；i＝1,2,3,4；

为机体系到导航系之间的姿态矩阵，通过下式计算：

采用下式预测位置信息：

其中，

分别为k时刻无人机节点i在导航系X、Y、Z轴上的位置坐标；k-1时刻的位置

分别为k-1时刻无人机节点i在导航系X、Y、Z轴上的位置坐标。

进一步地，步骤(4)的具体过程如下：

(401)确定无人机节点1导航***的状态向量，无人机节点1～4的状态向量相同，如下：

其中，

为无人机节点i的平台误差角，δV_Ei,δV_Ni,δV_Ui为无人机节点i的陀螺东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人机节点i陀螺纬度、经度、高度误差，L_i,λ_i,h_i无人机节点i陀螺纬度、经度、高度值，ε_bxi,ε_byi,ε_bzi为无人机节点i陀螺随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人机节点i陀螺一阶马尔科夫随机噪声，

为无人机节点i加速度计一阶马尔科夫过程；

当i取1时即为无人机节点1导航***的状态向量；

(402)建立惯性导航***的误差方程，包括平台误差角方程、速度误差方程和位置误差方程；

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人机节点i惯性导航***东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人机节点i东、北、天向陀螺噪声角，R_M＝R_e(1-2f+3fsin²L_i)，R_N＝R_e(1+fsin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人机节点i惯性导航***天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人机节点i加速度计测得的加速度值，

为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

当i取1时即为无人机节点1的惯性导航***的误差方程；

(403)建立无人机节点1导航***的状态方程，无人机节点1～4的状态方程相同，如下：

其中，G_i为***噪声系数矩阵，W_i为噪声控制向量，

F_Ni为平台误差角、速度误差、位置误差构成的***矩阵，

T_gxi、T_gyi、T_gzi为三轴陀螺相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数；

当i取1时即为无人机节点1导航***的状态方程；

(404)建立无人机节点1导航***的量测方程：

无人机节点1导航***的速度量测矩阵H_v1和量测噪声V_v1：

H_v1＝[0_3×3 diag[1 1 1]0_3×12]

V_v1＝[M_E1 M_N1 M_U1]

上式中，M_E1,M_N1,M_U1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差；

无人机节点1导航***的位置量测矩阵H_p1和量测噪声V_p1：

H_p1＝[0_3×6 diag[R_M R_Ncos L₁ 1]0_3×9]

V_p1＝[N_E1 N_N1 N_U1]

上式中，N_E1,N_N1,N_U1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差；

(405)采用卡尔曼滤波器对无人机节点1的导航***进行更新。

进一步地，采用下式解算出无人机节点2和无人机节点3的绝对位置信息(x_i,y_i,z_i)：

其中，x、y、z为无人机节点1组合导航***得到的高精度经度、纬度、高度信息，d_i为无人机节点i相对于无人机节点1的距离，α_i、β_i为无人机节点i相对于无人机节点1的俯仰角、方位角，ν_iE,ν_iN,ν_iU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；i＝2,3。

进一步地，在步骤(7)中，首先建立无人机节点2和无人机节点3的导航***量测方程：

无人机节点2和无人机节点3的导航***速度量测矩阵H_vi和量测噪声V_vi：

H_vi＝[0_3×3 diag[1 1 0]0_3×12]

V_vi＝[M_DEi M_DNi 0]

上式中，M_DEi,M_DNi为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声；

无人机节点2和无人机节点3的导航***位置量测矩阵H_pi和量测噪声V_pi：

V_pi＝[ν_iE ν_iN ν_iU]

上式中，i＝2,3；

然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航***进行更新。

进一步地，在步骤(9)中，首先建立无人机节点4的导航***量测方程：

无人机节点4的导航***速度量测矩阵H_v4和量测噪声V_v4：

H_v4＝[0_3×3 diag[1 1 0]0_3×12]

V_v4＝[M_E4 M_N4 0]

上式中，M_E4,M_N4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声；

无人机节点4的导航***位置量测矩阵H_p4和量测噪声V_p4：

V_p4＝[ν_4E ν_4N ν_4U]

上式中，ν_4E,ν_4N,ν_4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；

然后采用卡尔曼滤波器无人机节点4的导航***进行更新。

进一步地，在步骤(8)中，设无人机节点1的位置为(x₁,y₁,z₁)，无人机节点2的位置为(x₂,y₂,z₂)，无人机节点3的位置为(x₃,y₃,z₃)，通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d₄、与无人机节点2的距离d₂、与无人机节点3的距离d₃，则：

通过最小二乘法解算上式求出无人机节点4的位置(x₄,y₄,z₄)。

采用上述技术方案带来的有益效果：

无人机编队协同飞行时，受限于成本、体积、重量等方面的限制，每个无人机节点不可能都搭载高精度的导航传感器以实现无人机编队高精度的导航定位，本发明采用主从式的无人机编队协同导航方式，通过编队中长机搭载高精度的导航传感器实现长机的高精度的导航定位，各从机节点通过视觉导航设备获得从机无人机节点相对长机的距离，方位信息数学解算出各从无人机节点的绝对位置信息，同时各从无人机节点通过视觉导航设备实时获得无人机节点相对地面的速度，从而实现各从无人机节点高精度导航定位，提高了导航***的稳定性、鲁棒性以及抗干扰性能。

附图说明

图1是本发明的总体框架图；

图2是本发明中无人机节点1的导航解算流程图；

图3是本发明中无人机节点2和3的导航解算流程图；

图4是本发明中无人机节点4的导航解算流程图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示，本发明设计的一种无人机编队协同导航方法，无人机编队包含4架无人机节点：无人机节点1～4，包括以下步骤：

步骤1：建立机体坐标系和导航坐标系；

步骤2：在无人机节点1上搭载惯性导航***和GPS接收机，周期读取k时刻无人机节点1的机载传感器信息，包括速度信息、位置信息，陀螺仪信息和加速度计信息；

步骤3：预测k时刻无人机节点i的姿态四元数、速度和位置信息，其中i＝1,2,3,4；

步骤4：利用卡尔曼滤波器，对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正，得到无人机节点1的高精度导航信息，如图2所示；

步骤5：在无人机节点i上搭载惯性导航***、视觉导航设备和光流传感器，周期性读取k时刻无人机节点i的机载传感器信息，包括相对于无人机节点1的距离信息、俯仰角和方位角，以及通过光流解算出的速度信息、陀螺仪信息和加速度计信息，其中i＝2,3,4；

步骤6：根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息；

步骤7：利用卡尔曼滤波器，根据无人机节点2和无人机节点3的位置信息和速度信息对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息，如图3所示；

步骤8：根据无人机节点1～3的位置信息推导出无人机节点4的绝对位置信息；

步骤9：利用卡尔曼滤波器，将无人机节点4的位置信息和速度信息作为量测信息，对k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点4的高精度导航信息，如图4所示。

在本实施例中，步骤1采用如下优选技术方案：

机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向，导航坐标系的X、Y、Z轴分别为东向、北向、天向。

在本实施例中，步骤3采用如下优选技术方案：

采用下式预测姿态四元数：

其中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；

通过下式计算：

其中

通过下式计算：

其中，

为k时刻陀螺仪读取的无人机节点i机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，

为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；i＝1,2,3,4；

采用下式预测速度信息：

其中，

为k时刻加速度计读取的无人机节点i机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；

为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值；

为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；

为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；i＝1,2,3,4；

为机体系到导航系之间的姿态矩阵，通过下式计算：

采用下式预测位置信息：

其中，

分别为k时刻无人机节点i在导航系X、Y、Z轴上的位置坐标；k-1时刻的位置

分别为k-1时刻无人机节点i在导航系X、Y、Z轴上的位置坐标。

在本实施例中，步骤4采用如下技术方案：

由于4个无人机节点的状态量和状态方程都相同，因此用i＝1,2,3,4统一表示。

1)无人机节点一组合导航***的状态量：

其中，

为无人机节点i的平台误差角，δV_Ei,δV_Ni,δV_Ui为无人机节点i的陀螺东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人机节点i陀螺纬度、经度、高度误差，L_i,λ_i,h_i无人机节点i陀螺纬度、经度、高度值，ε_bxi,ε_byi,ε_bzi为无人机节点i陀螺随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人机节点i陀螺一阶马尔科夫随机噪声，

为无人机节点i加速度计一阶马尔科夫过程。

2)建立惯性导航***的误差方程，包括平台误差角方程、速度误差方程和位置误差方程；

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人机节点i惯性导航***东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人机节点i东、北、天向陀螺噪声角，R_M＝R_e(1-2f+3f sin²L_i)，R_N＝R_e(1+f sin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人机节点i惯性导航***天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人机节点i加速度计测得的加速度值，

为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

3)建立无人机节点1导航***的状态方程，无人机节点1～4的状态方程相同，如下：

其中，G_i为***噪声系数矩阵，W_i为噪声控制向量，

F_Ni为平台误差角、速度误差、位置误差构成的***矩阵，

T_gxi、T_gyi、T_gzi为三轴陀螺相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数。

3)无人机节点1的***量测方程：

k时刻GPS接收机获得无人机载体的速度和位置信息可表示为：

上式中，λ_t1,L_t1,h_t1,N_E1,N_N1,N_U1,v_E1,v_N1,v_U1,M_E1,M_N1,M_U1分别为GPS测得载体沿地理坐标系下各轴下的位置真值、GPS接收机在东、北、天向的位置误差、GPS测得载体在地理坐标系下沿各轴的速度真值以及GPS接收机沿东、北、天向的测速误差。将GPS接收机获得载体的速度信息、位置信息与惯导解算出载体的速度、位置信息作差分并写成***量测方程形式Z＝HX+V，可得：

无人机节点1导航***的速度量测矩阵H_v1和量测噪声V_v1：

H_v1＝[0_3×3 diag[1 1 1]0_3×12]

V_v1＝[M_E1 M_N1 M_U1]

上式中，M_E1,M_N1,M_U1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差；

无人机节点1导航***的位置量测矩阵H_p1和量测噪声V_p1：

H_p1＝[0_3×6 diag[R_M R_Ncos L₁ 1]0_3×9]

V_p1＝[N_E1 N_N1 N_U1]

上式中，N_E1,N_N1,N_U1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差。

4)采用卡尔曼滤波器对无人机节点一导航***进行更新：

X_k+1＝F_k+1,kX_k+G_kW_k

Z_k+1＝H_k+1X_k+1+V_k+1

上式中，X_k+1为***k+1时刻的状态向量，W_k为k时刻的噪声控制向量，F_k+1,k为k到k+1时刻的***状态转移矩阵，G_k为***噪声系数矩阵；Z_k+1为k+1时刻的量测向量，H_k+1为k+1时刻***的量测矩阵，V_k+1为***k+1时刻***的量测噪声。

首先进行***一步状态预测：

其次求解一步预测的均方差：

上式中，

为k时刻状态变量X_k的估计值，

为利用计算得到的

预测k+1时刻状态变量X_k+1的值，P_k为k时刻估计均方差误差，Q_k为***的噪声方差矩阵，P_k+1/k为状态向量

的均方差。

然后求解滤波增益：

上式中，R_k+1为噪声的方差矩阵，通过以上各式完成对无人机节点1的状态估值，从而得到滤波结果：

同时更新估计***的均方差误差：

P_k+1＝[1-K_k+1H_k+1]P_k+1/k

在本实施例中，步骤6采用如下优选技术方案：

采用下式解算出无人机节点2和无人机节点3的绝对位置信息(x_i,y_i,z_i)：

其中，x、y、z为无人机节点1组合导航***得到的高精度经度、纬度、高度信息，d_i为无人机节点i相对于无人机节点1的距离，α_i、β_i为无人机节点i相对于无人机节点1的俯仰角、方位角，ν_iE,ν_iN,ν_iU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；i＝2,3。

在本实施例中，步骤7采用如下优选技术方案：

首先建立无人机节点2和无人机节点3的导航***量测方程：

无人机节点2和无人机节点3的导航***速度量测矩阵H_vi和量测噪声V_vi：

H_vi＝[0_3×3 diag[1 1 0]0_3×12]

V_vi＝[M_DEi,M_DNi,0]

上式中，M_DEi,M_DNi为无人机节点i的光流传感器在东、北向上的均值为零且方差为定值的量测白噪声；

无人机节点2和无人机节点3的导航***位置量测矩阵H_pi和量测噪声V_pi：

V_pi＝[ν_iE ν_iN ν_iU]

上式中，i＝2,3；

然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航***进行更新。

在本实施例中，步骤8采用如下优选技术方案：

设无人机节点1的位置为(x₁,y₁,z₁)，无人机节点2的位置为(x₂,y₂,z₂)，无人机节点3的位置为(x₃,y₃,z₃)，通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d₄、与无人机节点2的距离d₂、与无人机节点3的距离d₃，则：

通过最小二乘法解算上式求出无人机节点4的位置(x₄,y₄,z₄)。

在本实施例中，步骤9采用如下优选技术方案：

k时刻无人机节点4解算得到的位置信息与光流解算得到的速度信息可用如下公式表示：

无人机节点4惯导解算出的位置在地球地固坐标系(ECEF)可表示为：

上式中x_I,y_I,z_I为无人机节点4机载惯导***解算出的载***置在地球地固坐标系下的位置，x_t,y_t,z_t为惯导解算出的位置在地球地固坐标下的位置真值，δx,δy,δz为地球地固坐标系下的三个方向的位置误差。

无人机节点4在地球地固(ECEF)坐标系解算出的位置可表示为：

上式中，x、y、z为无人机节点4在地球地固坐标系下三个方向上的位置，x_t,y_t,z_t为三个方向上的位置真值，N_x,N_y,N_z为三个方向上的白噪声。

可得：

无人机节点4机载光流传感器解算得到的无人机水平方向的速度可表示为：

上式中，v_DE4,v_DN4为无人机节点4搭载的光流传感器获得无人机水平方向上对地速度，v_DET4,v_DNT4为水平方向对地速度真值。

将无人机节点4位置、速度信息与惯导解算出来的位置、速度信息作差分得到无人机节点四的量测量并改写成量测方程形式：

无人机节点4的导航***速度量测矩阵H_v4和量测噪声V_v4：

H_v4＝[0_3×3,diag[1,1,1],0_3×3]

V_v4＝[M_E4 M_N4 0]

上式中，M_E4,M_N4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声；

无人机节点4的导航***位置量测矩阵H_p4和量测噪声V_p4：

V_p4＝[ν_4E ν_4N ν_4U]

上式中，ν_4E,ν_4N,ν_4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差。

然后采用卡尔曼滤波器无人机节点4的导航***进行更新。

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

Claims (8)

1.一种无人机编队协同导航方法，其特征在于，无人机编队包含4架无人机节点：无人机节点1～4，包括以下步骤：

(1)建立机体坐标系和导航坐标系；

(2)在无人机节点1上搭载惯性导航***和GPS接收机，周期性读取k时刻无人机节点1的机载传感器信息，包括速度信息、位置信息、陀螺仪信息和加速度计信息；

(3)预测k时刻无人机节点i的姿态四元数、速度和位置信息，其中i＝1,2,3,4；

(4)利用卡尔曼滤波器，对k时刻的无人机节点1的姿态四元数、速度、位置信息进行校正，得到无人机节点1的高精度导航信息；

(5)在无人机节点i上搭载惯性导航***、视觉导航设备和光流传感器，周期性读取k时刻无人机节点i的机载传感器信息，包括相对于无人机节点1的距离信息、俯仰角和方位角，以及陀螺仪信息、加速度计信息以及通过光流解算出的速度信息，其中i＝2,3,4；

(6)根据相对于无人机节点1的距离信息、俯仰角和方位角解算出无人机节点2和无人机节点3的绝对位置信息；

(7)利用卡尔曼滤波器，根据无人机节点2和无人机节点3的位置信息和速度信息对无人机节点2和无人机节点3的k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点2和无人机节点3的高精度导航信息；

(8)根据无人机节点1～3的位置信息推导出无人机节点4的绝对位置信息；

(9)利用卡尔曼滤波器，将无人机节点4的位置信息和速度信息作为量测信息，对无人机节点4的k时刻的姿态四元数、速度、位置信息进行校正，得到无人机节点4的高精度导航信息。

2.根据权利要求1所述无人机编队协同导航方法，其特征在于，在步骤(1)中，机体坐标系的X、Y、Z轴分别为无人机的右向、机头方向、天向，导航坐标系的X、Y、Z轴分别为东向、北向、天向。

3.根据权利要求2所述无人机编队协同导航方法，其特征在于，在步骤(3)中，采用下式预测姿态四元数：

其中，Q_i(k)＝[q_i0(k) q_i1(k) q_i2(k) q_i3(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置；Q_i(k-1)＝[q_i0(k-1) q_i1(k-1) q_i2(k-1) q_i3(k-1)]^T为k-1时刻的姿态四元数；ΔT为离散采样周期；

通过下式计算：

其中

通过下式计算：

其中，

为k时刻陀螺仪读取的无人机节点i机体坐标系相对于导航坐标系的角速度在机体坐标系X、Y、Z轴上的分量，

为k时刻陀螺仪的零偏在机体坐标系X、Y、Z轴上的分量；i＝1,2,3,4；

采用下式预测速度信息：

其中，

为k时刻加速度计读取的无人机节点i机体坐标系相对于导航坐标系的加速度在机体坐标系X、Y、Z轴上的分量；

为k时刻加速度计零偏在机体坐标系X、Y、Z轴上的分量；g＝[0 0 g]^T，g为当地重力加速度值；

为k时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；

为k-1时刻机体坐标系相对于导航坐标系的线速度在导航坐标系X、Y、Z轴上的分量；i＝1,2,3,4；

为机体坐标系到导航坐标系之间的姿态矩阵，通过下式计算：

采用下式预测位置信息：

其中，

分别为k时刻无人机节点i在导航坐标系X、Y、Z轴上的位置坐标；k-1时刻的位置

分别为k-1时刻无人机节点i在导航坐标系X、Y、Z轴上的位置坐标。

4.根据权利要求2所述无人机编队协同导航方法，其特征在于，步骤(4)的具体过程如下：

(401)确定无人机节点1导航***的状态向量，无人机节点1～4的状态向量相同，如下：

其中，

为无人机节点i的平台误差角，δV_Ei,δV_Ni,δV_Ui为无人机节点i的陀螺东向、北向、天向速度误差，δL_i,δλ_i,δh_i为无人机节点i陀螺纬度、经度、高度误差，L_i,λ_i,h_i无人机节点i陀螺纬度、经度、高度值，ε_bxi,ε_byi,ε_bzi为无人机节点i陀螺随机常数误差，ε_rxi,ε_ryi,ε_rzi为无人机节点i陀螺一阶马尔科夫随机噪声，

为无人机节点i加速度计一阶马尔科夫过程；

当i取1时即为无人机节点1导航***的状态向量；

(402)建立惯性导航***的误差方程，包括平台误差角方程、速度误差方程和位置误差方程；

平台误差角方程：

其中，w_ie为地球自转角速度，v_Ei、v_Ni为无人机节点i惯性导航***东向、北向速度，δv_Ei、δv_Ni为对应的速度误差，ε_Ei、ε_Ni、ε_Ui为无人机节点i东、北、天向陀螺噪声角，R_M＝R_e(1-2f+3fsin²L_i)，R_N＝R_e(1+f sin²L_i)，R_e＝6378137米，f＝1/298.257；

速度误差方程：

其中，v_Ui为无人机节点i惯性导航***天向速度，δv_Ui为对应的速度误差，f_Ei、f_Ni、f_Ui为无人机节点i加速度计测得的加速度值，

为无人机节点i东向、北向、天向加速度计一阶马尔可夫过程白噪声，R为地球半径，g为当地重力加速度值；

位置误差方程：

当i取1时即为无人机节点1的惯性导航***的误差方程；

(403)建立无人机节点1导航***的状态方程，无人机节点1～4的状态方程相同，如下：

其中，G_i为***噪声系数矩阵，W_i为噪声控制向量，

F_Ni为平台误差角、速度误差、位置误差构成的***矩阵，

T_gxi、T_gyi、T_gzi为三轴陀螺相关时间常数，T_axi、T_ayi、T_azi为三轴加速度计相关时间常数；

当i取1时即为无人机节点1导航***的状态方程；

(404)建立无人机节点1导航***的量测方程：

无人机节点1导航***的速度量测矩阵H_v1和量测噪声V_v1：

H_v1＝[0_3×3 diag[1 1 1] 0_3×12]

V_v1＝[M_E1 M_N1 M_U1]

上式中，M_E1,M_N1,M_U1分别为无人机节点1的GPS接收机沿东、北、天向的测速误差；

无人机节点1导航***的位置量测矩阵H_p1和量测噪声V_p1：

H_p1＝[0_3×6 diag[R_M R_NcosL₁ 1] 0_3×9]

V_p1＝[N_E1 N_N1 N_U1]

上式中，N_E1,N_N1,N_U1分别为无人机节点1上的GPS接收机在东、北、天向的位置误差；

(405)采用卡尔曼滤波器对无人机节点1的导航***进行更新。

5.根据权利要求4所述无人机编队协同导航方法，其特征在于，在步骤(6)中，采用下式解算出无人机节点2和无人机节点3的绝对位置信息(x_i,y_i,z_i)：

其中，x、y、z为无人机节点1导航***得到的高精度经度、纬度、高度信息，d_i为无人机节点i相对于无人机节点1的距离，α_i、β_i为无人机节点i相对于无人机节点1的俯仰角、方位角，ν_iE,ν_iN,ν_iU为无人机节点i的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；i＝2,3。

6.根据权利要求5所述无人机编队协同导航方法，其特征在于，在步骤(7)中，首先建立无人机节点2和无人机节点3的导航***量测方程：

无人机节点2和无人机节点3的导航***速度量测矩阵H_vi和量测噪声V_vi：

H_vi＝[0_3×3 diag[1 1 0] 0_3×12]

V_vi＝[M_DEi M_DNi 0]

上式中，M_DEi,M_DNi为无人机节点i的光流传感器在东向、北向上的均值为零且方差为定值的量测白噪声；

无人机节点2和无人机节点3的导航***位置量测矩阵H_pi和量测噪声V_pi：

V_pi＝[ν_iE ν_iN ν_iU]

上式中，i＝2,3；

然后采用卡尔曼滤波器对无人机节点2和无人机节点3的导航***进行更新。

7.根据权利要求4所述无人机编队协同导航方法，其特征在于，在步骤(9)中，首先建立无人机节点4的导航***量测方程：

无人机节点4的导航***速度量测矩阵H_v4和量测噪声V_v4：

H_v4＝[0_3×3 diag[1 1 0] 0_3×12]

V_v4＝[M_E4 M_N4 0]

上式中，M_E4,M_N4为无人机节点4的光流传感器在东向、北向满足均值为零且方差为定值的速度量测白噪声；

无人机节点4的导航***位置量测矩阵H_p4和量测噪声V_p4：

V_p4＝[ν_4E ν_4N ν_4U]

上式中，ν_4E,ν_4N,ν_4U为无人机节点4的视觉导航设备解算的东向位置误差、北向位置误差、天向位置误差；

然后采用卡尔曼滤波器对无人机节点4的导航***进行更新。

8.根据权利要求1-7中任意一项所述无人机编队协同导航方法，其特征在于，在步骤(8)中，设无人机节点1的位置为(x₁,y₁,z₁)，无人机节点2的位置为(x₂,y₂,z₂)，无人机节点3的位置为(x₃,y₃,z₃)，通过无人机节点4上的视觉导航设备获得与无人机节点1的距离d₁、与无人机节点2的距离d₂、与无人机节点3的距离d₃，则：

通过最小二乘法解算上式求出无人机节点4的位置(x₄,y₄,z₄)。

Images