趙 興，贠 超，王 剛，朱鴻泰

(北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191)

碼垛機(jī)器人動(dòng)力學(xué)與控制分析*

趙 興，贠 超，王 剛，朱鴻泰

(北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191)

MDJ90碼垛機(jī)器人是一種用于完成生產(chǎn)線上物料搬運(yùn)和碼放的機(jī)器人。通過運(yùn)用機(jī)器人的相關(guān)理論，對(duì)該機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)分析，并總結(jié)分析了該機(jī)器人的特點(diǎn)。在建立了基于拉格朗日方程的動(dòng)力學(xué)模型基礎(chǔ)上，對(duì)機(jī)器人采用了實(shí)時(shí)重力補(bǔ)償?shù)目刂品桨? 通過仿真實(shí)驗(yàn)表明了該方案的有效性。目前，相應(yīng)的技術(shù)已經(jīng)應(yīng)用于機(jī)器人的現(xiàn)場(chǎng)作業(yè)中。

碼垛機(jī)器人 動(dòng)力學(xué)模型 實(shí)時(shí)重力補(bǔ)償

0 引 言

隨著工業(yè)自動(dòng)化的發(fā)展，工廠的物流技術(shù)有了顯著地提高，在物流大潮下，碼垛技術(shù)便成為物流自動(dòng)化技術(shù)領(lǐng)域的一門重要技術(shù)[1]。以此背景，碼垛機(jī)器人以其在機(jī)械結(jié)構(gòu)、適用范圍、靈活性、成本以及維護(hù)等方面的優(yōu)勢(shì)，使其應(yīng)用漸為廣泛, 并成為一種發(fā)展趨勢(shì)。

為了保證碼垛機(jī)器人能平穩(wěn)高速準(zhǔn)確地運(yùn)行，完成既定的工作要求，必須對(duì)機(jī)器人進(jìn)行有效的控制。對(duì)機(jī)器人來說，控制的目的是要控制機(jī)器人手端的位置和姿態(tài)，即所謂位置控制問題。期望位置或軌跡可在機(jī)器人任務(wù)空間中給出，也可通過逆運(yùn)動(dòng)學(xué)關(guān)系轉(zhuǎn)化為機(jī)器人關(guān)節(jié)空間中的期望軌跡，通常包括兩種形式：一種是一個(gè)固定位置(定點(diǎn))；另一種是一條隨時(shí)間變化的軌跡[2]。此課題研究的碼垛機(jī)器人在工作中只對(duì)抓取點(diǎn)和放置點(diǎn)有嚴(yán)格的位置要求，屬于定點(diǎn)控制問題；但由于機(jī)器人負(fù)載重慣量大，為了保證運(yùn)動(dòng)的平穩(wěn)流暢，則需可慮機(jī)器人的運(yùn)動(dòng)軌跡，即軌跡控制問題。

1 機(jī)器人動(dòng)力學(xué)模型

為了對(duì)機(jī)器人進(jìn)行有效的動(dòng)態(tài)控制，首先需要建立其力學(xué)模型并進(jìn)行性能分析。

1.1 碼垛機(jī)器人的結(jié)構(gòu)設(shè)計(jì)

碼垛機(jī)器人MDJ90為四驅(qū)動(dòng)關(guān)節(jié)，串并混聯(lián)式結(jié)構(gòu)，如圖1所示。其中腰部關(guān)節(jié)實(shí)現(xiàn)底座1與腰部框架2的相對(duì)轉(zhuǎn)動(dòng)，水平滑塊3與垂直滑塊4分別實(shí)現(xiàn)機(jī)器人末端的前后與上下運(yùn)動(dòng)，腕部關(guān)節(jié)實(shí)現(xiàn)腕部框架10與抓手11的相對(duì)轉(zhuǎn)動(dòng)，同時(shí)利用輔助平行四邊形結(jié)構(gòu)8使機(jī)器人手端始終保持水平。

圖1 碼垛機(jī)器人簡(jiǎn)圖1.基座 2.腰部框架 3.水平滑塊 4.豎直滑塊 5.大臂連桿 6.后大臂 7.前大臂 8.輔助平行四邊形機(jī)構(gòu) 9.小臂 10.腕部框架 11.抓手

1.2 機(jī)器人運(yùn)動(dòng)學(xué)分析

機(jī)器人采用4臺(tái)交流伺服電機(jī)進(jìn)行驅(qū)動(dòng)如圖2所示，其中腰部關(guān)節(jié)θ0和腕部關(guān)節(jié)θ3的回轉(zhuǎn)決定了所搬運(yùn)物料在水平面內(nèi)的位置和姿態(tài)，垂直滑塊和水平滑塊運(yùn)動(dòng)帶動(dòng)臂桿的運(yùn)動(dòng)確定了物料在垂直面內(nèi)的位置，將兩部分分開進(jìn)行討論。

依次定義各臂長(zhǎng)度:前大臂為l1，大臂連桿為l2，后大臂為l3，小臂為l4

首先分析臂部平行四桿機(jī)構(gòu)的運(yùn)動(dòng)關(guān)系，隨著垂直滑塊和水平滑塊的移動(dòng)，當(dāng)大臂和小臂分別從初始位置轉(zhuǎn)動(dòng)θ1和θ2后，這時(shí)根據(jù)幾何關(guān)系可得腕部的位移ΔXp與ΔZp為：

相應(yīng)的，也可得水平滑塊的位移ΔX和垂直滑塊

的位移ΔZ為：

如平行四邊形機(jī)構(gòu)滿足如下比例關(guān)系：

則有：

圖2 碼垛機(jī)器人臂桿機(jī)構(gòu)運(yùn)動(dòng)圖

這樣，可知該機(jī)器人在垂直面內(nèi)的運(yùn)動(dòng)上實(shí)現(xiàn)了解耦，為機(jī)器人末端軌跡的規(guī)劃提供了便利，為機(jī)器人的軌跡控制提供了基礎(chǔ)。

1.3 機(jī)器人動(dòng)力學(xué)分析

(1) 廣義坐標(biāo)和廣義力

由前面分析，大臂擺動(dòng)角度取為θ1，小臂擺動(dòng)角度為θ2，系統(tǒng)廣義坐標(biāo)即可描述該二自由度系統(tǒng)[3]。

該機(jī)器人的非保守力有

關(guān)節(jié)力矩為：

τ=[τ1,τ2]T

式中：τ1為驅(qū)動(dòng)水平移動(dòng)關(guān)節(jié)的電機(jī)輸出力矩，τ2為驅(qū)動(dòng)垂直移動(dòng)關(guān)節(jié)的電機(jī)輸出力矩。

τ對(duì)機(jī)器人的總需功為：

δW=τTδq=τ1δX+τ2δZ

根據(jù)幾何關(guān)系有：

整理得，系統(tǒng)的廣義力為：

(2) 四桿機(jī)構(gòu)的拉格朗日方程

根據(jù)系統(tǒng)的拉格朗日方程：

式中：L=T-U

首先應(yīng)分別求出每根桿件的動(dòng)能Ti和勢(shì)能Vi。

對(duì)于前大臂(桿1)：

V1=m1gl1ccosθ1

對(duì)于大臂連桿(桿2)：

V2=m2gl2ccosθ2

對(duì)于后大臂(桿3)：

當(dāng)桿2靜止，桿3隨桿1轉(zhuǎn)動(dòng)時(shí)有：

當(dāng)桿1靜止，桿3隨桿2端點(diǎn)平移時(shí)有：

整理計(jì)算,得桿3的動(dòng)能與勢(shì)能為：

V3=(l2cosθ2+l3ccosθ1)m3g

同理，對(duì)于小臂(桿4)：

V4=(l1cosθ1-l4ccosθ2)m4g

設(shè)末端執(zhí)行機(jī)構(gòu)和負(fù)載m0，由于始終隨小臂移動(dòng)，與其有相近的動(dòng)能和勢(shì)能表達(dá)式：

V0=(l1cosθ1-l4ccosθ2)m0g

拉格朗日函數(shù)為：

L=T1+T2+T3+T4+T0-V1-V2-V3-T4-T0

代入拉格朗日方程，可得到系統(tǒng)的動(dòng)力學(xué)方程:

其中：

H12=(m3l3cl2-m4l4cl1-m0l4l1)cos (θ1-θ2)

H21=(m3l3cl2-m4l4cl1-m0l4l1)cos (θ1-θ2)

C12=(m3l3cl2-m4l4cl1-m0l4cl1)sin (θ1-θ2)

C21=(m3l3cl2-m4l4cl1-m0l4cl1)sin (θ1-θ2)

G11=(m1l1c+m3l3c+m4l1+m0l1)gsinθ1

G21=(m2l2c+m3l2-m4l4c-m0l4)gsinθ2

通過分析可清楚地看出，系統(tǒng)中所有由哥氏力引起的慣性力消失了，因而沒有哥氏加速度的產(chǎn)生，只有慣性力、向心力和重力加速度，這樣機(jī)構(gòu)中臂桿之間的相對(duì)運(yùn)動(dòng)關(guān)系就變得相對(duì)簡(jiǎn)單，能夠改善機(jī)器人的動(dòng)態(tài)特性。

另外，在水平面內(nèi)，各臂桿僅隨腰座作回轉(zhuǎn)運(yùn)動(dòng)，并將各構(gòu)件看作集中質(zhì)量的剛性元件，因此，腰座回轉(zhuǎn)的廣義驅(qū)動(dòng)力矩為：

2 碼垛機(jī)器人控制分析

當(dāng)忽略重力和外加干擾時(shí)，采用獨(dú)立的PD控制[4]，能夠滿足機(jī)器人定點(diǎn)控制的要求，其控制方程為：

計(jì)算上式結(jié)果，整理得：

H(q)=

由于本碼垛機(jī)器人慣量大、負(fù)載重，為了完整地描述機(jī)器人的動(dòng)態(tài)特性則必須考慮重力，對(duì)機(jī)器人采用在線估計(jì)動(dòng)力補(bǔ)償[5]，此時(shí)控制方程變?yōu)椋?/p>

整理上式，可得：

而且在實(shí)際中，由于動(dòng)力學(xué)模型不可能絕對(duì)準(zhǔn)確，并且系統(tǒng)中還存在干擾和噪聲，僅采用開環(huán)控制是不實(shí)用的。構(gòu)造高性能控制系統(tǒng)的可行方法是實(shí)時(shí)反饋控制，用關(guān)節(jié)傳感器組成閉環(huán)系統(tǒng)，計(jì)算伺服誤差，這時(shí)控制系框圖如圖3所示。

圖3 機(jī)器人控制系統(tǒng)框圖

可看出，該系統(tǒng)屬于引入了進(jìn)行實(shí)時(shí)重力補(bǔ)償?shù)膬?nèi)控制回路，屬于考慮的機(jī)器人動(dòng)力學(xué)性質(zhì)的動(dòng)態(tài)控制方案[6]。

此時(shí)控制方程為：

取運(yùn)動(dòng)過程中機(jī)器人某典型位置，θ1=30°，θ2=30°。

根據(jù)上述控制方程，進(jìn)行計(jì)算機(jī)仿真，同時(shí)分別調(diào)整各環(huán)節(jié)Kd與Kp，得到相應(yīng)的階躍響應(yīng)曲線，如圖4所示。

圖4 控制系統(tǒng)階躍響應(yīng)曲線

從仿真結(jié)果可看到，采用本控制方案可得到較好的系統(tǒng)穩(wěn)定性和瞬態(tài)響應(yīng)。但是，為了得到較為理想的控制曲線，一方面要進(jìn)行動(dòng)力學(xué)的實(shí)時(shí)解算，另一方面也要根據(jù)機(jī)器人的運(yùn)行狀況，及時(shí)調(diào)整各環(huán)節(jié)的PID參數(shù)，對(duì)控制軟件的算法編寫提出了要求。

3 結(jié) 語

根據(jù)機(jī)器人理論分析了碼垛機(jī)器人的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)，然后根據(jù)建立的力學(xué)模型，分析了該機(jī)器人的運(yùn)動(dòng)特點(diǎn)與力學(xué)性能。在此基礎(chǔ)上，分析了采取實(shí)時(shí)重力補(bǔ)償?shù)目刂品桨?，并進(jìn)行了計(jì)算機(jī)仿真，得到該方案可滿足機(jī)器人的控制要求。同時(shí)，也對(duì)課題研究接下來的詳細(xì)設(shè)計(jì)提供了參考，對(duì)控制器參數(shù)的調(diào)節(jié)與上下位機(jī)軟件的算法編寫等工作提出了基礎(chǔ)。

[1] 胡洪國，高建華.碼垛技術(shù)綜述[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2000(6)：7-9.

[2] 熊有倫.機(jī)器人學(xué)[M].武漢：華中科技大學(xué)出版社，1996.

[3] 費(fèi)仁元，張慧慧.機(jī)器人機(jī)械設(shè)計(jì)和分析[M].北京：北京工業(yè)大學(xué)出版社，1998.

[4] Craig J. Introduction to Robotics-Mechanics and Control[M].Reading, Assachusetts: Addison - Wesley Publishing Company, 1986.

[5] 丁學(xué)恭.機(jī)器人控制研究[M].杭州：浙江大學(xué)出版社，2006.

[6] 霍 偉.機(jī)器人動(dòng)力學(xué)與控制[M].北京：高等教育出版社，2005.

Dynamics and Control Analysis of the Palletizer Robot

ZHAO Xing，YUN Chao，WANG Gang，ZHU Hong-tai

(CollegeofMechanicalEngineeringandAutomation，BeihangUniversity，Beijing100191，China)

The MDJ90 palletizer robot is used to transfer and palletize cargoes on the production line. Using robotics theory, the kinematics and dynamics of the robot are analyzed and summarized some feature. By the dynamics model based on Lagrange Equation, we take the control project of real-time compensation for the gravity and prove the validity of it by computer simulation. Now this technology has been applied to fieldwork of the palletizer robot.

palletizer robot；dynamics model；real-time compensation for the gravity

2014-03-10

趙 興(1987-)，男，遼寧沈陽人，碩士，主要從事工業(yè)機(jī)器人結(jié)構(gòu)設(shè)計(jì)與控制技術(shù)方面的研究工作。

TP242

A

1007-4414(2014)02-0008-03