[發明專利]波浪滑翔器虛擬錨泊導航控制算法

在審

• 申請號: 202010034275.2
• 申請日: 2020-01-13
• 公開（公告）號: CN111142541A
• 公開（公告）日: 2020-05-12
• 發明（設計）人: 桑宏強;孫秀軍;周瑩;于佩元;吳向陽
• 申請（專利權）人: 天津工業大學;中國海洋大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種虛擬錨泊導航控制算法，其特征在于，所述一種虛擬錨泊導航控制算法具有以下步驟：

步驟一：設置虛擬錨泊中心點，引入虛擬錨泊誤差圓，將虛擬錨泊問題轉變成路徑跟蹤問題，給出誤差圓半徑R，并建立直角坐標系。

步驟二：將虛擬錨泊誤差圓進行曲線離散化，即將圓形曲線軌跡跟蹤轉化為跟蹤多個目標點的直線路徑跟蹤，羅盤采集當前航向角ψ，GPS模塊對波浪滑翔器進行實時定位，LOS算法計算期望航向角ψ_los，實際航向角與期望航向角間的航向差輸入到PID控制器，解算出當前期望舵角δ并執行打舵。

步驟三：延時，再次采集波浪滑翔器當前位置和當前航向角，通過波浪滑翔器當前位置與目標點的距離偏差d判斷是否到達預設目標點范圍，如果未到達，則繼續重復之前的步驟二，如果到達，則切換到下一目標點，重復步驟二。通過上述步驟，使波浪滑翔器圍繞虛擬錨泊點進行近似圓形軌跡跟蹤運動，實現精確的虛擬錨泊導航控制。

2.根據權利要求1所述的一種虛擬錨泊導航控制算法，其特征在于，設置虛擬錨泊中心點，引入虛擬錨泊誤差圓，給出誤差圓半徑R，將虛擬錨泊誤差圓進行曲線離散化，取目標點，通過對目標點的跟蹤實現精確的錨泊導航控制。

3.根據權利要求1所述的一種虛擬錨泊導航控制算法，其特征在于，

式中，ψ_los為期望航向角，(X_LOS，Y_LOS)為期望航向點的坐標值，(X，Y)為實際位置的坐標值，Δ為可視距離，期望航向點與波浪滑翔器當前位置在期望航機上的投影點相距Δ＝nL_PP，這里n＝2-5，L_PP為波浪滑翔器的身長，Δ為波浪滑翔器的可視距離，式中e表示波浪滑翔器的橫向跟隨誤差，α_k為直角坐標系鉛直方向與期望航向的夾角。

4.根據權利要求1所述的一種虛擬錨泊導航控制算法，其特征在于，

ψ_e＝ψ_los-ψ (3)

其中，δ為舵角增量，K₁為比例系數，K₂為微分系數，K₃為積分系數，ψ_e為航向差，ψ_los為期望航向角，ψ為實際航向角，系統存在航向差ψ_e時，經過PID控制后會產生一個舵角，該舵角會控制波浪滑翔器偏向期望軌跡航行，通過對PID控制器參數合理的設置，可以得到穩定的收斂，使航向差ψ_e逐漸趨近于零，即波浪滑翔器的當前航向角趨近于期望航向角，能夠使波浪滑翔器不斷向目標航向點靠近，最終行駛到目標航向點，完成對目標點的跟蹤控制，當波浪滑翔器進入該目標點誤差范圍內時，轉入跟蹤下一個目標點，直至完成所有離散目標點的路徑跟蹤，實現精確的虛擬錨泊導航控制。