[發(fā)明專利]一種大潛深A(yù)UV下潛定位方法

有效

• 申請?zhí)枺?/td>: 201910365822.2
• 申請日: 2019-05-05
• 公開（公告）號: CN110057365B
• 公開（公告）日: 2022-06-21
• 發(fā)明（設(shè)計）人: 張強(qiáng);李曄;張雯;曹建;李岳明;姜言清;廖煜雷;李宏偉
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種大潛深A(yù)UV下潛定位方法，其特征在于，包括：

步驟一：通過大潛深A(yù)UV搭載水下通信節(jié)點、捷聯(lián)慣導(dǎo)系統(tǒng)以及溫鹽深傳感器，得到大潛深A(yù)UV的基本信息；

步驟二：構(gòu)建大潛深A(yù)UV的純距離誤差估計濾波模型；

1)設(shè)短時間內(nèi)捷聯(lián)慣導(dǎo)系統(tǒng)的位置誤差和速度誤差均為常值；

2)建立大潛深A(yù)UV的純距離誤差估計濾波模型的離散系統(tǒng)方程為：

X_k＝Φ_kX_k-1+w_k

其中，

上式中，分別為大潛深A(yù)UV在NED導(dǎo)航坐標(biāo)系下慣導(dǎo)系統(tǒng)北向和東向的位置誤差，分別為大潛深A(yù)UV在載體坐標(biāo)系下縱向和橫向測速誤差，T為采樣時間，w_k～N(0,Q)，Q為w_k的4×4協(xié)方差矩陣，ψ、θ、γ分別為大潛深A(yù)UV的艏向、縱傾角和橫傾角；

3)將母船同大潛深A(yù)UV的測距信息r_k和大潛深A(yù)UV溫鹽深傳感器輸出的深度信息d_k作為量測信息，設(shè)觀測噪聲為加性白噪聲，建立大潛深A(yù)UV純距離誤差估計濾波模型的觀測方程為：

z_k＝h(X_k)+v_k

其中，

上式中，為NED導(dǎo)航坐標(biāo)系下大潛深A(yù)UV捷聯(lián)慣導(dǎo)系統(tǒng)北向和東向定位信息，為NED導(dǎo)航坐標(biāo)系下水面母船北向和東向定位信息，v_k～N(0,R)，R為v_k的1×1協(xié)方差；

步驟三：根據(jù)同步水聲通信協(xié)議，水面母船向大潛深A(yù)UV發(fā)送水面母船各時刻在NED坐標(biāo)系下的水平位置信息以及相應(yīng)的時間信息；

步驟四：根據(jù)大潛深A(yù)UV水聲通信系統(tǒng)接收到的步驟三所述時間信息，大潛深A(yù)UV記錄此時的捷聯(lián)慣導(dǎo)系統(tǒng)定位信息以及此時的溫鹽深傳感器輸出的溫度、導(dǎo)電率和深度信息；

步驟五：根據(jù)同步水聲通信協(xié)議，通過傳輸時間和溫鹽深傳感器信息，大潛深A(yù)UV計算得到自身到母船的測距信息；

步驟六：構(gòu)建強(qiáng)跟蹤UKF算法模型；

1)設(shè)強(qiáng)跟蹤UKF的初始狀態(tài)為：

上式中，0_4×1、0_4×4分別為4維零矢量和4維全零方陣；

2)時間更新：為狀態(tài)的估計值，不考慮系統(tǒng)協(xié)方差矩陣Q，則濾波狀態(tài)協(xié)方差矩陣的時間更新為：

3)根據(jù)水面母船水平定位信息以及大潛深A(yù)UV捷聯(lián)慣導(dǎo)系統(tǒng)定位信息，計算觀測方程參數(shù)和

4)根據(jù)觀測方程參數(shù)和基于濾波狀態(tài)的時間更新通過確定性觀測模型z_k＝h(X_k)預(yù)測量測信息為：

上式中，為狀態(tài)矢量的前兩項；

5)根據(jù)母船和大潛深A(yù)UV的測距信息r和大潛深A(yù)UV溫鹽深傳感器輸出的深度信息d，得到強(qiáng)跟蹤UKF的觀測信息z_k為：

6)計算量測信息殘差為：

7)計算殘差協(xié)方差矩陣為：

上式中，ρ為遺忘因子，且0ρ≤1，ρ＝0.95；

8)基于UT變換計算系統(tǒng)協(xié)方差矩陣Q通過確定性觀測模型h的傳播Q_zz；根據(jù)和Q，求取Sigma點集{ζ_i}，(i＝1,…，L)；計算Sigma點{ζ_i}通過確定性觀測模型h的傳播ξ_i＝h(ζ_i)，最后計算Qzz為：

上式中，為求二階統(tǒng)計特性時的權(quán)系數(shù)；

9)計算殘差中非正交噪聲矢量協(xié)方差為：

N_k＝V_k-R-Q_zz；

10)基于UT變換計算k-1時刻狀態(tài)協(xié)方差矩陣P_k,k-1的時間更新通過確定性觀測模型h的傳播P_zk,k-1；根據(jù)和P_k,k-1求取Sigma點集{χ_i}，(i＝1,…，L)；計算Sigma點{χ_i}通過確定性觀測模型h的傳播δ_i＝h(χ_i)；根據(jù)計算狀態(tài)協(xié)方差矩陣P_k,k-1的時間更新通過確定性觀測模型h的傳播P_zk,k-1；

上式中，為求二階統(tǒng)計特性時的權(quán)系數(shù)；

11)計算漸消因子矩陣Λ_k；令M_k＝P_zk,k-1，計算基于計算漸消因子矩陣元素λ_i(i＝1,…，4)，構(gòu)建漸消因子矩陣Λ_k＝diag(λ₁,λ₂,λ₃,λ₄)；

12)狀態(tài)協(xié)方差矩陣的強(qiáng)跟蹤時間更新為：

13)以和為觀測方程參數(shù)，基于UT變換計算量測信息預(yù)測值；根據(jù)和P_k,k-1求取Sigma點集{ζ_i}，(i＝1,…，L)；計算Sigma點{ζ_i}通過確定性觀測模型h的傳播ξ_i＝h(ζ_i)；根據(jù)

計算和

上式中，和分別為求一、二階統(tǒng)計特性時的權(quán)系數(shù)；

14)計算濾波增益為：

15)濾波狀態(tài)量測更新為：

16)濾波狀態(tài)協(xié)方差矩陣量測更新為：

步驟七：根據(jù)強(qiáng)跟蹤UKF算法，融合水面母船水平定位信息、大潛深A(yù)UV同母船的測距信息和大潛深A(yù)UV捷聯(lián)慣導(dǎo)系統(tǒng)定位信息、大潛深A(yù)UV深度信息，跟蹤深潛過程中大潛深A(yù)UV在NED坐標(biāo)系下的北向和東向水平定位誤差以及載體坐標(biāo)系下AUV的縱向和橫向測速誤差，對捷聯(lián)慣導(dǎo)系統(tǒng)輸出進(jìn)行校正，得到精確的大潛深A(yù)UV下潛定位信息。