[發明專利]一種基于稀疏長基線緊組合的AUV水下導航方法

有效

• 申請號: 201711395940.5
• 申請日: 2017-12-21
• 公開（公告）號: CN107966145B
• 公開（公告）日: 2020-12-15
• 發明（設計）人: 王彬;梁瑾;王彥國;汪湛清;宮京
• 申請（專利權）人: 中國船舶重工集團公司第七0七研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S5/22
• 代理公司: 天津盛理知識產權代理有限公司 12209
• 代理人: 王雨晴

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為橢球偏心率和地球半徑。