基于变分推断的混合熵因子图多无人系统协同定位方法

本发明涉及多无人系统协同定位，具体而言，涉及基于变分推断的混合熵因子图多无人系统协同定位方法。

背景技术：

1、在军事、农业、交通等领域，如无人机、无人车、水下无人航行器等多无人系统凭借相互通信与协作，能够有效解决单个无人系统作业覆盖范围有限、感知与环境建模困难等问题，精确的定位信息是多无人系统完成协同作业的关键保障。在典型的协同定位架构中，部分无人系统配备高精度惯性导航系统作为主无人系统，其余从无人系统采用低成本导航定位系统，通过与主无人系统进行通信测距，从无人系统利用协同定位算法修正自身导航信息，从而实现降低成本并提升整体定位精度的目标。

2、现有技术中，基于因子图的协同定位方法通过将各无人系统的位置信息和观测信息抽象为因子节点，利用和积算法进行消息传递与信息融合，已成为多无人系统协同定位的主流方案之一。为进一步提升算法在非高斯噪声环境下的鲁棒性，部分现有技术引入了基于相关熵（correntropy）的代价函数，以替代传统的最小均方误差准则，取得了较好的抗干扰效果。特别是采用两个不同核带宽高斯核函数凸组合的混合熵方法，相较于单一核带宽方法，能够适应更复杂的噪声分布形态。然而，上述现有技术均将混合熵中的核带宽参数和混合系数设置为预先离线确定的固定超参数。在实际长航时作业中，无人系统的作业环境并非一成不变，例如，无人系统从开阔水域驶入港区、从室外进入室内、或遭遇多径效应突变的区域时，测距传感器所面临的非高斯噪声的尖峰强度和拖尾厚度会随着环境的变化而动态演化。当预设的固定核带宽与当前时刻真实噪声分布形态发生失配时，相关熵作为局部相似性度量的有效性将大幅降低，导致因子图消息传递过程中虽然混合熵在数值上被最大化，但其对应的最优解却严重偏离从无人系统的真实位置，即定位精度骤降。

3、因此，有必要提供基于变分推断的混合熵因子图多无人系统协同定位方法，旨在解决当前技术中核参数在时变非高斯噪声环境下无法跟随噪声变化而自适应调整的问题。

技术实现思路

1、鉴于此，本发明提出了基于变分推断的混合熵因子图多无人系统协同定位方法，旨在解决当前技术中核参数在时变非高斯噪声环境下无法跟随噪声变化而自适应调整的问题。

2、本发明提出了基于变分推断的混合熵因子图多无人系统协同定位方法，包括以下步骤：

3、s100、构建多无人系统协同定位的扩展因子图模型，基于两个具有不同核带宽的高斯核函数的凸组合构建混合相关熵代价函数；其中，所述扩展因子图模型的节点分别由多无人系统的状态变量和观测约束、以及两个高斯核函数的核参数抽象得到；

4、s200、对所述节点中的变量节点分别进行先验分布初始化设置，在完成先验分布初始化设置后，基于固定采样周期获取主无人系统的位置信息与从无人系统的测距量测数据；

5、s300、以所述位置信息和测距量测数据为输入，基于所述扩展因子图模型，并以最大化混合相关熵为整体优化目标，采用变分贝叶斯推断对从无人系统的位置状态和所述核参数进行联合后验估计，得到当前采样周期的所述位置状态的近似后验分布和所述核参数的近似后验分布；

6、s400、确定变分贝叶斯推断是否满足预设迭代终止条件，当满足预设迭代终止条件时，基于所述位置状态的近似后验分布，确定当前采样周期的从无人系统的最优位置坐标，并将当前采样周期的所述位置状态的近似后验分布和所述核参数的近似后验分布分别作为下一采样周期对应位置状态和核参数的先验分布。

7、进一步的，所述扩展因子图模型的节点分别由多无人系统的状态变量和观测约束、以及两个高斯核函数的核参数抽象得到时，包括：

8、将从无人系统的位置状态变量抽象为所述扩展因子图模型的位置状态变量节点；

9、将两个高斯核函数的核参数抽象为所述扩展因子图模型的核参数变量节点；其中，

10、所述核参数变量节点包括两个高斯核函数各自的核带宽变量节点以及两个高斯核函数凸组合的混合系数变量节点；

11、将从无人系统的运动状态递推约束抽象为所述扩展因子图模型的运动因子节点；

12、将主无人系统与从无人系统的测距量测约束抽象为所述扩展因子图模型的测距因子节点；

13、基于核参数增设核参数先验因子节点，所述核参数先验因子节点与对应所述核参数变量节点连接。

14、进一步的，对所述节点中的变量节点分别进行先验分布初始化设置时，包括：

15、对从无人系统的位置状态变量节点设置位置状态初始高斯先验分布，并基于从无人系统的初始航位信息确定所述位置状态初始高斯先验分布的超参数；

16、对两个高斯核函数凸组合的混合系数变量节点设置贝塔共轭先验分布；

17、对两个高斯核函数各自的核带宽变量节点设置逆伽马共轭先验分布。

18、进一步的，采用变分贝叶斯推断对从无人系统的位置状态和所述核参数进行联合后验估计时，包括：

19、采用坐标上升方式，以证据下界为依据进行交替迭代求解，得到当前采样周期的所述位置状态的近似后验分布和所述核参数的近似后验分布。

20、进一步的，以证据下界为依据进行交替迭代求解时的每一轮交替迭代过程中，包括：

21、基于所述混合系数变量节点的近似后验分布的均值作为当前交替迭代轮次中混合相关熵代价函数的混合系数，并基于两个所述核带宽变量节点的近似后验分布的均值分别作为当前交替迭代轮次中混合相关熵代价函数的两个核带宽，确定当前交替迭代轮次中混合相关熵代价函数；基于当前交替迭代轮次中混合相关熵代价函数以及扩展因子图模型，更新得到位置状态的近似后验分布；

22、固定更新后的所述位置状态的近似后验分布不变，以及固定两个核带宽变量节点的近似后验分布不变，基于混合系数变量节点的贝塔共轭先验分布，更新得到混合系数的近似后验分布；

23、固定更新后的所述位置状态的近似后验分布不变，以及固定更新后的混合系数的近似后验分布不变，基于各核带宽变量节点的逆伽马共轭先验分布，分别更新得到两个高斯核函数各自核带宽的近似后验分布。

24、进一步的，基于当前交替迭代轮次中混合相关熵代价函数以及扩展因子图模型，更新得到位置状态的近似后验分布时，包括：

25、基于当前交替迭代轮次中混合相关熵代价函数，在扩展因子图模型上进行和积运算，获得所述位置状态变量节点的高斯消息分布；其中，

26、所述高斯消息分布包括位置状态均值信息和位置状态方差信息；

27、将所述位置状态均值信息作为位置状态近似后验分布的均值参数，将所述位置状态方差信息作为位置状态近似后验分布的方差参数，基于所述均值参数和方差参数确定更新后的位置状态的近似后验分布。

28、进一步的，基于混合系数变量节点的贝塔共轭先验分布，更新得到混合系数的近似后验分布时，包括：

29、获取所述混合系数变量节点在当前交替迭代轮次的贝塔分布，所述贝塔分布包括第一形状参数的第一当前值和第二形状参数的第二当前值；

30、获取所述混合系数变量节点对应的混合系数，以贝塔分布作为概率测度，在贝塔分布的概率测度下将混合系数的第一自然对数的期望值作为第一统计量，并获取1减所述混合系数的差值的第二自然对数，将所述第二自然对数的期望值作为第二统计量；

31、将所述第一统计量的取值加到所述第一形状参数的所述第一当前值上，得到更新后的第一形状参数；将所述第二统计量的取值加到所述第二形状参数的所述第二当前值上，得到更新后的第二形状参数；

32、基于所述更新后的第一形状参数和所述更新后的第二形状参数，构造更新后的贝塔分布，并将所述更新后的贝塔分布作为更新后的混合系数变量节点的近似后验分布。

33、进一步的，基于各核带宽变量节点的逆伽马共轭先验分布，分别更新得到两个高斯核函数各自核带宽的近似后验分布时，包括：

34、获取各核带宽变量节点在当前交替迭代轮次的逆伽马分布，所述逆伽马分布包括第三形状参数的第三当前值和尺度参数的第四当前值；

35、获取各核带宽变量节点对应的核带宽，以更新后的位置状态的近似后验分布作为概率测度，在更新后的位置状态的近似后验分布的概率测度下，获取测距残差平方项的数学期望，得到各核带宽对应的第三统计量；其中，所述测距残差平方项为主无人系统与从无人系统之间的距离估计值与距离观测值之差的平方；

36、将所述第三统计量的取值加到所述尺度参数的第四当前值上，得到更新后的尺度参数；

37、基于第三当前值和更新后的尺度参数，构造更新后的逆伽马分布，并将更新后的逆伽马分布作为更新后的对应核带宽变量节点的近似后验分布。

38、进一步的，确定变分贝叶斯推断是否满足预设迭代终止条件时，包括：

39、当所述证据下界连续预设数量相邻轮次的增加量小于或等于预设收敛阈值时，确定变分贝叶斯推断满足预设迭代终止条件；

40、否则，确定变分贝叶斯推断不满足预设迭代终止条件。

41、进一步的，当满足预设迭代终止条件时，基于所述位置状态的近似后验分布，确定当前采样周期的从无人系统的最优位置坐标时，包括：

42、当满足预设迭代终止条件时，获取所述位置状态的近似后验分布的均值向量，将所述均值向量作为当前采样周期的从无人系统的最优位置坐标。

43、与现有技术相比，本发明的有益效果在于：构建多无人系统协同定位的扩展因子图模型，基于两个具有不同核带宽的高斯核函数的凸组合构建混合相关熵代价函数；其中，所述扩展因子图模型的节点分别由多无人系统的状态变量和观测约束、以及两个高斯核函数的核参数抽象得到，将核参数作为节点纳入拓展因子图模型中，使得核参数成为因子图模型中具有概率属性的内生变量，而非固定常数，使得核参数与从无人系统的位置状态在同一概率框架下进行联合估计，从根本上解决了传统因子图方法中核参数作为离线固定超参数无法跟随环境变化而自适应调整的技术缺陷。对所述节点中的变量节点分别进行先验分布初始化设置，在完成先验分布初始化设置后，基于固定采样周期获取主无人系统的位置信息与从无人系统的测距量测数据，完成先验分布的初始化设置，可为后续在线推断提供初始的先验信息，获取的主无人系统的位置信息与从无人系统的测距量测数据，可为后续基于变分贝叶斯推断对从无人系统的位置状态和所述核参数进行联合后验估计提供数据基础。以所述位置信息和测距量测数据为输入，基于所述扩展因子图模型，并以最大化混合相关熵为整体优化目标，采用变分贝叶斯推断对从无人系统的位置状态和所述核参数进行联合后验估计，得到当前采样周期的所述位置状态的近似后验分布和所述核参数的近似后验分布，采用贝叶斯推断对从无人系统的位置状态和核参数进行联合后验估计，同时更新位置状态和核参数的后验分布，使得核参数能够跟随当前噪声环境的变化而自适应调整。确定变分贝叶斯推断是否满足预设迭代终止条件，当满足预设迭代终止条件时，基于所述位置状态的近似后验分布，确定当前采样周期的从无人系统的最优位置坐标。通过设置预设迭代终止条件，能够在保证估计精度的前提下及时收敛，避免了计算资源的消耗。并将当前采样周期的所述位置状态的近似后验分布和所述核参数的近似后验分布分别作为下一采样周期对应位置状态和核参数的先验分布，通过将当前采样周期的后验分布传递至下一采样周期作为先验分布，使得核参数的估计能够在时间轴上进行持续积累，能够确保系统在整个任务周期内始终保持对时变噪声环境的自适应跟踪能力。

44、综上所述，本发明将核参数纳入扩展因子图模型中，和位置状态采用变分推断进行联合后验估计，在线迭代更新核宽带参数和混合系数，使得核参数能够跟随环境噪声的变化而自适应调整，能够在非高斯噪声环境下显著提升系统状态估计的鲁棒性和定位精度。