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

在審

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

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

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

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

步驟2：記錄USBL接收到應答器報文時刻的方位角，SINS姿態、速度和位置信息；

在應答器報文接收時刻t₂,應答器發出的信號到基陣原點的聲線與x軸和y軸的夾角分別為α和β，SINS姿態轉移矩陣為速度更新結果為表示載體東向、北向、天向速度,位置更新結果為

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

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

3.1在t₂時刻，應答器在n系下的位置為USBL發送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根據上式中的遞推結果，計算的斜距：

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

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

其中，表示SINS與USBL之間的安裝誤差角，表示t₂時刻的姿態轉移矩陣，表示n系到e系下的轉移矩陣，表示SINS與USBL之間的桿臂誤差；

3.4SINS計算的姿態轉移矩陣為則將t₂時刻計算的斜距遞推至t₁時刻：

3.5根據步驟3.4所計算的斜距，則t₂時刻的斜距修正為：

步驟4：根據步驟3修正的USBL斜距和步驟2采集的方位角、高度角和SINS解算信息，建立SINS/USBL緊組合系統模型；

在t₂時刻，建立系統模型，步驟如下：

4.1系統狀態模型為：

其中，F(k)為狀態轉移矩陣，W(k)為系統噪聲，狀態向量X(k)為：

其中，

X_USBL＝[θ_x θ_y θ_z δα δβ δD]^T；

其中，δθ＝[θ_x θ_y θ_z]^T表示安裝角誤差，[δα δβ]^T表示、測角誤差，δD表示測距誤差，

φⁿ＝[φ_x φ_y φ_z]^T表示載體俯仰角、橫滾角、航向角誤差；δV＝[δV_E δV_N δV_N]^T表示載體東向、北向、天向速度誤差；δP＝[δL δλ δh]^T表示載體地理經度、緯度、高度誤差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；

其中，F_USBL＝0_6×6，

其中，

F₂₂＝(Vⁿ×)F₁₂-(a×)

其中，ω_ie表示地球自轉角速率。

4.2系統量測模型為：

Z(k)＝HX(k)+V

其中，Z表示系統量測值，H表示量測方程轉移矩陣，V表示量測噪聲；

根據步驟4.3修正后的斜距以及t₂時刻測量的方位角，USBL量測信息表示為：

利用SINS位置計算得到應答器在a系下的相對位置矢量為則應答器的相對位置矢量和方位角、斜距之間的關系表達式為：

對上式求偏微分得：

其中，矢量為表述方便，記

則矩陣A_TS的表達式為：

根據SINS的位置計算的應答器在a系下的相對位置矢量為：

忽略高階誤差小量，則位置矢量誤差為：

其中，

其中，C_λ＝cosλ(t₂)，

則利用SINS位置及應答器位置計算得到的a系下的斜距方位角信息重新表述為：

其中，[α β R]^T為方位角及斜距的真值，

矩陣分別為

其中，

則系統量測為：

其中，H_u＝[0 0 -1]，δU＝[δα δβ δD]^T；

觀測矩陣H的表達式為：

H＝[H_a 0_3×3 H_p0_3×6 H_I H_u]；

步驟5：計算動力學模型誤差自適應因子和根據最大熵原則計算增益矩陣；

步驟6：對步驟5的模型進行卡爾曼濾波融合，對SINS進行反饋校正，輸出導航結果，重復上述步驟，直至導航結束。