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

权利要求书

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。