[發明專利]水下航行器及其路徑跟蹤控制方法和裝置

在審

• 申請號: 202310233735.8
• 申請日: 2023-03-03
• 公開（公告）號: CN116300982A
• 公開（公告）日: 2023-06-23
• 發明（設計）人: 趙宏亮;張木;張育瑋;薛任宇欣;孔凡忠;王銳;高宏宇;黃磊;劉鵬;柯南極
• 申請（專利權）人: 新興際華（北京）智能裝備技術研究院有限公司
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 北京路浩知識產權代理有限公司 11002
• 代理人: 龔利波

1.一種水下航行器的路徑跟蹤控制方法，其特征在于，所述方法包括：

獲取用于監測待測物體的期望路徑；

基于所述期望路徑和所述水下航行器的運行參數，確定路徑跟蹤誤差；

預估運動學不確定項；

基于所述路徑跟蹤誤差和所述運動學不確定項，對所述水下航行器進行路徑跟蹤控制；

其中，所述運動學不確定項是所述水下航行器運行狀態下由非零時變橫滾角、攻角和側滑角所引起的非線性干擾。

2.根據權利要求1所述的水下航行器的路徑跟蹤控制方法，其特征在于，所述期望路徑是以所述待測物體的軸線為鉛垂線、以1/R為曲率、以L為螺距且以μ為曲線參數的螺旋線；

其中，所述μ是與虛擬導向控制律和時間有關的參量，所述R為所述期望路徑的半徑。

3.根據權利要求2所述的水下航行器的路徑跟蹤控制方法，其特征在于，所述期望路徑上的任意點p_t的表達式如下所示：

所述μ的表達式如下所示：

其中，p_t為所述水下航行器在航行t時間時的虛擬目標點，x_H、y_H、z_H分別為p_H的橫坐標、縱坐標和豎坐標，t為時間，為虛擬導向控制律。

4.根據權利要求1所述的水下航行器的路徑跟蹤控制方法，其特征在于，所述運行參數包括但不限于：姿態角和重心位置坐標；

所述基于所述期望路徑和所述水下航行器的運行參數，確定路徑跟蹤誤差，包括：

在所述期望路徑上定位所述水下航行器的虛擬目標點；

確定所述虛擬目標點的姿態角以及位置坐標；

根據所述水下航行器的重心位置坐標和所述虛擬目標點的位置坐標，確定期望路徑坐標系下的位置跟蹤誤差；

根據所述水下航行器的姿態角和所述虛擬目標點的姿態角，確定期望路徑坐標系下的姿態跟蹤誤差；

將期望路徑坐標系下的所述位置跟蹤誤差和所述姿態跟蹤誤差整體作為所述路徑跟蹤誤差。

5.根據權利要求1所述的水下航行器的路徑跟蹤控制方法，其特征在于，所述預估運動學不確定項，包括：

利用干擾觀測器預估所述運動學不確定項；

其中，所述干擾觀測器的表達式如下所示：

上式中，為D_χ的估計值，為D_υ的估計值，ω₁為所述干擾觀測器的第一帶寬，ω₂為所述干擾觀測器的第二帶寬，ξ₁為所述干擾觀測器的第一輔助變量，ξ₂為所述干擾觀測器的第二輔助變量，χ_e和υ_e分別為期望路徑坐標系下所述水下航行器跟蹤其虛擬目標點時的航跡角跟蹤誤差和潛伏角跟蹤誤差，χ_Q和υ_Q分別為所述水下航行器的虛擬目標點的航跡角和潛伏角，θ和ψ分別為所述水下航行器的俯仰角和偏航角，D_χ為與所述水下航行器的航跡角對應的運動學不確定項，D_υ為與所述水下航行器的潛伏角對應的運動學不確定項，·為求導符號。

6.根據權利要求4所述的水下航行器的路徑跟蹤控制方法，其特征在于，所述基于所述路徑跟蹤誤差和所述運動學不確定項，對所述水下航行器進行路徑跟蹤控制，包括：

確定載體坐標系下所述水下航行器的合速度；

將所述路徑跟蹤誤差、所述運動學不確定項和所述合速度代入運動學控制器中，得到輸出值；

根據所述輸出值對所述水下航行器進行路徑跟蹤控制；

其中，所述運動學控制器，是以補償所述運動學不確定項為目的設計的。