本发明属于水下航行器的协同定位领域，涉及一种基于拓扑信息的水下航行器协同定位方法。
背景技术：
水下航行器在水下作业时，由于接收不到精准的gps信号，只能依赖内部的传感器进行导航，像惯性导航等，在这种传统的方式下，误差会随着时间的延长而积累。鉴于此，本文发明了一种利用拓扑因子的协同定位方法，此方法把水下航行器间的距离作为约束因子，使得水下航行器不用浮上水面也可以获得比之前高的定位精度。而且，在加上gps信号的情况下，使用了拓扑因子作为约束的定位方法也比传统方法的定位精度高。
技术实现要素：
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种基于拓扑信息的水下航行器协同定位方法，利用拓扑因子增加协同定位的精确度。实验用georgiatechsmoothingandmapping(gtsam)来进行。
技术方案
一种基于拓扑信息的水下航行器协同定位方法，其特征在于步骤如下：
步骤1：采用matlab模拟出四个水下航行器的运行轨迹图，并取得四个水下航行器的运行轨迹图的测量数据：
步骤2：以下式表示拓扑信息,计算因子图中的水下航行器之间的距离
其中：l表示航行器的数量，其中
步骤3：根据计算的拓扑信息，采用gtsam构建因子图；x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，每个水下航行器本时刻坐标与下一时刻坐标之间的联系为内部导航系统的里程计因子fin，每个时刻四个水下航行器之间的联系为四个水下航行器之间的距离约束ftop；
步骤4：采用levenberg-marquardt法对拓扑约束的因子图进行优化，得到水下航行器各个时刻的估计位姿。
有益效果
本发明提出的一种基于拓扑信息的水下航行器协同定位方法，用matlab模拟出四个水下航行器的运行轨迹图，用gtsam构建出因子图，以水下航行器之间的距离作为约束条件，加入到对位姿的优化中去，使得定位更加的精确。本发明解决水下航行器由于接收不到高精度的gps信号而产生的定位误差，经过实验验证，该方法确实可以提高定位的精度，减小误差。
附图说明
图1：水下航行器的拓扑因子图
图2：水下航行器的行驶轨迹
图3：使用拓扑因子协同定位与传统方法的实验结果图
具体实施方式
现结合实施例、附图对本发明作进一步描述：
在本实验中，水面船和四个水下航行器都配备有声呐和gps,且水下航行器有内部的导航系统。
第一步：我们用matlab模拟出四个水下航行器的运行轨迹图(设定初始位置，方向，和增长速度的直线)，轨迹图如附图2，并记录下数据：速度，位置坐标，行驶距离。
第二步：用gtsam构建出因子图并进行优化。因子图如附图1，其中，变量x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，fin是内部导航系统的里程计因子，表示两个位置之间的距离变化，把两个相邻的位置关联起来。ftop是四个水下航行器之间的距离约束，也就是拓扑因子，把四个航行器在同一时刻的位置关联起来。
在构建拓扑因子时，采用如下公式计算因子图中的水下航行器之间的距离
上式中l表示航行器的数量，其中
在计算拓扑信息时，由误差引起的不确定性也会随之传递，我们用下面的公式来表示拓扑信息的不确定性：
cy＝fxcxfxt(2)
其中cx是指从声呐测量的最初的不确定性，是一个协方差矩阵。fx是传递函数
由以上的数据关联和计算公式，我们可以用gtsam构建出因子图，构建的因子图如附图1。
其中，变量x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，fin是内部导航系统的里程计因子，ftop是四个水下航行器之间的距离约束，也就是拓扑因子。
得到有拓扑约束的因子图后，此处采用levenberg-marquardt法进行优化，得到水下航行器各个时刻的估计位姿。
第三步：比较加上拓扑约束的协同定位方法和传统方法的差别，得出此发明可以更精确的定位。
在此发明中，采用计算均方根偏差(rmse)来比较两种方法的误差大小，计算公式如下：
上式中n表示测量的总步数，k表示第k步，l表示航行器的个数，其中
本发明的一个具体实施步骤如下：
⑴设置gps精度为10倍的方差为1的高斯白噪声，内部里程计的精度为方差为1的高斯白噪声，声呐的精度为0.1倍的方差为1的高斯白噪声，以此进行实验。
⑵确定水下航行器的初始位置和x，y方向的速度
航行器①(100,200,18,5)航行器②(-100,300，5,20)
航行器③(-300,-200，-17,20)航行器④(300，-200,19，-17)
括号里的四个数据分别为航行器的x，y初始位置和x，y方向上的速度。以此速度状态进行300步，得出每个水下航行器的运行轨迹图。在图2中，每个航行器都朝着不同的方向行驶了不同的距离，在上面采集真值和测量值。
⑶实验中对每个水下航行器进行300次数据采集，得出仿真图进行对比分析。在图3中，红色的实线代表使用了拓扑因子作为约束后的误差大小，绿色的虚线表示传统方法的误差大小。由图3可以看出，使用拓扑信息后的协同定位方法比传统方法的均方根误差要小1.5，可见，引入拓扑因子作为约束能使得对水下航行器的定位更加的精准。