基于相对定位的自组网编队导航方法、系统及存储介质

本发明涉及组网定位，尤其涉及一种基于相对定位的自组网编队导航方法、系统及存储介质。

背景技术：

1、现有主流多机组网内相对位置估计技术框架主要由观测信号筛选模块、卡尔曼滤波估计器以及融合惯导的位置结果解算器这三个部分构成。其中，观测信号筛选模块主要从节点收到的包括提供伪距估计的信标数据、导航源数据等在内的无线信号中，筛选出质量较高的导航源及其伪距估计结果；卡尔曼滤波估计器主要将筛选出的导航源、伪距估计结果、以及相关的质量信息进行滤波处理，以获得在给定条件约束下的最优估计结果；融合惯导的位置结果解算器主要基于卡尔曼估计结果，融合惯导传感信息，周期性推算当前位置结果，以避免无线信号不稳定情况导致卡尔曼滤波解算停滞问题。

2、现有主流多机组网内分布式自适应决策调度算法主要包括：领航-跟随等中心化算法和基于一致性的分布式算法。其中，领航-跟随算法是领航节点按任务规划行进，而其余节点基于领航节点运动状态，依照预设编队目标及几何架构进行自适应协同调度；基于一致性的分布式算法需要通过信息交互，实现组网内各节点在位置、速度等参数上达成一致性共识，即各网络节点就网络拓扑几何状态估计及运动状态估计达成一致，等效于各节点均可获得全局信息，各节点基于此计算与目标任务所要求的位置差值，最终实现控制与调度。此外，在简单特定场景下，可通过人工设计势函数或流函数等代价函数的方式引导节点运动以达到目标位置形成目标编队的算法。

3、现有的组网相对位置估计方法对无线信号和惯导传感器的利用率较低，没有充分挖掘质量较差的测量信息效用，也没有对多源数据进行有效的融合，导致定位精度差、收敛速度慢。分布式自适应决策调度算法方面，领航-跟随算法是对领航节点的依赖性极高，系统鲁棒性低，多机协同优势未能充分发挥；基于一致性的分布式算法虽然容易分布式实现，并且在无噪声下的收敛性条件已有比较充分的研究，但在有噪声情况下的收敛性和精度令人堪忧，因为其并未充分考虑定位阶段误差，容易导致组网定位不准确、编队能力受限，影响设备的正常使用。

技术实现思路

1、本发明提供一种基于相对定位的自组网编队导航方法、系统及存储介质，用以解决现有卫星拒止或卫星信号微弱情况下导致的定位导航不稳定、无人自组网集群编队性能受限问题。

2、本发明提供一种基于相对定位的自组网编队导航方法，包括：

3、获取自组网基本信息，基于预设的自组网编队网络协议进行最优目标编队的选择，将所述最优目标编队进行拆分并下发至各个自组网节点；

4、通过预设的卡尔曼滤波的多传感融合位置估计器，获取相邻自组网节点间实时相对测量结果，生成当前相对位置估计信息及估计误差；

5、通过预设的分布式自适应决策调度算法，基于所述相对位置估计信息及估计误差对比节点当前任务目标，计算生成运动控制策略，执行所述运动控制策略进行自组网编队。

6、根据本发明提供的一种基于相对定位的自组网编队导航方法，所述获取自组网基本信息，基于预设的自组网编队网络协议进行最优目标编队的选择，将所述最优目标编队进行拆分并下发至各个自组网节点，具体包括：

7、所述预设的自组网编队网络协议包括自组网络基本信息收集、确立最优任务目标、任务目标的拆解下达至各节点；

8、所述自组网络基本信息包括节点终端的身份表示、节点终端初始位置的估计结果和估计误差协方差；

9、所述节点终端初始位置的估计结果和估计误差协方差来源于先验信息或者来源于节点终端绝对定位或相对定位方法的解算结果。

10、根据本发明提供的一种基于相对定位的自组网编队导航方法，所述确立最优任务目标，具体包括：

11、给定自组网络目标编队等价类，自组网络基于节点间相对测量估计网络位置信息；

12、基于位置估计结果和估计误差，在目标编队等价类中选择最优目标编队并广播至各个节点；

13、各节点对邻居节点进行相对测量，各节点利用卡尔曼滤波或其他方法得到对所处子编队的估计结果和估计误差的协方差；

14、各节点比较所述子编队与目标子编队的形状差异，确立运动控制策略，各节点执行运动控制策略。

15、根据本发明提供的一种基于相对定位的自组网编队导航方法，所述通过预设的卡尔曼滤波的多传感融合位置估计器，获取相邻自组网节点间实时相对测量结果，生成当前相对位置估计信息及估计误差，具体包括：

16、所述多传感融合位置估计器，利用各节点周期性对相邻节点进行实时相对测量，并融合节点当前传感器获取的相关信息，通过卡尔曼滤波等滤波算法，获得当前相对位置估计信息及估计误差，其中估计误差通过估计误差协方差矩阵表示。

17、根据本发明提供的一种基于相对定位的自组网编队导航方法，所述相邻节点进行实时相对测量方式包括：距离测量、角度测量、激光雷达、视觉定位；

18、所述位置估计的方式包括：扩展卡尔曼滤波器、贝叶斯滤波、粒子滤波、最小二乘。

19、根据本发明提供的一种基于相对定位的自组网编队导航方法，所述通过预设的分布式自适应决策调度算法，基于所述相对位置估计信息及估计误差对比节点当前任务目标，计算生成运动控制策略，执行所述运动控制策略进行自组网编队，具体包括：

20、所述分布式自适应决策调度算法，利用各节点周期性基于实时位置及运动状态信息，对比所处子编队与目标子编队的形状差异，计算求解自身的运动控制策略并执行。

21、本发明还提供一种基于相对定位的自组网编队导航系统，所述系统包括：

22、通导自组网模块，用于获取自组网基本信息，基于预设的自组网编队网络协议进行最优目标编队的选择，将所述最优目标编队进行拆分并下发至各个自组网节点；

23、相对定位模块，用于通过预设的卡尔曼滤波的多传感融合位置估计器，获取相邻自组网节点间实时相对测量结果，生成当前相对位置估计信息及估计误差；

24、编队控制模块，用于通过预设的分布式自适应决策调度算法，基于所述相对位置估计信息及估计误差对比节点当前任务目标，计算生成运动控制策略，执行所述运动控制策略进行自组网编队。

25、本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述基于相对定位的自组网编队导航方法。

26、本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述基于相对定位的自组网编队导航方法。

27、本发明还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述基于相对定位的自组网编队导航方法。

28、本发明提供的一种基于相对定位的自组网编队导航方法、系统及存储介质，基于通导融合网络的编队网络协议，实现编队任务拆分及子目标任务下达，基于卡尔曼滤波的多传感融合位置估计器，实时估计当前多机组网内相对位置及精度，为调度决策处理提供信息支撑，能够根据各节点实时观测信息，实现了卫星拒止或卫星信号不稳定下，保证自组网编队性能及编队结果稳定。此外，本发明设计了融合当前运动状态的分布式调度决策方案，分布式自适应决策调度算法，各节点基于当前自身及周边节点实时观测信息及估计精度，结合目标任务进行自适应决策调度，在较快收敛至任务目标的同时，具备了高效的自主维持能力，可有效解决在处理复杂任务时，因调度决策收敛速度慢或可维持性较差导致的多机协同系统性能不佳的情况。