CN108151737B - 一种动态互观测关系条件下的无人机蜂群协同导航方法 - Google Patents

Abstract

本发明公开一种动态互观测关系条件下的无人机蜂群协同导航方法，包括如下步骤：步骤1，初始化，对无人机蜂群中的无人机按照1～n进行编号；步骤2，选取无人机蜂群导航状态量，建立导航状态方程和独立导航量测方程；步骤3，建立协同导航量测方程；步骤4，无人机蜂群中的无人机分别进行独立导航解算；步骤5，无人机蜂群中的无人机分别进行协同导航解算；步骤6，判断导航是否结束，如结束则停止导航，否则返回步骤4。此种方法可在各无人机利用既有自身量测信息进行独立导航的基础上，设计适用于视线矢量测量信息动态变化的协同导航方法对导航性能进行进一步优化，有效提高无人机蜂群中无人机的导航精度。

Description

一种动态互观测关系条件下的无人机蜂群协同导航方法

技术领域

本发明属于定位与导航技术领域，特别涉及一种动态互观测关系条件下的无人机蜂群协同导航方法。

背景技术

导航技术是无人机实现可靠控制和稳定飞行的关键技术之一。传统无人机主要依靠自身携带的组合导航***，在卫星导航等测量信息的辅助下通过不断修正惯导误差进行持续导航。在卫星导航信号干扰或遮蔽条件下，惯导误差难以得到有效抑制，使得导航性能下降。

由多架微无人机组成的蜂群不仅能够完成更为丰富的任务，而且有望引入蜂群中无人机之间的视线矢量测量信息，通过蜂群中微无人机的相互协同提高导航性能。但需要考虑各无人机自身既有独立导航和无人机之间协同导航两者之间的协调运行，并兼顾各无人机在飞行过程中相互位置变化所引起的视线矢量测量信息数量和实现矢量观测关系的动态变化。

发明内容

本发明的目的，在于提供一种动态互观测关系条件下的无人机蜂群协同导航方法，其可在各无人机利用既有自身量测信息进行独立导航的基础上，设计适用于视线矢量测量信息动态变化的协同导航方法对导航性能进行进一步优化，有效提高无人机蜂群中无人机的导航精度。

为了达成上述目的，本发明的解决方案是：

一种动态互观测关系条件下的无人机蜂群协同导航方法，包括如下步骤：

步骤1，初始化，对无人机蜂群中的无人机按照1～n进行编号；

步骤2，选取无人机蜂群导航状态量，建立导航状态方程和独立导航量测方程；

步骤3，建立协同导航量测方程；

步骤4，无人机蜂群中的无人机分别进行独立导航解算；

步骤5，无人机蜂群中的无人机分别进行协同导航解算；

步骤6，判断导航是否结束，如结束则停止导航，否则返回步骤4。

上述步骤2的详细内容是：

对于其中任意一架无人机i，i∈{1,2,…,n}，导航状态量表达式为：

其中，φ_E,φ_N,φ_U分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E,δv_N,δv_U分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx,ε_by,ε_bz，ε_rx,ε_ry,ε_rz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，T为转置；

导航状态方程表达式为：

X_i(k)＝Φ_i(k,k-1)X_i(k-1)+Γ_i(k,k-1)W_i(k-1)

其中，Φ_i(k-1)为t_k-1时刻至t_k时刻***的状态转移矩阵，W_i(k-1)为t_k时刻***的噪声矢量，Γ_i(k,k-1)为t_k-1时刻至t_k时刻***的噪声驱动矩阵；X_i(k-1)为t_k-1时刻无人机i机载惯性导航***误差状态量，X_i(k)为t_k时刻无人机i机载惯性导航***误差状态量；

独立导航量测方程表达式为：

Z_i(k)＝H_i(k)X_i(k)+V_i(k)

其中Z_i(k)为t_k时刻无人机i惯导输出的飞行器速度矢量，V_i(k)为t_k时刻无人机i的卫星导航接收机噪声，其协方差矩阵为R_i(k)，H_i(k)为t_k时刻无人机i的惯性/卫星组合量测矩阵。

上述步骤3中，协同导航量测方程的表达式为：

Z_ij(k)＝H_ij(k)X_i(k)+V_ij(k)

其中Z_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差，V_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差测量噪声，H_ij(k)为t_k时刻无人机j对于无人机i的协同导航量测矩阵，其表达式为：

H_ij＝[0_3×6 diag[R_N R_M 1] 0_3×9]

其中R_N为无人机i所在当地卯酉圈曲率半径，R_M为无人机i所在当地子午圈曲率半径，h_i为无人机i所在高度。

上述步骤4的详细内容是：对于其中任意一架无人机i，i∈{1,2,…,n}，独立导航解算包括如下子步骤：

步骤41，采集惯性器件数据，进行惯性导航解算；

步骤42，判断是否有卫星导航数据，如果有，则继续执行步骤43，否则执行步骤45；

步骤43，采集卫星导航数据，根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差，其表达式为：

P_i(k)＝[I-K_i(k)H_i(k)][Φ_i(k,k-1)P_i(k)Φ_i(k,k-1)^T+Γ_i(k,k-1)Q_iΓ_i(k,k-1)^T]

K_i(k)＝P_i(k)H_i(k)[H_i(k)P_i(k)H_i(k)+R_i(k)]^-1

其中，

为k-1时刻惯导误差状态量估计值；

步骤44，利用步骤43估计的惯导误差进行惯性导航修正；

步骤45，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的独立导航解算结果。

上述步骤5的详细内容是：对于其中任意一架无人机i，i∈{1,2,…,n}，协同导航解算包括如下子步骤：

步骤51，令j＝1；

步骤52，判断是否j＝i，如果是则执行步骤58，否则继续执行步骤53；

步骤53，判断是否可以获得无人机i到无人机j的视线矢量数据，如是则执行步骤54，否则执行步骤58；

步骤54，采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据d_ij，并根据步骤45获得的无人机i和无人机j的独立导航解算结果，进行协同导航位置偏差解算，其表达式为：

其中r_j为无人机j在无人机i所在当地地理坐标系下的位置矢量，其表达式为

r_j＝[(λ_j-λ_i)R_N (L_j-L_i)R_M h_j-h_i]^T

其中λ_i、L_i、h_i分别为无人机i的独立导航解算结果中的经度、纬度、高度；λ_j、L_j、h_j分别为无人机j的独立导航解算结果中的经度、纬度、高度；

步骤55，进行协同导航位置偏差协方差解算，其表达式为：

其中σ_dλ,ij，σ_dL,ij，σ_dh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的为相对视线矢量测量误差协方差，其表达式为：

其中上标^(s)表示矩阵对角线上的第s个元素,δθ表示相对视线矢量方向测量误差，ε表示相对距离测量误差；

其中σ_rλ,ij，σ_rL,ij，σ_rh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差，其表达式为：

步骤56，利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差，根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差，其表达式为：

P_ij(k)＝[I-K_ij(k)H_ij(k)]P_i(k)

K_ij(k)＝P_i(k)H_ij(k)[H_ij(k)P_i(k)H_ij(k)+R_ij(k)]^-1

步骤57，利用步骤56得到的惯导误差状态量估计值进行惯性导航修正，并令

P_i(k)＝P_ij(k)；

步骤58：判断是否j＜n，如果是则令j＝j+1，并执行步骤52，否则执行步骤59；

步骤59，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的协同导航解算结果。

采用上述方案后，本发明能够适应无人机蜂群在飞行过程中相互视线矢量观测条件的不断变化，有效实现协同导航与自身既有独立导航***的协调运行，与无人机独立导航相比可有效提升导航精度，具有良好的应用前景。

附图说明

图1是本发明的原理流程示意图；

图2是采用本发明协同导航和未优化的无人机蜂群中无人机平均导航误差对比图。

具体实施方式

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

如图1所示，本发明提供一种动态互观测关系条件下的无人机蜂群协同导航方法，包括如下步骤：

步骤1，初始化，对无人机蜂群中的无人机按照1～n进行编号；

步骤2，选取无人机蜂群导航状态量，建立导航状态方程和独立导航量测方程；对于其中任意一架无人机i，i∈{1,2,…,n}，导航状态量表达式为：

其中，φ_E,φ_N,φ_U分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E,δv_N,δv_U分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx,ε_by,ε_bz，ε_rx,ε_ry,ε_rz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，^T为转置；

导航状态方程表达式为：

X_i(k)＝Φ_i(k,k-1)X_i(k-1)+Γ_i(k,k-1)W_i(k-1)

其中，Φ_i(k-1)为t_k-1时刻至t_k时刻***的状态转移矩阵，W_i(k-1)为t_k时刻***的噪声矢量，Γ_i(k,k-1)为t_k-1时刻至t_k时刻***的噪声驱动矩阵；X_i(k-1)为t_k-1时刻无人机i机载惯性导航***误差状态量，X_i(k)为t_k时刻无人机i机载惯性导航***误差状态量；

独立导航量测方程表达式为：

Z_i(k)＝H_i(k)X_i(k)+V_i(k)

其中Z_i(k)为t_k时刻无人机i惯导输出的飞行器速度矢量，V_i(k)为t_k时刻无人机i的卫星导航接收机噪声，其协方差矩阵为R_i(k)，H_i(k)为t_k时刻无人机i的惯性/卫星组合量测矩阵；

步骤3，建立协同导航量测方程，其表达式为：

Z_ij(k)＝H_ij(k)X_i(k)+V_ij(k)

其中Z_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差，V_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差测量噪声，H_ij(k)为t_k时刻无人机j对于无人机i的协同导航量测矩阵，其表达式为：

H_ij＝[0_3×6 diag[R_N R_M 1] 0_3×9]

其中R_N为无人机i所在当地卯酉圈曲率半径，R_M为无人机i所在当地子午圈曲率半径，h_i为无人机i所在高度；

步骤4，无人机蜂群中的无人机分别进行独立导航解算，对于其中任意一架无人机i，i∈{1,2,…,n}，独立导航解算包括如下子步骤：

步骤41，采集惯性器件数据，进行惯性导航解算；

步骤42，判断是否有卫星导航数据，如果有，则继续执行步骤43，否则执行步骤45；

步骤43，采集卫星导航数据，根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差，其表达式为：

P_i(k)＝[I-K_i(k)H_i(k)][Φ_i(k,k-1)P_i(k)Φ_i(k,k-1)^T+Γ_i(k,k-1)Q_iΓ_i(k,k-1)^T]

K_i(k)＝P_i(k)H_i(k)[H_i(k)P_i(k)H_i(k)+R_i(k)]^-1

其中，

为k-1时刻惯导误差状态量估计值，

步骤44，利用步骤43估计的惯导误差进行惯性导航修正；

步骤45，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的独立导航解算结果；

步骤5，无人机蜂群中的无人机分别进行协同导航解算，对于其中任意一架无人机i，i∈{1,2,…,n}，协同导航解算包括如下子步骤：

步骤51，令j＝1；

步骤52，判断是否j＝i，如果是则执行步骤58，否则继续执行步骤53；

步骤53，判断是否可以获得无人机i到无人机j的视线矢量数据，如是则执行步骤54，否则执行步骤58；

步骤54，采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据d_ij，并根据步骤45获得的无人机i和无人机j的独立导航解算结果，进行协同导航位置偏差解算，其表达式为：

其中r_j为无人机j在无人机i所在当地地理坐标系下的位置矢量，其表达式为

r_j＝[(λ_j-λ_i)R_N (L_j-L_i)R_M h_j-h_i]^T

其中λ_i、L_i、h_i分别为无人机i的独立导航解算结果中的经度、纬度、高度；λ_j、L_j、h_j分别为无人机j的独立导航解算结果中的经度、纬度、高度；

步骤55，进行协同导航位置偏差协方差解算，其表达式为：

其中σ_dλ,ij，σ_dL,ij，σ_dh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的为相对视线矢量测量误差协方差，其表达式为：

其中上标^(s)表示矩阵对角线上的第s个元素，δθ表示相对视线矢量方向测量误差，ε表示相对距离测量误差；

其中σ_rλ,ij，σ_rL,ij，σ_rh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差，其表达式为：

步骤56，利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差，根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差，其表达式为：

P_ij(k)＝[I-K_ij(k)H_ij(k)]P_i(k)

K_ij(k)＝P_i(k)H_ij(k)[H_ij(k)P_i(k)H_ij(k)+R_ij(k)]^-1

步骤57，利用步骤56得到的惯导误差状态量估计值进行惯性导航修正，并令

P_i(k)＝P_ij(k)；

步骤58：判断是否j＜n，如果是则令j＝j+1，并执行步骤52，否则执行步骤59；

步骤59，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的协同导航解算结果；

步骤6，判断导航是否结束，如结束则停止导航，否则返回步骤4。

为了验证本发明所提出的用于动态观测关系条件的无人机蜂群协同导航方法的有效性，进行数字仿真分析。仿真中采用的无人机蜂群中无人机数量为60～240架，视线矢量的方向测量精度为0.1度，视线矢量的距离测量精度为20厘米。图2为采用本发明方法优化和未优化的无人机蜂群中无人机平均导航误差对比图。

由图2可以看出，采用本发明所提出的一种用于动态观测关系条件的无人机蜂群协同导航方法后，蜂群中无人机平均定位误差与独立导航相比降低10.4％～30.9％。此外，采用本发明方法能够适应无人机蜂群在飞行过程中相互视线矢量观测条件的不断变化，且随着蜂群中无人机数量的增加定位误差可以通过协同持续显著降低，具有良好的应用价值。

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

Claims (4)

1.一种动态互观测关系条件下的无人机蜂群协同导航方法，其特征在于包括如下步骤：

步骤1，初始化，对无人机蜂群中的无人机按照1～n进行编号；

步骤2，选取无人机蜂群导航状态量，建立导航状态方程和独立导航量测方程；

步骤3，建立协同导航量测方程；

步骤4，无人机蜂群中的无人机分别进行独立导航解算；

步骤5，无人机蜂群中的无人机分别进行协同导航解算；

所述步骤5的详细内容是：对于其中任意一架无人机i，i∈{1,2,…,n}，协同导航解算包括如下子步骤：

步骤51，令j＝1；

步骤52，判断是否j＝i，如果是则执行步骤58，否则继续执行步骤53；

步骤53，判断是否可以获得无人机i到无人机j的视线矢量数据，如是则执行步骤54，否则执行步骤58；

步骤54，采集无人机i机体坐标系下无人机i到无人机j的视线矢量数据d_ij，并根据步骤4获得的无人机i和无人机j的独立导航解算结果，进行协同导航位置偏差解算，其表达式为：

其中r_j为无人机j在无人机i所在当地地理坐标系下的位置矢量，其表达式为

r_j＝[(λ_j-λ_i)R_N (L_j-L_i)R_M h_j-h_i]^T

其中λ_i、L_i、h_i分别为无人机i的独立导航解算结果中的经度、纬度、高度；λ_j、L_j、h_j分别为无人机j的独立导航解算结果中的经度、纬度、高度；R_N为无人机i所在当地卯酉圈曲率半径，R_M为无人机i所在当地子午圈曲率半径；

步骤55，进行协同导航位置偏差协方差解算，其表达式为：

其中σ_dλ,ij，σ_dL,ij，σ_dh,ij分别为无人机j在无人机i所在当地地理坐标系经度、纬度、高度方向的相对视线矢量测量误差协方差，其表达式为：

其中上标^(s)表示矩阵对角线上的第s个元素，δθ表示相对视线矢量方向测量误差，ε表示相对距离测量误差；

其中σ_rλ,ij，σ_rL,ij，σ_rh,ij分别为在无人机i所在当地地理坐标系经度、纬度、高度方向的无人机j独立导航解算误差协方差，其表达式为：

步骤56，利用步骤54获得的协同导航位置偏差和步骤55获得的协同导航位置偏差协方差，根据步骤3建立的协同导航量测方程进行协同导航滤波并更新惯导误差状态量估计值和惯导误差状态量估计协方差，其表达式为：

P_ij(k)＝[I-K_ij(k)H_ij(k)]P_i(k)

K_ij(k)＝P_i(k)H_ij(k)[H_ij(k)P_i(k)H_ij(k)+R_ij(k)]^-1

其中，Z_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差，H_ij(k)为t_k时刻无人机j对于无人机i的协同导航量测矩阵；

步骤57，利用步骤56得到的惯导误差状态量估计值进行惯性导航修正，并令

P_i(k)＝P_ij(k)；

步骤58：判断是否j＜n，如果是则令j＝j+1，并执行步骤52，否则执行步骤59；

步骤59，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的协同导航解算结果；

步骤6，判断导航是否结束，如结束则停止导航，否则返回步骤4。

2.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法，其特征在于：所述步骤2的详细内容是：

对于其中任意一架无人机i，i∈{1,2,…,n}，导航状态量表达式为：

其中，φ_E,φ_N,φ_U分别表示无人机i机载惯性导航***误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E,δv_N,δv_U分别表示无人机i机载惯性导航***误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL,δλ,δh分别表示无人机i机载惯性导航***误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx,ε_by,ε_bz，ε_rx,ε_ry,ε_rz分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示无人机i机载惯性导航***误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，T为转置；

导航状态方程表达式为：

X_i(k)＝Φ_i(k,k-1)X_i(k-1)+Γ_i(k,k-1)W_i(k-1)

其中，Φ_i(k,k-1)为t_k-1时刻至t_k时刻***的状态转移矩阵，W_i(k-1)为t_k-1时刻***的噪声矢量，Γ_i(k,k-1)为t_k-1时刻至t_k时刻***的噪声驱动矩阵；X_i(k-1)为t_k-1时刻无人机i机载惯性导航***误差状态量，X_i(k)为t_k时刻无人机i机载惯性导航***误差状态量；

独立导航量测方程表达式为：

Z_i(k)＝H_i(k)X_i(k)+V_i(k)

其中Z_i(k)为t_k时刻无人机i惯导输出的飞行器速度矢量，V_i(k)为t_k时刻无人机i的卫星导航接收机噪声，其协方差矩阵为R_i(k)，H_i(k)为t_k时刻无人机i的惯性/卫星组合量测矩阵。

3.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法，其特征在于：所述步骤3中，协同导航量测方程的表达式为：

Z_ij(k)＝H_ij(k)X_i(k)+V_ij(k)

其中，X_i(k)为t_k时刻无人机i机载惯性导航***误差状态量，Z_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差，V_ij(k)为t_k时刻无人机j对于无人机i的协同导航位置偏差测量噪声，H_ij(k)为t_k时刻无人机j对于无人机i的协同导航量测矩阵，其表达式为：

H_ij＝[0_3×6 diag[R_N R_M 1] 0_3×9]

其中R_N为无人机i所在当地卯酉圈曲率半径，R_M为无人机i所在当地子午圈曲率半径，h_i为无人机i所在高度。

4.如权利要求1所述的一种动态互观测关系条件下的无人机蜂群协同导航方法，其特征在于：所述步骤4的详细内容是：对于其中任意一架无人机i，i∈{1,2,…,n}，独立导航解算包括如下子步骤：

步骤41，采集惯性器件数据，进行惯性导航解算；

步骤42，判断是否有卫星导航数据，如果有，则继续执行步骤43，否则执行步骤45；

步骤43，采集卫星导航数据，根据步骤2建立的独立导航量测方程进行惯性/卫星组合滤波并计算惯导误差状态估计值及惯导误差状态估计协方差，其表达式为：

P_i(k)＝[I-K_i(k)H_i(k)][Φ_i(k,k-1)P_i(k)Φ_i(k,k-1)^T+Γ_i(k,k-1)Q_iΓ_i(k,k-1)^T]

K_i(k)＝P_i(k)H_i(k)[H_i(k)P_i(k)H_i(k)+R_i(k)]^-1

其中，

为k-1时刻惯导误差状态量估计值，Φ_i(k,k-1)为t_k-1时刻至t_k时刻***的状态转移矩阵，Z_i(k)为t_k时刻无人机i惯导输出的飞行器速度矢量，H_i(k)为t_k时刻无人机i的惯性/卫星组合量测矩阵，Γ_i(k,k-1)为t_k-1时刻至t_k时刻***的噪声驱动矩阵，R_i(k)为t_k时刻无人机i的卫星导航接收机噪声的协方差矩阵；

步骤44，利用步骤43估计的惯导误差进行惯性导航修正；

步骤45，输出惯性导航***指示的经度λ_i、纬度L_i、高度h_i作为无人机i的独立导航解算结果。

Images