CN111595348B - 一种自主水下航行器组合导航系统的主从式协同定位方法 - Google Patents

Abstract

本发明公开了一种自主水下航行器组合导航系统的主从式协同定位方法，该方法包括对于单领航定位系统和双领航定位系统下的主、从AUV建立三维协同导航系统模型，通过主AUV广播自身的位置信息，从AUV根据声速和时延获取两者时间的相对距离，进而有主AUV利用测速信息、测距信息对任意的从AUV进行协同定位，修正两者之间的距离。本发明所述方法还针对主动式协同定位的精度问题提出新的增加深度测量信息的三维协同导航模型，包括在AUV没有收到距离测量信息情况下的测量方式，能够提高主从式协同定位的精度。

Description

一种自主水下航行器组合导航系统的主从式协同定位方法

技术领域

本发明涉及水下航行器编队控制技术，具体涉及一种自主水下航行器组合导航系统的主从式协同定位方法。

背景技术

自主水下航行器(Autonomous underwater vehicles，AUVs)是无人水下航行器(Unmanned underwater vehicle，UUV)的一种，用于海洋资源的开发和研究。由于AUV摆脱了缆线的束缚，因此，AUV具有作业范围广，作业时间长，不怕线缆缠绕，成本低等优点，这些优点使得AUV在军事侦察和监视、反潜和巡逻、海洋测绘、海洋资源勘探和潜水支援等方面发挥着重要作用。目前，AUV仍面临着高精度水下定位问题带来的技术性问题。AUV有长时间和远距离作业需求，并且考虑到其体积、质量、特殊的水下环境、能量的约束等因素，准确定位AUV成为一项艰巨的任务。

根据定位误差是否有界，将水下潜器的定位方式归为两类，一类是定位误差有界，如水下声学定位系统，其要求母船跟踪或事先布放并标定长基线阵，作业成本高；另一类是定位误差会随着时间累积，将导致误差无界，如惯性导航系统、航位推算系统等，其工作方式独立自主、抗干扰性好，然而其固有的误差随着时间累积的特性在很大程度上限制其工作能力。

针对以上定位方式的弊端，通过各AUV之间进行信息共享而使团队中所有AUV的定位精度都得到提升并使得误差有界的方法逐渐成为水下AUV定位的主流研究方向，被称之为协同定位。协同定位概念最早是由日本学者Ryo Kurazume和Shigemi Nagata两人在20世纪90年代研究多机器人定位时提出，逐渐变成在该领域最有研究前景的方向之一。

目前，在陆地多机器人定位、无线传感器网络、卫星定位等领域己经广泛进行研究和发展协同定位。随着水下通信研究的进步和AUV产品的成熟，基于水声研究的协同定位方法得到了长足的发展和进步。在此趋势下，未来AUV进行协同集群化作业将成为主要作业方式。协同作业允许每个AUV负责不同的任务，使得AUV可以完成搭载不同传感器设备并实现某一特定的操作能力，大大提升了AUV团队的水下持续工作时间和单次作业能力。当AUV在某些特定情况下，无法获得外界的GPS信息、没有母船跟踪，也无法通过地形匹配信息或重力来获得自身的位置信息，若AUV定位能力丧失，多个AUV可以在每个定位周期通过AUV之间进行信息共享，从而使各个AUV的定位精度达到相同量级，定位能力得以恢复。根据上述分析可以得到该研究的必要性。本发明所研究的协同定位方法正是针对无法预知地形信息或地标并且没有母船跟踪的前提下，满足多AUV作业过程中对高精度位置的要求。

发明内容

发明目的：针对现有技术在无法预知地形信息或地标并且没有母船跟踪的前提下，多AUV作业过程中对位置的定位不精准问题，本发明提供一种自主水下航行器组合导航系统的主从式协同定位方法。

技术方案：一种自主水下航行器组合导航系统的主从式协同定位方法，所述主从式协同定位方法通过增加深度测量信息的三维协同导航模型，适用于单领航定位系统和双领航定位系统下的协同定位，定位过程如下：

(1)主AUV广播自身位置信息；

(2)在主AUV通信范围内的从AUV收到来自主AUV的位置信息后，根据声速和时延获得两者之间的距离，从AUV获得测速信息；

(3)主AUV利用测速信息、测距信息对任意从AUV进行协同定位，给出从AUV修正后的位置；

(4)从AUV广播自身修正后的位置信息。

进一步的，所述主从式协同定位方法中，系统的状态向量的表达式如下：

x_k＝[(pⁿ)^T Θ^T (v^b)^T]^T

＝[x y z ψ θ φ u v w]^T

由航位推算公式可以得到状态转移方程：

式中，依次选取导航坐标系下位置信息、载体坐标系下的速度信息、三维姿态信息作为系统的状态向量。

述主从式协同定位方法中，系统的观测向量的表达式如下：

z_k＝[(z_depth)^T (z_attitude)^T (z_velocity)^T(z_range) ^T]^T

＝[z φ θ ψ u v w r^M1 … r^Mi]^T

在AUV取到距离测量信息时，对应的测量方程为

式中，依次选取深度信息、姿态信息、载体速度信息以及从AUV之间的相对距离信息构成观测向量。

更进一步的，所述主从式协同定位方法中针对AUV没有收到距离测量信息的情况下的测量方程如下：

更进一步的，所述方法中单领航协同定位具体过程如下：

在每一个定位周期内，由主AUV进行发起定位，主AUV将自身的位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，所有AUV在入水前进行对时，保持时间同步，通过获取的时延和声速剖面得到主从AUV之间的距离，从AUV利用主AUV提供的位置信息、测距信息和自身的测速信息，通过滤波算法计算出自身位置并对位置更新。

更进一步的，所述方法中双领航协同定位具体过程如下：

在每一个定位周期内，双领航AUV同时将自身位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，获得两个不同方位的测距信息，从AUV通过分别两个领航AUV的测距信息、位置信息，以及测距信息，通过滤波算法计算出自身位置并对位置更新。

有益效果：与现有技术相比，本发明显著效果包括如下几点：

(1)本发明针对AUV路径点跟随问题，给出了纯跟踪制导时系统下的全局渐进收敛至期望路径点的充分条件，能够方便结合S面控制算法设计基于纯跟踪制导的路径点跟随控制器；

(2)通过将前视距离表示为路径跟随法向误差的函数，改进现有法制导的路径跟随控制方法，提高了算法及函数计算的收敛速度；

(3)针对多AUV编队问题，本发明适用于路径跟随的多AUV编队控制，并且也提高了系统控制的同步和稳定性；

(4)本发明在方法中涉及到极大似然估计，对由水声通信获得的领队导航信息和前视声呐探测得到的相应信息进行融合，改进了对领队跟踪过程中导航信息传输不稳定的情况。

附图说明

图1是单领航定位模式示意图；

图2是双领航定位模式示意图。

具体实施方式

为了详细的说明本发明所公开的技术方案，下面结合说明书附图和具体实施例做进一步的阐述。

在主从式协同定位系统中，其中主AUV为其他从AUV提供自己精确的位置信息，以及相互间的距离信息，同时从AUV通过自己携带的低精度、低成本传感器获取速度信息，完成自身低精度航位推算，但是由于传感器累计误差的存在，长时间作业会导致定位误差不断增加甚至发散，为控制误差有界，需要外部的绝对位置信息作为参考实现对从AUV定位误差的修正，实现主从式AUV协同定位。

本发明所述的方法在应用中，对于主从式的自主水下航行器组合导航系统，首先由主AUV配备了高精度设备(或通过定期上浮获得GPS信息)以获得高精度位置，并通过与从AUV进行通信，来传递信息。主AUV利用精确的测距信息以及从AUV提供的不精确的位置进行，对从AUV的位置进行修正，实现从AUV的精确定位，以此来实现自主水下航行器组合导航系统的协同定位和导航。

如图1和图2所示，主从式协同定位将主从式AUV协同定位分为以下两种模式，分别是单领航协同定位和双领航协同定位。其工作场景的定位模式如下：

单领航系统定位为：在每一个定位周期内，定位由主AUV进行发起，主AUV将自身的位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，由于所有AUV在入水前进行对时，保持时间同步，因此可通过获取的时延和声速剖面得到主从AUV之间的距离，从AUV利用主AUV提供的位置信息、测距信息和自身的测速信息，通过滤波算法计算出自身位置并对位置更新。

双领航系统定位为：在每一个定位周期内，双领航AUV同时将自身位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，可以获得两个不同方位的测距信息，该从AUV利用分别两个领航AUV的测距信息、位置信息，以及测距信息，通过滤波算法计算出自身位置并对位置更新。

双领航模式较单领航模式具备更多优势：第一，可以减弱单领航条件下对从AUV的机动性要求；第二，相比于单领航的定位收敛速度，双领航有更好的提升效果。

下面详细说明本发明所述方法的具体实施过程，协同定位流程概述如下：

(1)主AUV广播自身位置信息。

(2)在主AUV通信范围内的从AUV收到来自主AUV的位置信息后，根据声速和时延获得两者之间的距离，从AUV获得测速信息。

(3)主AUV利用测速信息、测距信息对任意从AUV进行协同定位，给出从AUV修正后的位置。

(4)从AUV广播自身修正后的位置信息。

进一步的，本发明所述方法中，根据二维协同导航模型利用系统中各台AUV上配备的深度传感器测得的深度信息，将所有的AUV转化到同一深度维上，把原本的立体空间上的问题简化为一个平面上的问题，能够减少系统的运算开销提高系统的运行速度。

对于在实际运用中受到传感器自身精度、海平面起伏、海水密度变化等因素的影响，深度传感器的测量并没有办法达到标称的理想精度，直接使用深度的数据会带来一定的误差问题。本发明建立主从式导航系统建立协同导航空间状态运动模型，增加深度测量信息作为模型的参量。深度信息可以作为一个测量值运用到导航系统中，且加入深度维信息后配合三维姿态信息能使模型更加精准、符合实际；另外，二维模型都是假设AUV俯仰变化不大、作业以定深运动为主，而实际AUV作业过程中往往有较长时间的下潜和爬升，并且还需AUV做上下yo-yo运动或锯齿盘旋来进行海洋物理观测。具体过程如下：

1)依次选取导航坐标系下位置信息、载体坐标系下的速度信息、三维姿态信息作为系统的状态向量：

x_k＝[(pⁿ)^T Θ^T (v^b)^T]^T

＝[x y z ψ θ φ u v w]^T

由航位推算公式可以得到状态转移方程

2)考虑由AUV搭载的其它导航传感器，其中AUV之间的距离信息可由声通测得，但是其测量的频率较低，本发明中采用的实际实验设备为0.125Hz；深度信息可由压力传感器测得；航行器的速度信息可由DVL测得；姿态信息可由电子罗盘测得。依次选取深度信息、姿态信息、载体速度信息以及从AUV之间的相对距离信息构成观测向量，其表达式如下所示：

z_k＝[(z_depth)^T (z_attitude)^T (z_velocity)^T(z_range)^T]^T

＝[z φ θ ψ u v w r^M1 … r^Mi]^T

在AUV取到距离测量信息时，对应的测量方程如下所示：

考虑到AUV之间利用声通测量距离的频率较低，故在较大比例的时间内仅使用H1k作为观测矩阵是不准确的。

3)针对AUV没有收到距离测量信息的情况，考虑使用新的测量方程如下：

式中，AUV在t_k时刻

4)构建基于扩展卡尔曼滤波协同定位模型

利用扩展卡尔曼滤波方法，构建具有高精度位置信息的主AUV带领多个低精度位置信息的从AUV的定位模型，从AUV利用主AUV的高精度位置信息以及其他量测信息进行滤波计算，从而改善整个系统定位性能。

AUV运动模型构建AUV运动的数学模型分为动力学模型和运动学模型两种。AUV动力学模型用来描述外力矩对AUV的作用，对动力学模型的建模有利于对AUV控制方法的确定。与动力学模型相比，运动学模型一般用于描述目标在坐标系下的位移、速度等运动学参数之间的关系。

另一方面，由于本发明主要针对AUV协同系统的定位性能进行分析与研究，并不需要对动力学系统深入了解与运用，因此，仅考虑对AUV的运动学模型进行建模，具体如下：

S1、考虑到AUV在水下进行的是三维运动，由于深度可由深度计直接获得，在实际应用中深度计对深度的测量精度高，且在AUV稳定工作后，深度俯仰角变化较小。为了便于计算，采用简化模型维数的方式，可以近似的对深度和深度俯仰角进行忽略。因此，将三维运动模型简化为二维运动模型。

定义AUV在t_k时刻的二维运动方程为X(k)＝[x(k),v_x(k),y(k),v_y(k)]，分别标识AUV在定位坐标系下的X轴坐标位置，X轴方向速度，Y轴坐标位置，Y轴方向速度，设采样周期为T，则AUV在二维平面的运动模型：

该模型是在理想状态下建立的，在实际过程中传感器的输入会受到噪声干扰带来的影响，设噪声为高斯白噪声，则在噪声存在的情况下，输入应为：

可以将上式改写为：

u_mk＝u_k+W_k

其中，

v_mx和v_my为AUV在k时刻的X轴与Y轴方向的速度量测值，W_x和W_y为零均值高斯白噪声，噪声的协方差可以表示为：

AUV的二维运动学方程可以写为：

S2、构建AUV协同定位量测模型

在整个滤波过程中，由于设定测速周期为T_vs，测距周期为T_Rs，一般来说，测距周期要大于测速周期。因此，存在两个过程针对不同量测信息的滤波，即量测信息为测距和测速的滤波及量测信息仅为测速的滤波。设在有测距信息及测速信息的情况下，线性离散化后的量测方程为：

在目标的量测方程中，[x(k) y(k) z(k)]为从AUV在k时刻的坐标，主AUV在k时刻通过利用上述AUV的状态方程和量测方程，以及卡尔曼滤波的递推公式就可以得到系统在任意速度周期时刻的状态信息及协方差更新。

在整个滤波过程当中，假设卡尔曼滤波系统递推周期为&，通常情况下&＝k。

测距单元以&为定位周期，其周期一般大于卡尔曼滤波周期&。在一个卡尔曼滤波周期中，当系统未收到测距信息，可以仅利用测速信息进行滤波，推算位置；在收到测距信息后，利用测距信息及测速信息共同对从AUV位置进行修正，实现高速率输出目标位置。

Claims (6)

1.一种自主水下航行器组合导航系统的主从式协同定位方法，其特征在于，包括如下步骤：

(1)建立主从式组合导航系统，适用于单领航协同定位和双领航协同定位，由主AUV广播自身位置信息，根据声速和时延获得主AUV和从AUV之间的距离、航行速度信息，确定主AUV和从AUV之间的相对位置；

(2)基于自主水下航行器的深度数据建立主从式导航系统建立协同导航空间状态运动模型，所述协同导航空间状态运动模型以导航坐标系下位置信息、载体坐标系下的速度信息、三维姿态信息作为主从式组合导航系统的状态向量；

(3)在主AUV的位置信息下，基于扩展卡尔曼滤波协同定位模型和深度信息对从AUV进行位置纠偏，基于扩展卡尔曼滤波协同定位模型的纠偏过程具体如下：

(31)定义AUV在t_k时刻的二维运动方程为X(k)＝[x(k),v_x(k),y(k),v_y(k)]，分别标识AUV在定位坐标系下的X轴坐标位置，X轴方向速度，Y轴坐标位置，Y轴方向速度，设采样周期为T，则AUV在二维平面的运动模型表达式如下：

所述运动模型的输入表达式如下：

v_mx和v_my为AUV在k时刻的X轴与Y轴方向的速度量测值，W_x和W_y为零均值高斯白噪声；

(32)构建AUV协同定位量测模型，设定测速周期为T_vs，测距周期为T_Rs，设在有测距信息及测速信息的情况下，线性离散化后的量测方程如下所示：

在目标的量测方程中，[x(k) y(k) z(k)]为从AUV在k时刻的坐标，主AUV在k时刻通过利用上述AUV的状态方程和量测方程，以及卡尔曼滤波的递推公式得到系统在任意速度周期时刻的状态信息及协方差更新；

(33)在一个卡尔曼滤波周期中，当系统未收到测距信息，根据测速信息进行滤波，推算位置；在收到测距信息后，利用测距信息及测速信息共同对从AUV位置进行修正，实现高速率输出目标位置。

2.根据权利要求1所述的自主水下航行器组合导航系统的主从式协同定位方法，其特征在于：步骤(2)依次选取导航坐标系下位置信息、三维姿态信息和载体坐标系下的速度信息作为系统的状态向量，得到主从式导航系统在t_k时刻的状态向量的表达式如下：

x_k＝[(pⁿ)^T Θ^T(v^b)^T]^T

＝[x y z ψ θ φ u v w]^T

由航位推算公式得到状态转移方程如下：

3.根据权利要求1所述的自主水下航行器组合导航系统的主从式协同定位方法，其特征在于：所述主从式协同定位方法中，系统的观测向量的表达式如下：

z_k＝[(z_depth)^T (z_attitude)^T (z_velocity)^T (z_range)^T]^T

＝[z φ θ ψ u v w r^M1 … r^Mi]^T。

4.根据权利要求1所述的自主水下航行器组合导航系统的主从式协同定位方法，其特征在于：步骤(2)在从AUV收到距离测量信息的情况下的测量方程如下：

从AUV收到距离测量信息的情况下的测量方程如下：

式中，选取深度信息、姿态信息、载体速度信息以及从AUV之间的相对距离信息构成观测向量。

5.根据权利要求1所述的自主水下航行器组合导航系统的主从式协同定位方法，其特征在于：所述方法中单领航协同定位具体过程如下：

在每一个定位周期内，由主AUV进行发起定位，主AUV将自身的位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，所有AUV在入水前进行对时，保持时间同步，通过获取的时延和声速剖面得到主从AUV之间的距离，从AUV利用主AUV提供的位置信息、测距信息和自身的测速信息，通过滤波算法计算出自身位置并对位置更新。

6.根据权利要求1所述的自主水下航行器组合导航系统的主从式协同定位方法，其特征在于：所述方法中双领航协同定位具体过程如下：

在每一个定位周期内，双领航AUV同时将自身位置信息进行广播，当在主AUV通信范围内的从AUV收到信息后，获得两个不同方位的测距信息，从AUV通过分别两个领航AUV的测距信息、位置信息，以及测距信息，通过滤波算法计算出自身位置并对位置更新。

Images