CN112698664A - 一种用于无人集群协同导航优化的视线扇区动态估计方法 - Google Patents

Abstract

本发明公布了一种用于无人集群协同导航优化的视线扇区动态估计方法，根据无人机集群中各无人机机载导航***的定位精度和无人机的数量，确定定位精度区分阈，并将无人机划分为从机和锚机，并对个从机成员进行视线扇区动态估计并筛选协同成员后建立优化的协同导航模型并设计对应协同导航解算算法，解算出待辅助成员的位置信息，最后通过滤波器修正低精度成员机载导航设备精度。本发明采用协同导航技术利用多飞行器间的传感器信息，实现多飞行器间地信息协同，结合数据融合的方法以提高多飞行器间的定位精度，弥补了导航传感器的误差，并且对可能出现的故障进行识别、隔离和恢复。

Description

一种用于无人集群协同导航优化的视线扇区动态估计方法

技术领域

本发明属于定位与导航技术领域。

背景技术

近年来，飞行器集群飞行越来越受到国际上地关注，所谓飞行器集群飞行，即多架飞行器为适应任务要求而进行的某种队形排列和任务分配的组织模式。飞行器编队飞行能够扩大飞行器搜索侦查范围，具有可执行多重任务，可靠性高，整体效率高，减少飞行阻力等优点。飞行器协同集群飞行模式相比单架无人机具有更多优势以及更广阔的应用前景。采用多无人机协同的作战方式，进一步提高了无人机自主生存能力和整体作战效能，通过集群无人机之间的信息共享，将实战中协调任务、协同搜索、侦查与攻击等协同打击效能得以充分发挥。

因此，高精度的相对导航***为提高集群整体作战效能提供了支撑和保障，是实现无人机集群飞行的关键核心技术。目前，用于相对导航***的传感器主要有惯性测量器件和卫星导航***。由于惯性测量单元存在常值漂移，而卫星导航***信号易受外界干扰，可靠性低，无法满足集群无人机相对导航***对精度的要求。

发明内容

发明目的：为了解决上述现有技术存在的问题，本发明提供了一种用于无人集群协同导航优化的视线扇区动态估计方法。

技术方案：本方买那个提供了一种用于无人集群协同导航优化的视线扇区动态估计方法，具体包括如下步骤：

步骤1：根据无人机集群中各无人机机载导航***的定位精度和无人机的数量，确定定位精度区分阈值τ，将机载导航***的精度大于等于τ的无人机设置为锚机，将机载导航***的精度小于τ的无人机设置为从机；

步骤2：获取第ic个从机所能探测到的所有锚机，该所有锚机的总个数为n₁，ic＝1,2,…n_c，n_c为从机的总个数；采用方法1或方法2对第ic个从机进行视线扇区估计，从而得到筛选后的锚机，筛选后的锚机的个数为N_i鐰；所述方法1：基于第ic个从机所在位置与该第ic个从机机载光电平台相对测量空间范围，对第ic个从机的视线扇区进行划分，并分析各从机所属视线扇区的遮蔽关系；所述方法2：基于第ic个从机与n₁个锚机的相对距离，计算n₁个锚机相对第ic个从机视线的相对遮蔽角度范围，并基于该相对遮蔽角度范围对第ic个从机的视线扇区进行动态估计；

位于短距离区域，若第ic个从机与第iz个锚机的相对距离在d_mnD_ic～d_mfD_ic范围内，则认为该第iz个锚机位于中距离区域，若第ic个从机与第iz个锚机的相对距离在d_mfD_ic～D_ic的范围内，则步骤3：根据N_i鐰个锚机与第ic个从机的相对角度信息，建立基于相对角度测量信息的第ic个从机的协同导航模型；

步骤4：根据协同导航模型，计算第ic个从机的位置信息，并基于该位置信息和卡尔曼滤波方法修正从机的机载导航精度。

进一步的，所述步骤2中的方法1具体为：

步骤2-1：计算该第ic个从机与所有锚机的相对距离，以第ic个从机为圆心，设置圆形的短距离区域，环形的中距离区域和环形的远距离区域：若第ic个从机与第iz个锚机的相对距离在0～d_mnD_ic范围内，则认为该第iz个锚机认为该第iz个锚机位于远距离区域；其中d_mn为预设的短距离区域和中距离区域的划分阈值，d_mf为预设的中距离区域和远距离区域的划分阈值，D_ic为第ic个从机与最远锚机之间的相对距离，iz＝1，2，…，n₁；

步骤2-2：以第ic个从机作为球心，根据每个区域对应的角度划分参数，将短距离区域划分为M_ic1个扇形区间，将中距离区域划分为M_ic2个扇形区间，将远距离区域划分为M_ic3个扇形区间；

其中M_ic1＝360/a_ic1；

M_ic2＝360/a_ic2；

M_ic3＝360/a_ic3；

若不能整除，则将余数划分至后一个区间，其中

为短距离区域对应的角度划分参数，

为中距离区域对应的角度划分参数，

为远距离区域对应的角度划分参数；其中，l为n₁个锚机中最长的锚机的长度；

步骤2-3：对获取第I个区域的第i个扇形区间内锚机进行筛选，其中I＝1，2，3，当I＝1时为短距离区域，当I＝2时为中距离区域，当I＝3时为远距离区域，i＝1，2，…，M_ic,I，其中M_ic，I为第I区域中扇形区间的总个数；若I＝1，则转步骤2-5；若I＝2或者I＝3，则转步骤2-4；

步骤2-4：若I＝2，基于第I个区域的第i个扇形区间的角度，判断该角度范围内对应的短距离区域是否存在锚机，若存在，则不对该第I个区域的第i个扇形区间内的主机进行筛选，并转步骤2-6；否则转步骤2-5；若I＝3，基于第I个区域的第i个扇形区间的角度，判断该角度范围内对应的短距离区域和中距离区域是否存在锚机，若存在，则不对该第I个区域的第i个扇形区间内的主机进行筛选，并转步骤2-6；否则转步骤2-5；

步骤2-5：获取第I个区域的第i个扇形区间内锚机的总数量n_I，i，若数量n_I，i＝0，则转步骤2-6，若n_I，i＝1，则标记该锚机，同时获取和记录该锚机的相对俯仰角和相对方位角，并转步骤2-6；若n_I，i≥2，则标记与第ic个从机相对距离最小的锚机，获取和记录该锚机的相对俯仰角和相对方位角，并转步骤2-6；

步骤2-6：i＝i+1，判断i是否大于M_ic，I，若否，则转步骤2-3，若是，则转步骤2-7；

步骤2-7：I＝I+1，判断I是否大于3，若否，则转步骤2-3，若是，则转步骤2-8；

步骤2-8：得到短距离区域、中距离区域和远距离区域内被标记的锚机的个数N_c1,N_c2，N_c3。

进一步的，所述步骤2中的方法2具体为：

步骤A：计算第ic个从机所能探测到的所有锚机中第iz个锚机的相对位置角度范围：α_ic-iz～β_ic-iz，其中

其中

为第iz个锚机在第i鐰个从机机载雷达上的扫描角度，

其中l_iz为第iz个锚机机体的长度，d_ic-iz为第ic个从机与第iz个锚机之间的距离，iz＝1，2，…，n₁；

步骤B：设置视角扇区遮蔽阈值e，将第一个锚机的相对位置角度范围存入高精度锚机库，并以该锚机为起点按照顺时针方向遍历所有锚机；遍历过程中判断当前锚机的相对位置角度范围和高精度锚机库中所有无人机角度范围是否有重叠，若没有重叠，则将当前锚机添加到高精度锚机库；若有重叠，则判断重叠部分是否超过e，若重叠部分没有超过e，则将当前锚机添加到高精度锚机库；若重叠部分超过e，则在重叠的两个无人机中选择与第ic个从机的相对距离小的无人机，将该无人机添加到高精度锚机库，并删除另外一个无人机；

步骤C：得到筛选后的无人机的数量，以及每个无人机的相对俯仰角和相对方位角。

进一步的，所述步骤3具体为：

步骤3-1：在第ic个从机的视线扇态估计结果中随机选一个锚机作为参考锚机m，以参考锚机m所在位置作为原点建立相对机体坐标系；

步骤3-2：建立第ic个从机的协同导航模型：

A_icX_ic＝B_ic

其中，

其中，α_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对俯仰角，j＝1，2，…N_i鐰；β_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对方位角，

为第j个锚机在相对机体坐标系下x轴上的坐标值，

为第j个锚机在相对机体坐标系下y轴上的坐标值，

为第j个锚机在相对机体坐标系下z轴上的坐标值，

分别为第ic个从机在相对机体坐标系下的坐标值，T为矩阵转置。

进一步的，所述步骤4具体为：

步骤4-1：建立增广矩阵C_ic＝[A_ic B_ic]；

步骤4-2：对C_ic进行奇异值分解：

C_i鐰＝U_icS_icV_ic

其中，U_ic为奇异值分解得到的M阶酉矩阵，M为C_ic的行数；S_ic为奇异值分解得到的奇异值矩阵；V_ic为奇异值分解得到的n阶酉矩阵，其中n为C_ic的列数；

并根据分解后得到的矩阵计算位置坐标矢量X_ic：

其中，

为矩阵V_ic第1行第4列的数值，

为矩阵V_ic第2行第4列的数值，

为矩阵V_ic第3行第4列的数值，

为矩阵V_ic第4行第4列的数值；

步骤4-3：将根据步骤4-2计算第ic个从机在相对机体坐标下的位置坐标，并将该位置坐标转为地球坐标系下的位置坐标

其中，

为地理坐标系到地球坐标系的坐标转换矩阵，

机体坐标系到地理坐标系的坐标转换矩阵，

为参考锚机m在地球坐标系的位置坐标；

步骤4-4：根据步骤4-3计算的第ic个从机在地球坐标系下的位置坐标，建立卡尔曼滤波量测方程，结合第ic个从机的惯性导航***的状态方程，对从机ic机载惯性导航***的误差进行估计和修正。

有益效果：本发明是一种用于无人集群协同导航优化的视线扇区动态估计方法，应用于大型无人机集群时因无人机位置重叠导致相对测量信息出现较大误差情况，提出了两种高精度视线扇区动态估计方案，方案1是根据从机和锚机间相对距离信息并将从机周围锚机划分为短距离区域、中距离区域以及远距离区域并计算不同的角度划分参数，将各区域划分为较小区间并对各区间锚机进行筛选；方案2通过无人机机载光电平台上获取从机周围各锚机雷达平面视角范围，并根据各锚机雷达视角进行筛选，对雷达视角出现重叠且重叠超过一半的锚机进行筛选，留下相对距离较近的高精度锚机；最后对高精度视线扇区动态估计过后建立了单一角度信息的协同导航模型并设计了对应协同导航解算算法，解算出低精度无人机位置信息，最后通过滤波器修正低精度无人机机载导航设备精度。本发明通过对从机周围高精度锚机进行筛选从而减小了角度测量过程中出现的非视距误差以及导航信息传输出现混乱的问题，并通过建立了基于角度信息的协同导航模型来修正集群***中低精度无人机的位置精度，适合实际使用；本发明采用协同导航技术利用多飞行器间的传感器信息，实现多飞行器间地信息协同，结合数据融合的方法以提高多飞行器间的定位精度，弥补了导航传感器的误差，并且对可能出现的故障进行识别、隔离和恢复。

附图说明

图1为本发明的流程图；

图2为视线扇区动态估计方法1筛选流程图；

图3为视线扇区动态估计方法1示意图；

图4为视线扇区动态估计方法2示意图；

图5为视线扇区动态估计方法2筛选流程图；

图6为低精度无人机不同方法修正效果示意图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

本实施例提供了一种用于无人集群协同导航优化的视线扇区动态估计方法，针对大规模无人集群因相对视线中成员重叠，提出两种视线扇区动态估计方案以避免，方案1根据无人集群成员间相对距离信息并将从机周围主机划分为短距离区域、中距离区域以及远距离区域并计算不同的视线扇区划分参数，将各视线扇区划分为较小区间并对各区间协同成员筛选；方案2通过机载光电平台上获取从机周围各成员雷达视角扇区范围，并根据各成员雷达视角进行筛选，对雷达视角出现重叠且重叠超过特定范围的成员进行筛选，保留相对距离较近的高精度成员。

如图1所示，本实施例，具体为：

步骤(1)，确定参与集群的无人机的数量N，其中N≥3，根据集群无人机机载导航设备的精度和集群无人机数量，确定定位精度区分阈值τ，将集群无人机总体定位精度高于阈值τ划分为高精度层锚机和低于阈值τ的低精度从机；

步骤(2)，根据上述步骤(1)的集群无人机总体分类结果，从机数量为n_c，从机编号为ic，获取第ic个从机所能探测到的所有锚机，该所有锚机的总个数为n₁，ic＝1,2,…n_c，获取从机ic和锚机iz之间的相对俯仰角信息α_ic-iz和相对方位角信息β_ic-iz，并且依据各无人机上所携带的机载导航设备位置信息计算无人机间的相对距离信息d_ic-iz，iz＝1,2…n₁；设计两种方法建立实时视线扇区动态估计模式，从而对取第ic个从机所能探测到的所有锚机进行筛选，从而得到从机ic正常通信范围内的锚机数量N_ic。

步骤(3)，根据上述步骤(2)的视线扇区动态估计模式结果，获取所有从机通信范围内的所有锚机相对角度信息，建立基于相对角度测量信息的协同导航模型；

步骤(4)，根据步骤(3)建立的基于相对角度测量信息的协同导航模型，利用总体最小二乘算法解算上述协同导航模型，计算低精度从机的位置结果，并通过卡尔曼滤波方法来修正从机的机载导航精度。

步骤(5)，重复步骤(3)、(4)，依次修正无人机集群***所有从机的位置精度。

优选的，如图2所示步骤(2)方法1的具体过程如下所示：

步骤(2-1)，首先对编号为ic的从机进行视线扇区动态估计，其中，ic＝1；

步骤(2-2)，如图3所示，根据上述步骤(1)确定的集群无人机中高精度锚机数量以及低精度从机数量n_c，并且依据各无人机机载导航设备位置数据，计算从机和其附近锚机(所述附近锚机为该编号为ic的从机所能探测到的锚机)的相对距离d_ic-iz，并获取从机ic和其附近最远锚机的相对距离D_ic，并将从机ic周围锚机分别划分成为三个区域，短距离区域、中距离区域、远距离区域，短距离区域为和从机ic相对距离为0～d_mnD_ic，d_mn为预设的短距离区域和中距离区域的划分阈值，本实施例中d_mn取值为1/7，中距离区域为和从机ic相对距离为，d_mnD_ic～d_mfD_ic，d_mf为预设的中距离区域和远距离区域的划分阈值，本实施例中d_mn取值为3/7，远距离区域为和从机ic相对距离为d_mfD_ic～D_ic的区域；

步骤(2-3)，根据不同的区域设定不同的角度划分参数，短距离区域角度划分参数a_ic1计算如下所示：

中距离区域角度划分参数a_ic2计算如下所示：

远距离区域角度划分参数a_ic3计算如下所示：

步骤(2-4)，根据步骤(2-3)确定的角度划分参数，以编号为ic的从机为球形中心，短距离区域、中距离区域以及远距离区域分别以a_ic1、a_ic2、a_ic3为角度的划分参数，分别将短距离区域、中距离区域以及远距离区域划分为M_ic1、M_ic2、M_ic3个扇形区间，其中：M_ic1＝360/a_ic1、M_ic2＝360/a_ic2、M_ic3＝360/a_ic3，其中不够整除的余数空间自动划分至最后一个区间；

步骤(2-5)，根据步骤(2-4)划分的各区域区间，首先对短距离区域内各区间进行视线扇区动态估计，按顺时针方向对短距离区域各区间进行编号m₁，其中1≤m₁≤M_ic1，首先令m₁＝1；

步骤(2-6)，确定区间m₁内的高精度锚机数量

如果

则跳转至步骤(2-9)，进行下一区间的视线扇区动态估计，如果

则跳转步骤(2-8)直接获取该锚机的相对角度信息，如果

则转到步骤(2-7)进行视线扇区动态估计；

步骤(2-7)，根据步骤(2)确的从机ic通信范围内的锚机相对距离信息d_ic-iz，对区间m₁内的

架锚机的相对距离数据进行排序，标记相对距离最小锚机，即离从机ic相对距离最近的锚机，并将其设定为筛选后的锚机；

步骤(2-8)，获取上述步骤区间m₁内筛选后的锚机的相对俯仰角数据，和相对方位角数据；

步骤(2-9)，令m₁＝m₁+1，跳转至步骤(2-6)进行下一区间的视线扇区动态估计；

步骤(2-10)，经上述步骤后，可获得短距离区域从机ic至筛选后锚机数量N_ic1，以及相对应锚机的相对俯仰角数据和相对方位角数据；

步骤(2-11)，然后对中距离区域内锚机进行筛选，按顺时针方向对中距离区域各区间进行编号m₂，其中1≤m₂≤M_ic2，首先令m₂＝1；

步骤(2-12)，对区间m₂进行筛选，首先判断区间m₂同向短距离区域内有无锚机(即与该扇形区间m₂占相同角度的范围的短距离区域)，若有则区间无需进行筛选直接去除跳转至步骤(2-16)，若没有，则跳转至步骤(2-13)进行视线扇区动态估计，

步骤(2-13)，确定区间m₂内的高精度锚机数量

如果

则跳转至步骤(2-16)，进行下一区间的视线扇区动态估计，如果

则跳转步骤(2-15)直接获取该锚机的相对角度信息，如果

则转到步骤(2-14)进行视线扇区动态估计；

步骤(2-14)，根据步骤(2)确的从机ic通信范围内的锚机相对距离信息，对区间m₂内的

架锚机的相对距离数据进行排序，标记相对距离最小锚机，即离从机ic相对距离最近的锚机，并将其设定为筛选后的锚机；

步骤(2-15)，获取上述步骤区间m₂内筛选后的锚机的相对俯仰角数据，和相对方位角数据；

步骤(2-16)，令m₂＝m₂+1，跳转至步骤(2-12)进行下一区间的视线扇区动态估计；

步骤(2-17)，经上述步骤后，可获得中距离区域从机ic至筛选后锚机数量N_ic2，以及相对应锚机的相对俯仰角数据和相对方位角数据；

步骤(2-18)，然后对远距离区域内锚机进行筛选，按顺时针方向对远距离区域各区间进行编号m₃，其中1≤m₃≤M_ic3，首先令m₃＝1；

步骤(2-19)，对区间m₃进行筛选，首先判断区间m₃同向短距离区域和中距离区域内有无锚机，若有则区间无需进行筛选直接去除跳转至步骤(2-23)，若没有，则跳转至步骤(2-20)进行视线扇区动态估计，

步骤(2-20)，确定区间m₃内的高精度锚机数量n_m3，如果n_m3＝0，则跳转至步骤(2-23)，进行下一区间的视线扇区动态估计，如果n_m3＝1，则跳转步骤(2-22)直接获取该锚机的相对角度信息，如果n_m3≥2，则转到步骤(2-21)进行视线扇区动态估计；

步骤(2-21)，根据步骤(2)确的从机ic通信范围内的锚机相对距离信息d_ic-iz，对区间m₃内的n_m3架锚机的相对距离数据进行排序，标记相对距离最小锚机，即离从机ic相对距离最近的锚机，并将其设定为筛选后的锚机；

步骤(2-22)，获取上述步骤区间m₃内筛选后的锚机的相对俯仰角数据α_ic-iz，和相对方位角数据β_ic-iz，其中iz为区间m₃筛选后的锚机编号；

步骤(2-23)，令m₃＝m₃+1，跳转至步骤(2-12)进行下一区间的视线扇区动态估计；

步骤(2-24)，经上述步骤后，可获得远距离区域从机ic至筛选后锚机数量N_ic3，以及相对应锚机的相对俯仰角数据和相对方位角数据；

步骤(2-25)，经过上述步骤，对从机ic周围短距离区域、中距离区域以及远距离区域内所有锚机都进行了筛选，并获取了短距离区域、中距离区域、远距离区域筛选后的锚机数量N_ic1、N_ic2、N_ic3，令ic＝ic+1，跳转至步骤(2-2)对集群***其余从机周围锚机进行筛选；

步骤(2-26)，经过上述步骤，集群***所有从机周围进行了视线扇区动态估计并获取了筛选后对应锚机的相对俯仰角和相对方位角数据。

优选的，如图4、5所示，步骤(2)方法2具体过程如下所示：

步骤(3-1)，首先对编号为ic的从机进行视线扇区动态估计，其中，ic＝1；

步骤(3-2)，从机以东方向为基准方向，并开始按顺时针方向对其附近无人机进行扫描，默认无人机机载光电平台测角范围为0度-360度，首先从基准方向顺时针扫描所有无人机的方位信息，并且无人机在雷达成像上的扫描角度

步骤(3-3)，设定第iz个锚机机体的长度为l_iz，第ic个从机与第iz个锚机之间的距离为d_ic-iz为，考虑l_iz＜＜d_ic-iz则无人机在机载光电雷达平台上测量视角范围为θ_ic-iz如下所示：

步骤(3-4)，经过步骤(3-2)和步骤(3-3)，可得到从机ic附近各锚机的相对位置角度范围信息：α_ic-iz～β_ic-iz，其中

步骤(3-5)，根据上述机载光电雷达扫描角度数值范围α_ic-iz～β_ic-iz，对从机ic其周围360度范围按照角度数值范围进行高精度视线扇区动态估计，并按顺时针方向对从机ic周围锚机进行重新编号，锚机iz＝1自动成为备选高精度锚机库，令iz＝2进行筛选；

步骤(3-6)，视线扇区动态估计分为三种情况：

1、若锚机iz其角度范围α_ic-iz～β_ic-iz和备选锚机库中所有无人机角度范围没有重叠部分，则将该锚机添加到备选锚机库中；

2、若锚机iz其角度范围和备选锚机库中无人机角度重叠部分超过，预设的视角扇区遮蔽阈值e，本实施例中e取其机载光电雷达平台视角角度范围θ_iz的一半，则比较角度重叠的两架无人机和从机ic的相对距离，若锚机iz相对距离较小，则将其添加到备选锚机库中，同时将备选锚机库中和其角度重叠的锚机从库中剔除；

3、若锚机iz其角度范围和备选锚机库中无人机角度重叠部分低于其机载光电雷达平台视角角度范围θ_iz的一半，则其传感器测量不受影响，直接将其添加到备选锚机库中；

步骤(3-7)，比较iz和锚机数量n₁大小关系，若iz＜n₁，令iz＝iz+1，并跳转至步骤(3-6)，继续对其他锚机进行筛选，若iz≥n₁，则跳转至步骤(3-8)；

步骤(3-8)，经过上述视线扇区动态估计机制，从机ic附近所有锚机都经过筛选，最终在备选锚机库中的锚机为筛选后留下的锚机；

4、步骤(3)的具体过程如下所示：

步骤(3-1)，令ic＝1，建立编号为ic的从机的协同导航模型；

步骤(3-2)，根据步骤(2)从机ic周围锚机的筛选结果，获取从机ic正常通信范围内的锚机数量N_ic，锚机z的纬度为λ_z，经度为L_z，锚机高度为H_z和从机ic相对角度传感器获取的相对俯仰角α_ic-z，相对方位角β_ic-z，其中z＝1,2…N_ic；

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

其中，R为地球半径，e为地球椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

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

步骤(3-4)，经过上述步骤(3-3)、步骤(3-4)，可获取从机ic周围锚机的大地直角坐标系坐标

步骤(3-5)，随机选取一个锚机m作为参考点，以该锚机的所在位置为相对坐标系原点，建立相对大地直角坐标系，1≤m≤N_ic，本实施例中选择第一个锚机作为参考点，也即m＝1其中X轴在赤道平面内与零度子午线相交，Z轴沿地球自转轴方向，Y轴与X轴、Z轴构成右手直角坐标系，则第z个锚机在相对大地直角坐标系下坐

的表达式如下所示：

其中，z＝2，3…N_ic；

步骤(3-6)，将上述步骤(3-6)建立的锚机相对大地直角坐标系坐标转换为以锚机m＝1为原点的相对机体坐标系坐标，其中X轴沿锚机1横轴向右，Y轴沿锚机1纵轴向前，Z轴沿锚机1竖轴向上，转换表达式如下所示：

其中，

为转换后的相对机体坐标系X轴坐标，

为转换后的相对机体坐标系Y轴坐标，

为转换后的相对机体坐标系Z轴坐标，z＝2,3…N_ic，其中，锚机1的相对坐标为

为地理坐标系和大地坐标系转换矩阵，

为地理坐标系和机体坐标系转换矩阵，其表达式如下所示：

其中，λ₁、L₁、H₁分别为锚机1的经度、纬度和高度，

θ₁、ψ₁分别为锚机1的横滚角、俯仰角和航向角，R_N为地球卯酉圈半径，e为地球椭球偏心率；

步骤(3-7)，根据步骤(3-7)计算的以锚机1为坐标原点的相对机体坐标系坐标，以及步骤(3-2)获取的角度观测信息来建立基于相对角度信息的协同导航模型，观测方程如下所示：

其中，α_ic-z为从机ic和锚机z之间的相对俯仰角，β_ic-z为从机ic和锚机z之间的相对方位角，

为第z个锚机在相对机体坐标系中的坐标，z＝1,2…N_ic，

为待计算的从机ic在相对机体坐标系下坐标；

步骤(3-8)，对上述步骤(3-7)表达式进行化简，如下所示：

其中z＝1,2…N_ic，

并将其转换为矩阵表达形式：

A_icX_ic＝B_ic

其中：

其中，其中，α_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对俯仰角，j＝1，2，…N_ic；β_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对方位角，

为第j个锚机在相对机体坐标系下x轴上的坐标值，

为第j个锚机在相对机体坐标系下y轴上的坐标值，

为第j个锚机在相对机体坐标系下z轴上的坐标值；

为在相对机体坐标系下第ic个从机和第N_i鐰个锚机之间的相对俯仰角，

为在相对机体坐标系下第ic和第N_i鐰个锚机之间的相对方位角，

为第N_i鐰个锚机在相对机体坐标系下x轴上的坐标值，

为第N_i鐰个锚机在相对机体坐标系下y轴上的坐标值，

为第N_i鐰个锚机在相对机体坐标系下z轴上的坐标值，

分别为第ic个从机在相对机体坐标系下的坐标值，T为矩阵转置。

优选的，步骤(4)的具体过程如下所示：

步骤(4-1)，根据步骤(3-9)确定的矩阵Α_ic、B_ic，组建由矩阵Α_ic、B_ic的增广矩阵C_ic，如下所示：

C_ic＝[A_ic B_ic]

步骤(4-2)，对步骤(4-1)建立的增广矩阵C_ic进行奇异值分解，如下所示：

C_ic＝U_icS_icV_ic；

其中，U_ic为奇异值分解得到的M阶酉矩阵，M为C_ic的行数；S_ic为奇异值分解得到的奇异值矩阵；V_ic为奇异值分解得到的n阶酉矩阵，其中n为C_ic的列数；

步骤(4-3)，上述步骤(4-2)奇异值分解后的矩阵V_ic，解算出步骤(3-9)方程中的X_ic，如下所示：

其中，

是矩阵V_ic第1行第四列、第2行第四列、第3行第四列、第4行第四列矩阵数值；

步骤(4-4)，根据步骤(4-3)计算的从机ic在以参考锚机为坐标原点的相对机体坐标系上坐标，将其转换为相对大地直角坐标系下坐标，如下所示：

其中，

分别为X_ic的前三项元素，

分别为从机ic在以主机1为坐标原点的相对大地直角坐标系下的坐标；

步骤(4-5)，从机ic在以锚机m为坐标原点的相对大地直角坐标系下的坐标转换为在以地心为坐标原点的大地直角坐标系下坐标：

其中，

为参考锚机m在地球坐标系的位置坐标；

则结合步骤步骤(4-4)和步骤(4-5)将第ic个从机在相对机体坐标下的位置坐标转为地球坐标系下的位置坐标

其中，

为地理坐标系到地球坐标系的坐标转换矩阵，

机体坐标系到地理坐标系的坐标转换矩阵，

步骤(4-6)，将从机ic在大地直角坐标系下三维坐标转换为经度λ_i′_c、纬度L_i′_c和高度H_i′_c数据，如下所示：

步骤(4-7)，采用迭代算法将其转化为大地坐标系下的纬度L′_ic和高度H′_ic，令迭代次数l＝1，初始L′_ic＝0.1；

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

l＝l+1

其中，N为地球半径，e为地球偏心率；

步骤(4-9)，根据步骤(4-8)的迭代次数l的值，比较l与10的大小关系，如果l≤10，则执行步骤(4-8)，如果l＞10，执行步骤(4-10)；

步骤(4-10)，获取最后的经度λ′_ic，纬度L′_ic和高度H′_ic；

步骤(4-11)，根据计算的从机的位置数据，建立卡尔曼滤波量测方程，计算表达式如下所示：

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

步骤(4-12)，根据步骤(4-11)所计算的卡尔曼滤波量测方程，结合从机ic惯性导航***的状态方程，经卡尔曼滤波程序后，输出从机ic机载惯性导航***经度，纬度和高度数据修正值(λ′_t,L′_t,H′_t)，λ′_t为经度修正值，L′_t为纬度修正值，H′_t为高度修正值。

图6为低精度无人机不同方法修正效果示意图。由图6可以看出，采用本发明中的方法1和方法2均能够有效实现对非遮蔽视线扇区的优选，提高角度信息协同辅助下的导航精度。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (5)

1.一种用于无人集群协同导航优化的视线扇区动态估计方法，其特征在于，具体包括如下步骤：

步骤1：根据无人机集群中各无人机机载导航***的定位精度和无人机的数量，确定定位精度区分阈值τ，将机载导航***的精度大于等于τ的无人机设置为锚机，将机载导航***的精度小于τ的无人机设置为从机；

步骤2：获取第ic个从机所能探测到的所有锚机，该所有锚机的总个数为n₁，ic＝1，2，…n_c，n_c为从机的总个数；采用方法1或方法2对第ic个从机进行视线扇区估计，从而得到筛选后的锚机，筛选后的锚机的个数为N_ic；所述方法1：基于第ic个从机所在位置与该第ic个从机机载光电平台相对测量空间范围，对第ic个从机的视线扇区进行划分，并分析各从机所属视线扇区的遮蔽关系；所述方法2：基于第ic个从机与n₁个锚机的相对距离，计算n₁个锚机相对第ic个从机视线的相对遮蔽角度范围，并基于该相对遮蔽角度范围对第ic个从机的视线扇区进行动态估计；

步骤3：根据N_ic个锚机与第ic个从机的相对角度信息，建立基于相对角度测量信息的第ic个从机的协同导航模型；

步骤4：根据协同导航模型，计算第ic个从机的位置信息，并基于该位置信息和卡尔曼滤波方法修正从机的机载导航精度。

2.根据权利要求1所述的一种用于无人集群协同导航优化的视线扇区动态估计方法，其特征在于，所述步骤2中的方法1具体为：

步骤2-1：计算该第ic个从机与所有锚机的相对距离，以第ic个从机为圆心，设置圆形的短距离区域，环形的中距离区域和环形的远距离区域：若第ic个从机与第iz个锚机的相对距离在0～d_mnD_ic范围内，则认为该第iz个锚机位于短距离区域，若第ic个从机与第iz个锚机的相对距离在d_mnD_ic～d_mfD_ic范围内，则认为该第iz个锚机位于中距离区域，若第ic个从机与第iz个锚机的相对距离在d_mfD_ic～D_ic的范围内，则认为该第iz个锚机位于远距离区域；其中d_mn为预设的短距离区域和中距离区域的划分阈值，d_mf为预设的中距离区域和远距离区域的划分阈值，D_ic为第ic个从机与最远锚机之间的相对距离，iz＝1,2，…，n₁；

步骤2-2：以第ic个从机作为球心，根据每个区域对应的角度划分参数，将短距离区域划分为M_ic1个扇形区间，将中距离区域划分为M_ic2个扇形区间，将远距离区域划分为M_ic3个扇形区间；

其中M_ic1＝360/a_ic1；

M_ic2＝360/a_ic2；

M_ic3＝360/a_ic3；

若不能整除，则将余数划分至后一个区间，其中

为短距离区域对应的角度划分参数，

为中距离区域对应的角度划分参数，

为远距离区域对应的角度划分参数；其中，l为n₁个锚机中最长的锚机的长度；

步骤2-3：对获取第I个区域的第i个扇形区间内锚机进行筛选，其中I＝1,2,3，当I＝1时为短距离区域，当I＝2时为中距离区域，当I＝3时为远距离区域，i＝1，2，…，M_ic，I，其中M_ic,I为第I区域中扇形区间的总个数；若I＝1，则转步骤2-5；若I＝2或者I＝3，则转步骤2-4；

步骤2-4：若I＝2，基于第I个区域的第i个扇形区间的角度，判断该角度范围内对应的短距离区域是否存在锚机，若存在，则不对该第I个区域的第i个扇形区间内的主机进行筛选，并转步骤2-6；否则转步骤2-5；若I＝3，基于第I个区域的第i个扇形区间的角度，判断该角度范围内对应的短距离区域和中距离区域是否存在锚机，若存在，则不对该第I个区域的第i个扇形区间内的主机进行筛选，并转步骤2-6；否则转步骤2-5；

步骤2-5：获取第I个区域的第i个扇形区间内锚机的总数量n_I,i，若数量n_I,i＝0，则转步骤2-6，若n_I，i＝1，则标记该锚机，同时获取和记录该锚机的相对俯仰角和相对方位角，并转步骤2-6；若n_I，i≥2，则标记与第ic个从机相对距离最小的锚机，获取和记录该锚机的相对俯仰角和相对方位角，并转步骤2-6；

步骤2-6：i＝i+1，判断i是否大于M_ic，I，若否，则转步骤2-3，若是，则转步骤2-7；

步骤2-7：I＝I+1，判断I是否大于3，若否，则转步骤2-3，若是，则转步骤2-8；

步骤2-8：得到短距离区域、中距离区域和远距离区域内被标记的锚机的个数N_c1，N_c2，N_c3。

3.根据权利要求1所述的一种用于无人集群协同导航优化的视线扇区动态估计方法，其特征在于，所述步骤2中的方法2具体为：

步骤A：计算第ic个从机所能探测到的所有锚机中第iz个锚机的相对位置角度范围：α_ic-iz～β_ic-iz，其中

其中

为第iz个锚机在第ic个从机机载雷达上的扫描角度，

其中l_iz为第iz个锚机机体的长度，d_ic-iz为第ic个从机与第iz个锚机之间的距离，iz＝1，2，…，n1；

步骤B：设置视角扇区遮蔽阈值e，将第一个锚机的相对位置角度范围存入高精度锚机库，并以该锚机为起点按照顺时针方向遍历所有锚机；遍历过程中判断当前锚机的相对位置角度范围和高精度锚机库中所有无人机角度范围是否有重叠，若没有重叠，则将当前锚机添加到高精度锚机库；若有重叠，则判断重叠部分是否超过e，若重叠部分没有超过e，则将当前锚机添加到高精度锚机库；若重叠部分超过e，则在重叠的两个无人机中选择与第ic个从机的相对距离小的无人机，将该无人机添加到高精度锚机库，并删除另外一个无人机；

步骤C：得到筛选后的无人机的数量，以及每个无人机的相对俯仰角和相对方位角。

4.根据权利要求1所述的一种用于无人集群协同导航优化的视线扇区动态估计方法，其特征在于，所述步骤3具体为：

步骤3-1：在第ic个从机的视线扇态估计结果中随机选一个锚机作为参考锚机m，以参考锚机m所在位置作为原点建立相对机体坐标系；

步骤3-2：建立第ic个从机的协同导航模型：

A_icX_ic＝B_ic

其中，

其中，α_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对俯仰角，j＝1，2，…N_ic；β_ic-j为在相对机体坐标系下第ic个从机和第j个锚机之间的相对方位角，

为第j个锚机在相对机体坐标系下x轴上的坐标值，

为第j个锚机在相对机体坐标系下y轴上的坐标值，

为第j个锚机在相对机体坐标系下z轴上的坐标值，

分别为第ic个从机在相对机体坐标系下的坐标值，T为矩阵转置。

5.根据权利要求4所述的一种用于无人集群协同导航优化的视线扇区动态估计方法，其特征在于，所述步骤4具体为：

步骤4-1：建立增广矩阵C_ic＝[A_ic B_ic]；

步骤4-2：对C_ic进行奇异值分解：

C_ic＝U_icS_icV_ic

其中，U_ic为奇异值分解得到的M阶酉矩阵，M为C_ic的行数；S_ic为奇异值分解得到的奇异值矩阵；V_ic为奇异值分解得到的n阶酉矩阵，其中n为C_ic的列数；

并根据分解后得到的矩阵计算位置坐标矢量X_ic：

其中，

为矩阵V_ic第1行第4列的数值，

为矩阵V_ic第2行第4列的数值，

为矩阵V_ic第3行第4列的数值，

为矩阵V_ic第4行第4列的数值；

步骤4-3：将根据步骤4-2计算第ic个从机在相对机体坐标下的位置坐标，并将该位置坐标转为地球坐标系下的位置坐标

其中，

为地理坐标系到地球坐标系的坐标转换矩阵，

机体坐标系到地理坐标系的坐标转换矩阵，

为参考锚机m在地球坐标系的位置坐标；

步骤4-4：根据步骤4-3计算的第ic个从机在地球坐标系下的位置坐标，建立卡尔曼滤波量测方程，结合第ic个从机的惯性导航***的状态方程，对从机ic机载惯性导航***的误差进行估计和修正。

Images