[發(fā)明專利]一種基于最大熵自適應抗差估計的USBL斜距修正方法

在審

• 申請?zhí)枺?/td>: 202111268907.2
• 申請日: 2021-10-29
• 公開（公告）號: CN114279443A
• 公開（公告）日: 2022-04-05
• 發(fā)明（設計）人: 張濤;劉射德;張亮;夏茂棟
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01S5/18
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 呂書桁

一種基于最大熵自適應抗差估計的USBL斜距修正方法，1)記錄USBL發(fā)送請求報文時刻的SINS姿態(tài)轉移矩陣、速度和位置信息，2)記錄USBL接收到應答器報文時刻的方位角，SINS姿態(tài)、速度和位置信息，3)根據(jù)發(fā)送時刻和接收時刻SINS的解算結果修正USBL的斜距，4)根據(jù)3)修正的USBL斜距和2)采集的方位角、高度角和SINS解算信息，建立SINS/USBL緊組合系統(tǒng)模型，5)計算動力學模型誤差自適應因子和根據(jù)最大熵原則計算增益矩陣，6)對5)的模型進行卡爾曼濾波融合，對SINS進行反饋校正，輸出導航結果。重復上述步驟，直至導航結束，利用捷聯(lián)慣導系統(tǒng)測量的姿態(tài)和位置建立USBL發(fā)射和接收時刻之間的變化模型，推導接收時刻的斜距修正值。最后，為了抑制異常噪聲對定位精度的影響。

技術領域

本發(fā)明涉及水下航行器SINS/USBL組合導航技術，具體為一種基于最大熵自適應抗差估計的USBL斜距修正方法。

背景技術

SINS/USBL組合導航系統(tǒng)是實現(xiàn)水下AUV高精度導航定位的主要途徑之一。由于USBL更新頻率低，當報文發(fā)送和接收之間存在徑向速度，傳統(tǒng)的斜距計算方法將引入較大誤差。并且，由于水下環(huán)境的復雜性和水聲多徑等因素的干擾，USBL的測量通常會包含較大的異常噪聲。

發(fā)明內(nèi)容

為解決現(xiàn)有技術中存在的問題，本發(fā)明提出了一種基于最大熵自適應抗差估計的USBL斜距修正方法，利用捷聯(lián)慣導系統(tǒng)(SINS)測量的姿態(tài)和位置建立USBL發(fā)射和接收時刻之間的變化模型，推導接收時刻的斜距修正值。最后，為了抑制異常噪聲對定位精度的影響。

一種基于最大熵自適應抗差估計的USBL斜距修正方法，包括：

步驟1：記錄USBL發(fā)送請求報文時刻的SINS姿態(tài)轉移矩陣、速度和位置信息；

在USBL發(fā)送請求報文時刻t₁，SINS姿態(tài)轉移矩陣為速度更新結果為位置更新結果為表示慣導解算地理經(jīng)度值，表示慣導解算地理緯度值，表示慣導解算高度值；

步驟2：記錄USBL接收到應答器報文時刻的方位角，SINS姿態(tài)、速度和位置信息；在應答器報文接收時刻t₂,應答器發(fā)出的信號到基陣原點的聲線與x軸和y軸的夾角分別為α和β，SINS姿態(tài)轉移矩陣為速度更新結果為表示載體東向、北向、天向速度,位置更新結果為

步驟3：根據(jù)發(fā)送時刻和接收時刻SINS的解算結果修正USBL的斜距；

對USBL斜距進行修正，包括如下步驟：

3.1在t₂時刻，應答器在n系下的位置為USBL發(fā)送t₁時刻與接收時刻t₂之間，SINS更新了m個周期，則將t₂時刻慣性解算位置遞推至t₁時刻

其中，M_PV(m)表示第m個采樣周期時的SINS位置更新矩陣，

表示第m個采樣周期慣性解算的速度信息，T表示SINS采樣間隔；R_Nh＝R_N+h(m)，R_Mh＝R_M+h(m),R_e表示地球長半軸半徑，e表示地球偏心率，h(m)表示第m個采樣周期慣導解算的高度值，L(m)表示第m個采樣周期慣導解算的緯度值。

3.2根據(jù)上式中的遞推結果，計算的斜距：

3.3在t₁時刻，根據(jù)組合的高精度位置，計算發(fā)送時刻的斜距：

則應答器在α系下的位置表示為：