[发明专利]一种基于分层式结构的集群飞行器协同导航方法

权利要求书

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时刻的系统噪声矢量。