CN109596128A - 一种基于多水听器提高多auv协同定位性能的方法 - Google Patents

Abstract

本发明属于自主式水下航行领域，具体涉及一种基于多水听器提高多AUV协同定位性能的方法。该方法包括：在跟随AUV上配置多个水听器，设置水听器的位置并固定水听器；领航AUV与跟随AUV通过水听器进行水声测距，同时领航AUV将自身位置广播发送给跟随AUV；建立单领航AUV模式的协同定位***状态空间模型；利用EKF滤波方法，对跟随AUV定位。通过设计多个水听器在跟随AUV上的位置，增加状态空间模型量测矩阵维数，***的可观测性大大提高，有效降低了对单领航方案中的各AUV高机动性要求；减少了多AUV协同导航***领航AUV数量，避免了由于设置多个领航AUV所需要配备的高精度惯性测量单元，降低成本；不涉及到多领航AUV时间同步的问题，容易实施，而且定位精度较高。

Description

一种基于多水听器提高多AUV协同定位性能的方法

技术领域

本发明属于自主式水下航行领域，具体涉及一种基于多水听器提高多AUV协同定位性能的方法。

背景技术

协同导航是目前中间层区域多自主式水下航行器最有效的导航方法之一，具有广阔的应用前景。基于水声通信网络的自主水下航行器AUV协同导航技术是伴随多AUV协作***发展而来的一种新的AUV水下导航方案，相比传统的惯性导航、声学基线导航，协同导航不仅精度高，而且成本低、鲁棒性好，可以大大增强多AUV***的协同作业能力，具有重要的研究价值。据领航AUV数目的不同，又可以分为单领航模式和多领航模式。单领航模式结构简单、容易部署且操作、成本需求低，但是***整体导航性能取决于单个领航AUV，对领航AUV的可靠性要求较高；其次，单领航AUV***可观测性较低，需要领航AUV不断进行航路机动以提高***的可观测性。相对单领航AUV方案来说，多领航方案由于领航AUV数目的增多，***对单个领航AUV的可靠性要求大大降低。同时，由于***的可观测性大大提高，在协同过程中通常不需要靠领航AUV的航路机动来保证***的可观测性，不仅提高了***的协同定位性能，同时更有利于按照预定的任务队形协作执行任务。但是相比于单领航模式来说，由于领航AUV数目的增多，***的导航成本有所增加。从水下协同导航技术的发展趋势来看，基于单领航AUV方案的协同导航无疑更具有吸引力，正受到越来越多的关注和发展。然而，***可观测是实现水下航行器协同导航的前提条件，尤其对于单领航方案来说至关重要。如果所描述的***是可观测的，则载体自身定位误差就可以通过融合领航AUV相对精确的位置信息以及二者间距离量测信息进行协同校正；反之，则不论采用什么样的滤波算法都无法实现位置状态的准确估计，因而如何增强***的可观测性，对提高多AUV协同导航***的协同定位性能来说至关重要。

发明内容

本发明的目的在于提供一种通过在跟随AUV上配置多个水听器并基于非线性滤波估计的多AUV协同导航方法，能够提高单领航模式下的主从式多AUV协同导航***可观测性，并最终提高***的协同定位性能。

本发明的目的是这样实现的：

一种基于多水听器提高多AUV协同定位性能的方法，该方法包括以下步骤：

步骤1：在跟随AUV上配置多个水听器，设置水听器的位置并固定水听器；

步骤2：领航AUV与跟随AUV通过水听器进行水声测距，得到跟随AUV上配置的多个水听器到领航AUV之间的距离，同时领航AUV将自身位置广播发送给跟随AUV；

步骤3：根据水听器的位置信息、跟随AUV上水听器到领航AUV之间的距离信息、领航AUV位置信息、跟随AUV组合推位的位置信息建立单领航AUV模式的协同定位***状态空间模型；

步骤4：利用EKF滤波方法，得到***状态的估计值，对跟随AUV定位。

步骤1所述的在跟随AUV上配置多个水听器包括：以跟随AUV的质心为坐标原点建立载体坐标系Ox_by_bz_b，载体坐标系的x_b轴沿跟随AUV横轴指右，y_b轴沿跟随AUV纵轴指前，z_b轴沿跟随AUV竖轴并与x_b、y_b轴构成右手直角坐标系；跟随AUV沿载体坐标系纵轴航行，跟随AUV上配置的多个水听器位置应均不处于同一纵轴；将水听器A和水听器B分别固定在跟随AUV头尾两侧，设水听器A和水听器B在该载体坐标系的位置坐标分别为(a,b,0)和(-c,-d,0)，其中a为水听器的x_b轴距离，c为跟随AUV质心的x_b轴距离，b为水听器的y_b轴距离，d为跟随AUV质心的y_b轴距离，单位为米。

步骤3所述的建立单领航AUV模式的协同定位***状态空间模型表示为：

式中，k+1表示t_k+1时刻；X_k＝(x_k,y_k,θ_k)^T为跟随AUV在t_k时刻的状态量，x_k为在导航坐标系中跟随AUV在t_k时刻的东向位置坐标，y_k为跟随AUV在t_k时刻的北向位置坐标，θ_k为跟随AUV的在t_k时刻的航向角；X_k+1为跟随AUV在t_k+1时刻的状态量；u_k＝(V_kθ_k)^T为t_k时刻***的传感器输入量，V_k为跟随AUV在t_k时刻的前向速度；Z_k+1为t_k+1时刻的量测向量；为领航AUV在t_k+1时刻的状态量，为跟随AUV和领航AUV在t_k+1时刻的东向位置坐标，为领航AUV在t_k+1时刻的北向位置坐标；w_k为过程噪声向量，v_k+1为量测噪声向量，且w_k、v_k+1均为Gauss白噪声；状态函数δt为采样周期；ω为角速率；量测函数表示为：

其中，x_k+1为在导航坐标系中跟随AUV在t_k+1时刻的东向位置坐标；y_k+1为跟随AUV在t_k+1时刻的北向位置坐标；为t_k+1时刻水听器A到跟随AUV质心在导航坐标系中的东向距离，为t_k+1时刻水听器A到跟随AUV质心在导航坐标系中的北向距离，为t_k+1时刻水听器B到跟随AUV质心在导航坐标系中的东向距离，为t_k+1时刻水听器B到跟随AUV质心在导航坐标系中的北向距离。

假设状态向量X_k＝[x_k,y_k]^T在t_k时刻***后验概率状态以及后验概率密度函数已知，P_kk为t_k时刻的状态估计方差阵，Z_k为t_k时刻由水声测距得到的跟随AUV上两个水听器到领航AUV的距离组成的量测向量，步骤4所述的利用EKF滤波方法，包括以下步骤：

步骤4.1：时间更新；

步骤4.2：量测更新；

步骤4.3：以此过程逐次递推至状态估计误差协方差达到稳定值。

步骤4.1所述时间更新包括，状态一步预测方程表示为下式：

状态预测方差阵表示为：

其中，***状态转移矩阵 为F_k的转置；***噪声激励阵 为G_k的转置；***噪声协方差阵w_k ^T为w_k转置，为传感器速度噪声方差，为航向噪声方差。

步骤4.2所述的量测更新包括，滤波增益表示为下式：

其中，量测矩阵表示为下式：

其中，为Η_k+1的转置；量测噪声协方差阵R_k+1＝E[v_k+1v_k+1 ^T]，v_k+1 ^T为v_k+1转置；

状态估计表示为下式：

状态估计方差阵表示为下式：

P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k

式中，I为单位阵，由滤波估计得到的跟随AUV在t_k+1时刻的状态量；

以此过程逐次递推至状态估计误差协方差达到稳定值，即得到***状态的估计，对跟随AUV定位。

本发明的有益效果在于：

1.跟随AUV配备多个水听器，通过设计多个水听器在跟随AUV上的位置，增加状态空间模型量测矩阵维数，***的可观测性大大提高，有效降低了对单领航方案中的各AUV高机动性要求；

2.减少了多AUV协同导航***领航AUV数量，避免了由于设置多个领航AUV所需要配备的高精度惯性测量单元，降低成本；

3.基于多水听器的协同导航方法不涉及到多领航AUV时间同步的问题，容易实施，而且定位精度较高。

附图说明

图1为本发明的流程图；

图2为跟随AUV上水听器的安装示意图；

图3为双水听器领航AUV与跟随AUV间的测距方案；

图4为EKF滤波流程图；

图5为领航AUV与跟随AUV的真实航行轨迹图；

图6为协同导航跟随AUV轨迹对比图；

图7定位误差对比图。

具体实施方式

下面结合附图对本发明做进一步描述。

单领航方案的多AUV协同导航***成本低、易操作，但其对领航AUV与跟随AUV运动状态要求较高。单领航模式下，若领航AUV、跟随AUV以相同的航速沿相互平行的直线路径机动时，将导致***全局不可观测；若领航AUV、跟随AUV以同速、同向航行时，或航行速度切线方向一致，将导致多AUV协同导航***局部不可观测。通过设计复杂的航路规划方案可以满足协同导航***的可观测性。然而对于航速慢、机动性差的AUV***来说，为了提高***的可观测性，在实际执行任务过程中进行复杂的航路机动往往是不现实的。针对以上问题，本专利设计一种基于多水听器提高多AUV协同导航***可观测性的方法，通过在跟随AUV上装备多个水听器，由水声测距得到各个水听器到领航AUV的距离及水听器与跟随AUV质心的坐标关系，建立多AUV协同导航***状态空间模型，增强了***的可观测性，提高了多AUV***协同定位性能。

步骤一：在跟随AUV上配置水听器。

以跟随AUV的质心为坐标原点建立载体坐标系Ox_by_bz_b，载体坐标系的x_b轴沿跟随AUV横轴指右，y_b轴沿跟随AUV纵轴指前，z_b轴沿跟随AUV竖轴并与x_b、y_b轴构成右手直角坐标系。跟随AUV沿载体坐标系纵轴航行，跟随AUV上配置的多个水听器位置应均不处于同一纵轴，并且各个水听器之间距离尽可能远。跟随AUV上安装多个水听器可提高协同导航***可观测性，但安装过多的水听器对提高协同导航***定位性能并不明显，且成本需求高。以跟随AUV上配置两个水听器为例，将水听器A和水听器B分别固定在在跟随AUV头尾两侧，设水听器A和水听器B在该载体坐标系的位置坐标分别为(a,b,0)和(-c,-d,0)，其中a、c为水听器与跟随AUV质心的x_b轴距离，b、d为水听器与跟随AUV质心的y_b轴距离，单位为米。

步骤二：领航AUV与跟随AUV进行水声测距，领航AUV广播自身位置。

领航AUV与跟随AUV通过水听器进行水声测距，可分别得到跟随AUV上配置的两个水听器到领航AUV之间的距离，同时，领航AUV将自身位置信息广播发送给跟随AUV。

步骤三：建立基于双水听器的单领航AUV协同定位***状态空间模型。

在实际水下协同定位中，AUV的深度和水平位置是相互独立的，通过压力传感器可以获得精确的深度信息，故可将三维的协同定位问题简化为二维，在讨论中将模型投影到二维水平面进行分析。在导航坐标系Ox_ty_t中，定义跟随AUV在t_k时刻和t_k+1时刻的位置向量分别为(x_k,y_k)和(x_k+1,y_k+1)，领航AUV在tk时刻和tk+1时刻的位置向量分别为和 分别为跟随AUV和领航AUV在t_k时刻的东向位置坐标；分别为跟随AUV和领航AUV在t_k时刻的北向位置坐标；分别为跟随AUV和领航AUV在t_k+1时刻的东向位置坐标；分别为跟随AUV和领航AUV在t_k+1时刻的北向位置坐标。基于相对位置量测的协同导航***状态方程具体为：

式中，x_k和x_k+1分别为在导航坐标系中跟随AUV在t_k时刻和t_k+1时刻的东向位置坐标；y_k和y_k+1分别为跟随AUV在t_k时刻和t_k+1时刻的北向位置坐标；δt为采样周期；V_k为跟随AUV在t_k时刻的前向速度，θ_k为跟随AUV的在t_k时刻的航向角；ω为角速率。

将跟随AUV上的水听器A到跟随AUV质心的x_b轴距离和y_b轴距离由载体坐标系投影到导航坐标系得到：

式中，为水听器A到跟随AUV质心距离投影到导航坐标系中的东向距离和北向距离，则水听器A在导航坐标系中位置向量为

同理：

式中，为水听器B到跟随AUV质心距离投影到导航坐标系中的东向距离和北向距离，得到水听器B在导航坐标系中位置向量为

跟随AUV在t_k时刻通过水声通信得到的相对观测信息为领航AUV在t_k时刻的位置信息及领航AUV与跟随AUV之间相对距离量测信息分别为和量测向量由以上信息可得领航AUV和跟随AUV之间的坐标位置关系为：

式中，分别为在导航坐标系中领航AUV在t_k时刻和t_k+1时刻的东向、北向位置坐标；

基于式(1)和式(2)，建立多AUV协同定位***的离散时间状态空间模型(状态方程和量测方程)如下：

式中，k+1表示t_k+1时刻；X_k＝(x_k,y_k,θ_k)^T为跟随AUV在t_k时刻的状态量；X_k+1为跟随AUV在t_k+1时刻的状态量；u_k＝(V_kθ_k)^T为t_k时刻***的传感器输入量；Z_k+1为t_k+1时刻的量测向量；为领航AUV在t_k+1时刻的状态量；w_k为过程噪声向量，v_k+1为量测噪声向量，且w_k、v_k+1均为Gauss白噪声；状态函数量测函数 为t_k+1时刻水听器A到跟随AUV质心在导航坐标系中的东向距离和北向距离，为t_k+1时刻水听器B到跟随AUV质心在导航坐标系中的东向距离和北向距离。

步骤四：利用EKF滤波方法，得到***状态的估计值，从而实现对跟随AUV的协同定位。

假设状态向量X_k＝[x_k,y_k]^T在t_k时刻***后验概率状态以及后验概率密度函数已知，P_kk为t_k时刻的状态估计方差阵，Z_k为t_k时刻由水声测距得到的跟随AUV上两个水听器到领航AUV的距离组成的量测向量。

(1)时间更新

状态一步预测方程：

状态预测方差阵：

其中***状态转移矩阵 为F_k的转置；***噪声激励阵 为G_k的转置；***噪声协方差阵w_k ^T为w_k转置，和分别为传感器速度噪声方差和航向噪声方差。

(2)量测更新

滤波增益：

其中量测矩阵 为Η_k+1的转置；量测噪声协方差阵R_k+1＝E[v_k+1v_k+1 ^T]，v_k+1 ^T为v_k+1转置。

状态估计：

状态估计方差阵：

P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k (10)

式中，I为单位阵，由滤波估计得到的跟随AUV在t_k+1时刻的状态量。

以此过程逐次递推至状态估计误差协方差达到稳定值，即得到***状态的估计，从而实现对跟随AUV的定位。

为进一步说明本发明的有益效果，模拟在领航AUV安装有惯性导航设备和多普勒计程仪，跟随AUV安装有MEMS惯性***和多普勒计程仪实验环境下，对基于双水听器的多AUV协同定位方案进行了仿真验证：

仿真时间为3600s；采样周期为1s；领航AUV以角速率大小为0.1°/s做曲线运动，跟随AUV沿航向角为60°的方向匀速直航；领航AUV与跟随AUV的速度均为4kn；跟随AUV速度传感器量测噪声取为的零均值高斯白噪声；跟随AUV航向角测量噪声取为的零均值高斯白噪声，且跟随AUV航向角存在5°/h的航向漂移误差；仿真中还引入领航AUV与跟随AUV间水声测距的量测噪声，方差为(0.5m)²；水听器A、水听器B分别安装在跟随AUV的右上角与左下角，两个水听器与跟随AUV质心的横向距离均为2米，纵向距离均为5米，进行仿真实验。

Claims (6)

1.一种基于多水听器提高多AUV协同定位性能的方法，其特征在于，该方法包括以下步骤：

步骤1：在跟随AUV上配置多个水听器，设置水听器的位置并固定水听器；

步骤2：领航AUV与跟随AUV通过水听器进行水声测距，得到跟随AUV上配置的多个水听器到领航AUV之间的距离，同时领航AUV将自身位置广播发送给跟随AUV；

步骤3：根据水听器的位置信息、跟随AUV上水听器到领航AUV之间的距离信息、领航AUV位置信息、跟随AUV组合推位的位置信息建立单领航AUV模式的协同定位***状态空间模型；

步骤4：利用EKF滤波方法，得到***状态的估计值，对跟随AUV定位。

2.根据权利要求1所述的一种基于多水听器提高多AUV协同定位性能的方法，其特征在于，步骤1所述的在跟随AUV上配置多个水听器包括：以跟随AUV的质心为坐标原点建立载体坐标系Ox_by_bz_b，载体坐标系的x_b轴沿跟随AUV横轴指右，y_b轴沿跟随AUV纵轴指前，z_b轴沿跟随AUV竖轴并与x_b、y_b轴构成右手直角坐标系；跟随AUV沿载体坐标系纵轴航行，跟随AUV上配置的多个水听器位置应均不处于同一纵轴；将水听器A和水听器B分别固定在跟随AUV头尾两侧，设水听器A和水听器B在该载体坐标系的位置坐标分别为(a,b,0)和(-c,-d,0)，其中a为水听器的x_b轴距离，c为跟随AUV质心的x_b轴距离，b为水听器的y_b轴距离，d为跟随AUV质心的y_b轴距离，单位为米。

3.根据权利要求2所述的一种基于多水听器提高多AUV协同定位性能的方法，其特征在于，步骤3所述的建立单领航AUV模式的协同定位***状态空间模型表示为：

式中，k+1表示t_k+1时刻；X_k＝(x_k,y_k,θ_k)^T为跟随AUV在t_k时刻的状态量，x_k为在导航坐标系中跟随AUV在t_k时刻的东向位置坐标，y_k为跟随AUV在t_k时刻的北向位置坐标，θ_k为跟随AUV的在t_k时刻的航向角；X_k+1为跟随AUV在t_k+1时刻的状态量；u_k＝(V_kθ_k)^T为t_k时刻***的传感器输入量，V_k为跟随AUV在t_k时刻的前向速度；Z_k+1为t_k+1时刻的量测向量；为领航AUV在t_k+1时刻的状态量，为跟随AUV和领航AUV在t_k+1时刻的东向位置坐标，为领航AUV在t_k+1时刻的北向位置坐标；w_k为过程噪声向量，v_k+1为量测噪声向量，且w_k、v_k+1均为Gauss白噪声；状态函数δt为采样周期；ω为角速率；量测函数表示为：

其中，x_k+1为在导航坐标系中跟随AUV在t_k+1时刻的东向位置坐标；y_k+1为跟随AUV在t_k+1时刻的北向位置坐标；为t_k+1时刻水听器A到跟随AUV质心在导航坐标系中的东向距离，为t_k+1时刻水听器A到跟随AUV质心在导航坐标系中的北向距离，为t_k+1时刻水听器B到跟随AUV质心在导航坐标系中的东向距离，为t_k+1时刻水听器B到跟随AUV质心在导航坐标系中的北向距离。

4.根据权利要求3所述的一种基于多水听器提高多AUV协同定位性能的方法，其特征在于：假设状态向量X_k＝[x_k,y_k]^T在t_k时刻***后验概率状态以及后验概率密度函数已知，P_k|k为t_k时刻的状态估计方差阵，Z_k为t_k时刻由水声测距得到的跟随AUV上两个水听器到领航AUV的距离组成的量测向量，步骤4所述的利用EKF滤波方法，包括以下步骤：

步骤4.1：时间更新；

步骤4.2：量测更新；

步骤4.3：以此过程逐次递推至状态估计误差协方差达到稳定值。

5.根据权利要求4所述的一种基于多水听器提高多AUV协同定位性能的方法，其特征在于，步骤4.1所述时间更新包括，状态一步预测方程表示为下式：

状态预测方差阵表示为：

其中，***状态转移矩阵 为F_k的转置；***噪声激励阵 为G_k的转置；***噪声协方差阵w_k ^T为w_k转置，为传感器速度噪声方差，为航向噪声方差。

6.根据权利要求4所述的一种基于多水听器提高多AUV协同定位性能的方法，其特征在于，步骤4.2所述的量测更新包括，滤波增益表示为下式：

其中，量测矩阵表示为下式：

其中，为Η_k+1的转置；量测噪声协方差阵R_k+1＝E[v_k+1v_k+1 ^T]，v_k+1 ^T为v_k+1转置；

状态估计表示为下式：

状态估计方差阵表示为下式：

P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k

式中，I为单位阵，由滤波估计得到的跟随AUV在t_k+1时刻的状态量；

以此过程逐次递推至状态估计误差协方差达到稳定值，即得到***状态的估计，对跟随AUV定位。