[發明專利]一種無人潛航器自主回收控制系統及其控制方法

有效

• 申請號: 202110274335.2
• 申請日: 2021-03-15
• 公開（公告）號: CN112947504B
• 公開（公告）日: 2022-09-16
• 發明（設計）人: 王曰英;陳匯資;吳乃龍;周衛祥;成敏;杜鑫;付俊;解相朋;姜斌;羅均;汪小帆;楊希祥;嚴懷成
• 申請（專利權）人: 王曰英
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 鄭州翊博專利代理事務所(普通合伙) 41155
• 代理人: 付紅莉

1.一種無人潛航器自主回收控制系統，其特征在于，包括雙體無人船主控系統、雙體無人船運動控制系統、潛航器自主回收控制系統、無人潛航器運動控制系統；雙體無人船主控系統用于接收回收指令，并將回收指令傳輸至雙體無人船運動控制系統；所述雙體無人船運動控制系統根據回收指令完成雙體無人船的動力定位，并將完成動力定位的指令反饋給雙體無人船主控系統；所述雙體無人船主控系統根據反饋的完成動力定位的指令向潛航器自主回收控制系統發出回收下降指令；所述潛航器自主回收控制系統根據回收下降指令將回收裝置下放到水下對接準備點位置，并將水下對接準備點位置信息發送給無人潛航器運動控制系統；所述無人潛航器運動控制系統根據水下對接準備點位置，規劃出潛航器與水下對接準備點位置間的期望路徑，控制潛航器進入回收裝置；所述潛航器自主回收控制系統將潛航器進入回收裝置的指令反饋給雙體無人船主控系統，雙體無人船主控系統向無人潛航器運動控制系統發送對接完成指令，同時，雙體無人船主控系統向潛航器自主回收控制系統發出回收上升指令；所述無人潛航器運動控制系統根據對接完成指令將潛航器調整為待機狀態；所述潛航器自主回收控制系統根據回收上升指令控制回收裝置的上升，完成潛航器的回收；

所述無人潛航器運動控制系統包括路徑規劃模塊、狀態矢量轉化模塊、L1自適應控制器、驅動器、水下潛航器、位姿矢量轉化模塊；所述路徑規劃模塊中運行有TangentBUG算法，所述路徑規劃模塊的輸入為潛航器的實時位姿信息、水下對接準備點位置，所述路徑規劃模塊的輸出為參考位置與角度矢量信息；所述狀態矢量轉化模塊的輸入為參考位置與角度矢量信息，所述狀態矢量轉化模塊的輸出為參考狀態矢量；所述L1自適應控制器的輸入為水下潛航器實時狀態矢量與參考狀態矢量間的偏差，所述L1自適應控制器的輸出為控制指令；所述驅動器的輸入為控制指令，驅動器將接收到的控制命令轉化為實際的推進力和力矩，控制水下潛航器改變自身的狀態矢量；所述位姿矢量轉化模塊的輸入為水下潛航器的實時狀態矢量，所述位姿矢量轉化模塊的輸出為潛航器實時的位姿信息；

所述L1自適應控制器的設計包括以下步驟：

(a1)構建潛航器動力學模型：

構建潛航器動力學模型，所述潛航器動力學模型公式如式(Ⅷ)所示：

式中，x₂＝[u₂,v₂,r₂,p₂,w₂]^T表示潛航器的速度矢量，u₂,v₂,r₂,p₂,w₂分別表示為潛航器在縱向方向的速度、橫向方向的速度、艏向方向的速度、俯仰方向的速度、升沉方向上的速度；表示潛航器的附加質量矩陣，且其中，m₂表示潛航器的質量，表示潛航器的重心；表示潛航器的線性阻尼矩陣；表示潛航器的非線性阻尼矩陣；表示柯氏力矩陣，其中，在上述表達式中，Y_r²,K_|p|p,均表示水動力系數，且均為正常數；τ₂＝[τ₂₁,τ₂₂,τ₂₃.τ₂₄,τ₂₅]^T表示驅動器的動力輸入，其中，τ₂₁為縱向驅動力、τ₂₂為橫向驅動力、τ₂₃為艏向驅動力、τ₂₄為俯仰方向驅動力、τ₂₅為升沉方向驅動力；τ_E2表示由海流引起的環境干擾；表示將速度信息轉化為位置信息的轉換矩陣；y₂＝[y₂₁,y₂₂,y₂₃,y₂₄,y₂₅]^T表示潛航器輸出的位置信息，其中，y₂₁為潛航器輸出的縱向位置信息，y₂₂為潛航器輸出的橫向位置信息，y₂₃為潛航器輸出的艏搖位置信息，y₂₄為潛航器輸出的俯仰位置信息，y₂₅為潛航器輸出的升沉位置信息，T表示對矩陣進行轉置；

(a2)構建狀態觀測器：

構建狀態觀測器，所述狀態觀測器公式如式(Ⅸ)所示：

式中，表示潛航器不確定性參數的估計值，表示潛航器所受的外界干擾和時變浮力；

(a3)構建自適應律：

構建自適應律，所述自適應律公式如式(Ⅹ)所示：

式中，x_2e＝x₂-x_2d，其中，x_2d表示潛航器在縱向方向、橫向方向、艏向方向、俯仰方向、升沉方向上的期望速度；P是一個赫爾維茲常數矩陣，所述P通過求解獲得，其中，Q為一個常數正定矩陣；Γ表示一個常數增益矩陣；表示潛航器不確定性參數的估計值；為潛航器所受的外界干擾和時變浮力；

(a4)閉環系統控制律：

在構建潛航器動力學模型、狀態觀測器、自適應律的基礎上構建閉環系統控制律，所述閉環系統控制律公式如式(Ⅺ)所示：

式中，起到一個低通濾波器的作用，s是時間t在頻域里對應的變量，k表示低通濾波常數；y₁為水下潛航器的期望位置和角度。