一种无人艇集群协同导航定位方法、系统、设备及介质

本发明涉及导航定位，特别涉及一种无人艇集群协同导航定位方法、系统、设备及介质。

背景技术：

1、无人艇集群作为多无人系统的重要发展方向，凭借灵活性高、容错性强和大规模任务执行能力，已广泛应用于军事侦察、海洋监测与资源勘探等领域。协同编队的高效运行依赖可靠的导航定位信息。但高精度传感器价格昂贵，不适合大规模装备，如何在低成本条件下实现高精度、强鲁棒性的协同导航已成为研究热点之一。

2、协同导航通过艇间信息共享提升整体定位精度，常见结构包括并行式和主从式，其中主从式因成本低、实现简单而应用更广。该模式下，领航艇配备高精度导航设备，跟随艇利用低成本传感器获取相对导航信息，并结合领航艇的高精度导航信息实现自定位。现有方法多基于卡尔曼滤波融合，但在实际应用中跟随艇搭载的低成本传感器极易受环境和机动状态的干扰，量测噪声常呈复杂非高斯分布，传统滤波方法无法有效处理复杂非高斯噪声，导致无人艇集群协同导航估计精度明显下降。

3、针对非高斯噪声问题，有粒子滤波等多种滤波方法。其中，粒子滤波理论上适应性强，但计算复杂度较高，不适合无人艇集群的实时应用。其他方法如sage-husa估计、变分贝叶斯估计与学生t估计等也存在收敛性或模型依赖问题。而信息论学习中的在非高斯噪声环境下具有比传统滤波方法更好的表现，特别是与非线性滤波方法容积滤波结合时，在非线性系统中具有更稳定和精确的状态估计能力。

4、然而，核函数的选择会影响最大相关熵滤波的鲁棒能力，现有的最大相关熵滤波采用高斯核函数，对噪声统计特性依赖较强，如果量测系统存在复杂重尾非高斯噪声，则系统状态估计会受到影响，甚至导致滤波发散。如前所述，跟随艇搭载的低成本相对导航传感器容易受到复杂海洋环境和机动状态的干扰，导致量测噪声不再服从高斯分布，从而无法实现准确的协同导航定位。

技术实现思路

1、本发明的目的在于提供一种无人艇集群协同导航定位方法、系统、设备及介质，可以解决现有最大相关熵准则无法有效处理非高斯噪声，导致无法实现准确协同导航定位的问题。

2、为解决上述技术问题，本发明的实施例提供了一种无人艇集群协同导航定位方法，包括以下步骤：

3、建立无人艇集群协同导航系统的状态模型和量测模型；

4、基于广义相关学习的统计线性回归方法，将非线性的量测模型转换为线性的量测模型；

5、采用基于

q-rényi核函数的最大相关熵准则，根据状态模型和线性的量测模型，确定无人艇集群的状态估计方程；

6、通过矩阵求逆引理对状态估计方程进行等价变换，以从等价变换后的状态估计方程中提取滤波增益；

7、采用非线性滤波器通过提取的滤波增益对无人艇集群进行状态估计，并在状态估计的过程中，采用最大后验准则进行迭代更新，以通过迭代更新获得的状态估计结果进行无人艇集群的协同导航定位。

8、可选的，所述建立无人艇集群协同导航系统的状态模型和量测模型，包括：

9、根据无人艇集群中每个无人艇的机动状态，建立状态模型；其中，机动状态包括匀速直线运动状态和匀速转弯运动状态，匀速直线运动状态和匀速转弯运动状态对应的状态模型采用交互多模型算法并行执行；

10、根据无人艇集群协同导航系统中协同导航传感器的性质，建立量测模型。

11、可选的，所述匀速直线运动状态和匀速转弯运动状态对应的状态模型为：

12、；

13、式中，为状态转移矩阵，为状态噪声，为系统状态向量，，和，分别代表跟随艇在地理坐标系

x轴和

y轴下的位置和速度；

14、匀速直线运动状态对应的状态转移矩阵为：

15、；

16、式中，为采样间隔；

17、匀速转弯运动状态对应的状态转移矩阵为：

18、；

19、式中，是转弯角速率，代表左转，代表右转；

20、量测模型为：

21、；

22、式中，表示

k时刻的量测值，包括无人艇集群中领航艇和跟随艇之间的相对速度、相对距离、方位角和俯仰角，和分别是领航艇的位置和速度，表示量测模型的非线性函数，是量测误差。

23、可选的，所述基于广义相关学习的统计线性回归方法，将非线性的量测模型转换为线性的量测模型，包括：

24、基于广义相关学习的统计线性回归方法，构建如下伪量测矩阵：

25、；

26、式中，为状态估计协方差，为互协方差；

27、根据伪量测矩阵，将非线性的量测模型转换为如下线性的量测模型：

28、；

29、式中，和分别表示状态预测矩阵和量测预测矩阵。

30、可选的，所述采用基于

q-rényi核函数的最大相关熵准则，根据状态模型和线性的量测模型，确定无人艇集群的状态估计方程，包括：

31、结合状态模型以及线性化的量测模型构建如下回归方程：

32、；

33、式中，，且的协方差矩阵的平方根形式为；

34、在回归方程的两边同时乘以，构建出如下线性回归模型：

35、；

36、式中，，，；

37、基于

q-rényi核函数的最大相关熵准则，构建如下代价函数：

38、；

39、式中，和分别是系统状态向量和量测向量的维度，是的第i个元素，表示二范数，是

q-rényi核，+表示选择和0之间的最大值，是形状参数；

40、通过最大化代价函数得到：

41、；

42、令代价函数对状态变量的一阶偏导为零，得到：

43、；

44、结合线性回归模型，得到：

45、；

46、；

47、；

48、；

49、展开得到如下状态估计方程：

50、；

51、式中，和分别为状态预测误差协方差矩阵的平方根和量测噪声协方差的平方根。

52、可选的，所述通过矩阵求逆引理对状态估计方程进行等价变换，以从等价变换后的状态估计方程中提取滤波增益，包括：

53、通过矩阵求逆引理对状态估计方程进行等价变换，得到：

54、；

55、从等价变换后的状态估计方程中提取滤波增益：

56、；

57、式中，和分别为提取出的

n阶和

m阶的块矩阵。

58、可选的，所述采用非线性滤波器通过提取的滤波增益对无人艇集群进行状态估计，并在状态估计的过程中，采用最大后验准则进行迭代更新，包括：

59、采用非线性滤波器通过提取的滤波增益对无人艇集群进行状态估计，并基于最大后验准则，获取在采用非线性滤波器进行状态估计过程中状态的后验概率密度函数：

60、；

61、式中，表示矩阵的行列式，是指数运算，并且；

62、通过最大化后验概率密度函数得到一个状态估计，并将和代入其中，得到：

63、；

64、使用牛顿-拉夫逊法，得到状态估计的迭代形式：

65、；

66、式中，表示迭代次数，和分别表示的雅可比矩阵和海森矩阵；

67、和为正定矩阵，则：

68、；

69、；

70、式中，，；

71、；

72、；

73、将当前迭代结果与上一次迭代结果进行比较，若满足，则终止迭代，并令状态估计为。

74、本发明的实施例还提供了一种无人艇集群协同导航定位系统，包括：

75、模型建立模块，用于建立无人艇集群协同导航系统的状态模型和量测模型；

76、模型转换模块，用于基于广义相关学习的统计线性回归方法，将非线性的量测模型转换为线性的量测模型；

77、状态估计模块，用于采用基于

q-rényi核函数的最大相关熵准则，根据状态模型和线性的量测模型，确定无人艇集群的状态估计方程；

78、增益提取模块，用于通过矩阵求逆引理对状态估计方程进行等价变换，以从等价变换后的状态估计方程中提取滤波增益；

79、导航定位模块，用于采用非线性滤波器通过提取的滤波增益对无人艇集群进行状态估计，并在状态估计的过程中，采用最大后验准则进行迭代更新，以通过迭代更新获得的状态估计结果进行无人艇集群的协同导航定位。

80、本发明的实施例还提供了一种计算机设备，包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器中存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述无人艇集群协同导航定位方法。

81、本发明的实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时实现上述无人艇集群协同导航定位方法。

82、本发明所提供的无人艇集群协同导航定位方法，至少具有以下有益效果：

83、本发明中最大相关熵准则采用

q-rényi核函数，在获得无人艇集群协同导航系统的状态模型和量测模型后，首先将非线性的量测模型转换为线性的量测模型，以结合状态模型，通过基于

q-rényi核函数的最大相关熵准则对无人艇集群进行状态估计，将该方法确定的状态估计方程通过引入矩阵求逆引理进行等价变换，从变换后的表达式中可显式提取出基于

q-rényi核函数加权的改进滤波增益，将改进滤波增益矩阵引入非线性滤波器的量测更新阶段，替代传统滤波器中的标准卡尔曼增益，用于更新状态估计，可以实现非线性抗差滤波，即在处理复杂非高斯量测噪声时表现出较强鲁棒性，而在状态估计过程中，采用最大后验准则进行迭代更新，实现最大后验准则与非线性抗差滤波方法的有机结合，能够进一步优化非线性滤波的迭代量测更新，充分利用新息信息提升状态估计精度，从而实现高精度的无人艇集群协同导航定位。