[發明專利]一種可估計未知有效聲速的自適應水下單信標定位方法

有效

• 申請號: 201910999922.0
• 申請日: 2019-10-21
• 公開（公告）號: CN110749891B
• 公開（公告）日: 2021-08-24
• 發明（設計）人: 朱仲本;余相;秦洪德;鄧忠超;萬磊;田瑞菊
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01S15/60
• 分類號: G01S15/60;G01C21/20;G01C21/16
• 代理公司: 北京興智翔達知識產權代理有限公司 11768
• 代理人: 張玉梅

1.一種可估計未知有效聲速的自適應水下單信標定位方法，其特征在于，包括以下步驟：

A.以定位區域內任意點為原點，東、北、天三個方向分別設為x，y，z軸，建立水下局部慣性坐標系；

B.通過水下航行器所搭載的GPS系統獲取該所述水下航行器在水下局部慣性系當中的初始位置；

C.建立所述水下航行器的運動學模型以及觀測模型并進行離散化；

所述運動學模型的建立方法為：

定義狀態向量為：

x＝[x y v_cx v_cy]^T

其中：x，y為所述水下航行器在所述水下局部慣性坐標系中的水平位置；v_cx，v_cy為未知的海流速度；

對x求導并加入所述水下航行器運動模型噪聲影響，得到所述水下航行器的運動學模型：

其中：v_wx為所述水下航行器x方向的對水速度；v_wy為所述水下航行器y方向的對水速度；v_wx及v_wy通過讀取螺旋槳轉速與電子羅盤測得的航行器艏向角計算得出；ω_x為所述水下航行器在x方向的位置不確定性；ω_y為所述水下航行器在y方向的位置不確定性；ω_cx為x方向的海流不確定性；ω_cy為y方向的海流不確定性；

v_wx及v_wy的計算公式為：式中v_w為根據所述螺旋槳轉速得出的所述水下航行器對水速度，為所述電子羅盤測得的艏向角；

所述觀測模型的建立方法為：

S1.建立水聲信號傳遞時間的觀測模型；

設所述水下航行器獲得水聲信標發射水聲信號的時刻為T_e，所述水聲信標在所述水下局部慣性坐標系中的空間位置坐標為X_Te，Y_Te，Z_Te，所述水下航行器接收到該水聲信號的時刻為T_a，觀測方程為：

其中：ν_t為對應的觀測噪聲；z為所述水下航行器的深度，由深度計精確測得，為已知量；v_e為有效聲速；

將觀測方程記作m＝h(x,v_e)，其中：

S2.建立海流流速觀測模型；

根據多普勒測速儀測得的所述水下航行器的絕對速度v_g，結合所述電子羅盤測得的艏向角計算得到所述水下航行器絕對速度在局部慣性坐標系下的分量v_gx,v_gy；

根據v_gx,v_gy以及v_wx,v_wy，計算得到海流速度觀測分量為：

海流觀測量為線性，滿足m_vc＝Hx+ν_vc；

其中：觀測向量m_vc＝[m_cx m_cy]^T；m_cx，m_cy分別為x，y方向海流速度觀測；ν_vc為海流觀測噪聲向量，ν_vc＝[ν_v,cx ν_v,cy]^T，其中ν_v,cx為x方向的海流不確定性；ν_v,cy為y方向的海流不確定性；H為海流觀測矩陣，滿足：

所述運動學模型以及所述觀測模型離散化方法為：

S1.運動學模型離散化；

以下標k為時間索引，以Δt＝t_k+1-t_k為離散間隔，運動學模型離散為：

x_k+1＝A_kx_k+B_ku_k+w_k

其中：A_k為運動學方程，滿足：

B_k為控制方程，滿足：

u_k為控制向量，滿足u_k＝[v_wx,k v_wy,k]^T，為已知量；

w_k過程噪聲向量，滿足w_k＝[ω_x,k ω_y,k ω_cx,k ω_cy,k]^T，對應各個狀態變量的不確定性；將系統狀態x_k,y_k,v_cx,k,v_cy,k的過程噪聲建模為零均值Gauss分布，其協方差矩陣滿足：

其中，σ_w為所述水下航行器對水速度觀測不確定性的標準差；σ_c為海流不確定性的標準差；

S2.觀測模型離散化；

所述水下航行器在k-1至k間接收到該水聲信號，將其假設為在k時刻接收到該水聲信號，即離散后的水聲信號傳遞時間觀測方程為：

其中，ν_t,k為觀測噪聲，假設其滿足方差為R_t,k的Gauss分布；考慮到有效聲速v_e,k的時變未知性，將v_e,k也看作隨機變量；離散形式的觀測方程寫作：

m_k＝h_k(x_k,v_e,k)，

由于海流觀測采樣頻率較高，假設在每一個離散時間點k處均可以得到海流觀測，故離散后的海流觀測方程為：

m_vc，k＝H_kx_k+ν_vc,k

其中，H_k為k時刻海流速度觀測矩陣，滿足：

ν_vc,k為k時刻海流觀測噪聲，為零均值Gauss分布，其觀測噪聲協方差矩陣滿足：

其中，σ_vc,m為海流速度觀測噪聲的標準差；

D.建立有效聲速及其不確定性統計參數的隨機模型以及函數模型；

將有效聲速初始先驗分布建模為Gauss分布：

其中：N(x|μ,Σ)表示以μ為均值，Σ為協方差矩陣，滿足Gauss分布的隨機向量x；為初始聲速估計均值；P_e,0為初始聲速不確定性方差；

將k-1時刻有效聲速的后驗分布也建模為Gauss分布：

P_e,k-1|k-1分別為k-1時刻有效聲速的后驗均值與方差；

有效聲速的動態擴散的函數模型記作：

v_e,k＝v_e,k-1+ω_e,k-1

其中：ω_e,k-1為有效聲速的不確定性，將其建模為均值及方差均未知的Gauss分布：

其中：μ_k-1與σ_e,k-1分別為聲速不確定性的均值與標準差；

據此，可以將有效聲速的概率擴散方程記作：

結合上式，獲得有效聲速的預測模型為：

其中P_e,k|k-1分別為k時刻有效聲速的先驗均值與方差，其計算公式為：

由于真實的有效聲速的不確定性均值和方差未知，以其名義值與進行聲速預測，預測方程為：

其中：與表示有效聲速先驗統計參數，即均值與方差的名義值；

通過在線估計P_e,k|k-1來補償σ_e,k-1設置誤差的影響；將μ_k-1與P_e,k|k-1的先驗分布建模為其共軛先驗，即Gauss-inverse Gama，簡寫為GIG分布：

其中：表示μ_k-1與P_e,k|k-1的先驗統計參數，GIG(a,A|τ,α,λ,ν)表示以τ,α,λ,ν為參數的GIG分布，可以被分解為：

GIG(a,A|τ,α,λ,ν)＝N(a|τ,αA)IG(A|λ,ν)

其中：IG(A|λ,ν)表示以λ,ν為參數的逆Gama分布；

μ_k-1與P_e,k|k-1的先驗分布可以被分解為：

μ_k-1與P_e,k|k-1的先驗估計需要與其名義值匹配，即μ_k-1與P_e,k|k-1的先驗統計參數滿足：

設計μ_k-1與P_e,k|k-1統計參數預測方程為：

其中：ρ_α與ρ_λ為調制參數；

E.水聲信標周期性廣播水聲信號，水聲信號發射時間及水聲信標位置已知；所述水下航行器在未接收到水聲信號時，通過自身配備的電子羅盤、深度計以及讀取自身的螺旋槳轉速信息進行航位推算，同時進行有效聲速隨機模型參數預測；所述水下航行器在接收到所搭載的多普勒測速儀測得的絕對速度觀測后，通過讀取所述螺旋槳轉速信息及電子羅盤信息，構造海流速度觀測量并通過Kalman濾波進行海流速度校正；

所述水下航行器利用自身配備的電子羅盤、深度計以及讀取自身的螺旋槳轉速信息進行航位推算的方法為：

根據Kalman濾波的預測環節，將系統狀態的先驗分布近似為Gauss分布：

其中：與P_k|k-1分別為k時刻的先驗狀態和先驗方差，其計算方法為：

其中：與P_k-1|k-1分別為k-1時刻的后驗狀態和后驗方差；

所述水下航行器接收到所述多普勒測速儀測得的絕對速度觀測后，進行海流速度校正的方法為：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

其中：K_k為海流更新Kalman增益；

F.所述水下航行器接收到水聲信號后，記錄接收時刻，根據已知的水聲信號發射時刻及水聲信標位置坐標，并考慮水下聲速及其不確定性統計參數的未知性，以此基于擴展Kalman濾波算法及變分貝葉斯近似，以水聲信號傳遞時間為觀測變量，進行所述水下航行器的位置更新；

S1.建立觀測似然函數、有效聲速后驗分布及其不確定性統計參數后驗概率密度分布模型；

根據離散形式的水聲信號傳遞時間觀測模型，得到水聲信號傳遞時間的觀測似然密度為：

p(m_k|v_e,k,x_k)＝N(m_k|h_k(x_k,v_e,k),R_k)

將有效聲速后驗分布建模為Gauss分布，即：

μ_k-1與P_e,k|k-1的后驗分布同樣建模為GIG分布，分解為：

S2.定義狀態變量、有效聲速及聲速不確定性統計參數后驗估計近似值；

通過變分貝葉斯近似，將狀態變量、有效聲速及聲速不確定性統計參數的聯合后驗估計近似為：

p(x_k,v_e,k,μ_k-1,P_e,k|k-1|m_1:k)≈q(x_k)q(v_e,k)q(μ_k-1)q(P_e,k|k-1)

以最小化近似前與近似后兩個概率密度函數之間的Kullback-Leibler散度(KLD)最小化為目標，得到近似解為：

其中：log(·)表示對數運算；θ表示x_k,v_e,k,μ_k-1,P_e,k|k-1中的任意元素；E_x[·]表示相對于x的期望；上標(-θ)表示整個集合當中除了θ以外的其它元素；c_θ表示與θ無關的常數；采用固定點迭代來求解q(θ)；

S3.求解狀態變量、有效聲速及聲速不確定性統計參數后驗估計近似值；

S3.1求解聯合概率密度對數值；

其對數形式表示為：

S3.2求解聲速不確定性統計參數P_e,k|k-1估計值；

令θ＝P_e,k|k-1，得到：

其中：

變量加上標(i)表示該變量在第i次迭代中的估計值；進而得到：

取P_e,k|k-1在第i+1次迭代中的估計值為：

S3.3求解聲速不確定性統計參數μ_k-1估計值；

令θ＝μ_k-1，得到：

進而得到：

取μ_k-1在第i+1次迭代中的估計值為其期望值，即：

S3.4求解有效聲速v_e,k估計值；

記選擇為展開點，對非線性的水聲信號傳遞時間觀測方程m_k＝h_k(x_k,v_e,k)進行線性化，只保留一階項，得到：

其中：

為觀測方程的雅克比矩陣；

由此得到線性化后的水聲信號傳遞時間觀測方程為：

令θ＝v_e,k，得到：

其中：

根據Kalman濾波更新式，得到有效聲速更新方程為：

其中：為第i+1次迭代中有效聲速更新的Kalman增益；

S3.5求解系統狀態x_k估計值；

以為展開點，對非線性的水聲信號傳遞時間觀測方程m_k＝h_k(x_k,v_e,k)進行線性化，只保留一階項，得到：

其中：

為觀測方程的雅克比矩陣；

由此得到線性化后的水聲信號傳遞時間觀測方程為：

令θ＝x_k，得到：

其中：

根據Kalman濾波更新式，得到系統狀態更新方程為：

其中：為第i+1次迭代中系統狀態更新的Kalman增益；

記總迭代次數為N，則最終有效聲速均值及方差、系統狀態均值及協方差矩陣的估計值分別為：

參數需要用于k+1時刻的預測過程，記其估計值為