基于因子图的无人艇集群地磁匹配协同定位方法

本发明涉及船舶组合导航领域，本发明是一种适用于无人艇集群海上长时间定位的方法，特别涉及一种基于因子图(factor graph)的地磁匹配协同定位方法(geomagneticmatching cooperative positioning method for unmanned vehicle cluster)。

背景技术：

1、无人艇具有体积小、速度高、成本低和无人员伤亡风险等优点。然而，由于海面情况日益复杂，单个无人艇已经无法满足救援需求，多个无人艇组成的无人艇集群不仅可以提高效率而且可以提升整个集群定位的精度和稳定度，具有更高的研究价值。目前在海上无人艇上应用的传统导航设备主要有全球卫星导航系统global positioning system(gps)和惯性导航系统inertial navigation system(ins)。其中，gps具有全球、全天候的导航定位授时服务，并且精度高、误差不会随着船舶航行的时间积累，在差分校准的模式下，定位精度可以达到厘米级。但是gps的抗干扰能力差，在复杂的海面情况下，gps很容易受到天气、无线电等干扰从而导致输出错误的导航信息。ins具有自主性强、不受外界干扰的优点，但是ins的误差会随着无人艇航行时间的增加而迅速积累，所以仅依靠惯性导航也无法满足高精度导航需求，而无人艇集群整体导航定位精度和稳定度直接影响其协同的能力。

2、地磁导航作为一种新兴的导航方式，一定程度上克服传统导航方法的缺陷，作为地球的一个基本物理场，地磁场可以在任何季节、气候以及地理位置(如高空、水下等)被检测到，使得地磁导航成为全天候、全地域的导航。地磁匹配是地磁导航的核心，地磁导航就是利用地磁图特征与当地即时测量磁特征信息进行匹配导航的过程。现有的地磁匹配技术主要包括基于magcom的地磁匹配算法和基于iccp算法的地磁匹配方法，前者无法旋转搜索，仅可以平动搜索，所以无法对航向的误差进行修正。而iccp算法借鉴了图像匹配中的最近迭代点匹配算法，既能平动又可以旋转搜索，有效的修正了航向误差，但是iccp算法要求载体的真实位置位于参考导航系统指示的位置附近，如果参考位置与真实位置相差很远，就很容易导致等值线算法失效，甚至引起误匹配。且考虑到成本问题，在单一无人艇上的惯导设备精度不会很高，提供的参考位置误差较大。

3、而现有的协同方法主要是通过把gps、ins等一系列导航源组合来实现协同定位。该研究方向主要包含两类，一类是以神经网络为代表的定位数据型融合，另一类是以卡尔曼为代表的定位结果型融合，但两类算法都具有计算复杂度高，实时性差的问题，并且复杂的海面和天气情况会使gps信号变差甚至无法接收，所以并不适合用于海上无人艇集群定位。

4、因此迫切需要一种不需要gps信号、不依赖ins、能长时间稳定导航的协同定位方法，来实现无人艇集群在复杂海面情况下的导航。

技术实现思路

1、本发明针对在海面情况复杂且无人艇需要长时间航行的情况下，gps信号易被干扰且ins误差较大问题，通过构建无人艇集群协同定位系统模型，提出了一种基于因子图的无人艇集群地磁匹配协同定位方法。首先利用基于因子图的协同定位，对无人艇集群的定位进行估计，再将该估计定位作为参考位置基于iccp算法进行地磁匹配，对估计定位进行修正，得到精确定位，本发明方法不再需要gps信号，不再依赖惯性导航，能够有效提高无人艇集群的定位精度和稳定度，从而能够实现复杂海面环境下无人艇集群的长时间自主导航定位。

2、本发明的技术方案为：

3、所述一种基于因子图的无人艇集群地磁匹配协同定位方法，其特征在于：包括以下步骤：

4、步骤1：构建无人艇集群协同定位网络，集群中每一个无人艇为一个带有位置模糊度的待定位节点，待定位节点通信范围内所有邻居节点为协同定位节点，无人艇上均配备有磁传感器、速度角度测量单元以及测距设备，可以获取无人艇的所处位置的磁感应强度、初始坐标以及无人艇之间的测距信息；

5、步骤2：在某一时刻k，对于每一个待定位节点，采用基于因子图的协同定位策略为所有无人艇提供定位，获得k时刻各个无人艇的位置估计；

6、步骤3：将步骤2中得到的位置估计看作无人艇各自的航迹点，对航迹点基于iccp算法进行地磁匹配，获得k时刻各个无人艇的准确位置。

7、进一步的，步骤2中采用基于因子图的协同定位策略为所有无人艇提供定位，具体包括以下子步骤：

8、步骤2.1：对于每个待定位节点，接收通信范围内所有邻居节点的位置信息，测量与邻居节点的距离，得到测距误差的统计特性；根据测距信息，构建待定位节点的因子图；所述因子图包括变量节点和函数节点；将待定位节点和所有协同定位节点作为变量节点，待定位节点和其它协同定位节点之间具有函数节点；所述函数节点用于定位信息的数据处理；

9、步骤2.2：构建待定位节点对应的内部因子图，所述内部因子图也包括变量节点和函数节点；

10、对于具体待定位节点mq，通信范围内有n邻居节点{mq1,…,mqn}，均为待定位节点；

11、由于在海面上无人艇坐标可近似为二维坐标，这里我们定义参考坐标系，其中x轴为经度，y轴为纬度，且待定位无人艇坐标解算过程中，各个坐标轴相互独立，因此本发明将二维定位问题转化为2个支路上一维问题，分为x坐标和y坐标，即经度值和纬度值，那么，待定位节点mq内部因子图中包含变量节点di，δxiq，δyiq，xq，yq，函数节点di，ei，ai，bi，其中i＝1,2,…,n；

12、表示待定位节点mq与其通信范围内第i个邻居节点的测距信息；di基于函数节点di产生，在数值上等于δxiq，δyiq表示待定位节点mq与其通信范围内第i个邻居节点在坐标轴上的相对距离；xq，yq表示待定位节点mq的位置估计；di表示测距信息的统计特性，服从均值为方差为的高斯分布；ei用于转换xq，yq之间的置信度信息，ai，bi用来描述待定位节点mq与相对于第i个邻居节点在两个轴上的方向和距离；

13、步骤2.3：对于具体待定位节点mq，通过循环迭代的方式得到待定位节点的位置信息；所述循环迭代，分为上行迭代与下行迭代；

14、首先待定位节点mq与通信范围内所有邻居节点的测距信息由函数节点进入该定位节点的内部因子图，在上行迭代过程中，通过函数节点依循和积算法的置信度信息传递准则更新变量节点的置信度信息；其次进行下行迭代为下一次的迭代更新做准备，最后得到待定位节点mq的位置信息；在判定达到最大迭代次数收敛时，循环结束；具体如下：

15、以第r次迭代为例，在进行第r次迭代时，将待定位节点mq与通信范围内第i个邻居节点之间的测距信息送入内部因子图中对应支路的函数节点得到对应变量节点对应的置信度信息；

16、

17、其中服从均值为方差为的高斯分布，bi()表示置信度信息，置信度信息包括均值和方差，n()表示待定位节点mq与通信范围内第i个邻居节点的测距信息统计特性分布；

18、(a)第r次上行迭代

19、利用函数节点更新变量节点的置信度信息：

20、

21、

22、其中，分别表示变量节点的均值，分别表示变量节点的方差；

23、利用函数节点实现相对位置和绝对位置信息的转换，更新变量节点的置信度信息：

24、

25、

26、(b)第r次下行迭代

27、变量节点基于和积算法计算准则，更新函数节点的置信度信息：

28、

29、

30、其中，分别是关于变量的高斯分布；

31、函数节点按照公式

32、

33、

34、更新变量节点的置信度信息；其中分别表示变量节点的均值，分别表示变量节点的方差；

35、(c)依据公式

36、

37、

38、更新待定位节点mq的置信度信息。

39、在协同定位网络中所有待定位节点并行完成上行迭代和下行迭代后，当变量节点xq、yq的信息收敛或达到最大迭代次数后，循环结束，得到所有待定位无人艇坐标mq的位置信息。

40、进一步的，在步骤3中对航迹点基于iccp算法进行地磁匹配，获得k时刻各个无人艇的准确位置，具体包括以下子步骤：

41、步骤3.1：将步骤2中的位置估计看作无人艇各自的航迹点，获取航迹点集合；

42、假设在步骤2中待定位无人艇坐标mq完成位置更新后的坐标为而k-n+1时刻到k-1时刻已经完成地磁匹配后的坐标为将位置估计看作无人艇各自的航迹点，则其中任意一艘无人艇的航迹点集合为h＝{hi}(i＝1,2,3…n)，其中前n-1个航迹点由组成，第n个航迹点由组成。

43、步骤3.2：基于步骤3.1获得的导航测得的航迹点hi的集合，获取地磁场强度等值线距离hi最近的点gi的集合；

44、在步骤3.1中获得的航迹点集合h＝{hi}(i＝1,2,3…n)为导航系统测出的航迹点集合，假设真实的航迹点集合为p＝{pi}(i＝1,2,3…n)，磁传感器测得的磁感应强度集合为fi(i＝1,2,3…n)，由于导航误差的存在，测量航迹点hi和真实航迹点pi一定不重合，但如果磁传感器没有测量误差，那么pi必然也在对应的地磁场强度等值线ci(i＝1,2,3…n)上，但是并不知道具体是哪一点。所以需要在ci上找出和hi距离最小的点gi(i＝1,2,3…n)，得到集合g＝{gi}(i＝1,2,3…n)，具体过程为：

45、假设取航迹点集合h＝{hi}中的某一测量航迹点，其坐标为hi(xi，yi)，即为待匹配的点，磁感应强度为f，f对应的等值线为c，搜索地磁基准图中区域内每个网格点的四条边，判断等值线是否经过其中两条边，所述地磁基准图是由地磁场特征量绘制成的参考图。若等值线值f在某条边的两个端点的值之间，则认为等值线经过该条边。假设等值线c与一条横边相交，这条横边的两个端点分别为a(xk，yk)、b(xk+1，yk)，磁感应强度分别为fk和fk+1，那么等值线与该条边的交点横坐标与纵坐标分别为：

46、

47、y′＝yk

48、同理，当等值线与一条纵边相交时，交点横坐标与纵坐标为：

49、x″＝xk

50、

51、搜索完毕之后，得到等值线c与网格线所有交点，将这些交点连接起来，得到一个近似等值线c的多段折线l＝(a0,a1,a2,…an)，定义其中的线段为等值线线段，其中aj(j＝1,2,3…n)是这些等值线线段的端点，即上面所求的(x′,y′)和(x″,y″)。

52、首先计算待匹配点hi向每一条等值线线段的最近距离，假设lj＝(aj,aj+1)是l中的一段线段，线段两端点为aj,aj+1，设lj上离hi最近的点为yj，若待匹配点hi对这一条等值线线段的投影bj在该线段上，则yj就是投影点bj，否则yj为两端点之一，其中bj的计算公式如下：

53、bj＝(1-μ)aj+μaj+1,

54、其中lj＝‖aj+1-aj‖，t为矩阵的转置，则

55、

56、比较各个最近距离，选择其中最小的yj作为最终的等值线距离hi最近的点gi。将该步骤应用到每一个航迹点上，得到集合g＝{gi}。

57、步骤3.3：求解刚性变换矩阵t，通过循环迭代，获得最终定位thi；

58、求解变换矩阵t，使得集合g＝{gi}和h＝{hi}间的欧氏距离最小，在距离度量中引入权系数wi以表征第i个点的重要程度，则距离为变换矩阵t由平移矢量t和旋转矩阵r组成，即thi＝t+rhi。

59、首先求出集合g和h的质心，即

60、

61、其中表示等值线最近点集合g的质心，表示航迹点集合h的质心，w表示权系数wi的和，旋转矩阵采用单位四元数q＝(q0,q1,q2,q3)t表示，其中q0表示单位四元数的实部，它对应四元数中的实数部分，q1、q2和q3分别表示单位四元数的虚部的三个独立分量，它们对应四元数中虚部的三个基本单位向量的系数，且有四元数方法最初用于三维情况，假设以矢量为旋转轴，其中v1,v2,v3分别表示旋转轴在x轴、y轴和z轴方向上的分量，旋转角度θ，用四元数法表示如下：

62、

63、由上式可知旋转轴和旋转角度θ可以用单位四元数描述：

64、

65、本发明中，地磁匹配时用四元数法处理的是二维地磁基准图，因此将旋转轴表示为则q的表达式化简为：

66、

67、根据iccp算法的数学原理，将旋转矩阵r表示为

68、

69、其中，旋转角θ的求解过程如下：

70、

71、

72、其中，分别表示测量航迹点hi、等值线距离hi最近的点gi相对它们各自点集质心的位置坐标，s表示测量航迹点hi和等值线距离hi最近的点gi构成的2阶方阵，矩阵w的四个特征值都是实数，由下式给出：

73、

74、记最大的特征值为λm，其对应的特征向量由下式求得：

75、(s11+s12-λm)q0+(s21-s12)q3＝0

76、由上式以及得：

77、

78、将θ代入r的表达式中，即得到旋转矩阵r，进而计算获得平移矢量

79、获得旋转矩阵r和平移矢量t即得到了刚性变换矩阵t，先旋转集合h＝{hi}使其对准集合g＝{gi}的方向，然后进行平移，使h的质心与g的质心重合，得到的新集合，求出所得新集合和g＝{gi}对应点的路径长度平方和，即判断是否满足收敛条件，如果不满足，将得到的结果：thi＝t+rhi作为下一次迭代的初始值，转至步骤3.2进行迭代；如果满足收敛条件，迭代结束，得到的thi就是最后匹配的结果，进而得到k时刻各无人艇的准确位置，完成定位。

80、有益效果

81、本发明提出的基于因子图的无人艇集群地磁匹配协同定位方法，通过先利用基于因子图的协同定位，对无人艇集群的进行定位，获得位置估计，再将该定位作为参考位置进行iccp算法地磁匹配，对位置估计进行修正，获得准确定位，从而不再需要gps信号，不再依赖惯性导航，因此能够有效解决复杂海面情况下gps容易受到干扰从而导致输出错误的导航信息以及ins的误差随着无人艇航行时间的增加而迅速积累的问题，可满足无人艇集群在不同海上环境下连续、长时间航行的高精度定位需求。