胡文靜，甘　戈，于吉鯤，姜芳芳

(1.大連海洋大學(xué) 應(yīng)用技術(shù)學(xué)院，遼寧 大連　116300；2.沈陽(yáng)新松機(jī)器人自動(dòng)化股份有限公司，沈陽(yáng)　110168；3.中國(guó)重型機(jī)械研究院股份有限公司，西安　710032)

?

真空手的插值算法與動(dòng)力學(xué)建模研究*

胡文靜1，甘戈2，于吉鯤1，姜芳芳3

(1.大連海洋大學(xué) 應(yīng)用技術(shù)學(xué)院，遼寧 大連116300；2.沈陽(yáng)新松機(jī)器人自動(dòng)化股份有限公司，沈陽(yáng)110168；3.中國(guó)重型機(jī)械研究院股份有限公司，西安710032)

摘要：文章針對(duì)真空手需要在高速運(yùn)動(dòng)的條件下需要保持手臂穩(wěn)定的問(wèn)題。首先利用MDH法建立了坐標(biāo)系，得到了各關(guān)節(jié)連桿的相對(duì)關(guān)系得到了正、反運(yùn)動(dòng)學(xué)描述。其次，討論了三次樣條插值在機(jī)器人路徑規(guī)劃中的應(yīng)用，利用該算法對(duì)示教路徑進(jìn)行插值以消除了機(jī)器人運(yùn)動(dòng)過(guò)程中的不連續(xù)現(xiàn)象。再次用Newton-Euler進(jìn)行多次迭代計(jì)算出力矩的數(shù)值方程求解，最后利用仿真工具軟件對(duì)真空手機(jī)器人力矩進(jìn)行了仿真，驗(yàn)證了真空手運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的正確性。實(shí)驗(yàn)表明，運(yùn)動(dòng)學(xué)可以讓機(jī)器人準(zhǔn)確的進(jìn)行運(yùn)動(dòng)，動(dòng)力學(xué)和插值算法可以有效的增加真空手機(jī)器人的運(yùn)動(dòng)性能和穩(wěn)定性。

關(guān)鍵詞：真空機(jī)械手；三次樣條插值；動(dòng)力學(xué)；牛頓-歐拉

0引言

真空手在晶圓加工過(guò)程中承擔(dān)著定位和傳輸晶圓到指定腔室兩項(xiàng)關(guān)鍵任務(wù)，是IC裝備的核心關(guān)鍵技術(shù)之一[1]。因?yàn)榧庸ぞA的工作環(huán)境和生產(chǎn)加工的工序要求,機(jī)器人持有晶圓在高速的條件下運(yùn)行時(shí)必須保持手臂穩(wěn)定，避免由于加速度的原因而造成硅片的飛出損壞，從而影響作業(yè)的正常執(zhí)行和生產(chǎn)率，導(dǎo)致真空設(shè)備的潔凈度下降報(bào)廢率增加。鑒于此，單臂真空手機(jī)器人要求特別苛刻，特別是清潔度、控制精度、運(yùn)動(dòng)性能、穩(wěn)定性和可靠性。因此，對(duì)加工晶圓傳輸?shù)臋C(jī)械手研究其運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)等其關(guān)鍵技術(shù)是對(duì)IC產(chǎn)業(yè)的蓬勃發(fā)展的重要保證。

近年來(lái)，動(dòng)力學(xué)技術(shù)已經(jīng)從理論階段逐步走向應(yīng)用階段。Degani A等研發(fā)了一種基于動(dòng)力學(xué)單傳動(dòng)裝置垂直攀爬機(jī)器人[2]。Swevers J等提出了一種工業(yè)機(jī)器人的動(dòng)力學(xué)模型識(shí)別[3]。Zomaya A Y研究了在機(jī)器人動(dòng)態(tài)模型:理論和應(yīng)用程序中提取和計(jì)算參數(shù)識(shí)別的問(wèn)題[4]。Dongsun Kim等提出了一種管理家庭服務(wù)機(jī)器人軟件的應(yīng)用動(dòng)力學(xué)軟件架構(gòu)[5]。Sung Jin Yoo等研究了利用柔性關(guān)節(jié)機(jī)器人的神經(jīng)網(wǎng)絡(luò)反饋控制自適應(yīng)輸出：動(dòng)力學(xué)表面設(shè)計(jì)方法[6]。

本文首先將單臂機(jī)械手分為上升軸、旋轉(zhuǎn)軸和伸縮軸三個(gè)部分。然后詳盡介紹了單臂真空手的運(yùn)動(dòng)學(xué)模型、插值算法和動(dòng)力學(xué)模型，經(jīng)過(guò)仿真試驗(yàn)表明，動(dòng)力學(xué)和插值算法的正確性[6]。

1真空手運(yùn)動(dòng)學(xué)

如圖1所示，為真空手坐標(biāo)系分布，機(jī)器人的MDH參數(shù)如表1所示[8]。

圖1　真空手機(jī)械手連桿坐標(biāo)系關(guān)系圖

通常把描述一個(gè)連桿和下一個(gè)連桿間相對(duì)關(guān)系的齊次變換叫做T矩陣[9]。本文首先采用MDH方法建立坐標(biāo)系，進(jìn)而用齊次變換矩陣來(lái)建立機(jī)器人末端執(zhí)行器坐標(biāo)系相對(duì)于基坐標(biāo)系的數(shù)學(xué)模型[10]。根據(jù)以上這些參數(shù)可以得到表1桿件坐標(biāo)系之間的關(guān)系，同時(shí)獲得各個(gè)關(guān)節(jié)之間的變換矩陣。

表1　真空手機(jī)器人的MDH桿件坐標(biāo)系

1.1機(jī)械臂幾何法運(yùn)動(dòng)學(xué)正解

本文提出了一種幾何法求解運(yùn)動(dòng)學(xué)方程，真空手運(yùn)動(dòng)學(xué)正解如下：

(1)

1.2機(jī)械臂運(yùn)動(dòng)學(xué)反解

機(jī)械臂運(yùn)動(dòng)學(xué)反解：根據(jù)真空手機(jī)的位置、姿態(tài)和連桿長(zhǎng)度，求解出機(jī)械臂各個(gè)軸應(yīng)該旋轉(zhuǎn)多少角度，以滿足已知的位置和姿態(tài)[11]。

通過(guò)逆運(yùn)動(dòng)求解得各個(gè)關(guān)節(jié)角度，可以實(shí)現(xiàn)對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析、編程和軌跡規(guī)劃等工作。用逆矩陣求解關(guān)節(jié)角度雖然看起來(lái)簡(jiǎn)單但是計(jì)算過(guò)程復(fù)雜[12]。本文根據(jù)MDH構(gòu)建的運(yùn)動(dòng)學(xué)方程，使用常規(guī)的方法來(lái)求解機(jī)器人位置值到機(jī)器人管家值的轉(zhuǎn)換，通過(guò)推到可以得到：

(2)

1.3運(yùn)動(dòng)學(xué)驗(yàn)證

機(jī)器人末端軌跡為一螺旋線，根據(jù)運(yùn)動(dòng)學(xué)逆解公式計(jì)算出各關(guān)節(jié)角度值。將上面螺旋線逆解求取出的一系列關(guān)節(jié)值代入正解運(yùn)算公式中，求解出對(duì)應(yīng)的一系列正解值，將正解值與上面規(guī)劃的螺旋線對(duì)比如下圖2所示。

圖2　運(yùn)動(dòng)學(xué)正反解驗(yàn)證

圖中藍(lán)色曲線代表規(guī)劃的螺旋線，紅色曲線代表規(guī)劃螺旋線經(jīng)一次反解→正解后計(jì)算得到的螺旋線，從圖中可看出兩條曲線處于重合狀態(tài)，證明了機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算的正確性。

2三次樣條插值

在數(shù)學(xué)上，光滑程度的定量描述就是函數(shù)的k階導(dǎo)數(shù)存在且連續(xù)，函數(shù)曲線具有k階光滑性，滿足的階數(shù)越高，就表現(xiàn)為光滑性越好[8]。分段線性插值具有零階光滑性，分段Hermite插值具有一階光滑性，三次樣條插值則具有二階光滑性[9]。

2.1三次樣條插值的原理

因此，由天然的約束條件可給出3(k-1)+k+1=4k-2個(gè)方程。而k個(gè)分段共有k個(gè)三次方程，共需4k個(gè)約束條件。

樣條本質(zhì)上是多段三次多項(xiàng)式拼合而成的曲線，在拼接處連續(xù)，且一階和二階導(dǎo)數(shù)均連續(xù)[11]。樣條最初是基于彈性條設(shè)計(jì)的工具，這是因?yàn)闃訔l函數(shù)是受到k+1個(gè)點(diǎn)約束的彈性條的最小能量形狀，而彈性條總是傾向于穩(wěn)定在能量最小的位置。

2.2三次樣條插值的實(shí)現(xiàn)

其中hi=xi+1-xi，i=0,1,…,k-1。積分兩次，再利用插值條件確定積分常數(shù)，可求得樣條函數(shù)如下：

(3)

鑒于此，借助MATLAB圖形工具箱，實(shí)現(xiàn)三次樣條插值的樣條曲線，得到圖3和圖4的曲線示圖，分別對(duì)應(yīng)最近鄰插值、分段線性插值和三次樣條插值的對(duì)比效果曲線[5]。從圖4可以看出，三次樣條插值曲線較光滑，保持了兩插值節(jié)點(diǎn)間的單調(diào)性。

圖3　最近鄰和分段線性插值

圖4　三次樣條插值

3真空手的動(dòng)力學(xué)建模

在控制精度和動(dòng)態(tài)控制指標(biāo)的要求下，傳統(tǒng)的控制效果很差，難以滿足較高的性能指標(biāo)的晶圓傳送需求[9]。要實(shí)現(xiàn)優(yōu)異的運(yùn)動(dòng)效果，必須要對(duì)機(jī)器人本機(jī)進(jìn)行動(dòng)力學(xué)分析[10-11]。鑒于此，引入動(dòng)力學(xué)的控制是一條有效的途徑。采用力矩前饋控制(動(dòng)力學(xué)前饋補(bǔ)償)法，可效的改善機(jī)器人的性能[12-13]。

在真空手傳送晶圓的工作過(guò)程中，升降運(yùn)動(dòng)、旋轉(zhuǎn)運(yùn)動(dòng)和水平伸縮運(yùn)動(dòng)都是單獨(dú)進(jìn)行運(yùn)動(dòng)的，也就是說(shuō)其中一個(gè)運(yùn)動(dòng)模塊運(yùn)動(dòng)時(shí)，其它模塊是保持不運(yùn)動(dòng)的。運(yùn)用牛頓-歐拉動(dòng)力學(xué)將上升、旋轉(zhuǎn)和伸縮進(jìn)行了動(dòng)力學(xué)的三個(gè)部分解耦，可以有效的簡(jiǎn)化動(dòng)力學(xué)方程的復(fù)雜度[9-10]。下面本文將從三個(gè)方面來(lái)分別計(jì)算動(dòng)力學(xué)的方程。

3.1機(jī)械參數(shù)

如表2所示，為真空手的實(shí)際機(jī)械參數(shù)。

表2　真空手機(jī)器人的機(jī)械參數(shù)

3.2牛頓-歐拉動(dòng)力學(xué)

牛頓-歐拉外推法

外推：i：0~5

(4)

牛頓-歐拉內(nèi)推法

內(nèi)推：i：6~1

(5)

3.3升降軸動(dòng)力學(xué)

升降軸運(yùn)動(dòng)：當(dāng)單臂SCAR機(jī)器人進(jìn)行升降運(yùn)動(dòng)時(shí)，旋轉(zhuǎn)和伸縮是不運(yùn)動(dòng)的。一般來(lái)說(shuō)對(duì)于升降軸，使用公式(5)計(jì)算動(dòng)力學(xué)：

(6)

3.4旋轉(zhuǎn)軸動(dòng)力學(xué)

一般來(lái)說(shuō)對(duì)于旋轉(zhuǎn)軸，使用如下公式(7)計(jì)算動(dòng)力學(xué)：

(7)

其中c包含靜摩擦力和克服真空隔離裝置的摩擦力系數(shù)，τ為旋轉(zhuǎn)關(guān)節(jié)電機(jī)提供的扭矩。

3.5伸縮軸動(dòng)力學(xué)(封閉形式)

牛頓-歐拉動(dòng)力學(xué)計(jì)算包括向外迭代和向內(nèi)迭代量部分：

外推i(2～4)：

對(duì)連桿2用向外迭代法求解如下：

(8)

對(duì)連桿3用向外迭代法求解如下：

(9)

對(duì)連桿4用向外迭代法求解如下：

(10)

內(nèi)推i(5～3)：

對(duì)連桿5用向外迭代法求解如下：

(11)

對(duì)連桿4用向外迭代法求解如下：

(12)

對(duì)連桿3用向外迭代法求解如下：

(13)

4計(jì)算結(jié)果及分析

首先規(guī)劃一段有的位置曲線值作為算法的輸入端，然后將動(dòng)力學(xué)計(jì)算值作為算法的輸出端。最后對(duì)各個(gè)軸的動(dòng)力學(xué)數(shù)值數(shù)據(jù)進(jìn)行采集和統(tǒng)計(jì)，如圖5所示，為牛頓-歐拉動(dòng)力學(xué)計(jì)算動(dòng)力學(xué)數(shù)值曲線，如圖6所示，為MATLAB機(jī)器人工具箱動(dòng)力學(xué)數(shù)值曲線。經(jīng)過(guò)對(duì)比分析，曲線一致。

經(jīng)過(guò)將牛頓-歐拉法計(jì)算得到的動(dòng)力學(xué)數(shù)值與MATLAB機(jī)器人工具箱中的動(dòng)力學(xué)算法得到的各個(gè)軸動(dòng)力數(shù)值相互比較，說(shuō)明該算法適用于真空機(jī)械手，并且可以推廣到各類(lèi)機(jī)器人當(dāng)中去，從而驗(yàn)證了算該算法的結(jié)果可靠性、再現(xiàn)性和普遍性。

圖5　牛頓-歐拉動(dòng)力學(xué)計(jì)算動(dòng)力學(xué)數(shù)值曲線

圖6　MATLAB機(jī)器人工具箱動(dòng)力學(xué)數(shù)值曲線

5結(jié)束語(yǔ)

本文針對(duì)真空手機(jī)器人必須在高速的條件運(yùn)行的下保持手臂穩(wěn)定的問(wèn)題，詳盡的介紹了運(yùn)動(dòng)學(xué)建模以及路徑曲線插值的過(guò)程。驗(yàn)證了機(jī)械臂數(shù)學(xué)模型以及正、逆運(yùn)動(dòng)學(xué)分析的正確性，同時(shí)與最近鄰插值相比，三次樣條插值能較好地保留原函數(shù)信息，插值函數(shù)比較光滑。將其動(dòng)力學(xué)技術(shù)應(yīng)用到該機(jī)器人當(dāng)中去。同時(shí)采用力矩前饋控制(動(dòng)力學(xué)前饋補(bǔ)償)法，對(duì)機(jī)械手的動(dòng)力學(xué)進(jìn)行了仿真求解。仿真實(shí)驗(yàn)表明，運(yùn)動(dòng)學(xué)、插值算法赫爾動(dòng)力學(xué)的正確性。

[參考文獻(xiàn)]

[1]趙彬,劉振宇,鄒風(fēng)山,等.單臂SCARA手的動(dòng)力學(xué)建模與參數(shù)辨識(shí)[J].機(jī)械設(shè)計(jì)與制造,2016,3(6):153-165.

[2] Degani A, ShapiroA, Choset H, et al. A dynamic single actuator vertical climbing robot[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems, 2007:2901-2906.

[3] Swevers J, Verdonck W, De Schutter J. Dynamic Model Identification for Industrial Robots[J]. IEEE Control Systems, 2007, 27(5): 58-71.

[4] Zomaya A Y. Extraction and computation of identifiable parameters in robot dynamic models: theory and application[J]. Control Theory and Applications, 2002, 141(1): 48-56.

[5] Dongsun Kim, Sooyong Park, Mun-Taek Choi, et al. Applying Dynamic Software Architecture Management to Home Service Robot Software[C]. The 16th IEEE International Symposium on Robot and Human interactive Communication, 2007: 285-290.

[6] Sung Jin Yoo, Jin Bae Park, Yoon Ho Choi. Adaptive Output Feedback Control of Flexible-Joint Robots Using Neural Networks: Dynamic Surface Design Approach[J]. IEEE Transactions on Neural Networks, 2008, 19(10): 1712-1726.

第3期

[7] Zhang Bin, Xiong Rong, Wu Jun. Kinematics analysis of a novel 7-DOF humanoid manipulator for table tennis[C].Electronics, Communications and Control (ICECC), 2011, 33(10):9-11.

[8] 趙彬. 基于機(jī)器視覺(jué)的工業(yè)機(jī)器人分揀技術(shù)研究[D]. 沈陽(yáng)：沈陽(yáng)工業(yè)大學(xué)，2013.

[9] Zhao Dingxuan, Wei Hailong, Zhang Hongyan. Explicit solution for inverse kinematics of 3-RPS parallel link manipulator[C]. Computer, Mechatronics, Control and Electronic Engineering (CMCE), 2010 (2):425-429.

[10] Zhao Jia-xin, Luo Qing-sheng, Mo Yang, et al. The analysis of kinematics and gait planning of a segmental appendage octopod robot[J]. Machinery Design & Manufacture, 2013 (10): 171-174.

[11] Nandy S, Shome S N, Chakraborty G. A modular approach to detailed dynamic formulation and control of wheeled mobile robot[C]. 2011 International Conference on Mechatronics and Automation, 2011:1471-1478.

[12] Jaeoh Lee, Seongik Han, Jangmyung Lee. Decoupled Dynamic Control for Pitch and Roll Axes of the Unicycle Robot[J]. IEEE Transactions on Industrial Electronics, 2012, 60(9):3814-3822.

[13] 鄭澤鈿,陳銀清,林文強(qiáng),等. 工業(yè)機(jī)器人上下料技術(shù)及數(shù)控車(chē)床加工技術(shù)組合應(yīng)用研究[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2013(7):105-109.

[14] 石磊,金忠全. 四自由度機(jī)器人的建模和仿真[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2012(3):37-39.

(編輯李秀敏)

Research Interpolation Algorithm and Dynamics Modeling of Single Vacuum Manipulator

HU Wen-jing1,GAN Ge2,YU Ji-kun1,JIANG Fang-fang3

(1.College of Applied Technology, Dalian Marine University, Dalian Liaoning 116300, China;2.Siasun Robot & Automation Co., Ltd, Shenyang 110168, China)

Abstract：Aimed at vacuum manipulator need to maintain stability of arms problem under the condition of high speed movement. Firstly, use MDH method to establish coordinate system, and obtain the joints connecting rod relative relationship of positive and inverse kinematics description. Then, bicubic interpolation is discussed in robot path planning, and the application of algorithm is used to analyze the teaching path interpolation to eliminate the discontinuous phenomenon in the process of robot movement. Finally，calculate the dynamic equation with Lagrange method. Using the Matlab robot toolbox executes kinematics and dynamics simulation experiment. The experimental results show that the kinematic algorithm, interpolation and dynamic algorithm has good robustness for the precise motion of robot and dynamic control has important significance.

Key words：vacuum manipulator;cubic spline interpolation;dynamic;Newton-Euler

中圖分類(lèi)號(hào)：TH166;TG506

文獻(xiàn)標(biāo)識(shí)碼：A

作者簡(jiǎn)介：胡文靜(1981—)，女，武漢人，大連海洋大學(xué)講師，碩士， 研究方向?yàn)闄C(jī)器人及自動(dòng)化，(E-mail) huwenjingAa@126.com。

*基金項(xiàng)目：國(guó)家863計(jì)劃(2012AA041405) ；國(guó)家自然科學(xué)基金(11126028)

收稿日期：2015-09-01；修回日期：2015-10-09

文章編號(hào)：1001-2265(2016)03-0029-04

DOI:10.13462/j.cnki.mmtamt.2016.03.008