[发明专利]一种基于稀疏长基线紧组合的AUV水下导航方法

权利要求书

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为椭球偏心率和地球半径。