CN109708629B - 一种用于差异定位性能条件的飞行器集群协同导航方法 - Google Patents

Abstract

本发明公布了一种用于差异定位性能条件的飞行器集群协同导航方法，属于定位与导航技术领域。本方法将不同类型和定位精度的飞行器编队飞行时，将定位精度较高的飞行器作为基准飞行器，利用飞行器之间的相互距离数据来提高定位精度较差的飞行器的定位精度；将飞行器机载平台导航系统输出的导航参数转化为地球坐标系下的坐标，通过构建三维到达时间差方程，并通过飞行器位置协同解算算法来求解待定位飞行器的位置坐标，利用飞行器位置误差迭代算法对求解结果进行优化，在此基础上，根据此算法的飞行器位置求解结果，利用卡尔曼滤波对机载导航信息进行修正。本发明在基准飞行器数量越多的情况下，定位精度越高，适合实际应用。

Description

一种用于差异定位性能条件的飞行器集群协同导航方法

技术领域

本发明涉及一种用于差异定位性能条件的飞行器集群协同导航方法，属于定位与导航技术领域。

背景技术

近年来，飞行器编队飞行越来越受到国际上的关注，所谓飞行器编队飞行，即多架飞行器为适应任务要求而进行的某种队形排列和任务分配的组织模式。飞行器编队飞行能够扩大飞行器搜索侦查范围，具有可执行多重任务，可靠性高，整体效率高，减少飞行阻力等优点。

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

发明内容

为了解决不同类型和性能飞行器在编队飞行过程因导航精度差距较大而影响编队飞行整体效果的问题，本发明提出了一种用于差异定位性能条件的飞行器集群协同导航方法，通过获取飞行器之间的距离信息来提高导航精度较差飞行器的定位精度，使整个编队飞行器之间的导航精度差距减小。

本发明为解决其技术问题采用如下技术方案：

一种用于差异定位性能条件的飞行器集群协同导航方法，包括以下步骤：

步骤(1)，获取飞行器集群协同定位所需的基准飞行器的经度、纬度和高度数据以及待定位飞行器与主基准飞行器和副基准飞行器之间的距离差数据；

步骤(2)，计算基准飞行器机载平台导航系统输出的经度、纬度和高度导航参数转化为地球坐标系下的坐标；

步骤(3)，构建待定位飞行器的到达时间差方程；

步骤(4)，利用飞行器位置协同解算算法求解到达时间差方程，得到待定位飞行器坐标；

步骤(5)，利用飞行器位置误差迭代算法对飞行器位置协同解算算法的求解结果进行优化；

步骤(6)，根据飞行器位置协同解算算法求解结果，利用卡尔曼滤波对待定位飞行器机载平台导航系统的经度、纬度和高度参数进行修正；

步骤(7)，重复上述步骤的算法，依次对定位精度较差的飞行器进行修正。

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

步骤(1-1)，确定所有参与集群飞行器的数量为M，若M≥5，则执行后续步骤，若M＜5，算法无法使用，则继续等待直至集群飞行器数量M≥5，并对所有飞行器进行编号，获取所有飞行器的机载惯性导航设备的定位误差E_j，其中j为飞行器编号，j＝1,2,3…M，表示含义为第j号飞行器，将待定位飞行器编号设定为变量f，其中初始设定f＝M，需要进行定位修正的飞行器数量变量设定为d，令d＝M-4；

步骤(1-2)，根据步骤(1-1)获取的所有飞行器的机载惯性导航设备定位误差，按照定位误差大小对所有飞行器进行排序，确定排序外循环次数变量n₁，其中n₁＝1，内循环次数变量n₂，其中n₂＝M-1，设定飞行器编号初始值i＝1；

步骤(1-3)，比较E_i和E_i+1的大小，如果E_i≥E_i+1，则编号为i的飞行器编号变为i+1，如果E_i＜E_i+1，则飞行器编号不变，并且i＝i+1，进行步骤(1-4)；

步骤(1-4)，比较i和n₂的大小，如果i≤n₂，则重复步骤(1-3)，如果i＞n₂，则n₁＝n₁+1并继续步骤(1-5)；

步骤(1-5)，比较n₁和n₂的大小，如果n₁≤n₂，则重复步骤(1-3)，如果n₁＞n₂，则所有飞行器已经重新排序和编号，飞行器编号越大，则机载设备定位误差越大；

步骤(1-6)，将编号为f的飞行器定为待定位飞行器，其余飞行器作为基准飞行器确定基准飞行器的数量N，其中编号为m的飞行器作为主基准飞行器，其中m＝1，其余基准飞行器作为副基准飞行器；

步骤(1-7)，基准飞行器通过自身机载平台上的导航系统获得基准飞行器所在位置的经度、纬度和高度数据；

步骤(1-8)，飞行器通过自身携带的相对测距传感器获得待定位飞行器到主基准飞行器与到副基准飞行器的距离差R_im，其计算公式如下：

R_im＝R_i-R_m＝c*t_im

其中，R_i为待定位飞行器到副基准飞行器的距离，i＝2,3…N，i为副基准飞行器的编号，R_m为待定位飞行器到主基准飞行器的距离，t_im为不同基准飞行器上的传感器测得的待定位飞行器发出的信号差，c为信号传播速度。

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

步骤(2-1)，根据步骤(1-7)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标x_i，其表达式为：

x_i＝(N+H)cosLcosλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-2)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标y_i，其表达式为：

y_i＝(N+H)cosLsinλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-3)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标z_i，其表达式为：

z_i＝[N(1-e²)+H]sin L

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，e为椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

N的计算表达式如下所示：

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

步骤(3-1)，根据步骤(2)中所获得的待定位飞行器和基准飞行器的坐标以及步骤(1-1)中获得的距离差数据，建立待定位飞行器的到达时间差方程，表达式如下所示：

其中(x,y,z)为待定位飞行器的三维坐标，是待求变量，其中x为待定位飞行器在地球坐标系下的横坐标，y为待定位飞行器在地球坐标系下的纵坐标，z为待定位飞行器在地球坐标系下的竖坐标，根据步骤(2-1)，步骤(2-2)，步骤(2-3)的计算结果，其中x_i为地球坐标系下的副基准飞行器横坐标，y_i为地球坐标系下的副基准飞行器纵坐标，z_i为地球坐标系下的副基准飞行器竖坐标，i＝2,3…N为副基准飞行器的编号，根据步骤(1-3)计算结果，R_2m,R_3m,R_4m…R_im，为待定位飞行器到副基准飞行器与主基准飞行器的距离差，其中i为副基准飞行器的编号，x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(3-2)，将步骤(3-1)建立的到达时间差方程转换成矩阵形式并化简，表达式为：

化简为：

其中X_i,m＝x_i-x_m，Y_i,m＝y_i-y_m，Z_i,m＝z_i-z_m，K_i＝x_i ²+y_i ²+z_i ²，i＝2,3…N,K_m＝x_m ²+y_m ²+z_m ²。

利用飞行器位置协同解算算法求解待定位飞行器的坐标，所述步骤(4)包括如下具体步骤：

步骤(4-1)，根据步骤(3-2)所建立的到达时间差方程，获取所需的方程参数h，G_a，Q，计算表达式为：

Q＝diag{σ_2,m,σ_3,m…σ_N,m}

其中，i＝2,3…N，其中，σ_i,m为高斯误差函数的标准差；

步骤(4-2)，令z_a＝[z_p ^t,R₁]为待求矢量，其中z_p＝[x,y,z]^T，R₁为待定位飞行器到主基准飞行器的距离；进行两次迭代算法计算误差矢量协方差矩阵ψ，设定迭代次数参数为n＝1，设定参数矩阵C＝Q；

步骤(4-3)，根据步骤(4-1)的参数计算结果计算z_a的估计值，表达式为：

z_a＝(G_a ^TC^-1G_a)^-1(G_a ^TC^-1h)；

其中，G_a ^T为步骤(4-1)计算结果G_a的转置矩阵，C^-1为步骤(4-2)计算结果C的逆矩阵；

步骤(4-4)，根据步骤(4-3)计算的z_a的估计值，计算z_a和各个副基准飞行器之间的距离V_ai，计算表达式为：

其中，z_a,1，z_a,2，z_a,3为z_a的前三项计算值，根据步骤(3-1)确定的结果，x_i为地球坐标系下的副基准飞行器的横坐标，y_i为地球坐标系下的副基准飞行器的纵坐标，z_i为地球坐标系下的副基准飞行器的竖坐标，i为副基准飞行器的编号；

步骤(4-5)，根据步骤(4-4)计算的V_ai的值，将其改为对角矩阵形式为B_a，其表达式为：

其中，V_a2，V_a3…V_aN分别为步骤(4-4)计算的V_ai中编号i取不同的值得计算结果，i＝2,3…N；

步骤(4-6)，根据步骤(4-1)中计算的Q和步骤(4-5)中计算的B_a，计算矩阵ψ_a，表达式为：

ψ_a＝B_a*Q*B_a；

步骤(4-7)，算法循环次数n加1，即：

n＝n+1；

步骤(4-8)，比较循环次数n和2的大小，如果n≤2，则执行步骤(4-3)，并且另参数矩阵C为步骤(4-6)计算的矩阵ψ_a，如果n＞2则执行步骤(4-9)；

步骤(4-9)，根据步骤(4-6)计算的ψ_a，计算z_a的误差协方差矩阵z_acov，表达式为：

z_acov＝(G_a ^Tψ_a ^-1G_a)^-1

其中，G_a ^T为矩阵G_a的转置，ψ_a ^-1为矩阵ψ_a的逆矩阵；

步骤(4-10)，构建新的方程参数h′、G′_a、z′_a，表达式为：

其中，z_a,1，z_a,2，z_a,3，z_a,4为z_a的前四项计算值，x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(4-11)，根据步骤(4-3)计算的z_a，构建矩阵B′，计算表达式为：

其中x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(4-12)，根据步骤(4-11)计算的B′和步骤(4-9)所计算的z_a的误差协方差矩阵z_acov，计算新的误差矢量ψ′，表达式为：

ψ′＝4*B′*z_acov*B′；

步骤(4-13)，根据步骤(4-10)计算的参数G_a′，h′和步骤(4-12)计算的误差矢量ψ′，得到新的坐标z_a′，表达式为：

z_a′＝(G_a′^Tψ′^-1G_a′)^-1(G_a′^Tψ′^-1h′)

其中，G_a′^T为矩阵G_a′的转置矩阵，ψ′^-1为矩阵ψ′的逆矩阵；

步骤(4-14)，根据步骤(4-13)计算的z_a′，得到待定位目标飞行器的最终解算坐标z_p，计算表达式为：

或

其中z_p1，z_p2为z_p的两个不同解算值；

步骤(4-15)，根据步骤(4-14)计算的z_p1和z_p2，分别计算其与步骤(4-6)计算结果z_a2的距离d₁,d₂，并计算d₁,d₂之差为a计算表达式为：

a＝d₁-d₂

其中，z_pi,1，z_pi,2，z_pi,3为解算值矩阵z_pi的前三项值i＝1,2，z_a,1为步骤(4-3)求解结果矩阵z_a的第一项求解值，z_a,2为步骤(4-3)求解结果矩阵z_a的第二项求解值，z_a,3为步骤(4-3)求解结果矩阵的第三项求解值；

步骤(4-16)，根据步骤(4-15)计算的a的值，如果a≥0，则解算算法求解结果z_p＝z_p2，如果a＜0，则z_p＝z_p1。

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

步骤(5-1)，根据步骤(4-16)飞行器位置协同解算算法的计算结果z_p，将其作为飞行器位置误差迭代算法的初值(x_v,y_v,z_v)，x_v为z_p的第一项值，y_v为z_p的第二项值，z_v为z_p的第三项值；

步骤(5-2)，确定飞行器位置误差迭代算法的门限值u；

步骤(5-3)，建立算法函数表达式f_i(x,y,z)，为：

其中，根据步骤(3-1)确定的基准飞行器的坐标，x_i+1为地球坐标系下的基准飞行器的横坐标，y_i+1为地球坐标系下的基准飞行器的纵坐标，z_i+1为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N-1，x为地球坐标系下的待定位飞行器的横坐标，y为地球坐标系下的待定位飞行器的纵坐标，z地球坐标系下的待定位飞行器的竖坐标；

步骤(5-4)，根据步骤(5-1)的初始值，以及步骤(1-1)和步骤(1-2)的基准飞行器的坐标和待定位飞行器和基准飞行器的距离差，计算飞行器位置误差迭代算法的部分参数，表达式为：

其中，(x_v,y_v,z_v)为初始坐标，x_v为矩阵z_p的第一项值，y_v为矩阵z_p的第二项值，z_v为矩阵z_p的第三项值，(x_i,y_i,z_i)为副基准飞行器的坐标，x_i为地球坐标系下的基准飞行器的横坐标，y_i为地球坐标系下的基准飞行器的纵坐标，z_i为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N，f_i,v待入初值后的待定位飞行器坐标和主副基准飞行器之间的距离差值，f_i(x_v,y_v,z_v)为步骤(5-3)中的函数带入设定初值后的函数值，α_i,1为函数f_i,v在X轴方向上的增量，α_i,2为函数f_i,v在Y轴方向上的增量，α_i,3为函数f_i,v在Z轴方向上的增量，

为待定位飞行器坐标值和基准飞行器之间的距离，i为基准飞行器的编号，

为

中的变量i取值为1时，

为基准飞行器编号i取值为i+1；

步骤(5-5)，根据步骤(5-4)确定的算法参数和步骤(4-2)确定的到达时间差协方差矩阵Q，求解算法矩阵表达式参数A、D，计算表达式为：

其中，

α_1,1为步骤(5-4)中的i取1时的第一列计算参数值，α_1,2为步骤(5-4)中的i取1时的第二列公式计算的参数值，α_1,3为步骤(5-4)中的i取1时的第三列公式计算值，α_2,1为步骤(5-4)中的i取2时的第一列计算参数值，α_2,2为步骤(5-4)中的i取2时的第二列公式计算的参数值，α_2,3为步骤(5-4)中的i取2时的第三列公式计算值，α_N-1,1为步骤(5-4)中的i取N-1时的第一列计算参数值，α_N-1,2为步骤(5-4)中的i取N-1时的第二列公式计算的参数值，α_N-1,3为步骤(5-4)中的i取N-1时的第三列公式计算值，f_1,v为步骤(5-4)中的i取1时的计算值，f_2,v为步骤(5-4)中的i取2时的计算值，f_N-1,v为步骤(5-4)中的i取N-1时的计算值；

步骤(5-6)，根据步骤(5-5)计算的算法方程参数，构建算法方程Aδ＝D+Q，求解未知的误差增量参数δ

δ＝[A^TQ^-1A]^-1A^TQ^-1D

其中，A^T为步骤(5-5)中矩阵A的转置，Q^-1为协方差矩阵Q的逆矩阵，

其中：δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-7)，计算矩阵δ的各项绝对值之和b，计算表达式为：

b＝|δ_x|+|δ_y|+|δ_z|

步骤(5-8)，如果b≥u，则更新算法初值(x_v,y_v,z_v)，并返回步骤(5-3)，如果b＜u，则输出最后一次更新坐标记为s，(x_v,y_v,z_v)更新表达式为：

x_v＝x_v+δ_x

y_v＝y_v+δ_y

z_v＝z_v+δ_z

其中，x_v，y_v，z_v为步骤(5-1)计算值，δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-9)，将步骤(5-8)计算的待定位飞行器坐标s转化为大地坐标系下经度λ′计算表达式为：

其中，s_x为步骤(5-8)中的计算矩阵s的第一项值，s_y为步骤(5-8)中的计算矩阵s的第二项值；

步骤(5-10)，根据步骤(5-8)计算的待定位飞行器坐标s，采用迭代算法将其转化为大地坐标系下的纬度L′和高度H′，令迭代次数t＝1，初始L′＝0.1；

步骤(5-11)，根据步骤(5-10)的初始L′，计算高度H′和纬度L′的不断修正值，并且迭代次数自加1，表达式如下：

t＝t+1

其中，s＝(s_x,s_y,s_z)^T，s_z为步骤(5-8)计算的矩阵s的第三项值，N为地球半径，e为地球偏心率；

步骤(5-12)，根据步骤(5-11)的迭代次数t的值，比较t与10的大小关系，如果t≤10，则执行步骤(5-11)，如果t＞10，执行步骤(5-13)；

步骤(5-13)，获取最后的经度λ′，纬度L′和高度H′。

所述步骤6包括如下几个步骤：

步骤(6-1)，根据步骤(5-13)计算的经度λ′，纬度L′和高度H′数据，建立卡尔曼滤波量测方程，计算表达式为：

其中，L_t为待定位飞行器机载惯性导航系统的输出的纬度，λ_t为待定位飞行器机载惯性导航系统的输出的纬度，H_t为待定位飞行器机载惯性导航系统的输出的高度数据，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，ε_L为解算算法在经度方向上误差，ε_λ为解算算法在纬度方向上误差，ε_H为解算算法在高度方向上误差；Z_c为量测方程，V_c为量测噪声；

步骤(6-2)，根据步骤(6-1)所计算的卡尔曼滤波量测方程，结合待定位飞行器惯性导航系统的状态方程，经卡尔曼滤波程序后，输出待定位飞行器机载惯性导航系统经度，纬度和高度数据修正值(λ_t′,L_t′,H_t′)，λ_t′为经度修正值，L_t′为纬度修正值，H_t′为高度修正值。

所述步骤(7)包括如下几个步骤：

步骤(7-1)，令f＝f-1，其中f为待定位飞行器的编号；

步骤(7-2)，根据步骤(1-1)的数据，比较f与4的大小，若f≤4，则所有需要修正定位精度的飞行器已经全部修正，若f＞4，继续执行步骤(7-3)；

步骤(7-3)，集群中各飞行器依靠自身机载导航设备继续进行下一时刻导航解算，然后跳转至步骤(1-2)。

本发明的有益效果如下：

本发明是一种用于差异定位性能条件的飞行器集群协同导航方法，应用于不同定位精度飞行器组成编队飞行情况，将基准飞行器机载导航信息转化为了地球坐标系下的坐标，测量了待定位飞行器和各个不同基准飞行器间的距离差，利用本方法计算了待定位飞行器的坐标，并优化了待定位飞行器的坐标，并通过卡尔曼滤波修正了待定位飞行器的机载导航信息。本发明能够提高集群飞行中定位精度较差的飞行器的定位精度，修正机载导航信息。

附图说明

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

具体实施方式

下面结合附图对本发明做更进一步的解释。

本发明方法通过对编队飞行中飞行器重新建立坐标系，将飞行器导航系统中的定位参数转化为地球坐标系，建立到达时间差方程，设计飞行器位置协同解算算法和飞行器位置误差迭代算法求解方程，从而提高定位精度较差的待定位飞行器的定位精度，缩小编队飞行中各飞行器的定位精度差距，增强编队飞行的效果。

如图1所示，一种用于差异定位性能条件的飞行器集群协同导航方法，包括以下步骤：

步骤(1)，对所有飞行器按照定位误差进行排序，并获取每个飞行器的机载惯性导航设备数据，包括如下具体步骤：

步骤(1-1)，确定所有参与集群飞行器的数量为M，若M≥5，则执行后续步骤，若M＜5，算法无法使用，则继续等待直至集群飞行器数量M≥5，并对所有飞行器进行编号，获取所有飞行器的机载惯性导航设备的定位误差E_j，其中j为飞行器编号，j＝1,2,3…M，表示含义为第j号飞行器，待定位飞行器编号设定为变量f，其中f＝M，需要进行定位修正的飞行器数量变量设定为d，令d＝M-4；

步骤(1-2)，根据步骤(1-1)获取的所有飞行器的机载惯性导航设备定位误差，按照定位误差大小对所有飞行器进行排序，确定排序外循环次数变量n₁，其中n₁＝1，内循环次数变量n₂，其中n₂＝M-1，设定飞行器编号初始值i＝1；

步骤(1-3)，比较E_i和E_i+1的大小，如果E_i≥E_i+1，则编号为i的飞行器编号变为i+1，如果E_i＜E_i+1，则飞行器编号不变，并且i＝i+1，进行步骤(1-4)；

步骤(1-4)，比较i和n₂的大小，如果i≤n₂，则重复步骤(1-3)，如果i＞n₂，则n₁＝n₁+1并继续步骤(1-5)；

步骤(1-5)，比较n₁和n₂的大小，如果n₁≤n₂，则重复步骤(1-3)，如果n₁＞n₂，则所有飞行器已经重新排序和编号，飞行器编号越大，则机载设备定位误差越大；

步骤(1-6)，将编号为f的飞行器定位待定位飞行器，其余飞行器作为基准飞行器确定基准飞行器的数量N，其中编号为m的飞行器作为主基准飞行器，其中m＝1，其余基准飞行器作为副基准飞行器；

步骤(1-7)，基准飞行器通过自身机载平台上的导航系统获得基准飞行器所在位置的经度、纬度和高度数据；

步骤(1-8)，飞行器通过自身携带的相对测距传感器获得待定位飞行器到主基准飞行器与到副基准飞行器的距离差R_im，其计算公式如下：

R_im＝R_i-R_m＝c*t_im

其中，R_i(i＝2,3…N)为待定位飞行器到副基准飞行器的距离，i为副基准飞行器的编号，R_m为待定位飞行器到主基准飞行器的距离，t_im(i＝2,3…N)为不同基准飞行器上的传感器测得的待定位飞行器发出的信号差，c为信号传播速度。

步骤(2)，计算基准飞行器机载平台导航系统输出的经度、纬度和高度导航参数转化为地球坐标系下的坐标，包括如下具体步骤：

步骤(2-1)，根据步骤(1-7)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标x_i，其表达式为：

x_i＝(N+H)cosLcosλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-2)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标y_i，其表达式为：

y_i＝(N+H)cosLsinλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-3)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标z_i，其表达式为：

z_i＝[N(1-e²)+H]sin L

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，e为椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

N的计算表达式如下所示：

步骤(3)，构建待定位飞行器的到达时间差方程，包括如下具体步骤：

步骤(3-1)，根据步骤(2)中所获得的待定位飞行器和基准飞行器的坐标以及步骤(1-1)中获得的距离差数据，建立待定位飞行器的到达时间差方程，表达式如下所示：

其中(x,y,z)为待定位飞行器的三维坐标，是待求变量，其中x为待定位飞行器在地球坐标系下的横坐标，y为待定位飞行器在地球坐标系下的纵坐标，z为待定位飞行器在地球坐标系下的竖坐标，根据步骤(2-1)，步骤(2-2)，步骤(2-3)的计算结果，(x_i,y_i,z_i)为副基准飞行器的坐标，其中x_i为地球坐标系下的副基准飞行器横坐标，y_i为地球坐标系下的副基准飞行器纵坐标，z_i为地球坐标系下的副基准飞行器竖坐标，i＝2,3…N为副基准飞行器的编号，根据步骤(1-3)计算结果，R_2m,R_3m,R_4m…R_im(i＝2,3…N)，为待定位飞行器到副基准飞行器与主基准飞行器的距离差，其中i为副基准飞行器的编号，(x_m,y_m,z_m)为主基准飞行器坐标，其中x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(3-2)，将步骤(3-1)建立的到达时间差方程转换成矩阵形式并化简，表达式为：

化简为：

其中X_i,m＝x_i-x_m，Y_i,m＝y_i-y_m，Z_i,m＝z_i-z_m，K_i＝x_i ²+y_i ²+z_i ²，(i＝2,3…N),K_m＝x_m ²+y_m ²+z_m ²。

步骤(4)，利用飞行器位置协同解算算法求解到达时间差方程，得到待定位飞行器坐标，包括如下具体步骤：

步骤(4-1)，根据步骤(3-2)所建立的到达时间差方程，获取所需的方程参数h，G_a，Q，计算表达式为：

Q＝diag{σ_2,m,σ_3,m…σ_N,m}

其中，i＝2,3…N，其中，σ_i,m(i＝2,3…N)为高斯误差函数的标准差；

步骤(4-2)，令z_a＝[z_p ^t,R₁]为待求矢量，其中z_p＝[x,y,z]^T，R₁为待定位飞行器到主基准飞行器的距离；进行两次迭代算法计算误差矢量协方差矩阵ψ，设定迭代次数参数为n＝1，设定参数矩阵C＝Q；

步骤(4-3)，根据步骤(4-1)的参数计算结果计算z_a的估计值，表达式为：

z_a＝(G_a ^TC^-1G_a)^-1(G_a ^TC^-1h)；

其中，G_a ^T为步骤(4-1)计算结果G_a的转置矩阵，C^-1为步骤(4-2)计算结果C的逆矩阵；

步骤(4-4)，根据步骤(4-3)计算的z_a的估计值，计算z_a和各个副基准飞行器之间的距离V_ai，计算表达式为：

其中，z_a,1，z_a,2，z_a,3为z_a的前三项计算值，根据步骤(3-1)确定的结果，(x_i,y_i,z_i)为副基准飞行器的坐标，(i＝2,3…N)，其中x_i为地球坐标系下的副基准飞行器的横坐标，y_i为地球坐标系下的副基准飞行器的纵坐标，z_i为地球坐标系下的副基准飞行器的竖坐标，i为副基准飞行器的编号；

步骤(4-5)，根据步骤(4-4)计算的V_ai的值，将其改为对角矩阵形式为B_a，其表达式为：

其中，V_a2，V_a3…V_aN分别为步骤(4-4)计算的V_ai中编号i取不同的值得计算结果，i＝2,3…N；

步骤(4-6)，根据步骤(4-1)中计算的Q和步骤(4-5)中计算的B_a，计算矩阵ψ_a，表达式为：

ψ_a＝B_a*Q*B_a；

步骤(4-7)，算法循环次数n加1，即：

n＝n+1；

步骤(4-8)，比较循环次数n和2的大小，如果n≤2，则执行步骤(4-3)，并且另参数矩阵C为步骤(4-6)计算的矩阵ψ_a，如果n＞2则执行步骤(4-9)；

步骤(4-9)，根据步骤(4-6)计算的ψ_a，计算z_a的误差协方差矩阵z_acov，表达式为：

z_acov＝(G_a ^Tψ_a ^-1G_a)^-1

其中，G_a ^T为矩阵G_a的转置，ψ_a ^-1为矩阵ψ_a的逆矩阵；

步骤(4-10)，构建新的方程参数h′、G′_a、z′_a，表达式为：

其中，z_a,1，z_a,2，z_a,3，z_a,4为z_a的前四项计算值，(x_m,y_m,z_m)为步骤(3-1)确定的主基准飞行器的坐标，其中x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标下的主基准飞行器竖坐标；

步骤(4-11)，根据步骤(4-3)计算的z_a，构建矩阵B′，计算表达式为：

(x_m,y_m,z_m)为步骤(3-1)确定的主基准飞行器的坐标，其中x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(4-12)，根据步骤(4-11)计算的B′和步骤(4-9)所计算的z_a的误差协方差矩阵z_acov，计算新的误差矢量ψ′，表达式为：

ψ′＝4*B′*z_acov*B′；

步骤(4-13)，根据步骤(4-10)计算的参数G_a′，h′和步骤(4-12)计算的误差矢量ψ′，得到新的坐标z_a′，表达式为：

z_a′＝(G_a′^Tψ′^-1G_a′)^-1(G_a′^Tψ′^-1h′)；

步骤(4-14)，根据步骤(4-13)计算的z_a′，得到待定位目标飞行器的最终解算坐标z_p，计算表达式为：

或

其中z_p1，z_p2为z_p的两个不同解算值；

步骤(4-15)，根据步骤(4-14)计算的z_p1和z_p2，分别计算其与步骤(4-6)计算结果z_a2的距离d₁,d₂，并计算d₁,d₂之差为a计算表达式为：

a＝d₁-d₂

其中，z_pi,1，z_pi,2，z_pi,3为解算值矩阵z_pi的前三项值i＝1,2，z_a,1为步骤(4-3)求解结果矩阵z_a的第一项求解值，z_a,2为步骤(4-3)求解结果矩阵z_a的第二项求解值，z_a,3为步骤(4-3)求解结果矩阵的第三项求解值；

步骤(4-16)，根据步骤(4-15)计算的a的值，如果a≥0，则解算算法求解结果z_p＝z_p2，如果a＜0，则z_p＝z_p1。

步骤(5)，利用飞行器位置误差迭代算法对飞行器位置协同解算算法的求解结果进行优化，包括如下具体步骤：

步骤(5-1)，根据步骤(4-16)飞行器位置协同解算算法的计算结果z_p，将其作为飞行器位置误差迭代算法的初值(x_v,y_v,z_v)，x_v为z_p的第一项值，y_v为z_p的第二项值，z_v为z_p的第三项值；

步骤(5-2)，确定飞行器位置误差迭代算法的门限值u；

步骤(5-3)，建立算法函数表达式f_i(x,y,z)，为：

其中，根据步骤(3-1)确定的基准飞行器的坐标，x_i+1为地球坐标系下的基准飞行器的横坐标，y_i+1为地球坐标系下的基准飞行器的纵坐标，z_i+1为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N-1；

步骤(5-4)，根据步骤(5-1)的初始值，以及步骤(1-1)和步骤(1-2)的基准飞行器的坐标和待定位飞行器和基准飞行器的距离差，计算飞行器位置误差迭代算法的部分参数，表达式为：

其中，(x_v,y_v,z_v)为初始坐标，x_v为矩阵z_p的第一项值，y_v为矩阵z_p的第二项值，z_v为矩阵z_p的第三项值，(x_i,y_i,z_i)为副基准飞行器的坐标，x_i为地球坐标系下的基准飞行器的横坐标，y_i为地球坐标系下的基准飞行器的纵坐标，z_i为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N，f_i,v待入初值后的待定位飞行器坐标和主副基准飞行器之间的距离差值，f_i(x_v,y_v,z_v)为步骤(5-3)中的函数带入设定初值后的函数值，α_i,1为函数f_i,v在X轴方向上的增量，α_i,2为函数f_i,v在Y轴方向上的增量，α_i,3为函数f_i,v在Z轴方向上的增量，

为待定位飞行器坐标值和基准飞行器之间的距离，i为基准飞行器的编号，

为

中的变量i取值为1时，

为基准飞行器编号i取值为i+1；

步骤(5-5)，根据步骤(5-4)确定的算法参数和步骤(4-2)确定的到达时间差协方差矩阵Q，求解算法矩阵表达式参数A、D，计算表达式为：

其中，

α_1,1为步骤(5-4)中的i取1时的第一列计算参数值，α_1,2为步骤(5-4)中的i取1时的第二列公式计算的参数值，α_1,3为步骤(5-4)中的i取1时的第三列公式计算值，α_2,1为步骤(5-4)中的i取2时的第一列计算参数值，α_2,2为步骤(5-4)中的i取2时的第二列公式计算的参数值，α_2,3为步骤(5-4)中的i取2时的第三列公式计算值，α_N-1,1为步骤(5-4)中的i取N-1时的第一列计算参数值，α_N-1,2为步骤(5-4)中的i取N-1时的第二列公式计算的参数值，α_N-1,3为步骤(5-4)中的i取N-1时的第三列公式计算值，f_1,v为步骤(5-4)中的i取1时的计算值，f_2,v为步骤(5-4)中的i取2时的计算值，f_N-1,v为步骤(5-4)中的i取N-1时的计算值；

步骤(5-6)，根据步骤(5-5)计算的算法方程参数，构建算法方程Aδ＝D+Q，求解未知的误差增量参数δ

δ＝[A^TQ^-1A]^-1A^TQ^-1D

其中，A^T为步骤(5-5)中矩阵A的转置，Q^-1为协方差矩阵Q的逆矩阵，

其中：δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-7)，计算矩阵δ的各项绝对值之和b，计算表达式为：

b＝|δ_x|+|δ_y|+|δ_z|

步骤(5-8)，如果b≥u，则更新算法初值(x_v,y_v,z_v)，并返回步骤(5-3)，如果b＜u，则输出最后一次更新坐标记为s，(x_v,y_v,z_v)更新表达式为：

x_v＝x_v+δ_x

y_v＝y_v+δ_y

z_v＝z_v+δ_z

其中，x_v，y_v，z_v为步骤(5-1)计算值，δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-9)，将步骤(5-8)计算的待定位飞行器坐标s转化为大地坐标系下经度λ′计算表达式为：

其中：s_x为步骤(5-8)中的计算矩阵s的第一项值，s_y为步骤(5-8)中的计算矩阵s的第二项值；

步骤(5-10)，根据步骤(5-8)计算的待定位飞行器坐标s，采用迭代算法将其转化为大地坐标系下的纬度L′和高度H′，令迭代次数t＝1，初始L′＝0.1；

步骤(5-11)，根据步骤(5-10)的初始L′，计算高度H′和纬度L′的不断修正值，并且迭代次数自加1，表达式如下：

t＝t+1

其中，s＝(s_x,s_y,s_z)^T，s_z为步骤(5-8)计算的矩阵s的第三项值，N为地球半径，e为地球偏心率；

步骤(5-12)，根据步骤(5-11)的迭代次数t的值，比较t与10的大小关系，如果t≤10，则执行步骤(5-11)，如果t＞10，执行步骤(5-13)；

步骤(5-13)，获取最后的经度λ′，纬度L′和高度H′。

步骤(6)，根据飞行器位置协同解算算法求解结果，利用卡尔曼滤波对待定位飞行器机载平台导航系统的经度、纬度和高度参数进行修正，包括如下几个步骤：

步骤(6-1)，根据步骤(5-13)计算的经度λ′，纬度L′和高度H′数据，建立卡尔曼滤波量测方程，计算表达式为：

其中，L_t为待定位飞行器机载惯性导航系统的输出的纬度，λ_t为待定位飞行器机载惯性导航系统的输出的纬度，H_t为待定位飞行器机载惯性导航系统的输出的高度数据，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，ε_L为解算算法在经度方向上误差，ε_λ为解算算法在纬度方向上误差，ε_H为解算算法在高度方向上误差；Z_c为量测方程，V_c为量测噪声；

步骤(6-2)，根据步骤(6-1)所计算的卡尔曼滤波量测方程，结合待定位飞行器惯性导航系统的状态方程，经卡尔曼滤波程序后，输出待定位飞行器机载惯性导航系统经度，纬度和高度数据修正值(λ_t′,L_t′,H_t′)，λ_t′为经度修正值，L_t′为纬度修正值，H_t′为高度修正值。

步骤(7)，重复上述算法步骤，依次对其他飞行器进行定位修正，所述步骤(7)包括如下几个步骤：

步骤(7-1)，令f＝f-1，其中f为待定位飞行器的编号；

步骤(7-2)，根据步骤(1-1)的数据，比较f与4的大小，若f≤4，则所有需要修正定位精度的飞行器已经全部修正，若f＞4，继续执行步骤(7-3)；

步骤(7-3)，集群中各飞行器依靠自身机载导航设备继续进行下一时刻导航解算，然后跳转至步骤(1-2)。

Claims (5)

1.一种用于差异定位性能条件的飞行器集群协同导航方法，其特征在于，包括以下步骤：

步骤(1)，获取飞行器集群协同定位所需的基准飞行器的经度、纬度和高度数据以及待定位飞行器与主基准飞行器和副基准飞行器之间的距离差数据；

步骤(2)，计算基准飞行器机载平台导航系统输出的经度、纬度和高度导航参数转化为地球坐标系下的坐标；

步骤(3)，构建待定位飞行器的到达时间差方程；包括如下具体步骤：

步骤(3-1)，根据步骤(2)中所获得的待定位飞行器和基准飞行器的坐标以及步骤(1-8)中获得的距离差数据，建立待定位飞行器的到达时间差方程，表达式如下所示：

其中(x,y,z)为待定位飞行器的三维坐标，是待求变量，其中x为待定位飞行器在地球坐标系下的横坐标，y为待定位飞行器在地球坐标系下的纵坐标，z为待定位飞行器在地球坐标系下的竖坐标，根据步骤(2-1)，步骤(2-2)，步骤(2-3)的计算结果，其中x_i为地球坐标系下的副基准飞行器横坐标，y_i为地球坐标系下的副基准飞行器纵坐标，z_i为地球坐标系下的副基准飞行器竖坐标，i＝2,3…N为副基准飞行器的编号，根据步骤(1-8)计算结果，R_2m,R_3m,R_4m…R_im为待定位飞行器到副基准飞行器与主基准飞行器的距离差，其中i为副基准飞行器的编号，x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(3-2)，将步骤(3-1)建立的到达时间差方程转换成矩阵形式并化简，表达式为：

化简为：

其中X_i,m＝x_i-x_m，Y_i,m＝y_i-y_m，Z_i,m＝z_i-z_m，K_i＝x_i ²+y_i ²+z_i ²，i＝2,3…N,K_m＝x_m ²+y_m ²+z_m ²；

步骤(4)，利用飞行器位置协同解算算法求解到达时间差方程，得到待定位飞行器坐标；

步骤(5)，利用飞行器位置误差迭代算法对飞行器位置协同解算算法的求解结果进行优化；

步骤(6)，根据步骤(5)优化结果，利用卡尔曼滤波对待定位飞行器机载平台导航系统的经度、纬度和高度参数进行修正，得到待定位飞行器最终定位结果；

步骤(7)，重复上述步骤，依次对定位精度较差的飞行器进行修正；

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

步骤(1-1)，确定所有参与集群飞行器的数量为M，若M≥5，则执行后续步骤，若M＜5，算法无法使用，则继续等待直至集群飞行器数量M≥5，并对所有飞行器进行编号，获取所有飞行器的机载惯性导航设备的定位误差E_j，其中j为飞行器编号，j＝1,2,3…M，表示含义为第j号飞行器，将待定位飞行器编号设定为变量f，其中初始设定f＝M，需要进行定位修正的飞行器数量变量设定为d，令d＝M-4；

步骤(1-2)，根据步骤(1-1)获取的所有飞行器的机载惯性导航设备定位误差，按照定位误差大小对所有飞行器进行排序，确定排序外循环次数变量n₁，其中n₁＝1，内循环次数变量n₂，其中n₂＝M-1，设定飞行器编号初始值i＝1；

步骤(1-3)，比较E_i和E_i+1的大小，如果E_i≥E_i+1，则编号为i的飞行器编号变为i+1，如果E_i＜E_i+1，则飞行器编号不变，并且i＝i+1，进行步骤(1-4)；

步骤(1-4)，比较i和n₂的大小，如果i≤n₂，则重复步骤(1-3)，如果i＞n₂，则n₁＝n₁+1并继续步骤(1-5)；

步骤(1-5)，比较n₁和n₂的大小，如果n₁≤n₂，则重复步骤(1-3)，如果n₁＞n₂，则所有飞行器已经重新排序和编号，飞行器编号越大，则机载设备定位误差越大；

步骤(1-6)，将编号为f的飞行器定为待定位飞行器，其余飞行器作为基准飞行器确定基准飞行器的数量N，其中编号为m的飞行器作为主基准飞行器，其中m＝1，其余基准飞行器作为副基准飞行器；

步骤(1-7)，基准飞行器通过自身机载平台上的导航系统获得基准飞行器所在位置的经度、纬度和高度数据；

步骤(1-8)，飞行器通过自身携带的相对测距传感器获得待定位飞行器到主基准飞行器与到副基准飞行器的距离差R_im，其计算公式如下：

R_im＝R_i-R_m＝c*t_im

其中，R_i为待定位飞行器到副基准飞行器的距离，i＝2,3…N，i为副基准飞行器的编号，R_m为待定位飞行器到主基准飞行器的距离，t_im为不同基准飞行器上的传感器测得的待定位飞行器发出的信号差，c为信号传播速度；

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

步骤(2-1)，根据步骤(1-7)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标x_i，其表达式为：

x_i＝(N+H)cosLcosλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-2)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标y_i，其表达式为：

y_i＝(N+H)cosLsinλ

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，L为纬度，λ为经度，N为地球半径，H为高度；

步骤(2-3)，根据步骤(1-1)获得的基准飞行器的经度，纬度，高度数据，计算地球坐标系下的坐标z_i，其表达式为：

z_i＝[N(1-e²)+H]sinL

其中，i＝m为主基准飞行器坐标，i＝2,3…N为副基准飞行器坐标，e为椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

N的计算表达式如下所示：

2.根据权利要求1所述的一种用于差异定位性能条件的飞行器集群协同导航方法，其特征在于，利用飞行器位置协同解算算法求解待定位飞行器的坐标，所述步骤(4)包括如下具体步骤：

步骤(4-1)，根据步骤(3-2)所建立的到达时间差方程，获取所需的方程参数h，G_a，Q，计算表达式为：

Q＝diag{σ_2,m,σ_3,m…σ_N,m}

其中，i＝2,3…N，其中，σ_i,m为高斯误差函数的标准差；

步骤(4-2)，令z_a＝[z_p ^t,R₁]为待求矢量，其中z_p＝[x,y,z]^T，R₁为待定位飞行器到主基准飞行器的距离；进行两次迭代算法计算误差矢量协方差矩阵ψ，设定迭代次数参数为n＝1，设定参数矩阵C＝Q；

步骤(4-3)，根据步骤(4-1)的参数计算结果计算z_a的估计值，表达式为：

z_a＝(G_a ^TC^-1G_a)^-1(G_a ^TC^-1h)；

其中，G_a ^T为步骤(4-1)计算结果G_a的转置矩阵，C^-1为步骤(4-2)计算结果C的逆矩阵；

步骤(4-4)，根据步骤(4-3)计算的z_a的估计值，计算z_a和各个副基准飞行器之间的距离V_ai，计算表达式为：

其中，z_a,1，z_a,2，z_a,3为z_a的前三项计算值，根据步骤(3-1)确定的结果，x_i为地球坐标系下的副基准飞行器的横坐标，y_i为地球坐标系下的副基准飞行器的纵坐标，z_i为地球坐标系下的副基准飞行器的竖坐标，i为副基准飞行器的编号；

步骤(4-5)，根据步骤(4-4)计算的V_ai的值，将其改为对角矩阵形式为B_a，其表达式为：

其中，V_a2，V_a3…V_aN分别为步骤(4-4)计算的V_ai中编号i取不同的值得计算结果，i＝2,3…N；

步骤(4-6)，根据步骤(4-1)中计算的Q和步骤(4-5)中计算的B_a，计算矩阵ψ_a，表达式为：

ψ_a＝B_a*Q*B_a；

步骤(4-7)，算法循环次数n加1，即：

n＝n+1；

步骤(4-8)，比较循环次数n和2的大小，如果n≤2，则执行步骤(4-3)，并且另参数矩阵C为步骤(4-6)计算的矩阵ψ_a，如果n＞2则执行步骤(4-9)；

步骤(4-9)，根据步骤(4-6)计算的ψ_a，计算z_a的误差协方差矩阵z_acov，表达式为：

z_acov＝(G_a ^Tψ_a ^-1G_a)^-1

其中，G_a ^T为矩阵G_a的转置，ψ_a ^-1为矩阵ψ_a的逆矩阵；

步骤(4-10)，构建新的方程参数h′、G′_a、z′_a，表达式为：

其中，z_a,1，z_a,2，z_a,3，z_a,4为z_a的前四项计算值，x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(4-11)，根据步骤(4-3)计算的z_a，构建矩阵B′，计算表达式为：

其中x_m为地球坐标系下的主基准飞行器横坐标，y_m为地球坐标系下的主基准飞行器纵坐标，z_m为地球坐标系下的主基准飞行器竖坐标；

步骤(4-12)，根据步骤(4-11)计算的B′和步骤(4-9)所计算的z_a的误差协方差矩阵z_acov，计算新的误差矢量ψ′，表达式为：

ψ′＝4*B′*z_acov*B′；

步骤(4-13)，根据步骤(4-10)计算的参数G_a′，h′和步骤(4-12)计算的误差矢量ψ′，得到新的坐标z_a′，表达式为：

z_a′＝(G_a′^Tψ′^-1G_a′)^-1(G_a′^Tψ′^-1h′)

其中，G_a′^T为矩阵G_a′的转置矩阵，ψ′^-1为矩阵ψ′的逆矩阵；

步骤(4-14)，根据步骤(4-13)计算的z_a′，得到待定位目标飞行器的最终解算坐标z_p，计算表达式为：

或

其中z_p1，z_p2为z_p的两个不同解算值；

步骤(4-15)，根据步骤(4-14)计算的z_p1和z_p2，分别计算其与计算结果z_a的距离d₁,d₂，并计算d₁,d₂之差为Δd计算表达式为：

Δd＝d₁-d₂

其中，z_pi,1，z_pi,2，z_pi,3为解算值矩阵z_pi的前三项值i＝1,2，z_a,1为步骤(4-3)求解结果矩阵z_a的第一项求解值，z_a,2为步骤(4-3)求解结果矩阵z_a的第二项求解值，z_a,3为步骤(4-3)求解结果矩阵的第三项求解值；

步骤(4-16)，根据步骤(4-15)计算的Δd的值，如果Δd≥0，则解算算法求解结果z_p＝z_p2，如果Δd＜0，则z_p＝z_p1。

3.根据权利要求2所述的一种用于差异定位性能条件的飞行器集群协同导航方法，其特征在于，在步骤(5)执行飞行器位置误差迭代算法，所述步骤(5)包括如下具体步骤：

步骤(5-1)，根据步骤(4-16)飞行器位置协同解算算法的计算结果z_p，将其作为初值(x_v,y_v,z_v)，开始执行飞行器位置误差迭代算法，x_v为z_p的第一项值，y_v为z_p的第二项值，z_v为z_p的第三项值；

步骤(5-2)，确定飞行器位置误差迭代算法的门限值u；

步骤(5-3)，建立飞行器位置误差迭代算法的函数表达式f_i(x,y,z)，为：

其中，根据步骤(3-1)确定的基准飞行器的坐标，x_i+1为地球坐标系下的基准飞行器的横坐标，y_i+1为地球坐标系下的基准飞行器的纵坐标，z_i+1为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N-1，x为地球坐标系下的待定位飞行器的横坐标，y为地球坐标系下的待定位飞行器的纵坐标，z地球坐标系下的待定位飞行器的竖坐标；

步骤(5-4)，根据步骤(5-1)的初始值，以及步骤(1-1)和步骤(1-2)的基准飞行器的坐标和待定位飞行器和基准飞行器的距离差，计算飞行器位置误差迭代算法的部分参数，表达式为：

其中，(x_v,y_v,z_v)为初始坐标，x_v为矩阵z_p的第一项值，y_v为矩阵z_p的第二项值，z_v为矩阵z_p的第三项值，(x_i,y_i,z_i)为副基准飞行器的坐标，x_i为地球坐标系下的基准飞行器的横坐标，y_i为地球坐标系下的基准飞行器的纵坐标，z_i为地球坐标系下的基准飞行器的竖坐标，i＝1,2…N，f_i,v待入初值后的待定位飞行器坐标和主副基准飞行器之间的距离差值，f_i(x_v,y_v,z_v)为步骤(5-3)中的函数带入设定初值后的函数值，α_i,1为函数f_i,v在X轴方向上的增量，α_i,2为函数f_i,v在Y轴方向上的增量，α_i,3为函数f_i,v在Z轴方向上的增量，

为待定位飞行器坐标值和基准飞行器之间的距离，i为基准飞行器的编号，

为

中的变量i取值为1时，

为基准飞行器编号i取值为i+1；

步骤(5-5)，根据步骤(5-4)确定的算法参数和步骤(4-1)确定的到达时间差协方差矩阵Q，求解算法矩阵表达式参数A、D，计算表达式为：

其中，

α_1,1为步骤(5-4)中的i取1时的第一列计算参数值，α_1,2为步骤(5-4)中的i取1时的第二列公式计算的参数值，α_1,3为步骤(5-4)中的i取1时的第三列公式计算值，α_2,1为步骤(5-4)中的i取2时的第一列计算参数值，α_2,2为步骤(5-4)中的i取2时的第二列公式计算的参数值，α_2,3为步骤(5-4)中的i取2时的第三列公式计算值，α_N-1,1为步骤(5-4)中的i取N-1时的第一列计算参数值，α_N-1,2为步骤(5-4)中的i取N-1时的第二列公式计算的参数值，α_N-1,3为步骤(5-4)中的i取N-1时的第三列公式计算值，f_1,v为步骤(5-4)中的i取1时的计算值，f_2,v为步骤(5-4)中的i取2时的计算值，f_N-1,v为步骤(5-4)中的i取N-1时的计算值；

步骤(5-6)，根据步骤(5-5)计算的算法方程参数，构建算法方程Aδ＝D+Q，求解未知的误差增量参数δ

δ＝[A^TQ^-1A]^-1A^TQ^-1D

其中，A^T为步骤(5-5)中矩阵A的转置，Q^-1为协方差矩阵Q的逆矩阵，

其中：δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-7)，计算矩阵δ的各项绝对值之和b，计算表达式为：

b＝|δ_x|+|δ_y|+|δ_z|

步骤(5-8)，根据步骤(5-2)确定的误差迭代算法门限值u，如果b≥u，则更新算法初值(x_v,y_v,z_v)，并返回步骤(5-3)，如果b＜u，则输出最后一次更新坐标记为s，(x_v,y_v,z_v)更新表达式为：

x_v＝x_v+δ_x

y_v＝y_v+δ_y

z_v＝z_v+δ_z

其中，x_v，y_v，z_v为步骤(5-1)计算值，δ_x为地球坐标系下的X轴方向误差增量，δ_y为地球坐标系下的Y轴方向误差增量，δ_z为地球坐标系下的Z轴方向误差增量；

步骤(5-9)，将步骤(5-8)计算的待定位飞行器坐标s转化为大地坐标系下经度λ′计算表达式为：

其中，s_x为步骤(5-8)中的计算矩阵s的第一项值，s_y为步骤(5-8)中的计算矩阵s的第二项值；

步骤(5-10)，根据步骤(5-8)计算的待定位飞行器坐标s，采用飞行器位置误差迭代算法将其转化为大地坐标系下的纬度L′和高度H′，令迭代次数t＝1，初始L′＝0.1；

步骤(5-11)，根据步骤(5-10)的初始L′，计算高度H′和纬度L′的不断修正值，并且迭代次数自加1，表达式如下：

t＝t+1

其中，s＝(s_x,s_y,s_z)^T，s_z为步骤(5-8)计算的矩阵s的第三项值，N为地球半径，e为地球偏心率；

步骤(5-12)，根据步骤(5-11)的迭代次数t的值，比较t与10的大小关系，如果t≤10，则执行步骤(5-11)，如果t＞10，执行步骤(5-13)；

步骤(5-13)，获取最后的经度λ′，纬度L′和高度H′，终止飞行器位置误差迭代算法。

4.根据权利要求3所述的一种用于差异定位性能条件的飞行器集群协同导航方法，其特征在于，所述步骤(6)包括如下几个步骤：

步骤(6-1)，根据步骤(5-13)计算的经度λ′，纬度L′和高度H′数据，建立卡尔曼滤波量测方程，计算表达式为：

其中，L_t为待定位飞行器机载惯性导航系统的输出的纬度，λ_t为待定位飞行器机载惯性导航系统的输出的纬度，H_t为待定位飞行器机载惯性导航系统的输出的高度数据，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，ε_L为解算算法在经度方向上误差，ε_λ为解算算法在纬度方向上误差，ε_H为解算算法在高度方向上误差；Z_c为量测方程，V_c为量测噪声；

步骤(6-2)，根据步骤(6-1)所计算的卡尔曼滤波量测方程，结合待定位飞行器惯性导航系统的状态方程，经卡尔曼滤波程序后，输出待定位飞行器机载惯性导航系统经度，纬度和高度数据修正值(λ_t′,L_t′,H_t′)，λ_t′为经度修正值，L_t′为纬度修正值，H_t′为高度修正值。

5.根据权利要求4所述的一种用于差异定位性能条件的飞行器集群协同导航方法，其特征在于，所述步骤(7)包括如下几个步骤：

步骤(7-1)，令f＝f-1，其中f为待定位飞行器的编号；

步骤(7-2)，根据步骤(1-1)的待定位飞行器编号f，比较f与4的大小，若f≤4，则所有需要修正定位精度的飞行器已经全部修正，若f＞4，继续执行步骤(7-3)；

步骤(7-3)，集群中各飞行器依靠自身机载导航设备继续进行下一时刻导航解算，然后跳转至步骤(1-2)。

Images