基于感知的异构双格网相对定位方法

本发明属于多无人机相对导航，具体地，涉及一种基于感知的异构双格网相对定位方法。

背景技术：

1、随着无人机技术的飞速发展与工程化应用不断深化，无人机集群协同作业凭借高效灵活、多机协作、场景适配性强等多重优势，在军事侦察、民用测绘、应急救援、环境巡检等多方面领域得到愈发广泛的应用。高精度的相对定位是保障无人机集群稳定编队、安全避障、顺利执行任务的核心技术，是无人机集群协同作业的关键前提。

2、当前无人机集群相对导航在复杂实际环境中面临两大关键约束：其一，在gnss信号拒止环境（如室内、城市楼宇、森林、强电磁干扰战场环境等）下，传统依赖卫星信号的导航方案完全失效，无法为集群提供基础的绝对定位保障，进而无法提供可靠的相对定位；其二，高空作业场景环境特征稀疏，视觉导航、激光雷达导航等依托环境特征匹配的技术，因有效特征匮乏导致定位性能大幅衰减，难以满足集群协同的高精度需求。在此背景下，仅提供机间相对测距信息的数据链、超宽带（ultra-wideband, uwb）等导航设备，因受外部环境制约小的特点脱颖而出。仅依靠机间测距信息的相对导航算法，成为gnss拒止、环境特征稀疏场景下无人机集群导航的主流研究方向，其中最具代表性的有基于扩展卡尔曼滤波的相对导航算法与基于因子图优化的相对导航算法。

3、扩展卡尔曼滤波算法具备计算量小、实时性强的突出优势，在量测非线性较弱的场景中，可实现较高精度的状态估计，因此被广泛应用于机载计算资源受限的无人机导航系统。但该算法本质是对非线性系统的一阶线性化近似，当环境呈现强量测非线性特性时，线性化误差会直接导致状态估计结果不一致，甚至引发滤波发散，严重降低导航精度。

4、因子图优化算法通过图模型对导航问题进行建模，将状态估计转化为非线性最小二乘全局优化问题，在强量测非线性场景中可稳定输出高精度状态估计结果，有效弥补了ekf算法线性化近似的固有缺陷。然而，fgo算法依赖全局迭代优化，存在计算资源消耗大、实时性不足的短板。

技术实现思路

1、为了解决现有的仅测距相对导航方法在状态估计精度与计算效率两方面无法兼顾的问题，本发明提出了基于感知的异构双格网相对定位方法。

2、本发明通过以下技术方案实现：

3、基于感知的异构双格网相对定位方法，所述方法具体包括以下步骤：

4、步骤1：规定无人机的状态模型与量测模型，假定无人机集群中共有n个无人机节点，其中有一个为nc节点，其余为普通节点，

5、步骤2：所有无人机节点在收到新的imu量测数据时，进行惯导解算、ekf时间更新、imu预积分因子构建；

6、步骤3：每个无人机节点判断当前是否收到量测，无人机收到量测时，建立量测模型；若未收到任何量测，返回步骤2；

7、步骤4：获得量测后的无人机进行ekf量测更新，获取7维物理特征；

8、步骤5：向训练后的网络输入7维物理特征，利用注意力门控残差网络进行推理，输出1维的归一化预测误差；

9、步骤6：运行双向重置策略，并根据结果采取ekf或fgo；通过神经网络预测的归一化预测误差来评估选择ekf模式或fgo模式，根据无人机状态后验估计，计算出相对坐标系下无人机的相对位置估计。

10、进一步地，在步骤1中，

11、每个无人机均搭载imu元件进行自身的位姿推位，搭载高度计以获得节点所在的地理坐标系下的天向高度量测，搭载数据链以实现无人机节点间的通信及测距；

12、nc节点建模自身的绝对导航状态；其ekf估计误差状态包括ecef坐标系下的位置误差、速度误差以及姿态失准角，其fgo优化变量包括ecef坐标系下的位置、速度以及载体系与ecef系对应的姿态四元数；

13、普通节点同时建模自身状态和nc的状态，自身导航状态的构成与导航控制节点nc的状态构成一致，且普通节点的ekf估计误差状态与fgo优化变量中均额外包含导航控制节点nc的ecef位置误差、速度误差以及姿态失准角。

14、进一步地，在步骤3中，

15、当接收到量测信息时，针对每类量测分别构建扩展卡尔曼滤波ekf量测更新模型与因子图优化fgo量测因子；

16、所述量测信息包括无人机间相对伪距量测、无人机自身高度计量测、无人机等效高度计量测；

17、针对无人机间相对伪距量测，基于量测发送端与接收端的位置信息构建对应的量测模型与优化因子；

18、针对无人机自身高度计量测，基于节点自身的位置信息构建对应的量测模型与优化因子；

19、针对无人机等效高度计量测，针对无人机等效高度计量测，基于普通节点所建模的nc节点位置信息，构建对应的量测模型与优化因子；

20、全部量测的模型与因子构建完成后，进入后续处理步骤。

21、进一步地，在步骤5中网络推理流程具体为：

22、步骤5.1：计算任意无人机节点当前7维物特征；

23、步骤5.2：输入7维导航状态数据，特征加权单元计算各维度权重，原始数据与对应权重逐元素相乘，得到加权后的7维特征；

24、步骤5.3：输入7维特征分为两路，主路径经线性变换、批量归一化得到24维特征；残差支路通过线性投影将输入转为24维特征以匹配维度；两路特征相加后经非线性激活，输出24维特征；

25、步骤5.4：输入24维特征分为两路，主路径经线性变换、批量归一化得到16维特征；残差支路通过线性投影将输入转为16维特征以匹配维度；两路特征相加后经非线性激活，输出16维特征；

26、步骤5.5：输入16维特征分为两路，主路径经线性变换得到8维特征；残差支路通过线性投影将输入转为8维特征以匹配维度；两路特征相加后经非线性激活，输出8维特征；

27、步骤5.6：将8维特征输入输出单元，经线性映射得到1维的归一化预测误差。

28、进一步地，在步骤6中，双向重置策略具体流程为：

29、步骤6.1；设定算法核心参数，包括一致性阈值，窗口长度；初始化窗口计数器；

30、步骤6.2；进行窗口计数器迭代；在每一个关键帧将计数器减1，直至计数器归零，以此管理ekf、fgo的切换频率；

31、步骤6.3；进行窗口计数器重置，输入，通过比较与一致性阈值，判断ekf是否健康；若，则重置窗口计数器；若，则不重置；

32、步骤6.4；根据计数器判断当前是ekf模式或fgo模式，当计数器数值大于零时启用fgo模式，当计数器数值为零时启用ekf模式；

33、最终根据选定模式执行状态重置，启用扩展ekf模式时输出其更新结果并重置fgo的迭代初值，启用fgo模式时输出其优化结果并重置ekf的后验估计。

34、步骤6.5；完成ekf/fgo的状态估计后，每个普通无人机节点计算出自身和nc节点在格网坐标系下的相对位置：

35、采用自身惯导输出的ecef位置投影得到nc节点的相对位置；

36、以普通节点自身惯导指示的ecef位置为基础，叠加上本节点估计出的nc惯导误差投影进行修正，得到普通节点相对位置估计结果。

37、一种基于感知的异构双格网相对定位系统；

38、所述系统包括初始化模块、更新模块、量测模型构建模块、量测更新模块、误差预测模块和双向重置解算模块；

39、所述初始化模块，用于规定无人机的状态模型与量测模型，假定无人机集群中共有n个无人机节点，其中有一个为nc节点，其余为普通节点，

40、所述更新模块，用于在所有无人机节点在收到新的imu量测数据时，进行惯导解算、ekf时间更新、imu预积分因子构建；

41、所述量测模型构建模块，用于每个无人机节点判断当前是否收到量测，无人机收到量测时，建立量测模型；若未收到任何量测，返回步骤2；

42、所述量测更新模块，用于获得量测后的无人机进行ekf量测更新，获取7维物理特征；

43、所述误差预测模块，用于向训练后的网络输入7维物理特征，利用注意力门控残差网络进行推理，输出1维的归一化预测误差；

44、所述双向重置解算模块，用于运行双向重置策略，并根据结果采取ekf或fgo；通过神经网络预测的归一化预测误差来评估选择ekf模式或fgo模式，根据无人机状态后验估计，计算出相对坐标系下无人机的相对位置估计。

45、相较于现有技术，本发明的有益效果为：

46、本发明首先基于感知的扩展卡尔曼滤波器（ekf）健康评估方法，有效评估ekf的状态估计不一致程度；其次提出了一种神经驱动的双向重置策略，保证了两种算法切换时的可靠性；在此基础上提出一种基于感知的异构双格网相对定位框架，在保障高精度定位的同时，显著降低计算负担。