CN113670307B - 基于角度混合定位精度因子的无人集群协同导航方法 - Google Patents

Abstract

本发明公开了一种基于角度混合定位精度因子的无人集群协同导航方法，包括以下步骤：判断是否需要进行观测机筛选；获取观测无人平台筛选所需的数据；设置观测无人平台筛选所需的参数；选取一对理想的观测无人平台；选取进行后续观测无人平台。根据待定位无人平台自身导航系统测量的空间位置以及可见星位置，预先解算出具有期望定位精度表示(ROPP)的观测无人平台位置范围，在此范围内进行观测无人平台的筛选或使观测无人平台机动至该范围。或者根据观测无人平台与待定位无人平台自身导航系统测量的空间位置以及可见星位置，计算定位精度表示(ROPP)，逐步筛选出能达到满足需求的定位精度表示(ROPP)的观测无人平台。

Description

基于角度混合定位精度因子的无人集群协同导航方法

技术领域

本发明涉及一种基于角度混合定位精度因子的无人集群协同导航方法，属于定位与导航技术领域。

背景技术

近年来，无人集群协同技术受到国内外越来越多的关注。无人集群具有工作范围大、可靠性高、可同时执行多重任务、整体效率高等优点，可用于灾害勘测和救援等，在军事民用领域都有着广阔的前景。

高精度的协同导航技术极大地提高了集群整体的稳定性、安全性和可靠性，为无人集群技术在编队导航、目标监测与跟踪等诸多方面的应用提供了支撑和保障，是实现无人集群协同技术的关键核心。传统的基于相对距离测量的集群协同导航方式对精密授时的依赖性强，还受到基准飞行器数量的限制。基于角度测量的定位导航方式不依赖精密授时，仅需两基准飞行器即可实现导航定位。而且可见光、红外相机等被动测角设备，与激光雷达等测距设备相比，具有重量轻、功耗低的明显优势，方便飞行器搭载。为了弥补传统协同导航方式在无人机集群作战应用中的不足，本发明方法基于角度协同定位的优势，提出了一种基于角度混合定位精度因子的无人集群协同导航方法。由于单个被动传感器仅能获得目标信号的到达角度与到达时间，为了获得目标的位置信息，通常需要多个传感器协同工作，可以组成无人集群被动协同导航系统，用以实现多无人平台协同导航。其协同定位的效果与集群构型分布相关，多无人平台之间形成的几何构型对相对导航定位的精度影响显著，需要对无人集群进行合理配置，才能获得高的协同导航精度。本发明方法以几何精度因子为指标实时分析基准飞行器筛选部分的有效性，以定位误差为指标对比验证了所提出导航方法的定位精度可用性。

发明内容

本发明的目的是提供一种基于角度混合定位精度因子的无人集群协同导航方法，以扩大集群无人平台定位算法的适用性并提升定位精度，使其在非规则的编组的无人集群中也能得到良好的运用。

为实现上述目的，本发明采用如下技术方案：

一种基于角度混合定位精度因子的无人集群协同导航方法，所述无人集群由多架无人平台组成，包含观测无人平台和待定位无人平台；

所述方法包括以下步骤：

步骤(1)，判断是否需要进行观测机筛选；

步骤(2)，获取观测无人平台筛选所需的数据；

步骤(3)，设置观测无人平台筛选所需的参数；

步骤(4)，选取一对理想的观测无人平台；

步骤(5)，选取进行后续观测无人平台。

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

步骤(1-1)，获取当前时刻相对于待定位无人平台的可见星数l；

步骤(1-2)，判断是否l≥4，若是，执行步骤(1-3)；否则执行步骤(2)；

步骤(1-3)，无需进行观测无人平台的筛选，直接通过卫星导航定位系统对待定位无人平台进行定位。

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

步骤(2-1)，通过无线电装置获取待定位无人平台与其之间没有信号遮挡的观测无人平台总数n；

步骤(2-2)，待定位无人平台target和观测无人平台i通过惯性导航系统测量所在位置的信息，包括：待定位无人平台target纬度λ_target，待定位无人平台经度L_target，待定位无人平台高度H_target，观测无人平台纬度λ_i，观测无人平台经度L_i，观测无人平台高度H_i，其中1≤i≤n；

步骤(2-3)，根据步骤(2-2)获取的待定位无人平台target和观测无人平台i的位置信息，将其位置坐标转换为大地直角坐标系，其中X轴在赤道平面内与零度子午线相交，Z轴沿地球自转方向，Y轴与X轴、Z轴构成右手直角坐标系，表达式如下所示：

其中，R_target和R_i分别为待定位无人平台和观测无人平台对应的地球半径，e为地球椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

R_target和R_i的计算表达式如下所示：

步骤(2-4)，经过上述步骤(2-2)、(2-3)，获取待定位无人平台和观测无人平台的大地直角坐标系坐标，分别为

步骤(2-5)，在大地直角坐标系中，获取观测无人平台与待定位无人平台的相对距离r_i和相对距离在大地直角坐标系XOY平面上的投影r_hi，其表达式为：

步骤(2-6)，使用星历参数求解导航定位过程中可见星在大地直角坐标系中的坐标位置(x_sj,y_sj,z_sj),j＝1,2,…,l，其中l为可见星总数；

步骤(2-7)，根据步骤(2-6)获取的可见星在大地直角坐标系中的坐标位置计算各可见星与待定位无人平台的相对距离，表达式如下所示：

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

步骤(3-1)，依据无人集群所搭载的计算机的计算能力，设置在无人集群中最大遍历无人平台数为N；

步骤(3-2)，通过设定期望的定位误差方差设置定位精度表示的下降阈值ΔROPP，定位精度表示与定位误差方差之间的关系，由下式表示：

其中，δx²、δy²、δz²分别为大地直角坐标系中，待定位无人平台在大地直角坐标系X轴、Y轴、Z轴方向上的定位误差方差，为等效时钟误差相应的距离平方，δx²、δy²、δz²、由以下公式计算：

其中，G_u为观测系数矩阵，为G_u的转置矩阵，δY_u为传感器测量误差向量，cov(δY_u)为δY_u协方差矩阵，G_u和cov(δY_u)的计算公式分别如下表示：

其中，为伪距测量误差，i＝1,2,…,l；为方位角测量误差方差，为俯仰角测量误差方差，i＝1,2,…,m；l为可见星的数量，m为筛选后得到的观测无人平台的数量。

所述步骤(4)有以下两种技术方案：

方案1的步骤为：

步骤(4a-1)，设置采样角度为Δα；

步骤(4a-2)，设置β＝90°；

步骤(4a-3)，根据待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4a-4)，寻找是否有两观测机分别到待定位无人平台的方向向量之间的夹角为β，若是，执行步骤(4a-5)；否则执行步骤(4a-7)；

步骤(4a-5)，判断寻找到的观测无人平台对数是否<2，若是，执行步骤(4a-6)；否则执行步骤(4a-8)；

步骤(4a-6)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取；

步骤(4a-7)，β＝β±Δα，执行步骤(4a-3)；

步骤(4a-8)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台，执行步骤(4a-6)。

方案2的步骤为：

步骤(4b-1)，设置观测无人平台分别到待定位无人平台的方向向量之间的最佳夹角裕度范围为β，β＝70°-90°；

步骤(4b-2)，根据待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4b-3)，寻找各对分别到待定位无人平台的方向向量之间的夹角在裕度范围β内的观测机；

步骤(4b-4)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台；

步骤(4b-5)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取。

所述步骤(5)有以下两种技术方案：

方案1的步骤为：

步骤(5a-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5a-2)，依据集群无人平台所搭载的计算机的计算能力，设置遍历时采样距离Δd，采样角度Δα，遍历最远距离dmax；

步骤(5a-3)，判断是否n＞N，若是，执行步骤(5a-4)；否则，执行步骤(5a-10)；

步骤(5a-4)，根据采样参数，遍历以待定位无人平台target为圆心的空间位置，获取若第i_g架观测无人平台在空间位置k时的定位精度表示，记作ROPP(i_gk)，其中，k＝1,2,…k_max，k_max为总采样次数，ROPP(i_gk)的表达式为：

其中，为第i_g架观测无人平台在空间位置k时对应的观测系数矩阵，cov(δY_u(k))为第i_g架观测无人平台在空间位置k时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表在执行步骤(5a-3)前已经筛选好的观测无人平台，ki_g代表进行遍历时在第k个空间位置的第i_g架观测无人平台；

步骤(5a-5)，判断遍历得到的ROPP(i_gk)中是否存在ROPP(i_gp)使ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，若存在，执行步骤(5a-6)，否则执行步骤(5a-17)；其中，p＝1,2,…,p_max，p_max为使上式成立的第i_g架观测无人平台可在的空间位置总数；

步骤(5a-6)，判断在未被筛选出的n+1-i_g架观测无人平台中是否有在步骤(5a-5)的ROPP(i_gp)相应位置范围内的无人平台，若有，执行步骤(5a-7)，否则，执行步骤(5a-15)；

步骤(5a-7)，ROPP(i_gp)中最小的几何精度因子记作ROPP(i_g)；

步骤(5a-8)，选择ROPP(i_g)对应的观测无人平台作第i_g架观测机；

步骤(5a-9)，i_g＝i_g+1,执行步骤(5a-3)；

步骤(5a-10)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示，定位精度表示ROPP(i_gk)的表达式为：

步骤(5a-11)，在步骤(5a-10)获取的个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5a-12)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5a-13)，否则执行步骤(5a-17)；

步骤(5a-13)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5a-14)，i_g＝i_g+1,执行步骤(5a-10)；

步骤(5a-15)，判断能否使n+1-i_g架观测无人平台中某一架机动到某一位置，以满足ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，如果能，执行步骤(5a-16)；否则执行步骤(5a-17)；

步骤(5a-16)，机动观测无人平台至该合适位置，此时的定位精度表示记为ROPP(i_g)，并执行步骤(5a-8)；

步骤(5a-17)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5a-18)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。

方案2的步骤为：

步骤(5b-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5b-2)，判断是否n＞N，若是，执行步骤(5b-3)；否则，执行步骤(5b-8)；

步骤(5b-3)，获取若未被筛选出的n+1-i_g架观测无人平台分别作第i_g架观测无人平台的定位精度表示；记作ROPP(i_gk)，其中，ROPP(i_gk)的表达式为：

其中，表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的观测系数矩阵，cov(δY_u(k))表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表执行步骤(5b-3)前已经筛选好的观测无人平台，ki_g代表在剩余观测无人平台中进行筛选时选取的第k个第i_g架观测无人平台；

步骤(5b-4)，寻找步骤(5b-3)获取的中最小定位精度表示记作ROPP(i_g)，；

步骤(5b-5)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP；若是，执行步骤(5b-6)，否则执行步骤(5b-13)；

步骤(5b-6)，选择ROPP(i_g)相应的观测无人平台作第i_g架观测无人平台；

步骤(5b-7)，i_g＝i_g+1,执行步骤(5b-3)；

步骤(5b-8)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示；

步骤(5b-9)，在个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5b-10)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5b-11)，否则执行步骤(5b-13)；

步骤(5b-11)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5b-12)，i_g＝i_g+1,执行步骤(5b-8)；

步骤(5b-13)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5b-14)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。

有益效果：本发明是一种基于角度混合定位精度因子的无人集群协同导航方法，适用于基于集群无人平台测角协同辅助增强卫星导航的观测无人平台筛选，基于测角协同定位提出两种观测无人平台筛选方案，方案1根据待定位无人平台自身导航系统测量的空间位置以及可见星位置，预先解算出具有期望定位精度表示(ROPP)的观测无人平台位置范围，在此范围内进行观测无人平台的筛选或使观测无人平台机动至该范围；方案2根据观测无人平台与待定位无人平台自身导航系统测量的空间位置以及可见星位置，计算定位精度表示(ROPP)，逐步筛选出能达到满足需求的定位精度表示(ROPP)的观测无人平台；考虑到观测无人平台的构型对定位结果的影响，综合利用集群定位信息优化提高了定位精度。和未采用优化的集群无人平台定位算法相比，本发明在不规则分布的集群中也能取得较好的定位精度，适合实际应用。

附图说明

图1为实施例1的示意图；

图2为实施例1的流程图；

图3为实施例1中步骤(5)的流程图；

图4为实施例2中步骤(5)的流程图；

图5低精度无人平台不同方法修正效果示意图。

具体实施方式

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

本发明方法通过对集群中观测无人平台构型进行优化，使得筛选出的观测无人平台提供的分布化测量参数更趋于精确合理，从而综合利用集群定位信息优化提高定位精度，同时增强集群无人平台定位算法对于多种集群编组情况的适应性。

本发明中所述的无人平台例如无人机。

根据下述实施例，可以更好的理解本发明。

实施例1

如图1所示，一种基于角度混合定位精度因子的无人集群协同导航方法，包括以下步骤：

步骤(1)，由多架无人平台组成无人集群，包含观测无人平台和待定位无人平台；根据自身导航系统测量的空间中观测无人平台与待定位无人平台的位置以及可见星位置，计算定位精度表示，逐步筛选出能达到期望定位精度表示的观测无人平台，包括如下具体步骤：

步骤(1-1)，获取当前时刻相对于待定位无人平台的可见星数l；

步骤(1-2)，判断是否l≥4，若是，执行步骤(1-3)；否则执行步骤(2)；

步骤(1-3)，无需进行观测无人平台的筛选，直接通过卫星导航定位系统对待定位无人平台进行定位。

步骤(2)，获取观测无人平台筛选所需的相关数据，包括如下具体步骤：

步骤(2-1)，通过无线电装置或其他手段获取待定位无人平台与其之间没有信号遮挡的观测无人平台总数n；

步骤(2-2)，待定位无人平台target和观测无人平台i通过惯性导航系统测量所在位置的纬度、经度和高度，待定位无人平台target纬度λ_target，待定位无人平台经度L_target，待定位无人平台高度H_target，观测无人平台纬度λ_i，观测无人平台经度L_i，观测无人平台高度H_i，其中1≤i≤n；

步骤(2-3)，根据步骤(2-2)获取的待定位无人平台target和观测无人平台i的位置信息，将其位置坐标转换为大地直角坐标系，其中X轴在赤道平面内与零度子午线相交，Z轴沿地球自转方向，Y轴与X轴、Z轴构成右手直角坐标系，表达式如下所示：

其中，R_target和R_i分别为待定位无人平台和观测无人平台对应的地球半径，e为地球椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

R_target和R_i的计算表达式如下所示：

步骤(2-4)，经过上述步骤(2-2)、(2-3)，获取待定位无人平台和观测无人平台的大地直角坐标系坐标，分别为其中1≤i≤n；

步骤(2-5)，在大地直角坐标系中，获取观测无人平台与待定位无人平台的相对距离r_i和相对距离在大地直角坐标系XOY平面上的投影r_hi，其表达式为：

其中，1≤i≤n；

步骤(2-6)，使用星历参数求解导航定位过程中可见星在大地直角坐标系中的坐标位置(x_sj,y_sj,z_sj),j＝1,2,...,l，其中l为可见星总数；

步骤(2-7)，根据步骤(2-6)获取的可见星在大地直角坐标系中的坐标位置计算各可见星与待定位无人平台的相对距离，表达式如下所示：

步骤(3)，设置观测无人平台筛选所需的相关参数，包括如下具体步骤：

步骤(3-1)，依据集群无人平台所搭载的计算机的计算能力，设置在无人集群中最大遍历无人平台数为N；

步骤(3-2)，通过设定期望的定位误差方差设置定位精度表示的下降阈值ΔROPP，定位精度表示与定位误差方差之间的关系由下式表示：

其中，δx²、δy²、δz²分别为大地直角坐标系中，待定位无人平台在大地直角坐标系X轴、Y轴、Z轴方向上的定位误差方差，为等效时钟误差相应的距离平方，δx²、δy²、δz²、由以下公式计算：

其中，G_u为观测系数矩阵，为G_u的转置矩阵，δY_u为传感器测量误差向量，cov(δY_u)为δY_u协方差矩阵，G_u和cov(δY_u)的计算公式分别如下表示：

其中，为伪距测量误差，i＝1,2,…,l；为方位角测量误差方差，为俯仰角测量误差方差，i＝1,2,…,m；l为可见星的数量，m为筛选后得到的观测无人平台的数量。

步骤(4)，选取一对理想的观测无人平台，包括如下具体步骤：

步骤(4a-1)，设置采样角度为Δα；

步骤(4a-2)，设置β＝90°；

步骤(4a-3)，根据步骤(2-4)获取的待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4a-4)，寻找是否有两观测机分别到待定位无人平台的方向向量之间的夹角为β，若是，执行步骤(4a-5)；否则执行步骤(4a-7)；

步骤(4a-5)，判断寻找到的观测无人平台对数是否<2，若是，执行步骤(4a-6)；否则执行步骤(4a-8)；

步骤(4a-6)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取；

步骤(4a-7)，β＝β±Δα，执行步骤(4a-3)；

步骤(4a-8)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台，执行步骤(4a-6)。

步骤(5)，选取进行后续观测无人平台，如图3，包括如下具体步骤：

步骤(5a-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5a-2)，依据集群无人平台所搭载的计算机的计算能力，设置遍历时采样距离Δd，采样角度Δα，遍历最远距离dmax；

步骤(5a-3)，判断是否n＞N，若是，执行步骤(5a-4)；否则，执行步骤(5a-10)；

步骤(5a-4)，根据采样参数，遍历以待定位无人平台target为圆心的空间位置，获取若第i_g架观测无人平台在空间位置k时的定位精度表示，记作ROPP(i_gk)，其中，k＝1,2,…k_max，k_max为总采样次数，ROPP(i_gk)的表达式为：

其中，为第i_g架观测无人平台在空间位置k时对应的观测系数矩阵，cov(δY_u(k))为第i_g架观测无人平台在空间位置k时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表在执行步骤(5a-3)前已经筛选好的观测无人平台，ki_g代表进行遍历时在第k个空间位置的第i_g架观测无人平台；

步骤(5a-5)，判断遍历得到的ROPP(i_gk)中是否存在ROPP(i_gp)使ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，若存在，执行步骤(5a-6)，否则执行步骤(5a-17)；其中，p＝1,2,…,p_max，p_max为使上式成立的第i_g架观测无人平台可在的空间位置总数；

步骤(5a-6)，判断在未被筛选出的n+1-i_g架观测无人平台中是否有在步骤(5a-5)的ROPP(i_gp)相应位置范围内的无人平台，若有，执行步骤(5a-7)，否则，执行步骤(5a-15)；

步骤(5a-7)，ROPP(i_gp)中最小的几何精度因子记作ROPP(i_g)

步骤(5a-8)，选择ROPP(i_g)对应的观测无人平台作第i_g架观测机；

步骤(5a-9)，i_g＝i_g+1,执行步骤(5a-3)；

步骤(5a-10)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示，定位精度表示ROPP(i_gk)的表达式为：

其中，和cov(δY_u(k))的表达式分别为：

其中，的表达式为：

其中，k1,k2,...,ki_g代表用于计算第k个每i_g架观测无人平台对应的定位精度表示时的各观测无人平台；

步骤(5a-11)，在步骤(5a-10)获取的个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5a-12)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5a-13)，否则执行步骤(5a-17)；

步骤(5a-13)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5a-14)，i_g＝i_g+1,执行步骤(5a-10)；

步骤(5a-15)，判断能否使n+1-i_g架观测无人平台中某一架机动到某一合适位置，以满足ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，如果能，执行步骤(5a-16)；否则执行步骤(5a-17)；

步骤(5a-16)，机动观测无人平台至该合适位置，此时的定位精度表示记为ROPP(i_g)，并执行步骤(5a-8)；

步骤(5a-17)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5a-18)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。

实施例2

本实施例的步骤(1)-(3)与实施例1相同，步骤(4)和(5)如下：

步骤(4)，选取一对理想的观测无人平台，包括如下具体步骤：

步骤(4b-1)，设置观测无人平台分别到待定位无人平台的方向向量之间的最佳夹角裕度范围为β，β＝70°-90°；

步骤(4b-2)，根据步骤(2-4)获取的待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4b-3)，寻找各对分别到待定位无人平台的方向向量之间的夹角在裕度范围β内的观测机；

步骤(4b-4)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台；

步骤(4b-5)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取。

步骤(5)，选取进行后续观测无人平台，如图4，包括如下具体步骤：

步骤(5b-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5b-2)，判断是否n＞N，若是，执行步骤(5b-3)；否则，执行步骤(5b-8)；

步骤(5b-3)，获取若未被筛选出的n+1-i_g架观测无人平台分别作第i_g架观测无人平台的定位精度表示；记作ROPP(i_gk)，其中，ROPP(i_gk)的表达式为：

其中，表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的观测系数矩阵，cov(δY_u(k))表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表执行步骤(5b-3)前已经筛选好的观测无人平台，ki_g代表在剩余观测无人平台中进行筛选时选取的第k个第i_g架观测无人平台；

步骤(5b-4)，寻找步骤(5b-3)获取的中最小定位精度表示记作ROPP(i_g)，；

步骤(5b-5)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP；若是，执行步骤(5b-6)，否则执行步骤(5b-13)；

步骤(5b-6)，选择ROPP(i_g)相应的观测无人平台作第i_g架观测无人平台；

步骤(5b-7)，i_g＝i_g+1,执行步骤(5a-3)；

步骤(5b-8)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示；

步骤(5b-9)，在个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5b-10)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5b-11)，否则执行步骤(5b-13)；

步骤(5b-11)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5b-12)，i_g＝i_g+1,执行步骤(5b-8)；

步骤(5b-13)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5b-14)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。

为了验证本发明所提出的一种基于角度混合定位精度因子的无人集群协同导航方法的有效性，进行数字仿真分析。仿真中采用的测角传感器的测角噪声标准差为0.05°。图5为在采用本发明方法前后平均定位误差对比曲线。

由图5可以看出，在飞行仿真过程中，本发明提出的基于角度混合定位精度因子的无人集群协同导航方法1和观测机筛选方法2，与未优化观测机筛选相比，集群中无人平台平均定位误差降低，能够有效改善集群条件下的协同定位性能，具有良好的应用价值。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (3)

1.一种基于角度混合定位精度因子的无人集群协同导航方法，其特征在于：所述无人集群由多架无人平台组成，包含观测无人平台和待定位无人平台；

所述方法包括以下步骤：

步骤(1)，判断是否需要进行观测机筛选；

步骤(2)，获取观测无人平台筛选所需的数据；

步骤(3)，设置观测无人平台筛选所需的参数；

步骤(4)，选取一对理想的观测无人平台；

步骤(5)，选取进行后续观测无人平台；

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

步骤(1-1)，获取当前时刻相对于待定位无人平台的可见星数l；

步骤(1-2)，判断是否l≥4，若是，执行步骤(1-3)；否则执行步骤(2)；

步骤(1-3)，无需进行观测无人平台的筛选，直接通过卫星导航定位系统对待定位无人平台进行定位；

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

步骤(2-1)，通过无线电装置获取待定位无人平台与其之间没有信号遮挡的观测无人平台总数n；

步骤(2-2)，待定位无人平台target和观测无人平台i通过惯性导航系统测量所在位置的信息，包括：待定位无人平台target纬度λ_target，待定位无人平台经度L_target，待定位无人平台高度H_target，观测无人平台纬度λ_i，观测无人平台经度L_i，观测无人平台高度H_i，其中1≤i≤n；

步骤(2-3)，根据步骤(2-2)获取的待定位无人平台target和观测无人平台i的位置信息，将其位置坐标转换为大地直角坐标系，其中X轴在赤道平面内与零度子午线相交，Z轴沿地球自转方向，Y轴与X轴、Z轴构成右手直角坐标系，表达式如下所示：

其中，R_target和R_i分别为待定位无人平台和观测无人平台对应的地球半径，e为地球椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

R_target和R_i的计算表达式如下所示：

步骤(2-4)，经过上述步骤(2-2)、(2-3)，获取待定位无人平台和观测无人平台的大地直角坐标系坐标，分别为

步骤(2-5)，在大地直角坐标系中，获取观测无人平台与待定位无人平台的相对距离r_i和相对距离在大地直角坐标系XOY平面上的投影r_hi，其表达式为：

步骤(2-6)，使用星历参数求解导航定位过程中可见星在大地直角坐标系中的坐标位置(x_sj,y_sj,z_sj),j＝1,2,...,l，其中l为可见星总数；

步骤(2-7)，根据步骤(2-6)获取的可见星在大地直角坐标系中的坐标位置计算各可见星与待定位无人平台的相对距离，表达式如下所示：

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

步骤(3-1)，依据无人集群所搭载的计算机的计算能力，设置在无人集群中最大遍历无人平台数为N；

步骤(3-2)，通过设定期望的定位误差方差设置定位精度表示的下降阈值ΔROPP，定位精度表示与定位误差方差之间的关系，由下式表示：

其中，δx²、δy²、δz²分别为大地直角坐标系中，待定位无人平台在大地直角坐标系X轴、Y轴、Z轴方向上的定位误差方差，为等效时钟误差相应的距离平方，δx²、δy²、δz²、由以下公式计算：

其中，G_u为观测系数矩阵，为G_u的转置矩阵，δY_u为传感器测量误差向量，cov(δY_u)为δY_u协方差矩阵，G_u和cov(δY_u)的计算公式分别如下表示：

其中，为伪距测量误差，i＝1,2,…,l；为方位角测量误差方差，为俯仰角测量误差方差，i＝1,2,…,m；l为可见星的数量，m为筛选后得到的观测无人平台的数量；

所述步骤(4)具体包括：

步骤(4a-1)，设置采样角度为Δα；

步骤(4a-2)，设置β＝90°；

步骤(4a-3)，根据待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4a-4)，寻找是否有两观测机分别到待定位无人平台的方向向量之间的夹角为β，若是，执行步骤(4a-5)；否则执行步骤(4a-7)；

步骤(4a-5)，判断寻找到的观测无人平台对数是否<2，若是，执行步骤(4a-6)；否则执行步骤(4a-8)；

步骤(4a-6)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取；

步骤(4a-7)，β＝β±Δα，执行步骤(4a-3)；

步骤(4a-8)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台，执行步骤(4a-6)；

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

步骤(5a-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5a-2)，依据集群无人平台所搭载的计算机的计算能力，设置遍历时采样距离Δd，采样角度Δα，遍历最远距离dmax；

步骤(5a-3)，判断是否n＞N，若是，执行步骤(5a-4)；否则，执行步骤(5a-10)；

步骤(5a-4)，根据采样参数，遍历以待定位无人平台target为圆心的空间位置，获取若第i_g架观测无人平台在空间位置k时的定位精度表示，记作ROPP(i_gk)，其中，k＝1,2,…k_max，k_max为总采样次数，ROPP(igk)的表达式为：

其中，为第i_g架观测无人平台在空间位置k时对应的观测系数矩阵，cov(δY_u(k))为第i_g架观测无人平台在空间位置k时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表在执行步骤(5a-3)前已经筛选好的观测无人平台，ki_g代表进行遍历时在第k个空间位置的第i_g架观测无人平台；

步骤(5a-5)，判断遍历得到的ROPP(i_gk)中是否存在ROPP(i_gp)使ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，若存在，执行步骤(5a-6)，否则执行步骤(5a-17)；其中，p＝1,2,…,p_max，p_max为使上式成立的第i_g架观测无人平台可在的空间位置总数；

步骤(5a-6)，判断在未被筛选出的n+1-i_g架观测无人平台中是否有在步骤(5a-5)的ROPP(i_gp)相应位置范围内的无人平台，若有，执行步骤(5a-7)，否则，执行步骤(5a-15)；

步骤(5a-7)，ROPP(i_gp)中最小的几何精度因子记作ROPP(i_g)；

步骤(5a-8)，选择ROPP(i_g)对应的观测无人平台作第i_g架观测机；

步骤(5a-9)，i_g＝i_g+1,执行步骤(5a-3)；

步骤(5a-10)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示，定位精度表示ROPP(i_gk)的表达式为：

步骤(5a-11)，在步骤(5a-10)获取的个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5a-12)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5a-13)，否则执行步骤(5a-17)；

步骤(5a-13)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5a-14)，i_g＝i_g+1,执行步骤(5a-10)；

步骤(5a-15)，判断能否使n+1-i_g架观测无人平台中某一架机动到某一位置，以满足ROPP(i_g-1)-ROPP(i_gp)＞ΔROPP，如果能，执行步骤(5a-16)；否则执行步骤(5a-17)；

步骤(5a-16)，机动观测无人平台至合适位置，此时的定位精度表示记为ROPP(i_g)，并执行步骤(5a-8)；

步骤(5a-17)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5a-18)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。

2.根据权利要求1所述的基于角度混合定位精度因子的无人集群协同导航方法，其特征在于：所述步骤(4)具体包括：

步骤(4b-1)，设置观测无人平台分别到待定位无人平台的方向向量之间的最佳夹角裕度范围为β，β＝70°-90°；

步骤(4b-2)，根据待定位无人平台和观测无人平台的大地直角坐标系坐标计算每两架观测无人平台分别到待定位无人平台的方向向量之间的夹角 由下式表示：

其中，r_i和r_j分别为两架不同的观测无人平台与待定位无人平台的相对距离，r_ij为该两架观测无人平台的相对距离，r_ij由下式表示：

步骤(4b-3)，寻找各对分别到待定位无人平台的方向向量之间的夹角在裕度范围β内的观测机；

步骤(4b-4)，在筛选出的各对观测无人平台中选取出到待定位无人平台距离之和最小的一对观测无人平台；

步骤(4b-5)，选其作为一对理想的观测无人平台，其对应的定位精度表示记为ROPP(2)，结束一对观测无人平台的选取。

3.根据权利要求1所述的基于角度混合定位精度因子的无人集群协同导航方法，其特征在于：所述步骤(5)具体包括：

步骤(5b-1)，设置i_g＝3，表示进行第i_g架观测无人平台的筛选；

步骤(5b-2)，判断是否n＞N，若是，执行步骤(5b-3)；否则，执行步骤(5b-8)；

步骤(5b-3)，获取若未被筛选出的n+1-i_g架观测无人平台分别作第i_g架观测无人平台的定位精度表示；记作ROPP(i_gk)，其中，ROPP(i_gk)的表达式为：

其中，表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的观测系数矩阵，cov(δY_u(k))表示未被筛选出的观测无人平台中第k架观测无人平台作第i_g架观测无人平台时的传感器测量误差协方差，和cov(δY_u(k))的表达式分别为：

其中，1,2,…(i_g-1)代表执行步骤(5b-3)前已经筛选好的观测无人平台，ki_g代表在剩余观测无人平台中进行筛选时选取的第k个第i_g架观测无人平台；

步骤(5b-4)，寻找步骤(5b-3)获取的中最小定位精度表示记作ROPP(i_g)；

步骤(5b-5)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP；若是，执行步骤(5b-6)，否则执行步骤(5b-13)；

步骤(5b-6)，选择ROPP(i_g)相应的观测无人平台作第i_g架观测无人平台；

步骤(5b-7)，i_g＝i_g+1,执行步骤(5b-3)；

步骤(5b-8)，在n架观测无人平台中每i_g架观测无人平台进行遍历，得到个定位精度表示；

步骤(5b-9)，在个定位精度表示中选出最小的定位精度表示记为ROPP(i_g)；

步骤(5b-10)，判断是否ROPP(i_g-1)-ROPP(i_g)＞ΔROPP，若是，执行步骤(5b-11)，否则执行步骤(5b-13)；

步骤(5b-11)，ROPP(i_g)对应的所有观测无人平台作为i_g架观测无人平台；

步骤(5b-12)，i_g＝i_g+1,执行步骤(5b-8)；

步骤(5b-13)，选取ROPP(i_g-1)所对应的观测无人平台作为最终的观测无人平台；

步骤(5b-14)，结束观测无人平台的筛选，共筛选出i_g-1架观测无人平台。