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

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

背景技术：

1、水下无人潜航器(简称潜器)集群因其具备机动性强、作用范围灵活、鲁棒性高等优势，在水下军事勘探、深海海洋勘探、生物监测等领域得到广泛应用。潜器自身位置的准确获取是多潜器集群高效执行任务的重要前提。在现有的潜器定位方法中，通常在相关的某一固定海域布放锚节点，通过多个位置已知的锚节点实现潜器定位，典型的如长基线定位系统。然而，锚节点的覆盖范围就广大海域而言十分有限，且锚节点的布放位置需要事先确定，恶劣的水下环境以及锚节点自身可携带电池容量有限等原因使得锚节点无法长时间处于工作状态。另一方面，当面临在移动范围内执行任务时，长基线定位系统无法实现在机动区域内对潜器的高精度定位。另一种方法是根据潜器配备的导航设备实现实时定位，虽具有灵活机动、无需任何锚节点的优势，但高精度导航设备价格昂贵，且存在累积误差使得潜器定位误差无限增长。随着水声通信技术的进步和潜器的智能化发展，多潜器协同定位具备实现高精度定位的潜力，从而得到广泛研究。

2、无论是基线类定位系统还是多潜器协同定位系统，均是以距离量测为基础，而准确的距离量测是以潜器之间、锚节点之间、锚节点与潜器之间的时钟同步为前提。然而，在水下环境中，由于潜器自身时钟相关元器件制作工艺以及受水下温度、盐度、深度等因素影响，多潜器之间的时钟难以保持同步，尤其是在长时间航行过程。另一方面，水声信道的复杂性和水声通信的延迟效应不仅降低了潜器系统的协同更新频率，而且造成的潜器移动进一步降低了协同定位精度。因此，在无锚节点辅助定位，且潜器之间时钟非同步的情况下，如何动态实时实现潜器的高精度协同定位亟需研究和解决。

3、申请号为202210102709.7的名称为“一种集成超短基线系统的uuv集群协同定位方法”的专利提供了一种基于距离测量和超短基线系统的方法，虽然可以提升主从式潜器系统的协同定位精度，但超短基线系统需要事先严格校准，且对于距离测量，不仅需要通信双方保证时钟同步，而且受限于水声通信延迟大的不足，使得双程水声通信带来了uuv位置的偏差以及距离误差的增大。申请号为202111610043.8的名称为“一种双主auv切换的auv集群大水深作业的协同导航定位方法”的专利提供了一种面向主从式潜器系统，利用主auv定期上浮至水面实现自身位置校正进而通过多潜器协同定位方法提升从auv定位精度，这不仅降低了系统协同更新频率，也无法解决主从auv时钟不同步的问题。申请号为202210213443.3的名称为“一种基于神经网络的分布式小平台水下纯方位定位方法”的专利提供了一种使用参考节点与待定位潜器间的方位信息的单潜器定位方法，但方位信息的准确程度依赖于测量阵列的结构，而潜器由于自身电池有限，阵列无法长时间工作，而且多阵元阵列由于体积等原因严重影响潜器的机动。综上而言，上述方法面向潜器定位问题均存在不足，无法实现实时定位，且未具体考虑潜器自身配备的导航设备累积误差对多潜器协同定位结果的影响。

4、因此，本领域的技术人员致力于开发一种基于水声通信时间差的多潜器实时协同定位方法。本发明面向水下无锚场景中时钟非同步的多潜器动态实时协同定位需求，提出一种基于水声通信时间差的多潜器实时协同定位方法。区别于以使用潜器间的相对距离测量为主的传统定位方法，面对水下无锚场景，针对仅具备主潜器和从潜器，且二者时钟不同步的潜器系统，利用主从潜器间的相邻时刻水声通信时间差和潜器自身导航信息，实现对从潜器的实时协同定位，以达到提升潜器系统定位精度的目的。

技术实现思路

1、有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是多潜器之间的时钟难以保持同步，潜器测量性能受限，潜器在水下无锚场景中实时定位难的问题。

2、为实现上述目的，本发明提供了一种基于水声通信时间差的多潜器实时协同定位方法，包括以下步骤：

3、步骤1、主潜器、从潜器设备初始化；

4、步骤2、主潜器、从潜器分别记录初始时间信息和初始位置信息，建立通信链接并完成单向水声通信，即主潜器发送信息，从潜器接收信息，具体为：主潜器发送自身位置实际值(三维笛卡尔坐标系下位置表示方法，下同)和发送信息时间从潜器接收消息后，记录接收信息的实际位置及接收时间

5、步骤3、在(i+1)(i≥0)时刻，主潜器、从潜器完成单向水声通信，从潜器获得主潜器时间信息主潜器的实际位置信息自身时间信息以及根据自身配备的低精度导航设备获取的位置粗预测值

6、步骤4、在(i+1)(i≥0)时刻，利用i时刻和(i+1)时刻主潜器、从潜器的时间信息获取水声通信时间差(time difference ofcommunication，tdoc)δτi+1；

7、步骤5、利用从潜器在i时刻的位置估计值和在(i+1)时刻的位置粗预测值获得从潜器的航向角、俯仰角和速度的粗量测值并基于从潜器在(i+1)时刻的位置粗预测值获得修正值

8、步骤6、根据主潜器、从潜器在i时刻和(i+1)时刻的位置信息和水声通信时间差δτi+1，建立动态时空关联的协同定位模型并构建关于从潜器航向角从潜器俯仰角及主潜器、从潜器传播时差δti的非线性量测方程；在充分考虑各项噪声的基础上线性化非线性量测方程；

9、步骤7、将线性化量测方程转化为加权最小二乘问题，得到从潜器在(i+1)时刻位置估计值的显示表达式；

10、步骤8、分析定位误差来源，得到从潜器在(i+1)时刻相对位置估计值的位置误差补偿量的显示表达式；

11、步骤9、根据从潜器在(i+1)时刻的位置估计值及其位置误差补偿量得到从潜器在(i+1)时刻位置高精度估计值

12、步骤10、重复执行所述步骤3到所述步骤9，实时估计从潜器的位置，直至航行结束。

13、进一步地，所述步骤1设备初始化包括确定导航设备、深度传感器和水声通讯机是否正常工作。

14、进一步地，所述步骤4，水声通信时间差其中，和和分别为主潜器和从潜器在(i+1)时刻和i时刻的时间。

15、进一步地，所述步骤5，当从潜器航向角从潜器俯仰角的误差和相对于真值较小时，可基于从潜器在(i+1)时刻的位置粗预测值获得修正值

16、进一步地，所述步骤6利用泰勒公式线性化非线性量测方程。

17、进一步地，所述步骤6包括以下步骤：

18、步骤6.1、利用主从潜器在和的位置真值和水声通信时间差真值，构建动态时空关联的协同定位模型；

19、步骤6.2、构建传播时差量测方程；

20、步骤6.3、构建方位(航向角、俯仰角)量测方程；

21、步骤6.4、构建线性化的量测方程。

22、进一步地，所述步骤6，传播时差δti可表示为其中c为声速，为从潜器(i+1)时刻位置和主潜器i时刻位置之间的距离，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，rd为主潜器和从潜器i时刻位置之间的距离。

23、进一步地，所述步骤6，传播时差与水声通信时间差的数学关系为其中，δti为传播时差，δτi+1为通信时间差，c为声速，为从潜器(i+1)时刻位置和从潜器i时刻位置之间的距离，为主潜器和从潜器(i+1)时刻位置之间的距离，ld为从潜器在(i+1)时刻位置和主潜器i时刻位置之间的距离。

24、进一步地，所述步骤8基于克拉美罗界原理分析各项误差对定位性能的影响。

25、进一步地，所述步骤8分析的定位误差来源包括模型误差，量测噪声，位置误差以及线性化引入的高阶截断误差。

26、进一步地，所述步骤8利用扰动法和大数定律，推导得到位置误差补偿量的显示表达式。

27、进一步地，所述协同定位方法基于贝叶斯理论框架。

28、在本发明的较佳实施方式中，潜器在未知区域执行特殊任务时，由于潜器机动范围内无锚节点、仅由一主潜器和一从潜器组成的系统可观测性弱、潜器可测量信息类型受限三方面影响，使得潜器在水下无锚场景中存在实时协同定位难的问题。本发明利用潜器相邻两时刻的水声通信时间差信息和潜器自身导航信息构建动态时空关联的协同定位模型，在此基础上，构建关于潜器位置的观测方程，利用加权最小二乘理论实现潜器系统在无锚环境中的实时协同定位。以三维场景为例，实现位置求解的显示表达式的前提是至少获取关于位置的三个观测方程。利用水声通信时间差和潜器自身导航信息构建关于基于航向角、俯仰角、传播时差的观测方程，根据泰勒公式线性化非线性观测方程，最后基于加权最小二乘理论实现实时协同定位。

29、潜器系统在复杂的水下无锚场景中，同步距离量测是有效的信息类型。然而基于同步距离量测的方法定位精度有限，且潜器系统无法长时间保持时钟同步，导致定位精度进一步下降。本发明提出基于水声通信时间差的测量手段，并利用时间相关性扩展潜器导航信息，解决了潜器系统时钟异步情况下信息不足的问题，从而为实现高精度的协同定位方法奠定基础。利用潜器相邻两时刻的水声通信时间差这一信息类型消除潜器系统在水下无锚场景中长时间同步难的问题，并利用时间相关性，将潜器自身导航信息扩展为方位(航向角、俯仰角)信息，实现有限信息的充分利用。

30、在获得定位结果的基础上，提供简单有效的协同定位精度补偿方法，弥补由噪声等因素造成的潜器系统协同定位精度低的问题。本发明基于克拉美罗界原理分析，得出潜器系统在无锚环境中的协同定位误差来源于模型误差、量测噪声和位置噪声，在定位结果的基础上，基于扰动分析方法和泰勒公式，推导出定位误差补偿量的显示表达式，进一步提升协同定位精度。

31、本发明与现有技术相比较，具有如下显而易见的实质性特点和显著优点：

32、1.本发明针对水下无锚场景中潜器系统协同定位难题，利用有限的信息，即仅利用水声通信时间差和潜器自身导航信息构建动态时空关联的协同定位模型，并进一步构建观测方程，实现了潜器系统的实时协同定位。

33、2.本发明利用水声通信时间差消除潜器系统实际中难以满足的时钟同步要求。并利用潜器自身导航信息及时间信息扩展出方位(航向角、俯仰角)信息，实现潜器有限信息的充分利用，为实现高精度的协同定位方法奠定基础。

34、3.本发明针对复杂的水下无锚场景，基于克拉美罗界理论分析协同定位精度低的原因包括模型误差、量测噪声和位置噪声和线性化过程中引入的高阶截断误差，进一步，基于加权最小二乘理论，结合扰动分析法和大数定律，利用泰克共识推导出基于定位结果的定位误差补偿量的显示表示式，提高协同定位精度。

35、以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。