運動控制和自適應模糊控制在碼垛機器人中的應用

劉 光 文 樺 梁 穎

(1.駐馬店職業技術學院機電工程系，河南 駐馬店 463000；2.黃淮學院信息工程學院，河南 駐馬店 463000；3.中原工學院機電學院，河南 鄭州 450007)

隨著食品冷鏈技術的發展，人工碼垛工作強度大、效率低、工作環境惡劣，對人體健康損害較大。而機器人碼垛機具有靈活性強、處理速度快、有效載荷大等特點，可適用于食品冷鏈技術中的物料搬運。將機器人應用于生產線進行碼垛，其難點多為應用程序的選擇和放置。碼垛機器人應用程序目標是通過生成一個代表其環境的計算機仿真模型及實際的機器人模型模擬機器人的4個關節(腰、肩、肘、腕)，并觀察機器人末端執行器的位置，最終實現對機器人的行為控制[1-2]。在模型開發中，常使用機器人工作單元仿真模型開發和機器人仿真執行兩種方法實現碼垛機器人的應用程序。上述兩種模型均以預定義的坐標、整體布局校驗、機器人的輸入輸出、工作區域、碰撞和接近目標檢測以及任務完成后的循環時間監控為基礎進行輸出。

作為一種運動學仿真工具，機器人仿真主要通過高精度的單元級校驗方法，仿真一個或多個運動設備的運動狀態發生變化的系統。還可為機器人應用程序提供一個“模擬”工作站，該工作站可以檢查和評估各種參數，如循環時間、最佳路徑、工作單元布局以及單元內的實體。文章擬提出一種基于計算機的機器人碼垛系統模型生成方法，用于整個機器人應用系統的仿真監控和評估。重點模擬機器人的4個關節(腰、肩、肘、腕)，使用拉格朗日動力學來分析和估計運動模型，并建立運動的逆方程，使用自適應模糊控制算法設計一個新的控制器，并利用模糊控制和自適應控制算法的優點以編程方式控制該控制器，可有效地滿足食品物料搬運的高速、重載要求，機構的動態性能得到大大改善。

1 機器人和機器人碼垛系統

1.1 碼垛系統

機器人碼垛系統應用程序使用機器人在線進行碼垛袋，并側重于取放應用。機器人在取貨點取包，然后將包放到托盤上，機器人模型為多關節四軸氣動動力的碼垛機器人(見圖1)[3-5]。其連桿由一個封閉的平行四桿機構組成，并將兩組小副四桿結構連接在一起，以滿足腕部定位的要求，從而改善機器人的動態特性。

圖1 碼垛機器人的結構

碼垛機器人在執行過程中循環周期最快可達1 600次/h，能處理高達140 kg的重量。其精度較高，可重復定位精度至1 mm[6]。常用的碼垛機器人由底座、轉子/Link1、下臂/Link2、上臂/Link3、連桿/Link4、法蘭/Link5、電機D-axis等部件組成，基于本體的坐標系統進行定位，末端夾手坐標采用基于三軸(X,Y,Z)的坐標系統。

1.2 工作單元仿真模型

(1)創建零件模型：在三維建模軟件中，采用實體建模來創建基本元素。

(2)建立設備模型：設備模型代表實際的工作單元組件，可分為機器人設備模型和非機器人設備模型兩類。設備模型的建立以零件模型的基礎定位為基礎坐標系。碼垛機器人的鏈接使用與其編號相關聯的附件特性，每個附加的鏈接受父/子關系的約束。

(3)布局中定位裝置型號：工作單元模型的布局以實際工作單元的環境為基礎，使用機器人末端的夾爪坐標系統，模型和設備在環境中的放置基于實際工作單元的布局。

(4)在布局中定義設備的運動目標：設備模型的運動屬性定義了設備模型的關節在零位、位置、速度和加速度方面的運動限制。每個連接都被認為是正在進行的鏈接中的一部分。碼垛機器人的關節1是一個腰關節之間的聯系基地和轉子，關節2是一個肩關節，轉子之間的聯系，關節3是一個手肘傳遞到上臂的連接關節，關節4是腕關節與上臂連接桿之間的連接點，關節5是一個爪關節連接法蘭的連接桿，每個關節都有各自的運動極限。

(5)設備行為和編程：設備運動是由一系列的幾何點(GP)決定的，這些點為機器人創建了運動路徑。GP和系列的定位是基于移動模式和布袋的安排，通過輸入每個關節的值、輸入X、Y和Z的絕對值以及單擊鼠標創建GP。其中，獲取準確位置最合適的方法是輸入GP的X、Y、Z的絕對值，目前已確定了74個GPs。

2 碼垛機器人的運動控制

碼垛機器人的連桿通過一個大地坐標系移動或是與r-關節一起旋轉，而第4個連桿之間會產生嚴重的耦合，通過大地坐標系移動的方法可以克服耦合現象，易于控制，但占用大量的工作空間，在貨物的搬運和碼垛方面靈活性較差[7-8]。經過大量的分析、計算和優化設計，碼垛機器人可用于包裝生產線上的貨物搬運和碼垛。該類型碼垛機器人具有響應快、控制便捷、操作范圍大、末端負荷大、安全可靠等特點。

2.1 正運動學計算

圖2為碼垛機器人空間幾何圖，利用幾何法進行運動學正反解析。利用幾何法建立的運動學更直觀，比傳統MDH方法更為簡便。

圖2 碼垛機器人空間幾何圖

連桿參數：關節軸為i，連桿轉角為аi-1，連桿長度為Di，連桿偏距為di，關節角為θi。從圖2中可以看出，機器人的姿態跟θ1、θ4有關。所以推導出機器人的姿態為：

(1)

機器人的位置跟θ2、θ3有關。所以推導出機器人的位置為：

(2)

根據姿態和位置可以得到碼垛機器人運動學正解

(3)

2.2 解析法反解運動

(1)第1個軸反解：當py≠0時，θ1=

或θ1=

(4)

當py=0時，θ1=±900。

θ1是唯一解。

(2)第4個軸反解：

θ4=arctan2[sin(θ1-θ4),cos(θ1-θ4)]+θ1。

(5)

θ4是唯一解。

(3)第2個軸反解：

(6)

所以θ2是唯一解。

(4)第3個軸反解：-900≤θ3≤900。由余弦定理可知cθ1≠0。

c(θ3+900)=

(7)

θ3是唯一解。

2.3 碼垛機模型的動力學分析

2.3.1 機器人的簡化模型 碼垛機的主要結構是封閉的平行四桿連桿，并連接兩組小平行四桿連桿的結構。4個馬達分別用于驅動手腕、水平滑塊和垂直滑塊的旋轉和移動[9-11]。為了計算方便，可僅考慮其腰部和平行四連桿的連接年齡。如圖3所示，將前臂AE、連桿AB、后臂CD及小臂DP的長度分別設為l2，l3，l5，l6；分別用l2c，l3c，l5c，l6c表示其質心和旋轉軸之間的距離；BC的長度、連桿的質心與B點之間的長度、后臂的質心與其C點之間的長度、小臂的質心與E點之間的長度、終點P點設為l0，r3，r5，r6，rr6；DP表示垂直滑塊與O1點之間的水平距離；θ1、θ2、θ3分別為腰部、前臂和小臂的旋轉角度。

利用拉格朗日第二動力學求解公式(即系統的勢能和動能之差)，分析機器人的動力學方程。

圖3 MJR機器人的簡化模型

2.3.2 系統的廣義坐標 為了方便計算，可忽略機器人手腕的運動，因此，該系統可化簡為3個自由度的三維模型。將兩個滑塊的位置坐標x1和y4轉換為XOY中的角度變化θ2和θ3。因此，將θ1、θ2和θ3作為系統的廣義協調。

2.3.3 機器人動力學模型 由于采用平行四連桿機構，連桿之間會產生很大的約束力[12]。因而，應首先導出系統的拉格朗日方程。假設坐標OXYZ中每個鏈接的向量半徑為ri(i= 1,2，…,7)，系統的動能為：

(8)

式中：

I1、I2、I3、I4——連桿1、2、3、4的轉動慣量;

Mi——關節的質量，i=1,2,…,4。

(9)

式中：

Pci——鏈接i的垂直坐標。

機器人的非保守力為關節轉矩τ1，τ2，τ3和τ4，分別為腰部、水平滑塊和垂直滑塊的電機輸出扭矩，機器人的非線性動力學方程可表示為矩陣形式：

(10)

3 自適應模糊PID控制

3.1 參數設置

自適應模糊PID控制器的推導過程是以自適應模糊規則表為基礎進行的，PID的參數可在線調整，通過對預先保存的模糊控制規則表的查詢可以改變參數[13]。傳統的PID控制器可描述為：

(11)

式中：

P、I、D——自適應模糊PID控制器的比例增益、積分增益、導數增益參數；

Out(t)——控制器輸出，(°)；

E(t)——誤差信號，(°)。

為使被控對象達到良好的動靜態性能，采用的PID控制器設計算法是以e和de(t)/dt等參數為基礎，通過計算在線調整KP、KI和KD參數。

3.2 控制器設計過程

(1)輸入和輸出變量：ec、e為輸入參數，而KP、KI和KD為輸出參數。

(2)輸入和輸出變量的模糊語言：將輸入變量e、ec的量子秩選擇為13個秩，e、ec={-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}。上述參數模糊集全部定義為{NB，NS，ZO，PS，PB}，其中NB，NS，ZO，PS和PB分別為負大、負、小、正、小和正大；此外，選擇KP，KI和KD的量子等級作為10個等級，KP，KI和KD={0,1,2,3,4,5,6,7,8,9}。每個變量的三角分布隸屬函數如圖4、5所示。參數e的量子因子Ke=10，量子因子Kec=100；KP，KI和KD的量子因子為KUP=3，KUI=0.7，KUD=0.5。

圖4 e和ec的模糊隸屬函數

圖5 KP，KI和KD的模糊隸屬函數

(3)PID參數調節：根據其模糊規則表進行調節，實際控制值是精確值和量子因子的乘積。

3.3 準確率

在數據集中分別按總量的1/4、1/3、1/2和3/4作為測試數據集，并按式(13)、(14)分別計算檢測準確率和誤報率。

(12)

式中：

A——準確分類數據；

B——測試數據集容量；

C——檢測率，%。

(13)

式中：

M——誤檢為其他類型的數據；

K——測試數據集容量；

L——誤報率，%。

4 仿真

為驗證試驗設計的控制器對堆垛機的有效性，利用Matlab軟件對其進行仿真。由圖6可知，當腰部輸入角度為1 rad，上升時間為50 ms，調節時間為10 ms時，穩態誤差為1.5%，說明自適應模糊PID控制在非線性系統響應方面比傳統的PID控制更具優勢，并且不存在過沖現象。因此，控制器可滿足碼垛機系統的性能要求。

圖6 傳統PID控制器與自適應模糊PID控制器的比較

機器人負載的變化導致控制器需較強的魯棒性。由圖7可知，機器人在10 N·m余弦信號的干擾下仍可正常工作，因此控制器具有堅固的魯棒性。

圖7 帶有干擾信號的系統的輸出響應

5 結論

采用拉格朗日動力學方程分析了碼垛機器人的動力學性能，建立了逆動力學模型。采用自適應模糊PID控制算法設計碼垛機器人的控制器，相對于傳統的PID控制算法更具優勢，可在很大程度上消除對沖，具有較強的魯棒性。試驗設計的碼垛機器人可有效地滿足食品物料搬運的高速、重載要求，機構的動態性能得到大大改善。后續可研究控制收斂去速度。