一种考虑水声通信延迟的多水下无人航行器极区协同导航方法与流程

本发明涉及一种考虑水声通信延迟的多水下无人航行器极区协同导航方法，是一种适用于多水下无人航行器在极区航行过程中存在水声通信延迟情况下的协同导航方法，属于多水下无人航行器协同导航方法领域。

背景技术：

水下无人航行器作为海洋开发的重要装备，近年来得到了巨大的发展。水下无人航行器可以完成海洋调查，失事飞机搜寻等重要任务。未来水下无人航行器的发展必将遵循两个重要趋势，一是向着更远更深的海域航行，另一个就是向着更加智能性，完成更加复杂任务的目标发展。作为完成各项任务的前提，导航至关重要。本发明主要关注水下无人航行器的这两个发展方向给水下无人航行器导航带来的机遇与挑战。一方面是解决水下无人航行器在极区的导航问题；另一方面，是由一队水下无人航行器来完成任务。本发明主要解决的是在水声通信存在延迟的情况下，多水下无人航行器的极区协同导航问题。

团队合作是人类工作学习中不可缺少的一部分。它可以大大提高人们的工作效率，对于水下无人航行器而言也是如此。单个水下无人航行器的智能性发展总会存在一定的限制，而多个水下无人航行器协同就会打破这种限制。尤其是在时间、效率上，多水下无人航行器体现出优于单个水下无人航行器的极大优势。常见的多水下无人航行器协同导航主要有两种形式，包括并行式和主从式。典型的并行式水下无人航行器协同导航系统是(virginiatech)研究团队开发的协同导航系统。以水声广播的方式向周围邻居发送位置信息。与并行式不同，主从式协同导航系统中的水下无人航行器可以有不同的配置。典型的主从式多水下无人航行器协同导航系统如葡萄牙波尔图大学(universityofporto)n.cruz研究小组的移动基线系统和麻省理工学院(mit)的cadre系统(cooperativeautonomyfordistributedreconnaissanceandexplorationsystem)。学者们针对非极区多智能体的协同导航问题提出了许多新方法，但是这些方法并不适用于多水下无人航行器在极区的导航。文献(xiaogd,wangat,wangb,etal.2016acooperativenavigationmethodbasedonusblproceedingsof2016chinainternationalconferenceoninertialtechnologyandnavigation586-590)和(xiaogd,wangb,dengzh,etal.2017anacousticcommunicationtimedelayscompensationapproachformaster-slaveauvcooperativenavigationieeesensorsjournal17(2)504-513)提出了一种针对主从式水下无人航行器的水声通信延迟补偿方法。然而这些方法在极区应用时，会受到经线快速收敛的影响。因而只适用在非极区。同时，滤波算法是基于标准的卡尔曼滤波算法，标准的卡尔曼滤波算法只适用于动态线性系统模型。相比于非极区多水下无人航行器协同导航的火热发展，极区多水下无人航行器协同导航正处于起步阶段，目前还没有涉及到多水下无人航行器协同导航的文章。本发明提出的方法主要解决考虑水声通信延迟的具有非线性系统模型的极区多水下无人航行器协同导航问题。

技术实现要素：

本发明的目的是为了提供一种考虑水声通信延迟的多水下无人航行器极区协同导航方法，是能够有效的用于多水下无人航行器极区协同导航，并考虑水声通信延迟的方法。

本发明的目的是这样实现的：步骤如下：

步骤一：主水下无人航行器与从水下无人航行器进行精确的时间同步；

步骤二：输入考虑通信延迟的水下无人航行器极区协同导航时间t、滤波周期t，计算滤波总次数n，并令初始滤波次数k为0；

步骤三：滤波次数k加1；

步骤四：主水下无人航行器利用自身携带的高精度导航设备以及极区导航算法确定其在极区的精确导航信息；

步骤五：从水下无人航行器利用自身携带的低精度导航设备以及极区导航算法确定其在极区的粗略导航信息；

步骤六：主水下无人航行器利用超短基线系统，测定主从水下无人航行器之间的方位与距离信息；

步骤七：主水下无人航行器向从水下无人航行器发送信息；

步骤八：从水下无人航行器结合收到的主水下无人航行器发来的信息确定通信延迟时间；

步骤九：根据通信延迟时间，利用改进的考虑水声通信延迟的滤波算法，融合得到从水下无人航行器精确的导航信息；

步骤十：判断当前滤波次数k是否大于等于总滤波次数n：当不满足k≥n时，则跳转至步骤三重复执行；当满足k≥n时，则考虑通信延迟的多水下无人航行器极区协同导航方法结束。

本发明还包括这样一些结构特征：

1.步骤五具体包括：选取格网坐标系g作为导航坐标系，考虑极区的导航环境特点，从水下无人航行器的状态方程建立如下：

其中，是失准角，是速度误差，δr^e＝[δxδyδz]^t是位置误差，f^g为比力；为载体坐标系b到格网坐标系g的方向余弦矩阵，为格网坐标系g到地球坐标系e的方向余弦矩阵；ε^b为陀螺漂移，它是由陀螺常值漂移和可以看作零均值高斯白噪声的陀螺随机漂移组成；▽^b是加速度计零偏、且由加速度计常值漂移和加速度计随机漂移组成，加速度计随机漂移可以被看作高斯白噪声；δkd和分别表示多普勒测速仪的刻度因数误差和随机速度误差，τv是相关时间，并且wv是白噪声；β^a是零均值白噪声，表示超短基线误差。

2.步骤八具体是：考虑水声通信延迟并且适用于多水下无人航行器极区协同导航方法的修正的自适应卡尔曼滤波算法的完整方程表述如下：

其中，φk,k-1，γk,k-1和hk分别是系统矩阵、控制矩阵和量测矩阵的离散形式；p是状态的协方差矩阵；kk是自适应卡尔曼滤波增益；系统噪声矩阵w的均值和方差分别为和和是量测噪声矩阵v的均值和方差；dk＝(1-b)/(1-b^k)并且0＜b＜1是遗忘因子，并且：

与现有技术相比，本发明的有益效果是：本发明解决传统水下无人航行器协同导航方法在极区无法应用的问题。同时考虑水声通信延迟对导航结果的影响，改进滤波算法，设计考虑水声通信延迟的自适应卡尔曼滤波算法。本发明提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法，可以有效解决在极区多水下无人航行器的协同导航问题。由于主水下无人航行器配备高精度惯性导航系统，从水下无人航行器只需要配备低精度惯性导航系统，通过极区协同导航算法可以提高从水下无人航行器的导航精度，因而本发明在提高水下无人航行器执行任务的效率的同时，也节约了导航成本。

附图说明

图1是本发明提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法流程图；

图2是本发明提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法中对于从水下无人航行器校正后的姿态误差曲线的对比；

图3是本发明提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法中对于从水下无人航行器校正后的速度误差曲线的对比；

图4是本发明提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法中对于从水下无人航行器校正后的位置误差曲线的对比。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图

本发明的主要目的是解决传统多水下无人航行器在极区无法应用的问题，设计多水下无人航行器极区协同导航方法。同时考虑水声通信延迟，改进自适应卡尔曼滤波算法，滤波融合得到多水下无人航行器精确的导航结果。

为了实现以上目的，本发明的技术方案主要包括以下步骤：

步骤一：主水下无人航行器与从水下无人航行器进行精确的时间同步。

步骤二：输入考虑通信延迟的水下无人航行器极区协同导航时间t、滤波周期t，计算滤波总次数n，并令初始滤波次数k为0。

步骤三：滤波次数k加1。

步骤四：主水下无人航行器利用自身携带的高精度导航设备以及极区导航算法确定其在极区的精确导航信息(包括姿态信息、位置信息和速度信息)。

由于主水下无人航行器上携带的是高精度惯性导航设备，可以实现主水下无人航行器在极区的精确导航。相比于从水下无人航行器由于携带低精度惯性导航设备而造成的较大的导航误差，主水下无人航行器的导航误差可以忽略不计。

步骤五：从水下无人航行器利用自身携带的低精度导航设备以及极区导航算法确定其在极区的粗略导航信息(包括姿态信息、位置信息和速度信息)。

选取格网坐标系g作为导航坐标系，考虑极区的导航环境特点，建立从水下无人航行器极区航行的状态方程和量测方程。从水下无人航行器的状态方程建立如下：

其中，是失准角，是速度误差，δr^e＝[δxδyδz]^t是位置误差，f^g为比力；为载体坐标系b到格网坐标系g的方向余弦矩阵，为格网坐标系g到地球坐标系e的方向余弦矩阵；ε^b为陀螺漂移，它是由陀螺常值漂移和可以看作零均值高斯白噪声的陀螺随机漂移组成；▽^b是加速度计零偏，它是由加速度计常值漂移和加速度计随机漂移组成，加速度计随机漂移可以被看作高斯白噪声；δkd和分别表示多普勒测速仪的刻度因数误差和随机速度误差，τv是相关时间，并且wv是白噪声；β^a是零均值白噪声，表示超短基线误差。并且：

其中，为了叙述方便，s(·)和c(·)分别表示sin(·)和cos(·)。

步骤六：主水下无人航行器利用超短基线系统，测定主从水下无人航行器之间的方位与距离信息。

选择从水下无人航行器自身携带的多普勒测速仪测量的速度解算出的速度误差，作为速度误差的观测量。同时选择超短基线定位系统测得的主从水下无人航行器之间的位置解算出的位置误差，作为位置误差的观测量。建立从水下无人航行器极区协同导航观测模型。

状态量量可以描述为：

观测量可以描述为：

z＝[(δv^g)^t(δr^e)^t]^t,(7)

观测模型可以描述为:

其中，h和v分别是观测矩阵和测量噪声矩阵。v可以看作零均值高斯白噪声，并且可以表示为:

步骤七：主水下无人航行器向从水下无人航行器发送信息(包括：时间、位置、姿态、速度、相对位置等)。

步骤八：从水下无人航行器结合收到的主水下无人航行器发来的信息确定通信延迟时间。

步骤九：根据通信延迟时间，利用改进的考虑水声通信延迟的滤波算法，融合得到从水下无人航行器精确的导航信息。考虑水声通信延迟并且适用于多水下无人航行器极区协同导航方法的修正的自适应卡尔曼滤波算法的完整方程表述如下(考虑水声通信延迟的改进的自适应卡尔曼滤波算法全过程如下)：

其中，φk,k-1，γk,k-1和hk分别是系统矩阵、控制矩阵和量测矩阵的离散形式；p是状态的协方差矩阵；kk是自适应卡尔曼滤波增益；系统噪声矩阵w的均值和方差分别为和和是量测噪声矩阵v的均值和方差；dk＝(1-b)/(1-b^k)并且0＜b＜1是遗忘因子。并且：

步骤十：判断当前滤波次数k是否大于等于总滤波次数n：当不满足k≥n时，则跳转至步骤三重复执行；当满足k≥n时，则考虑通信延迟的多水下无人航行器极区协同导航方法结束。

具体实施方案：

通过matlab仿真软件验证本发明所提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法的有效性。利用matlab仿真语言，建立考虑水声通信延迟的多水下无人航行器极区导航方法中的从水下无人航行器的导航状态模型和观测模型。设定初始条件，进行仿真实验。通过将考虑水声通信延迟的多水下无人航行器极区协同导航方法，与不考虑水声通信延迟的多水下无人航行器极区协同导航方法相比较，表明本发明的有效性。

matlab仿真条件设置如下：

系统仿真时间为12h，滤波周期为0.1s；多水下无人航行器中从水下无人航行器的初始位置为东经120°，北纬80°；横摇、纵摇和艏摇角的初始值均为0°，周期分别为3s，5s和7s，幅值分别为4°,5°和3°；陀螺常值漂移和随机漂移分别为0.03°/h和(0.001°/h)²；加速度计常值漂移和随机漂移分别为1×10^-4g0和(1×10^-6g0)²。

本发明仿真结果如下：

根据本发明所描述的考虑水声通信延迟的多水下无人航行器极区协同导航方法，并基于上述设置的仿真条件，通过对方法的仿真实验可以得到如图2到图4的仿真实验结果。

根据图2至图4的仿真实验结果对比，在仿真实验中，方法1表示考虑水声通信延迟的多水下无人航行器极区协同导航方法，方法2表示不考虑水声通信延迟的多水下无人航行器极区协同导航方法。通过图2至图4的仿真实验对比结果，可以发现，所提出的考虑水声通信延迟的多水下无人航行器极区协同导航方法优于不考虑水声通信延迟的多水下无人航行器极区协同导航方法。在考虑水声通信延迟的多水下无人航行器极区协同导航方法中，从水下无人航行器的姿态、速度和位置的估计误差快速收敛，并且这些误差稳定在零值附近。而不考虑水声通信延迟的多水下无人航行器极区协同导航方法中，从水下无人航行器的姿态、速度和位置误差较大。

综上，本发明公开了一种考虑水声通信延迟的多水下无人航行器极区协同导航方法。主水下无人航行器与从水下无人航行器进行精确的时间同步；输入考虑通信延迟的水下无人航行器极区协同导航时间t、滤波周期t，计算滤波总次数n，并令初始滤波次数k为0；滤波次数k加1；主水下无人航行器利用自身携带的高精度导航设备以及极区导航算法确定其在极区的精确导航信息(包括姿态信息、位置信息和速度信息)；从水下无人航行器利用自身携带的低精度导航设备以及极区导航算法确定其在极区的粗略导航信息(包括姿态信息、位置信息和速度信息)；主水下无人航行器利用超短基线系统，测定主从水下无人航行器之间的方位与距离信息；主水下无人航行器向从水下无人航行器发送信息(包括：时间、位置、姿态、速度、相对位置等)；从水下无人航行器结合收到的主水下无人航行器发来的信息确定通信延迟时间；根据通信延迟时间，利用改进的考虑水声通信延迟的滤波算法，融合得到从水下无人航行器精确的导航信息；判断当前滤波次数k是否大于等于总滤波次数n：当不满足k≥n时，则跳转至步骤三重复执行；当满足k≥n时，则考虑通信延迟的多水下无人航行器极区协同导航方法结束。本发明可有效实现考虑水声通信延迟的多水下无人航行器极区协同导航，相比于不考虑水声通信延迟的所水下无人航行器极区协同导航方法有更好的精度。