本發明屬于機器人控制，具體來說，特別涉及基于微分同胚映射的雙臂協作機器人的約束跟隨控制方法。

背景技術：

1、在現代制造業、物流及醫療康復等領域，雙臂協作機器人憑借其擬人化的操作模式與高度的靈活性，正發揮著日益重要的作用。它們能夠模擬人類雙臂，協同完成諸如精密裝配、大型物體搬運等復雜任務，這對機器人的控制精度和協調能力提出了極高要求。

2、傳統控制方法通常采用阻抗控制、基于任務的優先級控制或模型預測控制等策略。這些技術在結構化、已知環境中對于確定性系統表現出良好的性能。它們主要通過力/位混合控制或優化算法來處理約束，在很大程度上依賴于精確的系統動力學模型，往往假設工作環境中的不確定性是可忽略或已知的。

3、然而，現有技術在實際應用中面臨顯著挑戰。首先，機器人系統固有的參數不確定性、未建模動態以及不可預測的外部干擾，會嚴重惡化基于標稱模型設計的控制器的性能，導致約束跟隨誤差增大甚至任務失敗；其次，許多方法在應對這些不確定性時，缺乏有效的在線自適應機制，魯棒性不足；再者，將復雜的幾何約束直接融入關節空間的控制律設計中，常導致系統方程復雜、計算負擔重，難以實現實時、高效的控制，限制了雙臂機器人在動態、非結構化環境中的廣泛應用。

技術實現思路

1、（一）解決的技術問題

2、針對相關技術中的問題，本發明提供基于微分同胚映射的雙臂協作機器人的約束跟隨控制方法，以克服現有相關技術所存在的上述技術問題。

3、（二）技術方案

4、為解決上述技術問題，本發明是通過以下技術方案實現的：

5、第一方面，本發明提供一種基于微分同胚映射的雙臂協作機器人的約束跟隨控制方法，包括以下步驟：

6、s1、建立雙臂協作機器人的標稱系統動力學模型；

7、s2、根據雙臂協作任務定義任務空間中的幾何約束方程；基于所述幾何約束方程，構建全局微分同胚映射和完整系統動力學模型；

8、所述全局微分同胚映射通過定義約束跟蹤變量并構造互補坐標得到；所述完整系統動力學模型通過將幾何約束方程的雅可比矩陣以及不確定性數據引入標稱系統動力學模型中得到；

9、s3、基于全局微分同胚映射，將完整系統動力學模型的狀態空間轉化為至目標系統的狀態空間，得到映射后的目標系統動力學模型；

10、s4、基于映射后的目標系統動力學模型，定義跟蹤誤差與滑模面、設計自適應律，并綜合控制律，得到魯棒自適應控制器；

11、所述魯棒自適應控制器在線實時估計映射后的目標系統動力學模型的不確定性并生成實時控制力矩；

12、s5、將所述實時控制力矩，施加于所述雙臂協作機器人，驅動所述雙臂協作機器人各關節運動；

13、優選地，所述s1包括以下步驟：

14、s11、收集雙臂機器人所有關節的角位移數據以及線位移數據，得到雙臂機器人關節角位移集合和雙臂機器人關節線位移集合；

15、分別將雙臂機器人關節角位移集合中的所有角位移數據和雙臂機器人關節線位移集合中的所有線位移數據進行向量組合操作，得到廣義坐標向量；

16、s12、通過機器人設計圖紙、cad模型以及參數辨識實驗，收集雙臂機器人的每個連桿的質量數據、連桿的質心在連桿坐標系中的位置數據以及連桿的慣性張量數據；

17、基于廣義坐標向量、雙臂機器人的每個連桿的質量數據、連桿的質心在連桿坐標系中的位置數據以及連桿的慣性張量數據構建正定慣性矩陣、科里奧利力和向心力矩陣以及重力向量；

18、s13、基于正定慣性矩陣、科里奧利力和向心力矩陣以及重力向量，通過利用拉格朗日力學式，并引入摩擦力模型，構建雙臂機器人的標稱系統動力學模型；所述標稱動力學方程描述了在理想、無不確定性情況下的雙臂機器人的動力學；

19、優選地，所述s2包括以下步驟：

20、s21、根據雙臂協作的具體任務，將雙臂協作的具體任務的末端執行器之間的幾何關系轉化為數學方程，得到初始幾何約束方程；

21、將初始幾何約束方程轉化為代數向量方程，基于旋轉關節旋轉和平移關節平移的維度，確定代數向量方程中約束維度的值，得到幾何約束方程；

22、s22、計算幾何約束方程的雅可比矩陣并收集不確定性數據；將所述幾何約束的雅可比矩陣以及不確定性數據引入標稱系統動力學模型，得到完整系統動力學模型；

23、s23、基于幾何約束方程，通過定義約束跟蹤變量并構造互補坐標得到全局微分同胚映射；

24、優選地，所述s22包括以下步驟：

25、s221、通過全微分方程公式對幾何約束方程求導關于時間的全微分，得到約束的雅可比矩陣；

26、s222、收集參數不確定性數據、未建模動態數據、外部干擾數據、摩擦模型誤差數據，得到不確定性項數據；

27、s223、將約束的雅可比矩陣輸入至拉格朗日乘子法，當幾何約束方程等于零時，得到約束力；

28、將所述不確定性項數據和約束力引入標稱系統動力學模型中，得到完整系統動力學模型；

29、優選地，所述s23包括以下步驟：

30、s231、基于幾何約束方程和完整系統動力學模型，定義約束位置誤差變量、約束速度誤差變量，將約束位置誤差變量、約束速度誤差變量融合，得到約束跟蹤變量；

31、s232、根據完整系統動力學模型的關節數以及約束維度，以及互補變量的維度得到互補變量；

32、設定一個非線性映射函數等于互補變量，得到互補坐標變換函數；具體地：計算約束的雅可比矩陣的零空間投影矩陣；令互補變量對時間的一階導數等于零空間投影矩陣的轉置與廣義坐標向量對時間的一階導數相乘，使得互補坐標變換函數的梯度在約束流形上與約束雅可比矩陣的行向量張成的空間正交；

33、s233、基于互補變量和約束跟蹤變量構建新狀態向量；基于新狀態向量定義初始全局微分同胚映射；

34、基于互補坐標變換函數驗證初始全局微分同胚映射的可逆性；驗證成功則將初始全局微分同胚映射作為全局微分同胚映射；否則返回s232對互補坐標變換函數進行修正，直至驗證成功，得到全局微分同胚映射；

35、優選地，所述s231包括以下步驟：

36、s2311、設定一個變量等于幾何約束方程，得到約束位置誤差變量；所述約束位置誤差變量用于量化廣義坐標向量對于理想約束為零時的偏離程度；

37、s2312、求取約束位置誤差變量關于時間的一階導數，得到約束速度誤差變量；

38、s2313、將所述約束位置誤差變量、約束速度誤差變量進行融合，得到約束跟蹤變量；

39、優選地，所述s3包括以下步驟：

40、s31、利用鏈式法則，對新狀態向量中的約束速度誤差變量進行求導，得到約束速度誤差變量的一階導數變量；

41、s32、將完整系統動力學模型方程與約束速度誤差變量以及約束速度誤差變量的一階導數進行聯立，得到聯立后的方程；

42、s33、利用逆映射中的變換關系，將完整系統動力學模型的方程中的所有變量用新狀態向量中的變量表示，得到轉換關系后的變量；

43、s34、將轉換關系后的變量帶入聯立后的方程并化簡，得到新狀態向量下描述的映射后的目標系統動力學模型；所述目標系統動力學模型包括約束跟蹤子模型和互補子模型；

44、優選地，所述s4包括以下步驟：

45、s41、定義目標系統動力學模型中約束跟蹤子模型的控制目標為約束跟蹤子模型的值漸近收斂為零，得到定義的跟蹤誤差；基于約束跟蹤子模型的控制目標定義滑膜面，使約束跟蹤子模型加速收斂并增強魯棒性，得到滑膜面的方程；

46、s42、設定目標系統動力學模型中的不確定性項數據的上界；基于滑膜面，設計自適應律的方程用于在線評估上界，得到自適應律；

47、s43、基于李雅普諾夫穩定性理論，設計所述待設計的控制力矩向量τ，得到設計好的控制律；所述控制律包括前饋模型補償項、約束力補償項、pd反饋項以及魯棒自適應項；

48、所述定義的跟蹤誤差與滑模面、自適應律以及控制律共同構成魯棒自適應控制器；

49、優選地，所述s5包括以下步驟：

50、s51、通過機器人關節上的編碼器，實時讀取每個關節位置和每個關節速度，得到實時關節位置集和實時關節速度集；所述關節速度通過測速發電機得到；

51、s52、將實時關節位置集和實時關節速度集中的關節位置和關節速度代入s3的微分同胚映射中，計算得到當前時刻的目標系統狀態；

52、s53、將當前時刻的目標系統狀態中的誤差信號和自適應律估計出實時上界的估計值，將實時上界的估計值輸入至s4中的控制律方程中，得到施加到各關節的控制力矩，得到實時控制力矩集；

53、s54、將實時控制力矩集中的實時控制力矩傳輸至雙臂機器人的對應的各個關節驅動器，驅動機械臂運動，直至完成協作任務。

54、第二方面，本發明還提供一種基于微分同胚映射的雙臂協作機器人的約束跟隨控制系統，用于實現上述基于微分同胚映射的雙臂協作機器人的約束跟隨控制方法，所述系統包括：

55、系統建模模塊：構建雙臂機器人標稱系統動力學模型，通過拉格朗日法建立包含慣性、科里奧利力、重力及摩擦力的關節空間動力學方程；

56、約束與重構模塊：根據協作任務定義幾何約束，引入其雅可比矩陣、不確定性及外部干擾，構建完整系統動力學模型，并建立約束跟蹤變量與互補坐標的微分同胚映射；

57、模型解耦模塊：利用微分同胚映射將雙臂機器人完整系統動力學模型的系統變換至新狀態空間，解耦為包含約束跟蹤子模型和互補子模型的目標系統動力學模型；

58、控制器設計模塊：基于目標系統動力學模型，設計滑模面與自適應律，在線估計不確定性邊界，并綜合出包含模型補償、反饋及魯棒項的控制律，得到魯棒自適應控制器；

59、控制執行模塊：實時讀取關節狀態，通過微分同胚映射計算系統誤差與不確定性估計，由魯棒自適應控制器生成關節力矩并驅動機器人完成協作任務。

60、（三）有益效果

61、本發明具有以下有益效果：

62、本發明通過構建全局微分同胚映射，將復雜的幾何約束與關節空間動力學模型高效解耦，顯著提升了控制系統的設計與分析效率；該方法將原始非線性約束系統轉化為約束跟蹤與內部動態兩個獨立的子系統，使得控制器能夠直接針對簡化的誤差動態進行設計，有效克服了傳統方法中約束處理計算復雜、實時性差的難題。

63、本發明設計的魯棒自適應控制器集成了滑模控制與參數自適應機制，能夠在線實時估計并補償系統不確定性；這種設計確保了雙臂機器人在存在模型誤差和未知干擾的情況下，依然能精確、穩定地跟隨任務約束，顯著增強了系統在非結構化環境中的適應性和魯棒性。

64、本發明實現了從任務約束定義到關節力矩生成的全流程一體化控制，兼具理論嚴謹性與工程實用性；該控制方法不僅能夠保證雙臂協作任務的高精度執行，還因其模塊化的設計具有良好的通用性，可廣泛應用于各類需要滿足復雜幾何約束的機器人協同作業場景。

65、當然，實施本發明的任一產品并不一定需要同時達到以上所述的所有優點。