本發明涉及機械臂鎮定控制領域，尤其涉及一種復雜環境下水下游動機械臂鎮定控制及推力分配方法。

背景技術：

1、隨著我國對海洋資源的開發和利用，進行海洋勘察、探測等作業的載體，水下機器人等也得到了越來越廣泛的應用。作為一種新型的水下機器人，水下游動機械臂采用多關節鏈式仿生蛇形設計且搭載有多個推進器，具有自由度高、操作性強、靈活可控等特點，能夠適應多變的水下環境，且擅長在狹小空間中執行作業任務。水下游動機械臂的懸停鎮定控制是其實現自主操作任務的基礎，通過精確的懸停控制，機器人可以在特定位置保持穩定，執行長時間的觀測或操作任務。研究水下游動機械臂的懸停鎮定控制技術，有助于提高其在復雜海洋環境中的操作精度和穩定性，這對于精細的水下作業至關重要。

2、例如，中國專利公開號：cn114714359a,公開了一種機械臂系統的飽和參數化控制方法，該方法包括：建立機械臂系統動態模型、機械臂系統控制器設計、執行器飽和線性化、系統模型轉換、引入參數化矩陣、系統增益矩陣求解。該發明基于參數化方法，考慮到執行器飽和的影響，設計了一種適用于機械臂系統的控制器，通過對消開環系統的動態特性，來得到具有符合期望閉環特性的系統，避免了系統全驅特性受到的破壞，并實現了機械臂系統的鎮定控制。

3、例如，中國專利公開號：cn118456437a，公開了柔性機械臂的控制方法、控制裝置、電子設備和存儲介質，所述方法根據正定對角矩陣和軌跡跟蹤誤差構造非奇異積分終端滑模面，具有高魯棒性、高精度和快速瞬態響應等特性；基于柔性機械臂的動力學模型，結合非奇異積分終端滑模面，采用反步法分層設計策略逐層構造虛擬控制律，以得到柔性機械臂的輸入控制力矩，以及將輸入控制力矩作為柔性機械臂的控制指令，對柔性機械臂進行調節，以實現對期望軌跡的跟蹤控制，保證了滑模控制器能夠在有限時間內收斂到0，且收斂時間一定程度上可由參數控制，滿足了捕獲后復合體鎮定的快速需求，并保持較小的穩態誤差和力矩抖振。

4、但是，現有技術中還存在以下問題，

5、水下游動機械臂在執行作業任務過程中，依賴推進器實現自身位姿變化時，推進器操作死區的存在可能會對水下游動機械臂的作業任務產生不利影響，使得水下機器人的運動控制變得復雜，影響其穩定性和控制精度。

技術實現思路

1、為此，本發明提供一種復雜環境下水下游動機械臂鎮定控制及推力分配方法，用以解決水下游動機械臂在執行作業任務過程中，依賴推進器實現自身位姿變化時，推進器操作死區的存在可能會對水下游動機械臂的作業任務產生不利影響，使得水下機器人的運動控制變得復雜，影響其穩定性和控制精度的問題。

2、為實現上述目的，本發明提供一種復雜環境下水下游動機械臂鎮定控制及推力分配方法，其包括：

3、建立水下游動機械臂的運動學及動力學模型，將所述運動學與動力學模型解耦為雙積分線性化動力學模型；

4、設計內層廣義超螺旋自適應滑模控制器，以補償模型線性化帶來的誤差以及外部擾動；

5、基于所述雙積分線性化動力學模型確定模型預測控制器，根據內層滑模控制器輸出以及水下游動機械臂關節角度變化實時調整控制器約束，以使水下游動機械臂能夠得到滿足時變約束條件的最優控制序列；

6、基于所述模型預測控制器的輸出，設計帶有懲罰函數的序列二次規劃推力分配算法，計算水下游動機械臂的各個推進器的推力，以實現水下游動機械臂在復雜水下環境中的鎮定控制。

7、進一步地，所述建立水下游動機械臂的運動學及動力學模型的過程包括，

8、基于右手定則建立世界坐標系以及水下游動機械臂的連桿坐標系；

9、基于所述連桿坐標系相對于世界坐標系的旋轉矩陣計算水下游動機械臂的連桿在所述世界坐標系上的位姿；

10、基于所述連桿的位姿確定水下游動機械臂的運動學與動力學模型；

11、其中，所述位姿包括連桿在世界坐標系上的位置和姿態。

12、進一步地，計算水下游動機械臂的連桿在所述世界坐標系上的位姿的過程包括，

13、基于所述連桿坐標系確定相對于世界坐標系的旋轉矩陣以及位置向量；

14、確定所述旋轉矩陣以及位置向量的組合矩陣為所述連桿的姿態；

15、依據所述姿態以及沿旋量軸旋轉和平移兩種運動方式的組合確定位姿。

16、進一步地，將所述運動學與動力學模型解耦為雙積分線性化動力學模型的過程包括，

17、確定狀態變量；

18、基于所述狀態變量將水下游動機械臂運動學與動力學模型調整為狀態空間表達式；

19、基于所述運動學與動力學模型設計包含控制指令的逆動力學非線性補償控制律；

20、確定所述狀態空間表達式以及所述逆動力學非線性補償控制律的組合為雙積分線性化動力學模型。

21、進一步地，所述狀態變量包括位置向量以及速度向量。

22、進一步地，所述設計內層廣義超螺旋自適應滑模控制器的過程包括，

23、確定具有自適應增益的廣義超螺旋滑模控制器；

24、基于更新定律自適應地調整控制增益，以確定所述控制器的綜合控制性能；

25、確定帶有綜合控制性能的廣義超螺旋滑模控制器為內層廣義超螺旋自適應滑模控制器。

26、進一步地，所述基于所述雙積分線性化動力學模型確定模型預測控制器的過程包括，

27、確定水下游動機械臂的狀態量、輸入量以及輸出量；

28、基于所述狀態量、輸入量以及輸出量構建水下游動機械臂的狀態轉移方程；

29、基于所述狀態轉移方程確定模型預測控制器。

30、進一步地，所述模型預測控制器產生的是偽控制指令，需實時更新約束條件。

31、進一步地，所述約束條件包括系統廣義慣性矩陣、水下機器人推進器的配置和機械參數計算出的廣義力的極限值、水下機器人推進器的配置和機械參數計算出的力矩的極限值、滑模控制器的輸出以及模型預測控制器的輸出。

32、進一步地，所述設計帶有懲罰函數的序列二次規劃推力分配算法的過程包括，

33、基于水下游動機械臂的推進器配置，計算推力分配矩陣；

34、將原非線性約束問題的目標函數在迭代點附近進行泰勒展開，得到二次規劃子問題的描述式；

35、將不連續的不等式約束以懲罰函數的方式寫入二次規劃子問題的描述式；

36、基于所述推力分配矩陣以及二次規劃子問題的描述式確定二次規劃問題的描述式。

37、與現有技術相比，本發明建立水下游動機械臂的運動學及動力學模型，將所述運動學及動力學模型解耦為雙積分線性化動力學模型，設計內層廣義超螺旋自適應滑模控制器，以補償模型誤差及外部擾動，基于所述雙積分線性化動力學模型確定模型預測控制器，以保證被控系統能夠根據滑模控制器輸出以及水下游動機械臂關節角度變化實時調整；基于模型預測控制器的輸出，設計帶有懲罰函數的序列二次規劃推力分配算法，計算水下游動機械臂的各個推進器的推力，以實現其在復雜水下環境中的鎮定控制。本發明提出的控制方案能夠有效應對水下游動機械臂在懸停鎮定和推進器死區等方面的控制挑戰，適用于眾多具有強耦合非線性特征的系統以及存在死區或物理約束的運動控制系統，一定程度上消除了控制器的抖振現象，減小了水下游動機械臂的能耗，提高了機械臂在復雜環境中的精準控制能力。

38、尤其，本發明將水下游動機械臂的動力學模型解耦為雙積分線性化模型，在動力學模型中，誤差容易在各個自由度之間累積和傳播，導致控制精度下降，通過解耦，可以有效隔離誤差，減少誤差累積，從而提高控制精度，基于此，本發明將水下游動機械臂的動力學模型解耦為雙積分線性化模型，為后續進行控制器設計提供理論依據，解耦后的模型使得各個自由度之間的耦合效應被消除或減弱，可以對每個自由度進行獨立的控制，避免在控制一個自由度時，對其他自由度產生不良影響，提高控制系統的穩定性和性能，提高了機械臂在復雜環境中的精準控制。

39、尤其，本發明設計內層廣義超螺旋自適應滑模控制器，補償由于未建模動力學和線性化帶來的模型不確定性以及復雜環境擾動的影響，在實際對機械臂進行鎮定控制時，大多直接基于線性化模型進行控制器設計，未考慮模型不確定性及環境因素帶來的影響，使得控制器的計算結果發生偏差，導致機械臂不穩定，基于此，本發明設計內層廣義超螺旋自適應滑模控制器，對由于模型不準確及外部擾動產生的誤差進行補償，使機械臂更穩定，提高了機械臂在復雜環境中的精準控制。

40、尤其，本發明基于雙積分線性化模型確定模型預測控制器，以使模型預測控制器中的約束上下界能夠根據預模型預測控制器輸出以及水下游動機械臂關節角度變化實時調整，在實際情況中，機械臂在進行運作時，可能會受到外部干擾，偏離預定目標，無法完成預定工作任務，基于此，本發明確定模型預測控制器，實時接收水下游動機械臂的狀態信息和滑模控制器的輸出，并更新其輸出約束的邊界，保證了機械臂在滿足狀態和實時輸入約束的情況下實現最優控制，提高了機械臂在復雜環境中的精準控制。

41、尤其，本發明設計了帶懲罰函數的序列二次規劃推力分配算法，用以避免推進器的輸出處于死區或超出機械極限，實際情況中，如果不對控制器計算出的廣義力進行推進器上的有效處理及分配，部分推進器很有可能進入死區或者超出其物理極限，使得機械臂控制效果變差，完不成預設任務，甚至停止工作。基于此，本發明設計了序列二次規劃推力分配算法，并重構了二次規劃問題，將由推進器死區造成的不等式約束不連續的問題通過懲罰函數的形式融入目標函數中，從而降低水下游動機械臂能耗，使推進器推力值能夠時刻保持最優解，提高了機械臂在復雜環境中的精準控制。