CN113156442A - 一种基于长基线水声系统辅助导航的auv水下定位方法 - Google Patents

Abstract

本发明公开了一种基于长基线水声系统辅助导航的AUV水下定位方法，首先对水听器进行布放与位置标定，长基线水声系统接收来自AUV声源发出的信号，进行广义互相关运算，得到模糊相关峰；通过SINS解算出AUV当前位置来筛选主峰得到时延差；选定AUV的一个初始位置坐标，采用Taylor级数展开法进行迭代，通过长基线水听器基阵解算出位置信息；最后将SINS和长基线水声系统分别解算得到的位置坐标的差值输入到卡尔曼滤波器进行滤波，滤波结果对SINS和DVL的各项误差进行校正，得到精确的AUV位置信息。本发明修正了AUV水下航行过程中的累积误差，减小了声线弯曲和多路径效应产生的多相关峰对时延差估计的干扰，提高了AUV水下定位精度。

Description

一种基于长基线水声系统辅助导航的AUV水下定位方法

技术领域

本发明涉及AUV水下导航定位方法，尤其涉及一种基于长基线水声系统辅助导航的AUV水下定位方法。

背景技术

自主式水下航行器(Autonomous Underwater Vehicle，AUV)是一种可以完成海洋探测、水下排雷以及收集海洋及河流的水深测量数据多种功能的水下工具，因其具备活动范围广、体积小、重量轻、隐蔽性好的特点，现已应用在开发海洋资源，以及进行海洋的技术研究中。

在水下环境中，电磁波信号具有严重衰减的特性，在深海或者冰面下，采用 GPS及其他无线电定位方式均不能取得理想的定位效果。因此，AUV通常依靠本体传感器并通过船位推算的方法来估计位置。但采用船位推算时定位误差会随时间不断累积，对于多普勒计程仪(Doppler Velocity Log，DVL)，当声呐传感器远离海底时测速精度很差，仅当AUV贴近海底时定位精度较高，而对于GPS、 AUV需要中断潜行，定期浮到水面才能利用其信息，不仅影响工作效率，增加能耗，更容易暴露其位置，因此，非常有必要研究一种直接在水下进行辅助定位并消除误差的方法。

发明内容

发明目的：为了解决现有技术中AUV在水下航行时，由于声信号在水下传播时产生的声线弯曲和多路径效应而导致的定位误差较大的问题，本发明提供了一种基于长基线水声系统辅助导航的AUV水下定位方法。

技术方案：本发明基于长基线水声系统辅助导航的AUV水下定位方法，采用长基线水声定位系统来定位，该定位方法包括以下步骤：

(1)选取海底水听器基阵中的一个水听器，通过计算安装在水面母船上的两个换能器接收所述水听器发出的信号的时间差，得出距离差，确定该水听器相对于水面母船的距离和位置坐标，并推导出该水听器在地球直角坐标系下的位置坐标；

(2)布放在海底中的水听器接收AUV声源发出的信号至数据处理器，并进行互相关计算，得到模糊相关峰，通过捷联解算得到当前AUV的位置信息P_SINS来筛选主峰；等效声速计算单元利用上一周期的定位结果P_AUV'计算出等效声速c_ij，参考时延差计算单元根据c_ij和P_SINS计算出两个水听器接收到的声信号的参考时延差Δt_ij′；主相关峰筛选单元利用Δt_ij′对该模糊相关峰进行筛选，得到主峰对应的时延差Δt_ij；

(3)AUV位置解算单元根据解算出的AUV位置信息P_SINS，并结合等效声速c_ij和时延差Δt_ij计算距离差，列出双曲线方程，利用Taylor级数展开法解算出长基线水声系统定位的AUV位置信息P_LBL；

(4)Kalman滤波单元对P_SINS与P_LBL的差值进行滤波处理，滤波结果对捷联惯性导航系统和多普勒计程仪的各项误差进行校正，得出AUV的精确位置P_AUV。

其中，步骤(2)包括以下内容：

(2.1)在海底水听器基阵中选取第i个和第j个水听器，建立水听器接收到的信号关于时间的函数关系式，其中，第i个水听器接收到的声信号为：

第j个水听器接收到的声信号为：

其中：μ_i为第i个水听器接收到的声信号在水中传播的衰减系数，μ_j为第j 个水听器接收到的声信号在水中传播的衰减系数；

为互不相关的噪声信号，λ_i为声信号从AUV传播到第i个水听器的时间，λ_j为声信号从AUV传播到第j个到水听器的时间；

(2.2)根据声信号的函数表达式计算得到对应的互相关函数关系式：

其中：λ＝λ_j-λ_i表示到达时间差，T表示观测时间；

(2.3)通过计算第i个水听器和第j个水听器与AUV之间的距离差，结合等效声速，计算得到两个水听器接收到的信号时延差Δt_ij′；

(2.4)主相关峰筛选单元在时延差Δt_ij′附近寻找R_xixj(τ)的峰值，找出最接近所述时延差的相关峰作为主峰，最终得到对应的时延差。

步骤(2.3)包括以下内容：

第i个水听器的位置坐标为(x_i，y_i，z_i)，SINS计算得到的AUV位置坐标为 (x，y，z)，第i个水听器与该AUV的距离为：

第i个水听器和第j个水听器与AUV之间的距离差为

R_ij＝R_i-R_j(i≠j) (7)

第i个水听器与第j个水听器接收到的参考信号时延差为

Δt_ij′＝R_ij/c_ij (8)

其中c_ij为声信号在水中传播的等效声速。

等效声速的计算方法如下：

假设在上个定位周期t_k-1时刻的AUV定位结果为(x(k-1)，y(k-1)，z(k-1))，第i个水听器的位置为P_i(x_i，y_i，z_i)，则在t_k-1时刻，所述第i个水听器与AUV的距离为：

第i个水听器、第j个水听器与AUV的上个周期的距离差为

ΔR_ij(k-1)＝R_i(k-1)-R_j(k-1)

(10)

在t_k-1时刻筛选得到的时延差为t_ij(k-1)，则得出的等效声速为当前等效声速

c_ij(k)＝ΔR_ij(k-1)/t_ij(k-1) (11)

步骤(3)包括以下内容：

假设水听器基阵中有n个水听器，根据双曲线方程，列出定位解算方程组：

R_i1＝R_i-R₁＝c_i1t_i1(i＝2,3，...n,) (12)

将上式(12)在坐标

处按泰勒级数展开，略去二阶以上的小量，得：

ω＝r-Aρ (13)

式中，ω为距离差测量误差向量，ρ、A、r分别如式(14)(15)(17)所示：

式中，Q是ω的协方差矩阵；

选定一个初始坐标值，代入式(15)和(16)中计算A和

并将

代入式(17)中计算r，接着将A,r和Q代入式(14)对ρ进行更新；

若Δx、Δy、Δz满足

则

就是AUV的位置P_LBL，其中η为一很小的门限值；否则按式(19) 更新

并重复上述步骤，直至满足式(18)的条件为止；

则此时的

为AUV的位置P_LBL。

工作原理：在AUV水下航行的过程中，安装在AUV上的捷联惯性导航系统(Strapdown Inertial Navagation System,SINS)通过捷联解算连续输出载体的位置信息，但定位结果会产生累积误差并随着时间的增长而增加。

长基线(Long Base Line,LBL)水声系统利用水下目标与海底阵元之间的距离或距离差信息来求解目标位置，在局部区域内对水下载体进行精确定位，不存在累积误差，但是，声信号在水中传播的声速不能视为常值，同时声音在水下传播时会发生反射造成多途径传播，给斜距差的测量带来较大误差，影响定位结果的精度。

而本发明基于长基线水声系统辅助导航的AUV水下定位方法中，首先进行水听器的布放与位置标定，长基线水声系统接收来自AUV声源发出的信号，进行广义互相关运算，得到模糊相关峰；通过捷联惯性导航系统SINS解算出AUV的当前位置来筛选主峰得到时延差；选定AUV的一个初始位置坐标，并采用Taylor 级数展开法进行迭代，通过长基线水声系统解算出位置信息；最后将捷联惯性导航系统SINS和长基线水声系统分别解算得到的AUV位置坐标的差值输入到卡尔曼滤波器进行滤波，滤波结果对捷联惯性导航系统SINS和多普勒计程仪DVL的定位误差进行校正，得到精确的AUV位置信息。

有益效果：与现有技术相比，本发明具有以下优点：

(1)本发明结合捷联惯性导航系统SINS和多普勒计程仪DVL，以及长基线水声系统各自的优缺点，通过捷联惯性导航系统SINS和多普勒计程仪DVL解算的位置信息辅助估计到达时延差，修正声速，同时利用长基线水声系统估计的位置信息修正了AUV水下航行过程中，捷联惯性导航系统SINS和多普勒计程仪DVL 的累积误差，减小了声线弯曲和多路径效应产生的多相关峰对时延差估计的干扰，提高了AUV水下导航的定位精度，具有很强的实用性。

(2)本发明解决了捷联惯性导航系统SINS系统误差随时间累积的问题，保证了AUV在水下自主导航的精度，避免了GPS和其他无线电系统的应用，提高了 AUV水下作业效率。

(3)本发明提出的时延差方法减小了多路径效应产生的多相关峰对时延差估计的干扰，使最终的时延差计算值精度优于传统方法的时延差计算值。

(4)本发明提出了等效声速的计算方法，对声速进行了修正，解决了声线弯曲现象和声信号的反射带来的定位误差问题，大大减小了时延差和距离差的误差，提高了AUV水下定位精度。

附图说明

图1为本发明基于长基线水声系统辅助导航的AUV水下定位方法流程图；

图2为本发明长基线水声系统示意图；

图3为本发明的水面母船对海底中水听器进行布放与定位的模型示意图；

图4为本发明的水声信号在水下传播路径模型示意图。

具体实施方式

实施例：

如图1所示，本发明基于长基线水声系统辅助导航的AUV水下定位方法，通过长基线(Long Base Line,LBL)水声系统来实现。

该长基线水声系统包括布放在海底的水听器基阵，安装在AUV上的问答机和数据处理器。通过长基线水声系统辅助AUV上的捷联惯性导航系统SINS和多普勒计程仪DVL系统来提高水下定位的精度。其中，该水听器基阵包括多个布放在海底的水听器。

数据处理器包括声信号互相关计算单元、主相关峰筛选单元、参考时延差计算单元、等效声速计算单元、AUV位置解算单元和Kalman滤波单元。

其中，声信号互相关计算单元对其中的任意两个水听器接收到的声信号进行互相关运算得到模糊相关峰，同时捷联惯性导航系统SINS和多普勒计程仪DVL 系统通过捷联解算得到AUV的位置信息P_SINS；等效声速计算单元利用上一周期的定位结果P_AUV'计算出等效声速c_ij，时延差计算单元根据c_ij和P_SINS计算出第i号水听器和第j号水听器接收到的声信号的参考时延差Δt_ij′；主相关峰筛选单元利用Δt_ij′对前面声信号互相关计算单元得到的模糊相关峰进行筛选，得到主峰对应的时延差Δt_ij；AUV位置解算单元根据解算出的位置信息P_SINS，利用Taylor级数展开算法解算出长基线系统定位的位置信息P_LBL；Kalman滤波单元对P_SINS和 P_LBL的差值进行滤波处理，滤波结果用于校正捷联惯性导航系统SINS和多普勒计程仪DVL组合导航系统的各项误差，最终得出AUV的精确位置P_AUV。

本发明基于长基线水声系统辅助导航的AUV水下定位方法的具体实现步骤如下：

步骤1，如图2、图3所示，AUV随水面母船布放到指定水域，水面母船上设有换能器基阵，海底布设有水听器基阵。该换能器基阵对海底的水听器进行定位，位于海底的水听器基阵对AUV进行定位。具体布设过程如下：

首先在海底对水听器进行布放与位置标定；水面母船的底部已经预先安装有换能器基阵，该换能器基阵至少包括三个换能器，该换能器基阵用来对海底水听器进行定位的具体过程为：换能器基阵接收来自水下水听器发出的信号，根据短基线定位原理计算出海底的每个水听器相对于母船底部的换能器基阵的位置坐标，即，选取海底中的一个水听器，通过计算安装在水面母船上的两个换能器接收该水听器发出的信号的时间差，得出距离差，进而确定该海底的水听器相对于水面母船的距离和位置坐标，并推导出该海底的水听器在地球直角坐标系下的位置坐标。

具体操作方法为，先在海底进行水听器的布放，计算出每个水听器在大地坐标系下的坐标，其中第i个水听器的大地坐标用P_i(L，λ，h)表示，并将其转化为用地球直角坐标系P_i(x，y，z)表示。

其中，由公式

获得P_i(x，y，z)。

式中：L,λ,h分别为水听器所在位置的大地纬度，大地经度和大地高度，e 为第一偏心率，R_N为参考椭球体子午面内的曲率半径，

其中，a为地球椭球长半轴。

步骤2，如图2所示，布放在海底中的多个水听器接收AUV上的声源发出的声音信号，通过海底电缆将信号发送至水下数据处理器，水下数据处理器根据各水听器接收的信号进行广义互相关运算，得到模糊相关峰，捷联惯性导航系统 SINS提供当前AUV的位置信息P_SINS来筛选主峰，等效声速计算单元利用上一周期的定位结果P_AUV'计算出等效声速c_ij，参考时延差计算单元根据c_ij和P_AUV'计算出两个水听器接收到的声信号的参考时延差Δt_ij′；主相关峰筛选单元利用Δt_ij′对所述模糊相关峰进行筛选，得到主峰对应的时延差Δt_ij；具体步骤如下：

步骤2.1，在海底水听器基阵中选取第i个水听器和第j个水听器，考虑到声信号在水中传播时会产生衰减以及水中的噪声干扰信号，建立水听器接收到的信号关于时间的函数关系式，其中第i个水听器接收到的声信号为

第j个水听器接收到的声信号为

其中：μ_i为第i个水听器接收到的声信号在水中传播的衰减系数，μ_j为第j 个声信号在水中传播的衰减系数；

为互不相关的噪声信号，λ_i为声信号从AUV传播到第i个水听器的时间，λ_j为声信号从AUV传播到第j个水听器的时间。

步骤2.2，根据声信号的函数表达式计算得到对应的互相关函数关系式：

其中：λ＝λ_j-λ_i表示到达时间差，T表示观测时间。

步骤2.3，如图4所示，声信号从AUV传播到第一水听器T₁和第二水听器 T₂的过程中时，声信号采用了P_di，P_si，P_bi(i＝1,2)三种传播路径，由于多路径效应，声源信号x(t)经过多路径反射到达第一水听器和第二水听器后，接收到的声信号为：

其中β_di、β_si、β_bi(i＝1,2)分别为3种传播路径的衰减系数，τ_di、τ_si、τ_bi为传播时间，其中i＝1,2。

x₁(t)、x₂(t)的互相关函数为

R_x1x2(τ)＝

R_d1β_d2R_xx(τ_d1-τ_d2-τ)+β_d1β_s2R_xx(τ_d1-τ_s2-τ)

+β_d1β_b2R_xx(τ_d1-τ_b2-τ)+β_s1β_d2R_xx(τ_s1-τ_d2-τ)

+β_s1β_s2R_xx(τ_s1-τ_s2-τ)+β_s1β_b2R_xx(τ_s1-τ_b2-τ)

+β_b1β_d2R_xx(τ_b1-τ_d2-τ)+β_b1β_s2R_xx(τ_b1-τ_s2-τ)

+β_b1β_b2R_xx(τ_b1-τ_b2-τ)

其中R_xx(t)为x(t)的自相关函数；多路径效应的原因使得互相关函数出现多个相关峰。

则互相关函数在τ_d1-τ_d2,τ_d1-τ_s2,τ_d1-τ_b2,τ_s1-τ_d2,τ_s1-τ_s2,τ_s1-τ_b2, τ_b1-τ_d2,τ_b1-τ_s2,τ_b1-τ_b2这9个到达时延点上出现峰值，即出现模糊相关峰，实际情况中，我们只需要计算出直达路径的到达时延差，即主峰所对应的时延差，因为其它峰值将对选择正确的主峰产生干扰，使得无法正确估计出到达时延差。对此利用捷联惯性导航系统SINS提供的位置信息辅助选出精度较高的相关峰进行时延差的估计。即通过计算第i个水听器和第j个水听器与AUV之间的距离差，结合等效声速，计算得到两个水听器接收到的信号的参考时延差Δt_ij′。具体方法如下：

长基线水听器基阵中，第i个水听器的位置坐标为(x_i，y_i，z_i)，通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航系统计算得到的AUV位置为 (x，y，z)，第i个水听器与AUV的距离为：

第i个水听器和第j个水听器与AUV之间的距离差为

R_ij＝R_i-R_j(i≠j) (7)

第i个水听器与第j个水听器接收到的参考信号时延差为

Δt_ij′＝R_ij/c_ij (8)

其中c_ij为声信号在水中传播的等效声速，该等效声速的具体计算方法如下：

如图4所示，由于声信号在水下传播时会产生反射现象并伴有多途效应，故采用传统意义的声速计算时延差会带来较大的误差，本发明采用等效声速的计算方法得出等效声速，进而减小声线弯曲现象造成的水下定位误差，具体分析方法如下：

假设在上个定位周期t_k-1时刻AUV的定位结果为(x(k-1)，y(k-1)，z(k-1))，第i个水听器的位置为P_i(x_i，y_i，z_i)，第j个水听器的位置为P_j(x_j，y_j，z_j)，则在 t_k-1时刻，第i个水听器与AUV的距离、第j个水听器与AUV的距离为

第i个水听器、第j个水听器与AUV的上个周期的距离差为：

ΔR_ij(k-1)＝R_i(k-1)-R_j(k-1) (10)

在t_k-1时刻筛选得到的时延差为t_ij(k-1)，则得出的等效声速为当前等效声速：

c_ij(k)＝ΔR_ij(k-1)/t_ij(k-1) (11)

步骤2.4，如图1所示，主相关峰筛选单元在参考时延差Δt_ij′附近寻找相关峰R_xixj(τ)的峰值，找出最接近该时延差的相关峰作为主峰，该主峰对应的时延差Δt_ij为最终时延差。

步骤3，选定AUV的一个初始位置坐标值P_SINS进行迭代，AUV位置解算单元结合等效声速c_ij和时延差Δt_ij计算距离差，列出双曲线方程，以P_SINS为先验位置信息，采用Taylor级数展开法通过长基线水听器基阵解算出长基线定位的AUV 位置P_LBL。具体方法如下：

假设水听器基阵中有n个水听器，根据双曲线方程，列出定位解算方程组：

R_i1＝R_i-R₁＝c_i1t_i1(i＝2,3，...,n)

(12)

将上式在坐标

处按泰勒级数展开，略去二阶以上的小量，得：

ω＝r-Aρ (13)

式中，ω为距离差测量误差向量，ρ、A、r分别如式(14)(15)(17)所示：

式中，Q是ω的协方差矩阵；

选定一个初始坐标值，代入式(15)和(16)中计算A和

并将

代入式(17)中计算r，接着将A,r和Q代入式(14)对ρ进行更新。若Δx、Δy、Δz 足够小，满足

则

就是AUV的位置P_LBL，其中η为一很小的门限值；否则按式(19) 更新

并重复上述步骤，直至满足式(18)的条件为止。

则此时的

就是AUV的位置P_LBL，其中η为一很小的门限值。

步骤(3)中的初始位置坐标值为捷联惯性导航系统SINS和多普勒计程仪 DVL的解算结果P_SINS，该等效声速采用上个周期定位的第i个水听器和第j个水听器接收到目标声源的距离差除以时延差得到，该时延差即通过捷联惯性导航系统SINS定位结果辅助选择主相关峰的方法得出的时延差。

本发明选用捷联惯性导航系统SINS和多普勒计程仪DVL组合导航系统的解算结果P_SINS作为迭代初值既满足了泰勒级数方法的收敛性，也使迭代次数大大减少。

步骤4，将捷联惯性导航系统解算的AUV位置P_SINS，与长基线水声系统解算的AUV位置结果P_LBL的差值作为外部观测信息输入量输入到卡尔曼滤波器进行滤波处理，滤波结果对捷联惯性导航系统SINS和多普勒计程仪DVL组合导航系统的各项误差进行校正，最终得到AUV的精确位置P_AUV。

Claims (5)

1.一种基于长基线水声系统辅助导航的AUV水下定位方法，其特征在于：采用长基线水声定位系统来定位，所述方法包括以下步骤：

(1)选取海底水听器基阵中的一个水听器，通过计算安装在水面母船上的两个换能器接收所述水听器发出的信号的时间差，得出距离差，进而确定所述水听器相对于水面母船的距离和位置坐标，并推导出所述水听器在地球直角坐标系下的位置坐标；

(2)布放在海底中的水听器接收AUV声源发出的信号至数据处理器，并进行互相关计算，得到模糊相关峰，SINS通过捷联解算得到当前AUV的位置信息P_SINS来筛选主峰；等效声速计算单元利用上一周期的定位结果P_AUV'计算出等效声速c_ij，参考时延差计算单元根据c_ij和P_SINS计算出两个水听器接收到的声信号的参考时延差Δt_ij′；主相关峰筛选单元利用Δt_ij′对所述模糊相关峰进行筛选，得到主峰对应的时延差Δt_ij；

(3)AUV位置解算单元根据解算出的AUV位置信息P_SINS，并结合等效声速c_ij和时延差Δt_ij计算距离差，列出双曲线方程，利用Taylor级数展开法解算出长基线水声系统定位的AUV位置信息P_LBL；

(4)Kalman滤波单元对P_SINS与P_LBL的差值进行滤波处理，滤波结果对捷联惯性导航系统和多普勒计程仪的各项误差进行校正，得出AUV的精确位置P_AUV。

2.根据权利要求1所述的基于长基线水声系统辅助导航的AUV水下定位方法，其特征在于：所述步骤(2)包括以下内容：

(2.1)在海底水听器基阵中选取第i个和第j个水听器，建立水听器接收到的信号关于时间的函数关系式，其中，第i个水听器接收到的声信号为：

第j个水听器接收到的声信号为：

其中：μ_i为第i个水听器接收到的声信号在水中传播的衰减系数，μ_j为第j个声信号在水中传播的衰减系数；

为互不相关的噪声信号，λ_i为声信号从AUV传播到第i个水听器的时间，λ_j为声信号从AUV传播到第j个到水听器的时间；

(2.2)根据声信号的函数表达式计算得到对应的互相关函数关系式：

其中：λ＝λ_j-λ_i表示到达时间差，T表示观测时间；

(2.3)通过计算第i个水听器和第j个水听器与AUV之间的距离差，结合等效声速，计算得到两个水听器接收到的信号时延差Δt_ij′；

(2.4)主相关峰筛选单元在时延差Δt_ij′附近寻找R_xixj(τ)的峰值，找出最接近所述时延差的相关峰作为主峰，最终得到对应的时延差。

3.根据权利要求2所述的基于长基线水声系统辅助导航的AUV水下定位方法，其特征在于：步骤(2.3)包括以下内容：

所述第i个水听器的位置坐标为(x_i，y_i，z_i)，SINS计算得到的AUV位置坐标为(x，y，z)，第i个水听器与所述AUV的距离为：

第i个水听器和第j个水听器与AUV之间的距离差为

R_ij＝R_i—R_j(i≠j) (7)

第i个水听器与第j个水听器接收到的参考信号时延差为

Δt_ij′＝R_ij/c_ij (8)

其中c_ij为声信号在水中传播的等效声速。

4.根据权利要求3所述的基于长基线水声系统辅助导航的AUV水下定位方法，其特征在于：所述等效声速的计算方法如下：

假设在上个定位周期t_k-1时刻的AUV定位结果为(x(k-1)，y(k-1)，z(k-1))，第i个水听器的位置为P_i(x_i，y_i，z_i)，则在t_k-1时刻，所述第i个水听器与AUV的距离为：

第i个水听器、第j个水听器与AUV的上个周期的距离差为

ΔR_ij(k-1)＝R_i(k-1)-R_j(k-1) (10)

在t_k-1时刻筛选得到的时延差为t_ij(k-1)，则得出的等效声速为当前等效声速

c_ij(k)＝ΔR_ij(k-1)/t_ij(k-1) (11)

5.根据权利要求1所述的基于长基线水声系统辅助导航的AUV水下定位方法，其特征在于：所述步骤(3)包括以下内容：

假设水听器基阵中有n个水听器，根据双曲线方程，列出定位解算方程组：

将上式(12)在坐标

处按泰勒级数展开，略去二阶以上的小量，得：

ω＝r-Aρ (13)

式中，ω为距离差测量误差向量，ρ、A、r分别如式(14)(15)(17)所示：

式中，Q是ω的协方差矩阵；

选定一个初始坐标值，代入式(15)和(16)中计算A和

并将

代入式(17)中计算r，接着将A,r和Q代入式(14)对ρ进行更新；

若Δx、Δy、Δz满足

则

就是AUV的位置P_LBL，其中η为一门限值；否则按式(19)更新

并重复上述步骤，直至满足式(18)的条件为止；

则此时的

为AUV的位置P_LBL。

Images