運(yùn)動(dòng)控制和自適應(yīng)模糊控制在碼垛機(jī)器人中的應(yīng)用

劉 光 文 樺 梁 穎

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

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

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

1 機(jī)器人和機(jī)器人碼垛系統(tǒng)

1.1 碼垛系統(tǒng)

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

圖1 碼垛機(jī)器人的結(jié)構(gòu)

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

1.2 工作單元仿真模型

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

(2)建立設(shè)備模型：設(shè)備模型代表實(shí)際的工作單元組件，可分為機(jī)器人設(shè)備模型和非機(jī)器人設(shè)備模型兩類。設(shè)備模型的建立以零件模型的基礎(chǔ)定位為基礎(chǔ)坐標(biāo)系。碼垛機(jī)器人的鏈接使用與其編號(hào)相關(guān)聯(lián)的附件特性，每個(gè)附加的鏈接受父/子關(guān)系的約束。

(3)布局中定位裝置型號(hào)：工作單元模型的布局以實(shí)際工作單元的環(huán)境為基礎(chǔ)，使用機(jī)器人末端的夾爪坐標(biāo)系統(tǒng)，模型和設(shè)備在環(huán)境中的放置基于實(shí)際工作單元的布局。

(4)在布局中定義設(shè)備的運(yùn)動(dòng)目標(biāo)：設(shè)備模型的運(yùn)動(dòng)屬性定義了設(shè)備模型的關(guān)節(jié)在零位、位置、速度和加速度方面的運(yùn)動(dòng)限制。每個(gè)連接都被認(rèn)為是正在進(jìn)行的鏈接中的一部分。碼垛機(jī)器人的關(guān)節(jié)1是一個(gè)腰關(guān)節(jié)之間的聯(lián)系基地和轉(zhuǎn)子，關(guān)節(jié)2是一個(gè)肩關(guān)節(jié)，轉(zhuǎn)子之間的聯(lián)系，關(guān)節(jié)3是一個(gè)手肘傳遞到上臂的連接關(guān)節(jié)，關(guān)節(jié)4是腕關(guān)節(jié)與上臂連接桿之間的連接點(diǎn)，關(guān)節(jié)5是一個(gè)爪關(guān)節(jié)連接法蘭的連接桿，每個(gè)關(guān)節(jié)都有各自的運(yùn)動(dòng)極限。

(5)設(shè)備行為和編程：設(shè)備運(yùn)動(dòng)是由一系列的幾何點(diǎn)(GP)決定的，這些點(diǎn)為機(jī)器人創(chuàng)建了運(yùn)動(dòng)路徑。GP和系列的定位是基于移動(dòng)模式和布袋的安排，通過輸入每個(gè)關(guān)節(jié)的值、輸入X、Y和Z的絕對(duì)值以及單擊鼠標(biāo)創(chuàng)建GP。其中，獲取準(zhǔn)確位置最合適的方法是輸入GP的X、Y、Z的絕對(duì)值，目前已確定了74個(gè)GPs。

2 碼垛機(jī)器人的運(yùn)動(dòng)控制

碼垛機(jī)器人的連桿通過一個(gè)大地坐標(biāo)系移動(dòng)或是與r-關(guān)節(jié)一起旋轉(zhuǎn)，而第4個(gè)連桿之間會(huì)產(chǎn)生嚴(yán)重的耦合，通過大地坐標(biāo)系移動(dòng)的方法可以克服耦合現(xiàn)象，易于控制，但占用大量的工作空間，在貨物的搬運(yùn)和碼垛方面靈活性較差[7-8]。經(jīng)過大量的分析、計(jì)算和優(yōu)化設(shè)計(jì)，碼垛機(jī)器人可用于包裝生產(chǎn)線上的貨物搬運(yùn)和碼垛。該類型碼垛機(jī)器人具有響應(yīng)快、控制便捷、操作范圍大、末端負(fù)荷大、安全可靠等特點(diǎn)。

2.1 正運(yùn)動(dòng)學(xué)計(jì)算

圖2為碼垛機(jī)器人空間幾何圖，利用幾何法進(jìn)行運(yùn)動(dòng)學(xué)正反解析。利用幾何法建立的運(yùn)動(dòng)學(xué)更直觀，比傳統(tǒng)MDH方法更為簡(jiǎn)便。

圖2 碼垛機(jī)器人空間幾何圖

連桿參數(shù)：關(guān)節(jié)軸為i，連桿轉(zhuǎn)角為аi-1，連桿長(zhǎng)度為Di，連桿偏距為di，關(guān)節(jié)角為θi。從圖2中可以看出，機(jī)器人的姿態(tài)跟θ1、θ4有關(guān)。所以推導(dǎo)出機(jī)器人的姿態(tài)為：

(1)

機(jī)器人的位置跟θ2、θ3有關(guān)。所以推導(dǎo)出機(jī)器人的位置為：

(2)

根據(jù)姿態(tài)和位置可以得到碼垛機(jī)器人運(yùn)動(dòng)學(xué)正解

(3)

2.2 解析法反解運(yùn)動(dòng)

(1)第1個(gè)軸反解：當(dāng)py≠0時(shí)，θ1=

或θ1=

(4)

當(dāng)py=0時(shí)，θ1=±900。

θ1是唯一解。

(2)第4個(gè)軸反解：

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

(5)

θ4是唯一解。

(3)第2個(gè)軸反解：

(6)

所以θ2是唯一解。

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

c(θ3+900)=

(7)

θ3是唯一解。

2.3 碼垛機(jī)模型的動(dòng)力學(xué)分析

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

利用拉格朗日第二動(dòng)力學(xué)求解公式(即系統(tǒng)的勢(shì)能和動(dòng)能之差)，分析機(jī)器人的動(dòng)力學(xué)方程。

圖3 MJR機(jī)器人的簡(jiǎn)化模型

2.3.2 系統(tǒng)的廣義坐標(biāo) 為了方便計(jì)算，可忽略機(jī)器人手腕的運(yùn)動(dòng)，因此，該系統(tǒng)可化簡(jiǎn)為3個(gè)自由度的三維模型。將兩個(gè)滑塊的位置坐標(biāo)x1和y4轉(zhuǎn)換為XOY中的角度變化θ2和θ3。因此，將θ1、θ2和θ3作為系統(tǒng)的廣義協(xié)調(diào)。

2.3.3 機(jī)器人動(dòng)力學(xué)模型 由于采用平行四連桿機(jī)構(gòu)，連桿之間會(huì)產(chǎn)生很大的約束力[12]。因而，應(yīng)首先導(dǎo)出系統(tǒng)的拉格朗日方程。假設(shè)坐標(biāo)OXYZ中每個(gè)鏈接的向量半徑為ri(i= 1,2，…,7)，系統(tǒng)的動(dòng)能為：

(8)

式中：

I1、I2、I3、I4——連桿1、2、3、4的轉(zhuǎn)動(dòng)慣量;

Mi——關(guān)節(jié)的質(zhì)量，i=1,2,…,4。

(9)

式中：

Pci——鏈接i的垂直坐標(biāo)。

機(jī)器人的非保守力為關(guān)節(jié)轉(zhuǎn)矩τ1，τ2，τ3和τ4，分別為腰部、水平滑塊和垂直滑塊的電機(jī)輸出扭矩，機(jī)器人的非線性動(dòng)力學(xué)方程可表示為矩陣形式：

(10)

3 自適應(yīng)模糊PID控制

3.1 參數(shù)設(shè)置

自適應(yīng)模糊PID控制器的推導(dǎo)過程是以自適應(yīng)模糊規(guī)則表為基礎(chǔ)進(jìn)行的，PID的參數(shù)可在線調(diào)整，通過對(duì)預(yù)先保存的模糊控制規(guī)則表的查詢可以改變參數(shù)[13]。傳統(tǒng)的PID控制器可描述為：

(11)

式中：

P、I、D——自適應(yīng)模糊PID控制器的比例增益、積分增益、導(dǎo)數(shù)增益參數(shù)；

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

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

為使被控對(duì)象達(dá)到良好的動(dòng)靜態(tài)性能，采用的PID控制器設(shè)計(jì)算法是以e和de(t)/dt等參數(shù)為基礎(chǔ)，通過計(jì)算在線調(diào)整KP、KI和KD參數(shù)。

3.2 控制器設(shè)計(jì)過程

(1)輸入和輸出變量：ec、e為輸入?yún)?shù)，而KP、KI和KD為輸出參數(shù)。

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

圖4 e和ec的模糊隸屬函數(shù)

圖5 KP，KI和KD的模糊隸屬函數(shù)

(3)PID參數(shù)調(diào)節(jié)：根據(jù)其模糊規(guī)則表進(jìn)行調(diào)節(jié)，實(shí)際控制值是精確值和量子因子的乘積。

3.3 準(zhǔn)確率

在數(shù)據(jù)集中分別按總量的1/4、1/3、1/2和3/4作為測(cè)試數(shù)據(jù)集，并按式(13)、(14)分別計(jì)算檢測(cè)準(zhǔn)確率和誤報(bào)率。

(12)

式中：

A——準(zhǔn)確分類數(shù)據(jù)；

B——測(cè)試數(shù)據(jù)集容量；

C——檢測(cè)率，%。

(13)

式中：

M——誤檢為其他類型的數(shù)據(jù)；

K——測(cè)試數(shù)據(jù)集容量；

L——誤報(bào)率，%。

4 仿真

為驗(yàn)證試驗(yàn)設(shè)計(jì)的控制器對(duì)堆垛機(jī)的有效性，利用Matlab軟件對(duì)其進(jìn)行仿真。由圖6可知，當(dāng)腰部輸入角度為1 rad，上升時(shí)間為50 ms，調(diào)節(jié)時(shí)間為10 ms時(shí)，穩(wěn)態(tài)誤差為1.5%，說明自適應(yīng)模糊PID控制在非線性系統(tǒng)響應(yīng)方面比傳統(tǒng)的PID控制更具優(yōu)勢(shì)，并且不存在過沖現(xiàn)象。因此，控制器可滿足碼垛機(jī)系統(tǒng)的性能要求。

圖6 傳統(tǒng)PID控制器與自適應(yīng)模糊PID控制器的比較

機(jī)器人負(fù)載的變化導(dǎo)致控制器需較強(qiáng)的魯棒性。由圖7可知，機(jī)器人在10 N·m余弦信號(hào)的干擾下仍可正常工作，因此控制器具有堅(jiān)固的魯棒性。

圖7 帶有干擾信號(hào)的系統(tǒng)的輸出響應(yīng)

5 結(jié)論

采用拉格朗日動(dòng)力學(xué)方程分析了碼垛機(jī)器人的動(dòng)力學(xué)性能，建立了逆動(dòng)力學(xué)模型。采用自適應(yīng)模糊PID控制算法設(shè)計(jì)碼垛機(jī)器人的控制器，相對(duì)于傳統(tǒng)的PID控制算法更具優(yōu)勢(shì)，可在很大程度上消除對(duì)沖，具有較強(qiáng)的魯棒性。試驗(yàn)設(shè)計(jì)的碼垛機(jī)器人可有效地滿足食品物料搬運(yùn)的高速、重載要求，機(jī)構(gòu)的動(dòng)態(tài)性能得到大大改善。后續(xù)可研究控制收斂去速度。