CN107966145B - 一种基于稀疏长基线紧组合的auv水下导航方法 - Google Patents

Abstract

本发明涉及一种基于稀疏长基线紧组合的AUV水下导航方法，其技术特点在于：包括以下步骤：步骤1、在AUV的某作业区域内，主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正；步骤2、当AUV即将超出预先布放的两个水下应答器作用范围时，布放两个后续水下应答器，利用局部滤波器估计两个后续布放应答器的位置信息，当标定的位置精度达到要求时进入主滤波器，主滤波器融合后续的水下应答器进行水下AUV紧组合导航；当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行水下AUV紧组合导航，并重复执行上述过程。本发明节约应答器的布放成本，有效提升了AUV水下导航精度。

Description

一种基于稀疏长基线紧组合的AUV水下导航方法

技术领域

本发明属于惯性导航与水声定位技术领域，涉及AUV水下导航方法,尤其是一种基于稀疏长基线紧组合的AUV水下导航方法。

背景技术

目前，惯性导航系统(INS)为水下自主式航行器(AUV)在复杂的海洋环境执行任务与使命时不间断的提供载体姿态、速度和位置等信息，是保证其航行安全性、有效完成水下作业任务的关键。依靠单一的水下导航方式无法满足水下AUV高精度、长时间的定位需求，通常采用以惯性导航为核心的多源信息融合技术。

传统的长基线(LBL)水声定位技术是在海底以一定的几何形状布设4个水下应答器，使用时各应答器位置需要预先进行精密测量，通过测量安装在被测目标上的声学设备发出的应答信号来确定水下应答器与载体之间的相对位置，进而确定被测目标的位置坐标。

但传统的长基线(LBL)水声定位技术需要预先精确标定出应答器的位置，应答器的布放成本高，而且，由于AUV的任务区域受声学基阵配置范围的限制，导致其存在执行范围小、航行时间短，无法确保安全可靠作业的缺陷。

发明内容

本发明的目的在于提供一种设计合理、操作成本低且安全可靠的基于稀疏长基线紧组合的AUV水下导航方法。

本发明解决其现实问题是采取以下技术方案实现的：

一种基于稀疏长基线紧组合的AUV水下导航方法，包括以下步骤：

步骤1、在AUV的某作业区域内，预先布放水下应答器a和水下应答器b，主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正，进而完成水下AUV紧组合导航；

步骤2、当AUV即将超出预先布放的两个水下应答器作用范围时，布放两个后续水下应答器，利用局部滤波器估计两个后续布放应答器的位置信息，并当两个后续布放应答器的位置信息达到精度要求时进入主滤波器，主滤波器融合后续的水下应答器进行水下AUV紧组合导航，采用紧组合算法得到最终结果，同时反馈校正导航设备内部状态信息，抑制自身导航设备误差的发散增长；当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行水下AUV紧组合导航，并重复执行上述过程。

而且，所述步骤1的主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正的具体步骤包括：

(1)获得水下应答器a和水下应答器b的位置信息；

地球坐标系中，水下应答器a和水下应答器b的位置分别为(x_a,y_a,z_a)、(x_b,y_b,z_b)，可利用如下所示的地理坐标系转换到地球坐标系公式得到：

其中：

x,y,z为地球坐标系中位置坐标；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

R_E为与子午面垂直的法线平面的曲率半径，

e为椭球偏心率，

r_x,r_y为椭圆长半径和短半径；

(2)对水下应答器a和水下应答器b的位置信息进行解算，由INS推算AUV到应答器a和水下应答器b的计算距离；

当AUV作业距离内有2个位置已知的水下应答器(水下应答器a和水下应答器b)时，对2个应答器的位置信息进行解算，由INS推算AUV到2个应答器的计算距离为ρ_I1和ρ_I2：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：分别为2个应答器的位置坐标；

(x_I,y_I,z_I)为INS测量得到的AUV位置坐标。

(3)利用kalman滤波器实现稀疏长基线紧组合算法，对导航设备和声学测距信息误差进行校正；

kalman滤波器中状态变量X选取为姿态误差

速度误差δv、位置误差δp、陀螺漂移ε、加速度计零偏▽，以及由时钟引起的声学测距误差δρ_S；由声学测量得到AUV到2个应答器的声学测量距离为ρ_S1和ρ_S2，并将INS推算的距离ρ_Ii在(x,y,z)处进行泰勒级数展开，取一次项误差可得到AUV到2个应答器的计算距离与声学测量距离之差的观测方程Z如下所示：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：为2个应答器的位置；

ρ_I1、ρ_I2：为INS推算的AUV到2个应答器的计算距离；

ρ_S1、ρ_S2：声学测量得到AUV到2个应答器的声学测量距离。

地球坐标系与地理坐标系微分转换公式如下所示：

其中：(δx,δy,δz)为地球坐标系位置误差；

(δL,δλ,δh)为地理坐标系位置误差；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

e,R为椭球偏心率和地球半径。

而且，所述步骤2的利用局部滤波器估计两个后续布放应答器位置信息并使其达到精度要求的具体方法为：

当AUV运行到某作业区域，布放水下应答器，应答器沉入水底经触发后以等间隔时间发出声信息，提供自身与AUV相互间的距离信息；水下应答器布放后，载体进行机动航行，建立地球直角坐标系下AUV到水下应答器绝对位置X(x₀,y₀,z₀)校正的观测方程：

其中：F_i(i＝1,2,...,n)为无任何误差条件下的声学距离；

(x₀,y₀,z₀)为地球坐标系下应答器位置X初始值；

(x_i,y_i,z_i)为惯导设备第i(i＝1,2,...,n)次测量点的位置。

当水下AUV运行到作业区域或者校正区域时，布放水下应答器，应答器沉入水底经触发后发送声脉冲信号，可为其他载体提供自身精确的深度信息以及相互间的距离信息；设定水下应答器初值设置为AUV投放时的地理位置P₀(L₀,λ₀,h₀)，可根据INS导航系统中主滤波器输出得到，并经地理坐标系转换到地球坐标系为X(x₀,y₀,z₀)；水下应答器布放后，AUV进行机动航行，建立AUV到水下应答器位置绝对位置校正的观测方程：

式中：i＝1,2,...,n表示测量次数，(x_i,y_i,z_i)表示惯导设备第i次测量点的位置；

将上述公式在应答器初始值X(x₀,y₀,z₀)进行线性化泰勒展开，可得如下公式：

式中：F_i为无任何误差条件下的声学距离；

上述可写成向量形式为：

A·ΔX＝B

式中：ΔX＝[Δx Δy Δz]^T；

B＝[F₁-F₁₀ F₂-F₂₀ … F_n-F_n0]^T.

利用最小二乘法求解，可得：

ΔX＝(A^TA)^-1A^TB

将求得的ΔX迭代校正应答器位置X：

X＝X-ΔX

将所得结果作为初值带入F_i的无任何误差条件下的声学距离公式，利用泰勒级数展开法和最小二乘法迭代求解公式，重复上述过程，经数次迭代至精度满足需求，重复性可达到米级。

本发明的优点和有益效果：

1、本发明基于稀疏长基线定位技术，仅采用2个水下应答器声学测距紧组合方法，并辅以载体的一定的运动路径，便可实现传统长基线4个水下应答器的测量效果，有效节约了应答器的布放成本，并可在水下未知环境中实现增量式水下环境地图的构建与导航，有效提高了AUV水下校正精度。

2、本发明采用水下导航技术，AUV在未知环境中从一个未知的位置开始移动，在航行过程中依靠自身携带的环境感知传感器和惯性测量设备实现位置估计与环境地图构建。水下AUV利用此方法执行导航任务时，并不需要预先在任务区域精确布放水下声学应答器，节约了布放成本，同时使得AUV的任务区域不受声学基阵配置范围的限制，更加适合于执行大范围、长航时的任务，对AUV长航时安全、可靠作业具有重要意义。

3、本发明基于稀疏长基线紧组合的AUV水下导航算法，增强水下声学辅助惯性导航的智能化程度，降低其应用于水下AUV的复杂度，AUV通过所携带的声学传感器感知外部测量信息，将新布放的水下应答器融入到地图中，进行增量式的环境建模，从而构建环境地图并完成水下AUV自身的位置校正。稀疏长基线紧组合算法采用2个水下应答器声学测距功能可以达到4个应答器长基线测量效果。整个过程无需预先标定水下应答器的位置，应用方式灵活、布放成本低，可提升水下AUV在运行区域内的导航精度，组合导航系统定位误差为米级，更加适合于执行大范围、长航时的任务，对AUV长航时安全、可靠作业具有重要意义，为解决AUV导航设备水下校正提供了可能。

附图说明

图1为本发明的基于稀疏长基线紧组合的AUV水下导航方法应用场景示意图；

图2为本发明的基于稀疏长基线紧组合的AUV水下导航方法原理框图。

具体实施方式

以下结合附图对本发明实施例作进一步详述：

本发明针对水下自主式航行器(AUV)导航设备的特点和实际使用条件，设计了一种基于稀疏长基线紧组合的AUV水下导航方法，如图1和图2该融合算法由主滤波器和局部滤波器构成，采用并行处理方式。水下AUV运行到某作业区域后，主滤波器利用声学测距信息对布放水下2个水下应答器(应答器a和应答器b)进行稀疏长基线紧组合导航，抑制自身惯导设备(INS)定位误差的发散。当AUV即将远离上述水下应答器后，在载体运行路径上继续布放几个水下应答器(应答器c、应答器d……等)，局部滤波器是利用上述主滤波器所得到的最优导航输出结果，结合水声测距定位原理，采用非线性最小二乘算法滤波估计出新布放的应答器位置坐标，进行增量式的环境建模，构建水下环境地图并完成水下AUV自身的位置校正，实现了AUV长航程导航功能。在运行路径上始终保证主滤波器有2个水下应答器进行声学测距紧组合，对自身导航系统进行误差校正，整个过程无需预先标定水下应答器的位置，应用方式灵活、布放成本低，提升了水下AUV在航行区域内的导航精度。这种基于稀疏长基线紧组合的AUV水下导航方法应用场景示意图如图1所示。

一种基于稀疏长基线紧组合的AUV水下导航方法，包括以下步骤：

步骤1、在AUV的某作业区域内，预先布放水下应答器a和水下应答器b，主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正，进而完成水下AUV紧组合导航；

所述步骤1的主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正的具体步骤包括：

(1)获得水下应答器a和水下应答器b的位置信息；

地球坐标系中，水下应答器a和水下应答器b的位置分别为(x_a,y_a,z_a)、(x_b,y_b,z_b)，可利用如下所示的地理坐标系转换到地球坐标系公式得到：

其中：

x,y,z为地球坐标系中位置坐标；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

R_E为与子午面垂直的法线平面的曲率半径，

e为椭球偏心率，

r_x,r_y为椭圆长半径和短半径。

(2)对水下应答器a和水下应答器b的位置信息进行解算，由INS推算AUV到应答器a和水下应答器b的计算距离；

当AUV作业距离内有2个位置已知的水下应答器(水下应答器a和水下应答器b)时，对2个应答器的位置信息进行解算，由INS推算AUV到2个应答器的计算距离为ρ_I1和ρ_I2：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：分别为2个应答器的位置坐标；

(x_I,y_I,z_I)为INS测量得到的AUV位置坐标。

(3)利用kalman滤波器实现稀疏长基线紧组合算法，对导航设备和声学测距信息误差进行校正；

kalman滤波器中状态变量X选取为姿态误差

速度误差δv、位置误差δp、陀螺漂移ε、加速度计零偏▽，以及由时钟引起的声学测距误差δρ_S；由声学测量得到AUV到2个应答器的声学测量距离为ρ_S1和ρ_S2，并将INS推算的距离ρ_Ii在(x,y,z)处进行泰勒级数展开，取一次项误差可得到AUV到2个应答器的计算距离与声学测量距离之差的观测方程Z如式(4)所示：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：为2个应答器的位置；

ρ_I1、ρ_I2：为INS推算的AUV到2个应答器的计算距离；

ρ_S1、ρ_S2：声学测量得到AUV到2个应答器的声学测量距离。

地球坐标系与地理坐标系微分转换公式如下(5)所示：

其中：(δx,δy,δz)为地球坐标系位置误差；

(δL,δλ,δh)为地理坐标系位置误差；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

e,R为椭球偏心率和地球半径。

从观测方程Z可以看出，紧组合导航系统的信息融合是针对2个水下应答器进行数据处理的，当水下应答器数量不确定时，观测方程的维数可以随之自动切换改变，这也正是紧组合技术优越性的体现。仅从算法本身来说，稀疏长基线紧组合仅利用单一水下应答器即可用于滤波计算，考虑到误差状态可观测度大小，应当辅以特定的运动轨迹，同样可有效抑制导航参数误差发散，完成对INS导航误差(姿态误差、速度误差、位置误差及惯性元件漂移等)的校正，提高系统导航精度。当可用水下应答器为2个时，由于紧组合算法由于采用了最原始的水声测距信息，并不需要接收机进行定位解算，避免了观测量与观测噪声的时间相关问题，可有效提高水下AUV组合导航定位精度。

步骤2、当AUV即将超出预先布放的两个水下应答器作用范围时，布放两个后续水下应答器，利用局部滤波器估计两个后续布放应答器的位置信息，并当两个后续布放应答器的位置信息达到精度要求时进入主滤波器，主滤波器融合后续的水下应答器进行水下AUV紧组合导航，采用紧组合算法得到最终结果，同时反馈校正导航设备内部状态信息，抑制自身导航设备误差的发散增长；当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行水下AUV紧组合导航，并重复执行上述过程。

所述步骤2的利用局部滤波器估计两个后续布放应答器位置信息并使其达到精度要求的具体方法为：

当AUV运行到某作业区域，布放水下应答器，应答器沉入水底经触发后以等间隔时间发出声信息，提供自身与AUV相互间的距离信息；水下应答器布放后，载体进行机动航行，建立地球直角坐标系下AUV到水下应答器绝对位置X(x₀,y₀,z₀)校正的观测方程：

其中：F_i(i＝1,2,...,n)为无任何误差条件下的声学距离；

(x₀,y₀,z₀)为地球坐标系下应答器位置X初始值；

(x_i,y_i,z_i)为惯导设备第i(i＝1,2,...,n)次测量点的位置。

当水下AUV运行到作业区域或者校正区域时，布放水下应答器，应答器沉入水底经触发后发送声脉冲信号，可为其他载体提供自身精确的深度信息以及相互间的距离信息；设定水下应答器初值设置为AUV投放时的地理位置P₀(L₀,λ₀,h₀)，可根据INS导航系统中主滤波器输出得到，经公式(1)转换到地球坐标系为X(x₀,y₀,z₀)；水下应答器布放后，AUV进行机动航行，建立AUV到水下应答器位置绝对位置校正的观测方程：

式中：i＝1,2,...,n表示测量次数，(x_i,y_i,z_i)表示惯导设备第i次测量点的位置。

将公式(7)在应答器初始值X(x₀,y₀,z₀)进行线性化泰勒展开，可得公式(8)：

式中：F_i为无任何误差条件下的声学距离。

公式(8)可写成向量形式为：

A·ΔX＝B (9)

式中：ΔX＝[Δx Δy Δz]^T；

B＝[F₁-F₁₀ F₂-F₂₀ … F_n-F_n0]^T.

利用最小二乘法求解，可得：

ΔX＝(A^TA)^-1A^TB (10)

将求得的ΔX迭代校正应答器位置X：

X＝X-ΔX (11)

将所得结果作为初值带入式(8)，利用泰勒级数展开法和最小二乘法迭代求解公式重复式(9)-(11)过程，经数次迭代至精度满足需求，重复性可达到米级。

所述步骤2的工作原理为：

局部滤波器用来估计布放应答器的位置信息，每个布放的应答器均采用相应的局部滤波器去估计其位置信息，当各局部滤波器(局部滤波器c、局部滤波器d……局部滤波器z)分别估计水下应答器(水下应答器c、水下应答器d……水下应答器z)的位置精度达到要求时，进入主滤波器，主滤波器融合新的水下应答器，在运行路径上始终保证主滤波器有2个水下应答器进行声学测距紧组合，采用测距紧组合算法得到最终结果，同时反馈校正导航设备内部状态信息，抑制自身导航设备误差的发散增长，提升导航定位精度。

本发明的工作原理为：

本发明采用的水下导航方法是指自主式航行器(AUV)在未知环境中行驶时，通过外部传感器探测环境中的特征点，依靠所携带传感器增量式地构建环境地图，同时利用传感器观测与地图特征之间的对应关系对AUV自身位姿进行定位。根据AUV运行的水下环境的特殊性，特征点可选择为水下应答器，组合导航融合滤波器主要由主滤波器和局部滤波器两级构成，主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，完成载体导航设备(主要由陀螺仪和加速度计组成)的最优位置输出，由于声学距离信息主要是根据到达时间差乘以声速得到，融合滤波器同时还可估计由时钟误差引起的水声测距误差，紧组合算法同时可对导航设备和声学测距信息误差进行校正。局部滤波器用来估计布放应答器的位置信息，每个布放的应答器均采用相应的局部滤波器去估计其位置信息，当各局部滤波器(局部滤波器c、局部滤波器d……局部滤波器z)分别估计水下应答器(水下应答器c、水下应答器d……水下应答器z)的位置精度达到要求时，进入主滤波器，主滤波器融合新的水下应答器，在运行路径上始终保证主滤波器有2个水下应答器进行声学测距紧组合，采用测距紧组合算法得到最终结果，同时反馈校正导航设备内部状态信息，抑制自身导航设备误差的发散增长，提升导航定位精度。

定位是实现移动水下AUV自主导航的重要前提，利用水下特征预测位置与实际测量位置之间的偏差完成对自身位置及特征位置的校正，将水下AUV在航行过程中新观测到的路标向量加入到联合状态变量，以此增量式构建环境地图，同时更新AUV在环境中的位置。整个过程中AUV无需预知环境信息，完全通过自身携带惯性传感器以及声学测距单位在未知的水下环境中即可实现抑制自身导航误差发散的位置校正功能，从而在AUV航行路径中的较大范围内保持一定的定位精度，本发明原理框图如图2所示。

需要强调的是，本发明所述实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (2)

1.一种基于稀疏长基线紧组合的AUV水下导航方法，其特征在于：包括以下步骤：

步骤1、在AUV的某作业区域内，预先布放水下应答器a和水下应答器b，主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息进行误差校正，进而完成水下AUV紧组合导航；

步骤2、当AUV即将超出预先布放的两个水下应答器作用范围时，布放两个后续水下应答器，利用局部滤波器估计两个后续布放应答器的位置信息，并当两个后续布放应答器的位置信息达到精度要求时进入主滤波器，主滤波器融合后续的水下应答器进行水下AUV紧组合导航，采用紧组合算法得到最终结果，同时反馈校正导航设备内部状态信息，抑制自身导航设备误差的发散增长；当AUV运行区域超出了当前水下应答器的水声作用范围后，AUV再次与后续水下应答器进行水下AUV紧组合导航，并重复执行上述过程；

所述步骤2的利用局部滤波器估计两个后续布放应答器位置信息并使其达到精度要求的具体方法为：

当AUV运行到某作业区域，布放水下应答器，应答器沉入水底经触发后以等间隔时间发出声信息，提供自身与AUV相互间的距离信息；水下应答器布放后，载体进行机动航行，建立地球直角坐标系下AUV到水下应答器绝对位置X(x₀,y₀,z₀)校正的观测方程：

其中：F_i为无任何误差条件下的声学距离；其中，i＝1,2,...,n；

(x₀,y₀,z₀)为地球坐标系下应答器位置X初始值；

(x_i,y_i,z_i)为惯导设备第i次测量点的位置；其中，i＝1,2,...,n；

当水下AUV运行到作业区域或者校正区域时，布放水下应答器，应答器沉入水底经触发后发送声脉冲信号，可为其他载体提供自身精确的深度信息以及相互间的距离信息；设定水下应答器初值设置为AUV投放时的地理位置P₀(L₀,λ₀,h₀)，可根据INS导航系统中主滤波器输出得到，并经地理坐标系转换到地球坐标系为X(x₀,y₀,z₀)；水下应答器布放后，AUV进行机动航行，建立AUV到水下应答器绝对位置校正的观测方程：

式中：i＝1,2,...,n表示测量次数，(x_i,y_i,z_i)表示惯导设备第i次测量点的位置；

将上述公式在应答器初始值X(x₀,y₀,z₀)进行线性化泰勒展开，可得如下公式：

式中：F_i为无任何误差条件下的声学距离；

上述可写成向量形式为：

A·ΔX＝B

式中：ΔX＝[Δx Δy Δz]^T；

B＝[F₁-F₁₀ F₂-F₂₀ … F_n-F_n0]^T

利用最小二乘法求解，可得：

ΔX＝(A^TA)^-1A^TB

将求得的ΔX迭代校正应答器位置X：

X＝X-ΔX

将所得结果作为初值带入F_i的无任何误差条件下的声学距离公式，利用泰勒级数展开法和最小二乘法迭代求解公式，重复上述过程，经数次迭代至精度满足需求，重复性可达到米级。

2.根据权利要求1所述的一种基于稀疏长基线紧组合的AUV水下导航方法，其特征在于：所述步骤1的主滤波器通过与水下应答器a和水下应答器b进行稀疏长基线测距紧组合，对导航设备和声学测距信息误差校正的具体步骤包括：

(1)获得水下应答器a和水下应答器b的位置信息；

地球坐标系中，水下应答器a和水下应答器b的位置分别为(x_a,y_a,z_a)、(x_b,y_b,z_b)，可利用如下所示的地理坐标系转换到地球坐标系公式得到：

其中：

x,y,z为地球坐标系中位置坐标；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

R_E为与子午面垂直的法线平面的曲率半径，

e为椭球偏心率，

r_x,r_y为椭圆长半径和短半径；

(2)对水下应答器a和水下应答器b的位置信息进行解算，由INS推算AUV到应答器a和水下应答器b的计算距离；

当AUV作业距离内有2个位置已知的水下应答器a和水下应答器b时，对2个应答器的位置信息进行解算，由INS推算AUV到2个应答器的计算距离为ρ_I1和ρ_I2：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：分别为2个应答器的位置坐标；

(x_I,y_I,z_I)为INS测量得到的AUV位置坐标；

(3)利用kalman滤波器实现稀疏长基线紧组合算法，对导航设备和声学测距信息误差进行校正；

kalman滤波器中状态变量X选取为姿态误差

速度误差δv、位置误差δp、陀螺漂移ε、加速度计零偏

以及由时钟引起的声学测距误差δρ_S；由声学测量得到AUV到2个应答器的声学测量距离为ρ_S1和ρ_S2，并将INS推算的距离ρ_Ii在(x,y,z)处进行泰勒级数展开，取一次项误差可得到AUV到2个应答器的计算距离与声学测量距离之差的观测方程Z如下所示：

其中：(x_a,y_a,z_a)、(x_b,y_b,z_b)：为2个应答器的位置坐标；

ρ_I1、ρ_I2：为INS推算的AUV到2个应答器的计算距离；

ρ_S1、ρ_S2：声学测量得到AUV到2个应答器的声学测量距离；

地球坐标系与地理坐标系微分转换公式如下所示：

其中：(δx,δy,δz)为地球坐标系位置误差；

(δL,δλ,δh)为地理坐标系位置误差；

L,λ,h为地理坐标系中大地纬度，大地经度和大地高度；

e,R为椭球偏心率和地球半径。

Images