一种基于水声通信时间差的多潜器实时协同定位方法

1.本发明涉及潜航器定位领域，尤其涉及一种基于水声通信时间差的多潜器实时协同定位方法。


背景技术：

2.水下无人潜航器(简称潜器)集群因其具备机动性强、作用范围灵活、鲁棒性高等优势，在水下军事勘探、深海海洋勘探、生物监测等领域得到广泛应用。潜器自身位置的准确获取是多潜器集群高效执行任务的重要前提。在现有的潜器定位方法中，通常在相关的某一固定海域布放锚节点，通过多个位置已知的锚节点实现潜器定位，典型的如长基线定位系统。然而，锚节点的覆盖范围就广大海域而言十分有限，且锚节点的布放位置需要事先确定，恶劣的水下环境以及锚节点自身可携带电池容量有限等原因使得锚节点无法长时间处于工作状态。另一方面，当面临在移动范围内执行任务时，长基线定位系统无法实现在机动区域内对潜器的高精度定位。另一种方法是根据潜器配备的导航设备实现实时定位，虽具有灵活机动、无需任何锚节点的优势，但高精度导航设备价格昂贵，且存在累积误差使得潜器定位误差无限增长。随着水声通信技术的进步和潜器的智能化发展，多潜器协同定位具备实现高精度定位的潜力，从而得到广泛研究。
3.无论是基线类定位系统还是多潜器协同定位系统，均是以距离量测为基础，而准确的距离量测是以潜器之间、锚节点之间、锚节点与潜器之间的时钟同步为前提。然而，在水下环境中，由于潜器自身时钟相关元器件制作工艺以及受水下温度、盐度、深度等因素影响，多潜器之间的时钟难以保持同步，尤其是在长时间航行过程。另一方面，水声信道的复杂性和水声通信的延迟效应不仅降低了潜器系统的协同更新频率，而且造成的潜器移动进一步降低了协同定位精度。因此，在无锚节点辅助定位，且潜器之间时钟非同步的情况下，如何动态实时实现潜器的高精度协同定位亟需研究和解决。
4.申请号为202210102709.7的名称为“一种集成超短基线系统的uuv集群协同定位方法”的专利提供了一种基于距离测量和超短基线系统的方法，虽然可以提升主从式潜器系统的协同定位精度，但超短基线系统需要事先严格校准，且对于距离测量，不仅需要通信双方保证时钟同步，而且受限于水声通信延迟大的不足，使得双程水声通信带来了uuv位置的偏差以及距离误差的增大。申请号为202111610043.8的名称为“一种双主auv切换的auv集群大水深作业的协同导航定位方法”的专利提供了一种面向主从式潜器系统，利用主auv定期上浮至水面实现自身位置校正进而通过多潜器协同定位方法提升从auv定位精度，这不仅降低了系统协同更新频率，也无法解决主从auv时钟不同步的问题。申请号为202210213443.3的名称为“一种基于神经网络的分布式小平台水下纯方位定位方法”的专利提供了一种使用参考节点与待定位潜器间的方位信息的单潜器定位方法，但方位信息的准确程度依赖于测量阵列的结构，而潜器由于自身电池有限，阵列无法长时间工作，而且多阵元阵列由于体积等原因严重影响潜器的机动。综上而言，上述方法面向潜器定位问题均存在不足，无法实现实时定位，且未具体考虑潜器自身配备的导航设备累积误差对多潜器
协同定位结果的影响。
5.因此，本领域的技术人员致力于开发一种基于水声通信时间差的多潜器实时协同定位方法。本发明面向水下无锚场景中时钟非同步的多潜器动态实时协同定位需求，提出一种基于水声通信时间差的多潜器实时协同定位方法。区别于以使用潜器间的相对距离测量为主的传统定位方法，面对水下无锚场景，针对仅具备主潜器和从潜器，且二者时钟不同步的潜器系统，利用主从潜器间的相邻时刻水声通信时间差和潜器自身导航信息，实现对从潜器的实时协同定位，以达到提升潜器系统定位精度的目的。


技术实现要素：

6.有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是多潜器之间的时钟难以保持同步，潜器测量性能受限，潜器在水下无锚场景中实时定位难的问题。
7.为实现上述目的，本发明提供了一种基于水声通信时间差的多潜器实时协同定位方法，包括以下步骤：
8.步骤1、主潜器、从潜器设备初始化；
9.步骤2、主潜器、从潜器分别记录初始时间信息和初始位置信息，建立通信链接并完成单向水声通信，即主潜器发送信息，从潜器接收信息，具体为：主潜器发送自身位置实际值(三维笛卡尔坐标系下位置表示方法，下同)和发送信息时间从潜器接收消息后，记录接收信息的实际位置及接收时间
10.步骤3、在(i+1)(i≥0)时刻，主潜器、从潜器完成单向水声通信，从潜器获得主潜器时间信息主潜器的实际位置信息自身时间信息以及根据自身配备的低精度导航设备获取的位置粗预测值
11.步骤4、在(i+1)(i≥0)时刻，利用i时刻和(i+1)时刻主潜器、从潜器的时间信息获取水声通信时间差(time difference ofcommunication，tdoc)δτ
i+1
；
12.步骤5、利用从潜器在i时刻的位置估计值和在(i+1)时刻的位置粗预测值获得从潜器的航向角、俯仰角和速度的粗量测值并基于从潜器在(i+1)时刻的位置粗预测值获得修正值
13.步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息和水声通信时间差δτ
i+1
，建立动态时空关联的协同定位模型并构建关于从潜器航向角从潜器俯仰角及主潜器、从潜器传播时差δti的非线性量测方程；在充分考虑各项噪声的基础上线性化非线性量测方程；
14.步骤7、将线性化量测方程转化为加权最小二乘问题，得到从潜器在(i+1)时刻位置估计值的显示表达式；
15.步骤8、分析定位误差来源，得到从潜器在(i+1)时刻相对位置估计值的位置误差补偿量的显示表达式；
16.步骤9、根据从潜器在(i+1)时刻的位置估计值及其位置误差补偿量得
到从潜器在(i+1)时刻位置高精度估计值
17.步骤10、重复执行所述步骤3到所述步骤9，实时估计从潜器的位置，直至航行结束。
18.进一步地，所述步骤1设备初始化包括确定导航设备、深度传感器和水声通讯机是否正常工作。
19.进一步地，所述步骤4，水声通信时间差其中，和和分别为主潜器和从潜器在(i+1)时刻和i时刻的时间。
20.进一步地，所述步骤5，当从潜器航向角从潜器俯仰角的误差和相对于真值较小时，可基于从潜器在(i+1)时刻的位置粗预测值获得修正值
21.进一步地，所述步骤6利用泰勒公式线性化非线性量测方程。
22.进一步地，所述步骤6包括以下步骤：
23.步骤6.1、利用主从潜器在和的位置真值和水声通信时间差真值，构建动态时空关联的协同定位模型；
24.步骤6.2、构建传播时差量测方程；
25.步骤6.3、构建方位(航向角、俯仰角)量测方程；
26.步骤6.4、构建线性化的量测方程。
27.进一步地，所述步骤6，传播时差δti可表示为其中c为声速，为从潜器(i+1)时刻位置和主潜器i时刻位置之间的距离，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，rd为主潜器和从潜器i时刻位置之间的距离。
28.进一步地，所述步骤6，传播时差与水声通信时间差的数学关系为进一步地，所述步骤6，传播时差与水声通信时间差的数学关系为其中，δti为传播时差，δτ
i+1
为通信时间差，c为声速，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，为主潜器和从潜器(i+1)时刻位置之间的距离，ld为从潜器在(i+1)时刻位置和主潜器i时刻位置之间的距离。
29.进一步地，所述步骤8基于克拉美罗界原理分析各项误差对定位性能的影响。
30.进一步地，所述步骤8分析的定位误差来源包括模型误差，量测噪声，位置误差以及线性化引入的高阶截断误差。
31.进一步地，所述步骤8利用扰动法和大数定律，推导得到位置误差补偿量的显示表达式。
32.进一步地，所述协同定位方法基于贝叶斯理论框架。
33.在本发明的较佳实施方式中，潜器在未知区域执行特殊任务时，由于潜器机动范围内无锚节点、仅由一主潜器和一从潜器组成的系统可观测性弱、潜器可测量信息类型受限三方面影响，使得潜器在水下无锚场景中存在实时协同定位难的问题。本发明利用潜器相邻两时刻的水声通信时间差信息和潜器自身导航信息构建动态时空关联的协同定位模型，在此基础上，构建关于潜器位置的观测方程，利用加权最小二乘理论实现潜器系统在无锚环境中的实时协同定位。以三维场景为例，实现位置求解的显示表达式的前提是至少获取关于位置的三个观测方程。利用水声通信时间差和潜器自身导航信息构建关于基于航向
角、俯仰角、传播时差的观测方程，根据泰勒公式线性化非线性观测方程，最后基于加权最小二乘理论实现实时协同定位。
34.潜器系统在复杂的水下无锚场景中，同步距离量测是有效的信息类型。然而基于同步距离量测的方法定位精度有限，且潜器系统无法长时间保持时钟同步，导致定位精度进一步下降。本发明提出基于水声通信时间差的测量手段，并利用时间相关性扩展潜器导航信息，解决了潜器系统时钟异步情况下信息不足的问题，从而为实现高精度的协同定位方法奠定基础。利用潜器相邻两时刻的水声通信时间差这一信息类型消除潜器系统在水下无锚场景中长时间同步难的问题，并利用时间相关性，将潜器自身导航信息扩展为方位(航向角、俯仰角)信息，实现有限信息的充分利用。
35.在获得定位结果的基础上，提供简单有效的协同定位精度补偿方法，弥补由噪声等因素造成的潜器系统协同定位精度低的问题。本发明基于克拉美罗界原理分析，得出潜器系统在无锚环境中的协同定位误差来源于模型误差、量测噪声和位置噪声，在定位结果的基础上，基于扰动分析方法和泰勒公式，推导出定位误差补偿量的显示表达式，进一步提升协同定位精度。
36.本发明与现有技术相比较，具有如下显而易见的实质性特点和显著优点：
37.1.本发明针对水下无锚场景中潜器系统协同定位难题，利用有限的信息，即仅利用水声通信时间差和潜器自身导航信息构建动态时空关联的协同定位模型，并进一步构建观测方程，实现了潜器系统的实时协同定位。
38.2.本发明利用水声通信时间差消除潜器系统实际中难以满足的时钟同步要求。并利用潜器自身导航信息及时间信息扩展出方位(航向角、俯仰角)信息，实现潜器有限信息的充分利用，为实现高精度的协同定位方法奠定基础。
39.3.本发明针对复杂的水下无锚场景，基于克拉美罗界理论分析协同定位精度低的原因包括模型误差、量测噪声和位置噪声和线性化过程中引入的高阶截断误差，进一步，基于加权最小二乘理论，结合扰动分析法和大数定律，利用泰克共识推导出基于定位结果的定位误差补偿量的显示表示式，提高协同定位精度。
40.以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。
附图说明
41.图1是本发明的一个较佳实施例的动态时空关联的协同定位模型示意图；
42.图2是本发明的一个较佳实施例的协同定位算法流程图。
具体实施方式
43.以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
44.在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。
45.本发明面向现阶段海洋定位系统作用范围有限、不具备灵活性的发展现状，在水下无锚场景中，针对现阶段广泛使用的双潜器作业系统，分别设置为主、从潜器，为满足协同工作时高精度自身信息需求，提供了一种基于水声通信时间差的多潜器实时协同定位方法。
46.本发明提供的协同定位算法面向由一个主潜器和一个从潜器组成的定位系统，各潜器在保证正常机动的情况下，分别配备了导航设备，深度传感器和水声通讯机。具体来说，主潜器配备高精度导航设备，即主潜器可获取高精度的自身位置信息，从潜器仅配备精度较低的导航设备，即根据测得的姿态和运动信息通过航位推算实现自身位置信息的粗预测值，深度传感器用于获取潜器自身的航行深度，水声通信机用于实现主从潜器的水声通信。
47.本发明提供的协同定位方法基于贝叶斯理论框架，利用主从潜器的水声通信时间差以消除潜器间时钟非同步带来的影响，在此基础上，利用主从潜器的位置信息和通信时间差建立动态时空关联的协同定位模型，重构关于方位(航向角、俯仰角)和传播时差的量测方程，利用泰勒公式实现非线性量测方程的线性化，并根据加权最小二乘理论实现从潜器位置的实时估计，利用扰动法实现相对于从潜器位置估计值的位置误差补偿量的显示表达，进一步提高从潜器的协同定位精度。
48.多潜器协同定位方法执行如下：
49.(1)步骤1：主从潜器完成设备初始化，即确定导航设备、深度传感器和水声通讯机是否正常工作；
50.(2)步骤2：主从潜器分别记录初始时间信息和初始位置信息，建立通信链接并完成单向水声通信，即主潜器发送信息，从潜器接收信息，具体为：主潜器发送自身位置实际值(三维笛卡尔坐标系下位置表示方法，下同)和发送信息时间从潜器接收消息后，记录接收信息的实际位置及接收时间下面开始执行潜器的实时协同定位过程；
51.(3)步骤3：在(i+1)(i≥0)时刻，主潜器、从潜器完成单向水声通信，从潜器获得主潜器时间信息主潜器的实际位置信息自身时间信息以及根据自身配备的低精度导航设备获取的位置粗预测值
52.(4)步骤4：在(i+1)(i≥0)时刻，利用i时刻和(i+1)时刻主潜器、从潜器的时间信息获取水声通信时间差(time difference ofcommunication，tdoc)δτ
i+1
：
[0053][0054]
(5)步骤5：利用从潜器在i时刻的位置估计值和在(i+1)时刻的位置粗预测值获得从潜器的航向角、俯仰角和速度的粗量测值当的误差和相对于真值较小时，可得到基于从潜器在(i+1)时刻的位置粗预测值获得修正值实现第一阶段减弱从潜器低精度导航设备引入的累积导航误差造成的位置误差，其
计算公式可表示为：
[0055]
且
[0056][0057][0058][0059]
其中，和分别为和的方差，ts为相邻两时刻的时间间隔，代表求取(*)的均值。
[0060]
(6)步骤6：根据主从潜器在i时刻和在(i+1)时刻的水声通信时间差δτ
i+1
以消除潜器间时钟非同步带来的影响。在此基础上，根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息建立动态时空关联的协同定位模型并构建关于从潜器航向角从潜器俯仰角及主潜器、从潜器传播时差δti的非线性量测方程；在充分考虑各项噪声的基础上利用泰勒公式线性化关于量测向量的非线性量测方程。具体地，步骤6包含如下四部分：
[0061]
(61)步骤61：根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息(61)步骤61：根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息和水声通信时间差δτ
i+1
，建立动态时空关联的协同定位模型，其示意图如图1所示。
[0062]
(62)步骤62：构建传播时差量测方程。
[0063]
对应于图1中两个传播回路，相应的理论传播时延及可表示为：
[0064][0065][0066]
进一步，将水声通信时间差真值带入，有
[0067][0068]
其中c为水中声速。
[0069]
(63)步骤63：构建角度量测方程。
[0070]
对于从潜器，从i时刻到(i+1)时刻，关于从潜器的理论角度量测方程，可表示为：
[0071][0072]
(64)步骤64：构建线性化的量测方程。
[0073]
定义主从潜器的位置误差水声通信时间差的误差传播时间差误差角度误差均为加性零均值的高斯白噪声，则真值与实际量测值的关系可表示为
[0074][0075][0076][0077][0078][0079][0080]
补充说明的是，且对于δτ
i+1
，与与与等物理量均为上述关系，不再赘述。另外，为后续描述方便，定义等物理量均为上述关系，不再赘述。另外，为后续描述方便，定义
[0081]
将其带入步骤62和步骤63中，并采用泰勒公式将非线性量测方程(传播时差量测方程、方位量测方程)线性化，在忽略二阶误差项的同时，将线性化的量测方程转换成加权最小二乘问题(weighted least squares,wls)，可表示为
[0082][0083]
其中，
[0084][0085][0086]
进一步，
[0087][0088]
其中，
[0089][0090]
以及ci的维数为3
×
9，具体元素值为
[0091][0092][0093][0094][0095][0096][0097][0098][0099][0100]
并且ci的其余元素为零。
[0101]
(7)步骤7：将线性化量程方程转化为加权最小二乘问题，并得到从潜器在(i+1)时刻位置估计值可表示为
[0102][0103]
其中，
[0104]
[0105]
(8)步骤8：根据混合克拉美罗界(hcrlb)分析理论，从潜器在(i+1)时刻位置估计的误差包括三方面，(1)采用自身低精度导航设备信息构造动态时空关联的协同定位模型时造成的模型偏差；(2)量测信息误差和潜器位置误差(3)采用泰勒公式线性化非线性量测方程过程中，忽略的二阶及更高阶误差引入的截断误差。采用扰动分析法和泰勒公式，在(i+1)时刻相对位置估计值的位置误差补偿量可表示为
[0106][0107]
其中，
[0108][0109][0110]
进一步，假设噪声相互独立，根据大数定律，采用取均值的方式，实现在(i+1)时刻相对位置估计值的位置误差补偿量的求解，即
[0111][0112]
其中，
[0113][0114][0115][0116][0117]
且q
*
是由(*)对应量的协方差矩阵q
*
对角线元素构成的列向量。
[0118]
(9)步骤9：根据从潜器在(i+1)时刻的位置估计值机器位置误差补偿量在(i+1)时刻，从潜器位置的高精度估计值为
[0119][0120]
(10)步骤10：重复执行步骤3到步骤9，实时获取从潜器的位置估计，直至航行结束。
[0121]
图2为本发明提供的一种基于水声通信时间差的多潜器实时协同定位方法的具体实施流程。本发明面向多潜器协同任务执行过程中的定位问题，故在实际使用时，潜器数量仅需一台主潜器，任意数量台从潜器，且不对潜器做时钟同步要求，这里以一台主潜器，一台从潜器为例。
[0122]
为使用本发明提供的协同定位方法，各潜器在保证正常机动的情况下，分别配备了导航设备，深度传感器和水声通讯机。具体来说，主潜器配备高精度导航设备，即主潜器可获取较高精度的自身位置信息，从潜器仅配备精度较低的导航设备，即根据测得的姿态和运动信息通过航位推算实现自身位置信息的粗估计，深度传感器用于获取潜器自身的航行深度，水声通信机用于实现主从潜器的单向水声通信。
[0123]
如图1所示，本发明具体的执行步骤如下：
[0124]
步骤1：主从潜器完成设备初始化，即确定导航设备、深度传感器和水声通讯机是否正常工作；
[0125]
步骤2、主潜器、从潜器分别记录初始时间信息和初始位置信息，建立通信链接并完成单向水声通信，即主潜器发送信息，从潜器接收信息，具体为：主潜器发送自身位置实际值(三维笛卡尔坐标系下位置表示方法，下同)和发送信息时间从潜器接收消息后，记录接收信息的实际位置及接收时间
[0126]
步骤3、在(i+1)(i≥0)时刻，主潜器、从潜器完成单向水声通信，从潜器获得主潜器时间信息主潜器的实际位置信息自身时间信息以及根据自身配备的低精度导航设备获取的位置粗预测值
[0127]
步骤4、在(i+1)(i≥0)时刻，利用i时刻和(i+1)时刻主潜器、从潜器的时间信息获取水声通信时间差(time difference of communication，tdoc)δτ
i+1
为
[0128]
步骤5、利用从潜器在i时刻的位置估计值和在(i+1)时刻的位置粗预测值获得从潜器的航向角、俯仰角和速度的粗量测值当的误差和相对于真值较小时，可得到基于从潜器在(i+1)时刻的位置粗预测值获得修正值以提高由角度、传播时差构成的量测向量的准确性；
[0129]
步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息和水声通信时间差δτ
i+1
，建立动态时空关联的协同定位模型并构建关于从潜器航向角从潜器俯仰角及主潜器、从潜器传播时差δti的非线性量测方程；在充分考虑各项噪声的基础上，利用泰勒公式线性化非线性量测方程；
[0130]
步骤7、将线性化量测方程转化为加权最小二乘问题，得到从潜器在(i+1)时刻位置估计值的显示表达式
[0131]
步骤8、理论分析表明，位置估计误差来源于模型误差，量测噪声，位置误差以及线性化引入的高阶截断误差。利用扰动法和大数定律，基于泰勒公式推导并得到从潜器在(i+1)时刻相对位置估计值的位置误差补偿量的显示表达式为
[0132][0133]
步骤9、根据从潜器在(i+1)时刻的位置估计值及其位置误差补偿量得到从潜器在(i+1)时刻位置高精度估计值为
[0134]
步骤10、重复执行所述步骤3到所述步骤9，实时估计从潜器的位置，直至航行结束。
[0135]
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术
方案，皆应在由权利要求书所确定的保护范围内。

技术特征：
1.一种基于水声通信时间差的多潜器实时协同定位方法，其特征在于，包括以下步骤：步骤1、主潜器、从潜器设备初始化；步骤2、主潜器、从潜器分别记录初始时间信息和初始位置信息，建立通信链接并完成单向水声通信，即主潜器发送信息，从潜器接收信息，具体为：主潜器发送自身位置实际值和发送信息时间从潜器接收消息后，记录接收信息的实际位置及接收时间步骤3、在(i+1)(i≥00时刻，主潜器、从潜器完成单向水声通信，从潜器获得主潜器时间信息主潜器的实际位置信息自身时间信息以及根据自身配备的低精度导航设备获取的位置粗预测值步骤4、在(i+1)(i≥00时刻，利用i时刻和(i+1)时刻主潜器、从潜器的时间信息获取水声通信时间差δτ
i+1
；步骤5、利用从潜器在i时刻的位置估计值和在(i+1)时刻的位置粗预测值获得从潜器的航向角、俯仰角和速度的粗量测值并基于从潜器在(i+1)时刻的位置粗预测值获得修正值步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息和水声通信时间差δτ
i+1
，建立动态时空关联的协同定位模型并构建关于从潜器航向角从潜器俯仰角及主潜器、从潜器传播时差δt
i
的非线性量测方程；在充分考虑各项噪声的基础上线性化非线性量测方程；步骤7、将线性化量测方程转化为加权最小二乘问题，得到从潜器在(i+1)时刻位置估计值的显示表达式；步骤8、分析定位误差来源，得到从潜器在(i+1)时刻相对位置估计值的位置误差补偿量的显示表达式；步骤9、根据从潜器在(i+1)时刻的位置估计值及其位置误差补偿量得到从潜器在(i+1)时刻位置高精度估计值步骤10、重复执行所述步骤3到所述步骤9，实时估计从潜器的位置，直至航行结束。2.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤1设备初始化包括确定导航设备、深度传感器和水声通讯机是否正常工作。3.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤4，水声通信时间差其中，和和分别为主潜器和从潜器在(i+1)时刻和i时刻的时间。4.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤5，可基于从潜器在(i+1)时刻的位置粗预测值获得修正值5.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤6利用泰勒公式线性化非线性量测方程。6.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤6包括以下步骤：步骤6.1、利用主从潜器在i时刻和(i+1)时刻的位置和水声通信时间差，构建动态时空
关联的协同定位模型；步骤6.2、构建传播时差量测方程；步骤6.3、构建方位(航向角、俯仰角)量测方程；步骤6.4、构建线性化的量测方程。7.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤6传播时差δt
i
可表示为其中c为声速，为从潜器(i+1)时刻位置和主潜器i时刻位置之间的距离，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，r
d
为主潜器和从潜器i时刻位置之间的距离。8.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤6传播时差与水声通信时间差的数学关系为所述步骤6传播时差与水声通信时间差的数学关系为其中，δt
i
为传播时差，δτ
i+1
为通信时间差，c为声速，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，为主潜器和从潜器(i+1)时刻位置之间的距离，l
d
为从潜器在(i+1)时刻位置和主潜器i时刻位置之间的距离。9.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤8基于克拉美罗界原理分析各项噪声对定位性能的影响。10.如权利要求1所述的基于水声通信时间差的多潜器实时协同定位方法，其特征在于，所述步骤8分析的定位误差来源包括模型误差，量测噪声，位置误差以及线性化引入的高阶截断误差。

技术总结
本发明公开了一种基于水声通信时间差的多潜器实时协同定位方法，涉及潜航器定位领域。本发明面向水下无锚场景，利用主从潜器的水声通信时间差以消除潜器间时钟非同步带来的影响，在此基础上，利用主从潜器的位置信息和水声通信时间差建立动态时空关联的协同定位模型，重构关于从潜器方位和传播时差的非线性量测方程，利用泰勒公式线性化量测方程，并根据加权最小二乘理论获得关于从潜器位置的闭式表达式。在此基础上，针对由模型误差及各项噪声造成的定位不准确，利用扰动法推导出相对于从潜器位置估计值的位置误差补偿量的显示表达，进一步提高从潜器的协同定位精度。本发明解决了水下无锚场景中潜器系统协同定位难的问题。难的问题。难的问题。


技术研发人员：姜良宇 李一辰 王布依祎 于文彬 宋磊 关新平
受保护的技术使用者：上海交通大学
技术研发日：2023.04.23
技术公布日：2023/7/22