一种异构可观性驱动的空地集群协同定位与参考接力方法

本发明属于多智能体协同导航与定位，尤其涉及一种异构可观性驱动的空地集群协同定位与参考接力方法。

背景技术：

1、随着无人机和无人车技术的快速发展，由空中节点和地面节点组成的空地异构集群在环境监测、应急救援、物流配送等领域展现出广阔的应用前景。在这些应用中，高精度、高可靠性的协同定位能力是集群系统完成复杂任务的基础和关键。

2、现有协同定位方法已经能够利用惯性、视觉、轮式里程和节点间测距等信息进行状态估计，并可通过因子图、滤波或非线性优化实现多源融合。上述方法解决了多类量测如何进入定位后端的问题，但在节点退化、链路波动和通信资源受限同时出现时，仍需要动态决定哪些协作边应被启用、启用多少条边、各边应具有多大权重以及哪个节点应承担统一参考。

3、现有方案通常将异常边筛选、边权分配、邻接规模选择和参考节点选择作为相互独立的环节：局部质量较高的边不一定能够改善整体几何可观性，单纯增加邻接边又会提高通信与计算负担，参考节点切换还会改变参考可达性和拓扑约束。分别优化上述变量容易产生相互冲突的结果，例如选出的高权重边集合无法维持参考连通，或者为提高可观性而选取的邻接规模超过可用通信预算。

4、因此，本发明所关注的核心技术问题并非单条协作边残差的计算方式，而是如何形成能够表征边质量、节点状态、异构几何、系统资源和参考连续性的统一调度状态，并据此对多个相互影响的调度变量进行耦合决策。

5、第一，需要在同一调度目标下联合确定协作边拓扑权重、有效邻接规模和参考接力决策，避免三个变量分别求解造成的局部最优和决策冲突；

6、第二，需要在通信预算、局部图连通性、参考节点可达性和空地关键耦合约束下保证联合调度结果可执行，并在参考节点退化时避免频繁切换或无可达接替节点；

7、第三，需要使联合调度结果能够直接作用于定位后端，并利用后验不确定度、可观性和资源状态反向更新下一调度周期，形成可持续适应环境变化的闭环；

8、第四，在分布式实现中，还需要控制跨节点信息量并抑制重复路径和旧参考系信息造成的过度置信。

技术实现思路

1、本发明的目的在于提供一种以联合调度器为核心的空地异构集群协同定位与参考接力方法，将拓扑权重、有效邻接规模和参考接力决策置于同一目标函数和同一约束集合下耦合求解，并通过约束一致化回投影、后端权重注入和优化状态反馈，使调度结果在资源受限和参考节点退化条件下仍能够形成可执行、连续的协同定位拓扑。

2、实现本发明目的的技术解决方案为：一种异构可观性驱动的空地集群协同定位与参考接力方法，包括如下步骤：

3、步骤1、获取空中节点与地面节点的多源量测，根据节点类型建立包含节点状态、运动边和候选协作边的空地异构局部协作候选图；

4、步骤2、基于候选协作边的质量表征、边两端节点健康度、由候选协作边和本地先验形成的异构可观性指标、当前通信与计算资源状态以及参考连续性指标，形成联合调度状态；

5、步骤3、将所述联合调度状态输入联合调度器，在同一调度目标下耦合求取各候选协作边的拓扑权重、有效邻接规模和参考接力决策，所述调度目标同时考虑图可观测度增益、通信代价、拓扑切换代价和参考切换代价；

6、步骤4、将解析优化得到的初始调度结果与轻量学习推理得到的修正量进行融合，并将融合结果回投影至同时满足通信预算、拓扑规模、局部图连通性、参考节点可达性和空地关键耦合约束的可行域；按照回投影后的拓扑权重和有效邻接规模确定启用边集合，并在当前参考节点健康度低于退出阈值且候选参考节点满足切换优势裕量时执行参考接力；

7、步骤5、将所述启用边集合对应的拓扑权重通过信息矩阵缩放、协方差调整或鲁棒核参数调节注入固定滞后滑动窗口因子图，并以当前参考节点或参考接力后的参考节点为统一参考坐标系，对空中节点与地面节点状态进行联合优化；

8、步骤6、基于步骤5的联合优化结果形成后验状态，将后验不确定度、异构可观性指标和资源状态反馈至下一调度周期，并输出统一参考系下的集群位姿、速度与不确定度。

9、一种异构可观性驱动的空地集群协同定位与参考接力系统，该系统用于实现所述的异构可观性驱动的空地集群协同定位与参考接力方法，所述系统具体包括：

10、空中节点和地面节点，所述空中节点搭载惯性测量单元、视觉相机和高度传感器，所述地面节点搭载惯性测量单元、轮式里程计和无线电测距模块；

11、候选图构建模块，用于获取空中节点与地面节点的多源量测，并根据节点类型建立空地异构局部协作候选图；

12、联合调度状态构建模块，用于根据协作边质量表征、节点健康度、异构可观性指标、资源状态和参考连续性指标形成联合调度状态；

13、联合调度与回投影模块，用于在同一调度目标下耦合求取拓扑权重、有效邻接规模和参考接力决策，并将调度结果回投影至满足通信预算、局部图连通性、参考节点可达性和空地关键耦合约束的可行域；

14、因子图优化与反馈模块，用于将拓扑权重注入固定滞后滑动窗口因子图，输出统一参考系下的集群位姿、速度与不确定度，并将优化状态反馈至下一调度周期；

15、进一步地，所述系统还可包括分布式一致模块，用于构造跨节点因子摘要，并根据重复路径风险、信息新鲜度和参考切换来源进行保守一致性缩放与注入。

16、本发明与现有技术相比，其显著优点在于：

17、（1）联合调度器将拓扑权重、有效邻接规模和参考接力决策作为相互耦合的三个输出，在同一目标下统筹异构可观性增益、通信代价和切换代价，减少分别决策造成的拓扑冲突；

18、（2）解析优化提供可解释的初始调度结果，轻量学习模块仅对所述初值进行修正，修正结果再经约束一致化回投影，因而能够兼顾环境适应能力与预算、连通性和参考可达性等工程约束；

19、（3）参考接力决策与边权及邻接规模同步求取，并利用退出阈值和切换优势裕量触发接力，可降低参考节点退化和频繁切换对统一参考系连续性的影响；

20、（4）联合调度结果通过信息矩阵、协方差或鲁棒核参数作用于定位后端，后验不确定度和可观性状态再反馈至下一周期，使拓扑选择和状态估计形成闭环；

21、（5）跨节点协作约束以因子摘要形式传输，并依据重复路径、信息新鲜度和参考切换来源进行保守缩放，可降低通信负担并抑制重复信息造成的过度置信。

1.一种异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤1中任一节点的状态向量记为：

3.根据权利要求1所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤2中针对候选测距边的残差序列采用混合概率模型生成所述候选协作边的质量表征，所述混合概率模型满足：

4.根据权利要求1所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤3至步骤4中的联合调度器与约束一致化回投影具体如下：

5.根据权利要求4所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤3.1至步骤3.2中的联合调度状态构建与解析优化具体过程如下：

6.根据权利要求5所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤3.3至步骤3.4中对于解析初值执行轻量学习修正与约束一致化回投影，具体如下：

7.根据权利要求1所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤1中获取的跨平台视觉量测作为视觉协同边，优选采用编码标识边实现；当编码标识不可用时，共享环境目标边或自然特征重识别边作为等效实现方式构造视觉协同边；

8.根据权利要求4所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，步骤5中，滑动窗口优化的最优状态估计满足：

9.根据权利要求1所述的异构可观性驱动的空地集群协同定位与参考接力方法，其特征在于，在分布式实现中，将跨节点因子在当前线性化点处展开为：

10.一种异构可观性驱动的空地集群协同定位与参考接力系统，其特征在于，该系统用于实现权利要求1至9任一项所述的方法，所述系统具体包括：