[發明專利]一種基于牛頓迭代法的DVL誤差標定方法及系統

有效

• 申請號: 202210089031.3
• 申請日: 2022-01-25
• 公開（公告）號: CN114485730B
• 公開（公告）日: 2023-07-04
• 發明（設計）人: 秦洪懋;岳昊;王廣才;崔慶佳;秦曉輝;胡滿江;秦兆博;王曉偉;謝國濤;楊澤宇;徐彪;邊有鋼;丁榮軍
• 申請（專利權）人: 湖南大學;湖南大學無錫智能控制研究院
• 主分類號: G01C25/00
• 分類號: G01C25/00;G06F17/16;G06F17/10
• 代理公司: 北京匯智勝知識產權代理事務所(普通合伙) 11346
• 代理人: 趙立軍

1.一種DVL誤差優化方法，其特征在于，包括：

步驟1，構建DVL誤差損失函數f(Φ，δ)；

步驟2，通過牛頓迭代法逼近損失極小值點獲得Φ和δ的最優估計值，其具體包括：

步驟21，令X＝[Φ，δ]，Φ＝[φ₁，φ₂，φ₃]^T，φ₁表示SINS與DVL之間的安裝方位失準角，φ₂表示SINS與DVL之間的安裝俯仰失準角，φ₃表示SINS與DVL之間的安裝滾轉失準角，δ為DVL刻度因子誤差，將非線性優化函數f(X)進行在初始點二階泰勒展開，δ⁰分別為φ₁、φ₂、φ₃、δ的初始值；

步驟22，根據所述步驟21二階泰勒展開后的結果，確定迭代方程(3)，其中為A(X)逆矩陣：

其中，為f(X)梯度，表示為式(4)；A和A(X)均為海森矩陣，表示為式(6)：

式中，為偏導數；

步驟23，給定初始值通過i次迭代，求出每次迭代中不同的海森矩陣A(X)及其逆矩陣，從而得到Xⁱ，如果Xⁱ小于預設值，或者迭代次數達到設定的最大值，則Xⁱ作為最優解，停止迭代；否則返回方程(3)繼續迭代運行；

所述f(Φ，δ)具體包括被描述為式(2)：

式中，X_GPS，i、Y_GPS，i分別表示第i次航位推算時GPS數據在導航坐標系下的X和Y坐標值，X_DR，i、Y_DR，i分別表示第i次航位推算獲得的航位推算位置數據在導航坐標系下的X和Y坐標值，i＝1，2，3…N，||||表示范數。

2.如權利要求1所述的DVL誤差優化方法，其特征在于，所述步驟1具體包括：

步驟11，通過航位推算公式(1)，計算每一個航位推算位置數據：

式中，Z_DR，i表示第i次航位推算獲得的所述航位推算位置數據在導航坐標系下的Z坐標值，X_DR，i-1、Y_DR，i-1、Z_DR，i-1分別表示第i-1次航位推算獲得的所述航位推算位置數據在導航坐標系下的X、Y和Z坐標值，表示DVL速度，V_x、V_y、V_z分別為DVL速度沿x、y、z軸的分量，為載體坐標系到導航坐標系的轉換矩陣，表示SINS和DVL之間安裝誤差轉換矩陣，I為單位矩陣，Φ×表示Φ的反對稱矩陣，V_x、V_y、V_z分別表示第i次航位推算時DVL采集到的在DVL坐標系三個方向的速度值，Δt為DVL的數據更新時間；

步驟12，將GPS數據從經緯度轉換到導航坐標系下，得到[X_GPS，i，Y_GPS，i]^T；

步驟13，獲取f(Φ，δ)。

3.一種基于牛頓迭代法的DVL誤差標定方法，其特征在于，包括：

采用如權利要求1-2中任一項所述的DVL誤差優化方法獲得Φ和δ的最優估計值；

根據Φ和δ的最優估計值，完成標定過程。