[發明專利]一種基于超短基線和航位推算的水下導航定位方法

有效

• 申請號: 201910836803.3
• 申請日: 2019-09-05
• 公開（公告）號: CN110568407B
• 公開（公告）日: 2023-06-27
• 發明（設計）人: 馬杰;張煜;亓強強;彭智浩;李文楷
• 申請（專利權）人: 武漢理工大學
• 主分類號: G01S5/22
• 分類號: G01S5/22;G01S15/74;H03H17/02
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 張惠玲

1.一種基于超短基線和航位推算的水下導航定位方法，其特征在于，包括以下步驟：

S1、確定超短基線系統的定位原理；

S2、確定超短基線系統的誤差模型；

S3、建立狀態模型和觀測模型；

S4、建立考慮方向角量測偏差變化的擴展卡爾曼濾波算法；

在所述步驟S4中，建立考慮方向角量測偏差變化的卡爾曼濾波算法，包括以下步驟：

S4-1、根據上一時刻后驗估計位置以及其后驗協方差，通過狀態模型確定此時刻水下目標的先驗估計位置以及其先驗協方差；

S4-2、根據上一時刻的后驗估計位置，通過超短基線系統的誤差模型確定此時刻的觀測噪聲矩陣；

S4-3、根據此時刻觀測噪聲矩陣，確定增益；

S4-4、根據量測方程確定新息；

S4-5、根據增益及新息確定此時刻水下目標的后驗估計位置以及其后驗協方差；

S4-6、重復上述步驟，獲得水下航行器的實時位置；

在所述步驟S1中，換能器O布置在船體空間坐標系O-XYZ的原點位置，水聽器S1、S2、S3、S4在船體空間坐標系XOY平面中心對稱布置，其中，S1、S3陣元位于指向船艏的X軸上，S2、S4陣元位于垂直于船艏的Y軸上；陣元間距為d，即S1與S3間距、S2與S4間距；搭載聲學應答器的水下目標位于P點，其在船體空間坐標系下坐標為(x,y,z)；超短基線水聲定位系統利用水聽器接收信號之間的時延差T和相位差Δφ_x,Δφ_y解算水下目標的距離r：

r＝CT/2??????????????????????????(1)

和方位：

θ_x＝arcos(λΔφ_x/2πd),θ_y＝arcos(λΔφ_y/2πd)??????????????(2)

再根據公式(1)和公式(2)解算水下目標位置：

其中，公式(1)～公式(3)中，C為水下聲速，f為換能器收發頻率；

在所述步驟S2中，對水下目標位置偏微分并求標準差：

忽略聲波長誤差σ_λ、基陣安裝誤差σ_d，測向誤差依據克拉美羅下界取最優，簡化后的方向角θ_x,θ_y偏差模型為

在所述步驟S3中，規定水下目標的運動狀態由對地航向、對地航速和升沉速度三個參數決定：

公式(6)中，x(k),y(k),z(k)為k時刻水下目標相對換能器位置，COG(k)(Course?OfGround),SOG(k)(Speed?Of?Ground),Δz(k)分別為k時刻水下目標的對地航向、對地航速以及升沉速度，ε_COG(k),ε_SOG(k),ε_Δz(k)分別為k時刻水下目標的對地航向控制偏差、對地速度控制偏差以及升沉速度控制偏差，Δt為k時刻與k+1時刻的時間間隔；k+1時刻水下目標的狀態方程為：

超短基線系統對水下目標的位置進行觀測，

公式(8)～(9)中，r(k)、θ_x(k)、θ_y(k)分別為k時刻超短基線系統的觀測斜距、x方向上的方向角、y方向的方向角；δ_r(k)、分別為k時刻的觀測斜距偏差、x方向上的方向角偏差、y方向的方向角偏差，(a,b,c)為換能器的絕對坐標，則量測方程為：

公式(10)中，r(k)、θ_x(k)、θ_y(k)分別為k時刻超短基線系統的觀測斜距、x方向上的方向角、y方向的方向角、深度計測量值；δ_r(k)、分別為k時刻的觀測斜距偏差、x方向上的方向角偏差、y方向的方向角偏差；

在所述步驟4中，建立考慮方向角量測偏差變化的擴展卡爾曼濾波算法：

F(k+1)為x(k+1)＝g(x(k),u(k),ε(k))關于k+1時刻的水下目標位置的雅可比矩陣，其表達式為：

Q(k)為k時刻狀態向量的噪聲矩陣，將過程噪聲看作定值：

H(k+1)為觀測方程z(k)＝h(x(k),δ(k))相對于水下目標位置坐標的雅可比矩陣：

l,l₁,l₂為構造輔助變量：

R(k+1)為k+1時刻超短基線系統觀測向量的噪聲矩陣；設定各觀測向量相互獨立，且觀測斜距偏差為定值，將觀測噪聲矩陣分解為以下形式：

公示(16)中，Σ(k+1)為觀測噪聲調節因子，反映水下目標相對聲陣位置變化對觀測偏差的影響；R為系統最小觀測偏差；

物體運動的連續性使得相鄰兩時刻的θ_x與θ_y相差不大，用上一時刻的濾波值作為此時刻的真值：

根據下式取得：

濾波增益K修正為：

擴展卡爾曼濾波框架調整為：