基于六軸機械手的自動化設備控制方法及系統與流程

本發明涉及機械手，尤其涉及一種基于六軸機械手的自動化設備控制方法及系統。

背景技術：

1、六軸機械手是指具備六個自由度的機械手，這六個自由度分別是x軸自由度、y軸自由度、z軸自由度、繞x軸旋轉的自由度、繞y軸旋轉的自由度、繞z軸旋轉的自由度，基于六軸機械手的自動化設備控制是指對六軸機械手的運動動作與運動軌跡進行控制的過程。

2、目前，對六軸機械手進行控制之前需要先確定六軸機械手下一步要挪動的位置，一般是六軸機械手的末端要接觸到貨物，從而夾取貨物，現有技術一般僅控制六軸機械手的末端的運動動作與運動軌跡，而六軸機械手的末端的控制又依靠末端前面的機械臂與關節來實現，但現有技術并未明確末端前面的機械臂與關節的運動動作與運動軌跡，其次，從機械手的當前動作轉換成其他位置下的動作需要緩慢分間隔進行，并不是一次性動作，而是分化成需要小動作來完成當前動作轉換成其他位置下的動作，在這個過程中還需要考慮機械手是否保持穩定性，即防止機械手受外界干擾而產生抖動，現有技術常通過模糊pid的方式來保持機械手的穩定性，模糊pid控制的方式需要進行控制參數的調整、模糊集選擇及大量數據計算，而參數的調整需要一定的經驗和專業知識，需要由人工專家來決策選取，大量數據計算需要較大的計算資源，模糊集選擇對控制效果有較大影響，如果選擇不當，可能會導致控制效果下降或不穩定。因此，亟待一種解決方案可以明確末端前面的機械臂與關節的運動動作與運動軌跡，并代替模糊pid控制的方式來對六軸機械手進行控制。

技術實現思路

1、為了解決上述問題，本發明提供了一種基于六軸機械手的自動化設備控制方法及系統，可以明確末端前面的機械臂與關節的運動動作與運動軌跡，并代替模糊pid控制的方式來對六軸機械手進行控制。

2、第一方面，本發明提供了一種基于六軸機械手的自動化設備控制方法，包括：

3、確定六軸機械手的末端的末端坐標點，采集所述六軸機械手的當前位姿，其中，所述六軸機械手包括末端、第一連桿、第二連桿、第一連桿起點、第一連桿終點，所述當前位姿包括當前坐標點與當前角度值；

4、利用所述末端坐標點規劃所述六軸機械手的目標位姿，其中，所述目標位姿包括目標坐標點與目標角度值，基于所述當前坐標點與所述當前角度值，計算所述當前位姿至所述目標位姿的位姿成本，利用所述位姿成本從所述目標位姿中選取最終位姿；

5、計算所述當前位姿與所述最終位姿之間的位姿誤差，構建所述六軸機械手的傳遞函數，利用所述位姿誤差構建所述六軸機械手的pid控制器，基于所述傳遞函數，計算所述pid控制器的增益參數；

6、基于所述增益參數與所述位姿誤差，利用所述pid控制器輸出所述六軸機械手的規劃控制信號，并采集所述六軸機械手的當前控制信號；

7、計算從所述當前控制信號至所述規劃控制信號的信號控制風險，根據所述信號控制風險，從所述規劃控制信號中篩選設備控制信號，利用所述設備控制信號對所述六軸機械手進行設備控制，得到所述六軸機械手的設備控制結果。

8、在第一方面的一種可能實現方式中，所述利用所述末端坐標點規劃所述六軸機械手的目標位姿，包括：

9、獲取所述六軸機械手的旋轉角度范圍與連桿長度；

10、基于所述旋轉角度范圍與所述連桿長度，利用下述公式構建所述末端坐標點的目標函數：

11、

12、

13、

14、lx＝l1cosθx

15、ly＝l1cosθy

16、lz＝l1cosθz

17、

18、

19、

20、l′x＝l2cosθ′x

21、l′y＝l2cosθ′y

22、l′z＝l2cosθ′z

23、l″x＝lx+l′x

24、l″y＝ly+l′y

25、l″z＝lz+l′z

26、l″x＝l1cosθx+l2cosθ′x

27、l″y＝l1cosθy+l2cosθ′y

28、l″z＝l1cosθz+l2cosθ′z

29、

30、其中，f表示目標函數，f包括min|l″x-l″′x|、min|l″y-l″′y|及min|l″z-l″′z|，(l″′x,l″′y,l″′z)表示所述末端坐標點，(l″x,l″y,l″z)表示所述末端坐標點的待優化坐標點，l1表示所述連桿長度中第一連桿的長度，l2表示所述連桿長度中第二連桿的長度，(θx,θy,θz)表示以第一連桿起點為原點的直角坐標系中第一連桿相對于直角坐標系的偏轉角度，(θ′x,θ′y,θ′z)表示以第一連桿終點為原點的直角坐標系中第二連桿相對于直角坐標系的偏轉角度，lx表示以第一連桿起點為原點的直角坐標系中第一連桿在x軸上的映射長度，ly表示以第一連桿起點為原點的直角坐標系中第一連桿在y軸上的映射長度，lz表示以第一連桿起點為原點的直角坐標系中第一連桿在z軸上的映射長度，l′x表示以第一連桿終點為原點的直角坐標系中第二連桿在x軸上的映射長度，l′y表示以第一連桿終點為原點的直角坐標系中第二連桿在y軸上的映射長度，l′z表示以第一連桿終點為原點的直角坐標系中第二連桿在z軸上的映射長度，θx,θy,θz,θ′x,θ'y,θ'z屬于所述旋轉角度范圍；

31、利用下述公式對所述目標函數中的角度參數進行角度更新，得到更新角度：

32、v′ij＝vij+c1×rand×(pbestij-xij)+c2×rrand×(gbestij-xij)

33、x′ij＝xij+vij

34、其中，x′ij表示更新角度，v′ij表示對原始更新速度進行更新之后的速度，i表示θ的序號，j表示θ'的序號，vij表示θ'的原始更新速度，c1與c2表示學習因子，c1＝c2＝2，rand表示介于0～1之間的隨機數，xij表示θ'的當前數值，pbestij表示θ'的鄰近歷史取值中f值為0的鄰近歷史取值，gbestij表示θ'的全部歷史取值中f值為0的歷史取值，θ'包括θ'x,θ'y,θ'z，θ包括θx,θy,θz；

35、判斷所述更新角度在所述目標函數中對應的目標函數值是否為預設閾值；

36、當所述更新角度在所述目標函數中對應的目標函數值為所述預設閾值時，利用所述更新角度計算所述六軸機械手的目標坐標點與目標角度值；

37、將所述目標坐標點與所述目標角度值作為所述目標位姿。

38、在第一方面的一種可能實現方式中，所述基于所述當前坐標點與所述當前角度值，計算所述當前位姿至所述目標位姿的位姿成本，包括：

39、獲取所述目標位姿中的目標坐標點與目標角度值；

40、利用下述公式計算所述當前坐標點至所述目標坐標點的坐標成本：

41、u＝u1+u2

42、其中，u表示坐標成本，u1表示當前坐標點中第一連桿終點的坐標與目標坐標點中第一連桿終點的坐標之間的差值的絕對值，u2當前坐標點中末端的坐標與末端坐標點之間的差值的絕對值；

43、利用下述公式計算所述當前角度值至所述目標坐標點的角度成本：

44、v＝v1+v2

45、其中，v表示角度成本，v1表示當前角度值中第一連桿終點的角度與目標角度值中第一連桿終點的角度之間的差值的絕對值，v2當前角度值中末端的角度與末端角度值之間的差值的絕對值；

46、將所述坐標成本與所述角度成本之和作為所述當前位姿至所述目標位姿的位姿成本。

47、在第一方面的一種可能實現方式中，所述構建所述六軸機械手的傳遞函數，包括：

48、利用下述公式構建所述六軸機械手的傳遞函數：

49、

50、p(s)*(s2+αs+β)＝γ*q(s)

51、s2p(s)+αsp(s)+βp(s)＝γq(s)

52、

53、其中，表示所述六軸機械手的傳遞函數，t表示時域中的時間自變量，p(t)表示在pid控制器的控制下六軸機械手被調整之后的位姿，q(t)表示pid控制器向六軸機械手輸入的位姿控制信號，α、β、γ表示常數參數，s表示對p(t)進行拉普拉斯變換時將t進行變換而得到的自變量，p(s)表示p(t)的拉普拉斯變換形式，q(s)表示q(t)的拉普拉斯變換形式。

54、在第一方面的一種可能實現方式中，所述基于所述傳遞函數，計算所述pid控制器的增益參數，包括：

55、獲取pid控制器對應的輸出誤差；

56、基于所述輸出誤差與所述傳遞函數，構建所述六軸機械手的狀態方程矩陣；

57、構建所述pid控制器下的李雅普諾夫原模型；

58、對所述李雅普諾夫原模型進行模型求導，得到李雅普諾夫原模型的導數；

59、計算輸出誤差下所述pid控制器的反函數；

60、將所述狀態方程矩陣與所述反函數代入所述李雅普諾夫原模型的導數中，得到李雅普諾夫導數模型；

61、判斷是否可以構建所述李雅普諾夫原模型的正定矩陣；

62、在可以構建所述李雅普諾夫原模型的正定矩陣時，判定所述李雅普諾夫原模型大于預設閾值；

63、在所述李雅普諾夫原模型大于預設閾值時，構建所述李雅普諾夫導數模型的負定矩陣；

64、在所述pid控制器的比例增益、積分增益及微分增益符合所述負定矩陣時，確定所述pid控制器的增益參數。

65、在第一方面的一種可能實現方式中，所述基于所述增益參數與所述位姿誤差，利用所述pid控制器輸出所述六軸機械手的規劃控制信號，包括：

66、利用所述增益參數調整所述pid控制器，得到調整控制器；

67、將所述位姿誤差代入所述調整控制器中；

68、在所述調整控制器中輸出所述位姿誤差對應的規劃控制信號。

69、在第一方面的一種可能實現方式中，所述計算從所述當前控制信號至所述規劃控制信號的信號控制風險，包括：

70、查詢所述當前控制信號與所述規劃控制信號中每兩個相鄰控制信號之間的信號控制區間；

71、按照每個信號控制區間之間的相鄰關系，隨機合并所述信號控制區間，得到合并控制區間；

72、識別歷史時段中是否存在不小于所述合并控制區間的區間；

73、在歷史時段中存在不小于所述合并控制區間的區間時，將無風險作為所述信號控制風險；

74、在歷史時段中不存在不小于所述合并控制區間的區間時，計算所述合并控制區間的區間長度；

75、識別所述歷史時段中是否存在與所述區間長度一致的目標長度；

76、查詢所述區間長度下的區間規模與所述目標長度下的目標規模；

77、計算所述區間規模與所述目標規模之間的規模標準差；

78、利用所述規模標準差構建所述區間規模的標準差閾值；

79、在所述區間規模大于所述標準差閾值時，將有風險作為所述信號控制風險；

80、在所述區間規模不大于所述標準差閾值時，將無風險作為所述信號控制風險。

81、在第一方面的一種可能實現方式中，所述計算所述區間規模與所述目標規模之間的規模標準差，包括：

82、獲取所述區間規模中的區間角度差與區間坐標距離；

83、計算所述區間角度差與所述區間坐標距離之間的第一角度-坐標和；

84、獲取所述目標規模中的目標角度差與目標坐標距離；

85、計算所述目標角度差與所述目標坐標距離之間的第二角度-坐標和；

86、計算所述第一角度-坐標和與所述第二角度-坐標和之間的和均值；

87、根據所述第一角度-坐標和、所述第二角度-坐標和及所述和均值，計算所述區間規模與所述目標規模之間的規模標準差。

88、在第一方面的一種可能實現方式中，所述根據所述信號控制風險，從所述規劃控制信號中篩選設備控制信號，包括：

89、在所述信號控制風險為無風險時，保留所述無風險對應的合并控制區間，得到保留區間；

90、查詢所述保留區間中每個相鄰的保留區間之間是否區間連續；

91、在所述保留區間中每個相鄰的保留區間之間未區間連續時，根據所述保留區間中的信號控制區間，縮短所述保留區間的區間長度，得到縮短區間，以通過所述縮短區間對齊所述保留區間中每個相鄰的保留區間，得到對齊區間；

92、計算所述對齊區間的區間數目；

93、從所述區間數目中選取最小數目；

94、從所述對齊區間中獲取所述最小數目對應的目標對齊區間；

95、將所述目標對齊區間的邊界上的規劃控制信號作為所述設備控制信號。

96、第二方面，本發明提供了一種基于六軸機械手的自動化設備控制系統，所述系統包括：

97、位姿采集模塊，用于確定六軸機械手的末端的末端坐標點，采集所述六軸機械手的當前位姿，其中，所述六軸機械手包括末端、第一連桿、第二連桿、第一連桿起點、第一連桿終點，所述當前位姿包括當前坐標點與當前角度值；

98、位姿選取模塊，用于利用所述末端坐標點規劃所述六軸機械手的目標位姿，其中，所述目標位姿包括目標坐標點與目標角度值，基于所述當前坐標點與所述當前角度值，計算所述當前位姿至所述目標位姿的位姿成本，利用所述位姿成本從所述目標位姿中選取最終位姿；

99、參數計算模塊，用于計算所述當前位姿與所述最終位姿之間的位姿誤差，構建所述六軸機械手的傳遞函數，利用所述位姿誤差構建所述六軸機械手的pid控制器，基于所述傳遞函數，計算所述pid控制器的增益參數；

100、信號采集模塊，用于基于所述增益參數與所述位姿誤差，利用所述pid控制器輸出所述六軸機械手的規劃控制信號，并采集所述六軸機械手的當前控制信號；

101、設備控制模塊，用于計算從所述當前控制信號至所述規劃控制信號的信號控制風險，根據所述信號控制風險，從所述規劃控制信號中篩選設備控制信號，利用所述設備控制信號對所述六軸機械手進行設備控制，得到所述六軸機械手的設備控制結果。

102、與現有技術相比，本方案的技術原理及有益效果在于：

103、本發明實施例通過利用所述末端坐標點規劃所述六軸機械手的目標位姿，以用于明確末端前面的第一連桿、第二連桿及第一連桿終點的運動動作與運動軌跡，進一步地，本發明實施例通過基于所述當前坐標點與所述當前角度值，計算所述當前位姿至所述目標位姿的位姿成本，以用于選取位姿成本最小的目標位姿，進一步地，本發明實施例通過構建所述六軸機械手的傳遞函數，以用于構建pid控制器的輸出控制信號與六軸機械手輸出的位姿之間的傳遞關系，進一步地，本發明實施例通過基于所述傳遞函數，計算所述pid控制器的增益參數，以用于利用李雅普諾夫模型代替模糊pid控制的方式來對六軸機械手進行穩定性控制，本發明實施例通過計算從所述當前控制信號至所述規劃控制信號的信號控制風險，以用于在pid控制器輸出一序列的規劃控制信號的前提下，計算每個當前的控制信號至下一個控制信號的后面的控制信號的過程中是否會對六軸機械手造成機械障礙，進一步地，本發明實施例通過根據所述信號控制風險，從所述規劃控制信號中篩選設備控制信號，以用于將原本pid控制器輸出的連續的規劃控制信號中無風險的規劃控制信號擴大，這樣可以減少頻繁的調整控制信號的次數。因此，本發明實施例提出的一種基于六軸機械手的自動化設備控制方法及系統，可以明確末端前面的機械臂與關節的運動動作與運動軌跡，并代替模糊pid控制的方式來對六軸機械手進行控制。