趙 罡，張 佳，肖文磊

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

0 引言

目前，高精密運(yùn)動(dòng)平臺(tái)主要采用超聲電機(jī)驅(qū)動(dòng)方案，來實(shí)現(xiàn)亞微米或納米級(jí)的定位精度，國內(nèi)外對(duì)其研究已經(jīng)取得一定成果[1-3]。采用直線超聲電機(jī)驅(qū)動(dòng)的Tripod精密平臺(tái)是由Hunt[4]提出的3-RPS少自由度并聯(lián)機(jī)構(gòu)發(fā)展而來的混聯(lián)機(jī)構(gòu)，通過3-RPS并聯(lián)機(jī)構(gòu)實(shí)現(xiàn)Z軸、A軸及B軸的運(yùn)動(dòng)；通過串聯(lián)平臺(tái)實(shí)現(xiàn)X軸和Y軸的運(yùn)動(dòng)，使X軸與Y軸具有較大的運(yùn)動(dòng)范圍與運(yùn)動(dòng)速度。

動(dòng)力學(xué)建模是實(shí)施基于動(dòng)力學(xué)模型控制、提高控制精度的基礎(chǔ)。鄭魁敬[5]對(duì)3-RPS并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)和靜力學(xué)特性進(jìn)行分析；劉善曾[6]對(duì)3-RRS并聯(lián)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)分析，并基于Lagrange方程建立其動(dòng)力學(xué)模型；STAICUCAI[7]采用虛功原理和拉格朗日乘子法兩種方法對(duì)3-RPS機(jī)構(gòu)進(jìn)行逆動(dòng)力學(xué)分析；薛邦燦[8]根據(jù)拉格朗日方程建立三自由度并聯(lián)機(jī)器人的動(dòng)力學(xué)模型，并對(duì)其動(dòng)力學(xué)特性進(jìn)行仿真分析，為仿生肘關(guān)節(jié)的變剛度控制奠定理論基礎(chǔ)；賈凱凱[9]利用虛功原理建立了四自由度并聯(lián)機(jī)構(gòu)的完備剛體動(dòng)力學(xué)模型，并利用ADAMS校驗(yàn)該模型的正確性；李永剛[10]采用牛頓歐拉法對(duì)3-RPS機(jī)構(gòu)進(jìn)行了逆動(dòng)力學(xué)分析,求解機(jī)構(gòu)所需的驅(qū)動(dòng)力與約束力矩。

本文以控制精度高、響應(yīng)速度快的直線超聲電機(jī)驅(qū)動(dòng)的Tripod精密平臺(tái)為對(duì)象，針對(duì)其串并聯(lián)混合機(jī)構(gòu)的特點(diǎn)，建立動(dòng)力學(xué)模型，并基于PLC語言及C++混合編程開發(fā)控制系統(tǒng)，實(shí)現(xiàn)平臺(tái)按照預(yù)定軌跡運(yùn)動(dòng)，為Tripod精密平臺(tái)精密運(yùn)動(dòng)控制奠定基礎(chǔ)。

1 Tripod精密平臺(tái)簡(jiǎn)介及坐標(biāo)系建立

如圖1所示，Tripod精密平臺(tái)由精密二維平臺(tái)和Tripod并聯(lián)平臺(tái)兩部分組成，其中Tripod并聯(lián)平臺(tái)為3-RPS結(jié)構(gòu)，具有兩個(gè)旋轉(zhuǎn)自由度和一個(gè)平移自由度，精密二維平臺(tái)具有兩個(gè)平移自由度，實(shí)現(xiàn)X軸、Y軸、Z軸、A軸及B軸五軸聯(lián)動(dòng)。采用超聲電機(jī)直驅(qū)的方式驅(qū)動(dòng)負(fù)載，不經(jīng)過傳動(dòng)機(jī)構(gòu)，可以提高機(jī)械系統(tǒng)的傳動(dòng)剛度和傳動(dòng)精度。精密平臺(tái)內(nèi)部安裝有高精度的Micro-E光柵尺，以檢測(cè)運(yùn)動(dòng)平臺(tái)的當(dāng)前位置，以及作為伺服控制系統(tǒng)的反饋信號(hào)。

圖1 Tripod精密平臺(tái)

通過支持工業(yè)以太網(wǎng)EtherCAT總線的壓電伺服驅(qū)動(dòng)器，實(shí)現(xiàn)對(duì)超聲電機(jī)的網(wǎng)絡(luò)化伺服控制。采用德國的基于PC的支持EtherCAT工業(yè)以太網(wǎng)協(xié)議的實(shí)時(shí)控制的TwinCAT軟件系統(tǒng)，作為支持EtherCAT接口的超聲電機(jī)控制器，實(shí)現(xiàn)對(duì)Tripod精密平臺(tái)的控制。Tripod精密平臺(tái)系統(tǒng)結(jié)構(gòu)如圖2所示。

Tripod精密平臺(tái)為混聯(lián)結(jié)構(gòu)，為了求解方便，將3-RPS與XY平面運(yùn)動(dòng)平臺(tái)分開考慮。優(yōu)先考慮3-RPS機(jī)構(gòu)部分，由動(dòng)平臺(tái)、靜平臺(tái)及3個(gè)支鏈組成，3-RPS機(jī)構(gòu)結(jié)構(gòu)簡(jiǎn)圖如圖3所示。ai和bi(i=1,2,3)分別為支鏈i球面副(R副)和轉(zhuǎn)動(dòng)副(S副)的中心，構(gòu)成外接圓半徑分別為r和R的等邊三角形ΔA1A2A3和ΔB1B2B3。

以B1、B2、B3的分布圓圓心為中心點(diǎn)建立定平臺(tái)坐標(biāo)系{W}：OW-XYZ，Z軸垂直向上，X軸指向B1；以A1、A2、A3的分布圓的圓心為中心點(diǎn)建立動(dòng)平臺(tái)坐標(biāo)系{P}:Op-X′Y′Z′，Z′ 軸垂直向上，X′軸指向A1。

圖3 Tripod平臺(tái)3-RPS結(jié)構(gòu)簡(jiǎn)圖

2 機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)分析

對(duì)Tripod精密平臺(tái)進(jìn)行逆運(yùn)動(dòng)學(xué)建模，對(duì)其進(jìn)行位置分析、速度分析及加速度分析，為動(dòng)力學(xué)建模奠定基礎(chǔ)。

2.1 位置分析

以ZYZ歐拉角(φ，θ，-φ)表示平臺(tái)運(yùn)動(dòng)姿態(tài)，平臺(tái)傾角大小由進(jìn)動(dòng)角θ決定，脊線位置完全有由章動(dòng)角φ決定，則動(dòng)坐標(biāo)系Op-X′Y′Z′相對(duì)于定坐標(biāo)系OW-XYZ的旋轉(zhuǎn)矩陣表達(dá)式表示為：

(1)

其中,c為cos，s為sin。

(2)

圖2 Tripod精密平臺(tái)系統(tǒng)結(jié)構(gòu)

由空間向量關(guān)系可得到3個(gè)驅(qū)動(dòng)桿在{W}中矢量Li為：

Li=Wai-Wbi

(3)

(4)

動(dòng)平臺(tái)中心點(diǎn)的偏移量與動(dòng)平臺(tái)的進(jìn)動(dòng)角φ和章動(dòng)角θ有關(guān)。精密平臺(tái)X、Y、Z方向位移分別為X0、Y0、Z0。

串聯(lián)平臺(tái)實(shí)現(xiàn)X軸和Y軸的運(yùn)動(dòng)，使X軸與Y軸具有較大的運(yùn)動(dòng)范圍與運(yùn)動(dòng)速度，平臺(tái)X，Y方向位移分別為xp和yp，表達(dá)式如下：

(5)

2.2 速度分析

式(2)對(duì)時(shí)間求導(dǎo)，可得{W}系下R副中心Ai的速度：

(6)

驅(qū)動(dòng)速度li的可表示為Vai在ni上的投影:

(7)

式(4)對(duì)時(shí)間求導(dǎo)，進(jìn)而可得動(dòng)平臺(tái)中心點(diǎn)速度：

(8)

ZYZ型歐拉角參數(shù)一階運(yùn)動(dòng)[12]表示為：

(9)

通過式(7)～式(9)，可得驅(qū)動(dòng)桿速度：

(10)

式(5)對(duì)時(shí)間求導(dǎo)，即可得二維平臺(tái)運(yùn)動(dòng)速度：

(11)

2.3 加速度分析

式(7)對(duì)時(shí)間求導(dǎo)，可得驅(qū)動(dòng)桿加速度：

(12)

式(8)對(duì)時(shí)間求導(dǎo)可得動(dòng)平臺(tái)中心點(diǎn)加速度：

(13)

式(11)對(duì)時(shí)間求導(dǎo)，即可得二維平臺(tái)運(yùn)動(dòng)加速度：

(14)

3 機(jī)構(gòu)動(dòng)力學(xué)分析

圖4 支鏈簡(jiǎn)化結(jié)構(gòu)圖

3.1 支鏈動(dòng)力學(xué)分析

在進(jìn)行Tripod平臺(tái)機(jī)構(gòu)的動(dòng)力學(xué)分析之前，首先需要確定連桿角速度和加速度的表達(dá)式，進(jìn)而對(duì)支鏈進(jìn)行動(dòng)力學(xué)分析。

如圖5所示, 支鏈坐標(biāo)系可由{W}系繞z軸轉(zhuǎn)動(dòng)γi角，再繞yi′軸轉(zhuǎn)動(dòng)αi角得到[13]。

圖5 支鏈歐拉角

支鏈坐標(biāo)系相對(duì)定坐標(biāo)系的旋轉(zhuǎn)矩陣可表示為：

(15)

將支鏈角速度和角加速度、缸體和活塞質(zhì)心的加速度轉(zhuǎn)在支鏈坐標(biāo)系描述為：

(16)

ZinAi為作用于ai點(diǎn)的合外力矩：

(17)

其中,ZiMAi ,x支鏈坐標(biāo)系下機(jī)架作用于支鏈i的x方向的約束力矩。

根據(jù)支鏈歐拉方程[10]，有：

(18)

其中,I1iy和I2iy分別為上支鏈和下支鏈關(guān)于質(zhì)心的主慣性距在yi軸方向的分量。

由式(18)可得動(dòng)平臺(tái)對(duì)支鏈x方向的作用力：

(19)

3.2 平臺(tái)動(dòng)力學(xué)分析

動(dòng)平臺(tái)受支鏈反作用力、自身重力、動(dòng)平臺(tái)所受外力FCW及動(dòng)平臺(tái)所受外力矩MCW。

平臺(tái)力平衡方程為：

(20)

根據(jù)平臺(tái)歐拉方程，有：

(21)

由式(20)和式(21)可得：

(22)

其中：

因此，可得動(dòng)平臺(tái)對(duì)支鏈i，在z方向和y方向的作用力：

(23)

由支鏈力平衡方程可得電機(jī)驅(qū)動(dòng)力和約束力矩：

fi=m1a1i·ni-ifAi,z-m1G·ni

(24)

Mi=MAi,x=liifAi,y

(25)

二維精密平臺(tái)驅(qū)動(dòng)力為：

(26)

3.3 ADAMS動(dòng)力學(xué)仿真分析

利用ADAMS軟件對(duì)Tripod平臺(tái)進(jìn)行運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)仿真，并得出各桿軸向驅(qū)動(dòng)力的理論變化曲線。利用SolidWorks建立Tripod精密平臺(tái)三維模型，通過ADANS/Exchange模塊從外部導(dǎo)入Parasolid格式輸入模型文件并對(duì)模型材料、質(zhì)心、約束和仿真環(huán)境等參數(shù)進(jìn)行設(shè)置。

首先，給定動(dòng)平臺(tái)中心施加驅(qū)動(dòng)，仿真生成各移動(dòng)副位移、速度、加速度驅(qū)動(dòng)樣條曲線；將驅(qū)動(dòng)樣條曲線導(dǎo)入ADAMS模型驅(qū)動(dòng)設(shè)置中，分別進(jìn)行仿真得到動(dòng)力學(xué)模型驅(qū)動(dòng)力變化曲線。給定動(dòng)平臺(tái)運(yùn)動(dòng)運(yùn)動(dòng)軌跡為：

圖6為按照定動(dòng)平臺(tái)中心運(yùn)動(dòng)軌跡驅(qū)動(dòng)得到的各驅(qū)動(dòng)位移、速度和加速度誤差曲線，與MATLAB計(jì)算結(jié)果位移誤差在1%以內(nèi)。將MATLAB各移動(dòng)副位移樣條曲線作為ADAMS仿真驅(qū)動(dòng)函數(shù)，平臺(tái)目標(biāo)運(yùn)動(dòng)軌跡仿真結(jié)果如圖7所示，與平臺(tái)設(shè)定軌跡基本吻合，證明了運(yùn)動(dòng)學(xué)模型的正確性。

分別以位移樣條曲線、速度樣條曲線和加速度樣條曲線作為驅(qū)動(dòng)函數(shù)，經(jīng)ADAMS仿真得到的Tripod平臺(tái)3-RPS部分各超聲電機(jī)驅(qū)動(dòng)力隨時(shí)間變化的曲線如圖8所示，速度和加速度驅(qū)動(dòng)函數(shù)下得到的驅(qū)動(dòng)力連續(xù)穩(wěn)定。支鏈2驅(qū)動(dòng)力ADAMS仿真結(jié)果與MATLAB結(jié)果對(duì)比曲線如圖9所示，可知兩者總體吻合良好，數(shù)值誤差很小。由于MATLAB分析針對(duì)的是簡(jiǎn)化后的理想模型，與實(shí)際模型本身存在一定誤差，故仿真結(jié)果與理論值存在一定誤差是合理的，通過ADAMS動(dòng)力學(xué)仿真證明了動(dòng)力學(xué)分析的正確性，為動(dòng)力學(xué)前饋補(bǔ)償控制奠定基礎(chǔ)。

(a)驅(qū)動(dòng)位移誤差曲線 (b) 驅(qū)動(dòng)速度誤差曲線 (c) 驅(qū)動(dòng)加速度誤差曲線圖6 移動(dòng)副位移、速度及加速度曲線對(duì)比

(a)驅(qū)動(dòng)位移曲線 (b) 平臺(tái)位移曲線 (c) 平臺(tái)旋轉(zhuǎn)曲線圖7 ADAMS仿真驅(qū)動(dòng)位移及平臺(tái)目標(biāo)運(yùn)動(dòng)軌跡

(a)位置驅(qū)動(dòng)模式 3-RPS驅(qū)動(dòng)力 (b) 速度驅(qū)動(dòng)模式 3-RPS驅(qū)動(dòng)力 (c) 加速度驅(qū)動(dòng)模式 3-RPS驅(qū)動(dòng)力圖8 支鏈壓電馬達(dá)驅(qū)動(dòng)力曲線

圖9 支鏈2驅(qū)動(dòng)力仿真結(jié)果對(duì)比

4 控制系統(tǒng)開發(fā)

采用基于Windows軟硬件平臺(tái)的支持PLC語言及C++混合編程的實(shí)時(shí)控制軟件TwinCAT 3，實(shí)現(xiàn)對(duì)I/O模塊和驅(qū)動(dòng)器的實(shí)時(shí)控制。該軟件PLC包含豐富的運(yùn)動(dòng)控制庫可用于軌跡規(guī)劃及對(duì)NC軸控制，此外C++編程可實(shí)現(xiàn)PLC不能實(shí)現(xiàn)的復(fù)雜算法的開發(fā)。

利用TwinCAT C++調(diào)用newmat庫實(shí)現(xiàn)Tripod平臺(tái)運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)算法，將運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)工程文件生成可以集成到TwinCAT3環(huán)境的具有輸入輸出變量的C++語言的TcCOM模型。

TwinCAT NC PTP把一個(gè)電機(jī)的運(yùn)動(dòng)控制分為三層：PLC軸、NC軸和物理軸。NC軸如果關(guān)聯(lián)到物理軸，就是實(shí)軸，如果不關(guān)聯(lián)到硬件，就是虛軸。PLC中的運(yùn)動(dòng)控制程序并不區(qū)分虛軸和實(shí)軸，在實(shí)際調(diào)試的階段前采用虛軸進(jìn)行控制系統(tǒng)實(shí)驗(yàn)。

在TwinCAT NC中添加關(guān)節(jié)軸虛軸，Tripod平臺(tái)在笛卡爾坐標(biāo)系下具有X、Y、Z、A、B 這5個(gè)自由度，需控制5個(gè)直線超聲電機(jī)。給定的X、Y、Z、A、B位姿作為輸入變量，經(jīng)動(dòng)力學(xué)TcCOM模塊計(jì)算，得到驅(qū)動(dòng)位置、速度、加速度和驅(qū)動(dòng)力，作為關(guān)節(jié)軸控制輸入信息，調(diào)用運(yùn)動(dòng)控制庫中的凸輪、點(diǎn)動(dòng)等功能塊，實(shí)現(xiàn)平臺(tái)點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)及連續(xù)循環(huán)運(yùn)動(dòng)。Tripod控制系統(tǒng)HMI界面如圖10所示。

圖10 Tripod控制系統(tǒng)HMI界面

將Tripod平臺(tái)運(yùn)動(dòng)軌跡文件導(dǎo)入系統(tǒng)軟件，進(jìn)行連續(xù)運(yùn)動(dòng)測(cè)試。圖11為虛擬軸運(yùn)動(dòng)軌跡，與圖7a驅(qū)動(dòng)位移曲線對(duì)比，可知虛擬軸運(yùn)動(dòng)軌跡與滿足理論分析結(jié)果一致。通過測(cè)試Tripod平臺(tái)控制系統(tǒng)可實(shí)現(xiàn)單軸點(diǎn)動(dòng)、點(diǎn)對(duì)點(diǎn)定位運(yùn)動(dòng)、按照預(yù)置軌跡運(yùn)動(dòng)等操作，為 Tripod 精密平臺(tái)精密運(yùn)動(dòng)控制奠定基礎(chǔ)。

圖11 虛擬軸運(yùn)動(dòng)軌跡

5 結(jié)論

(1)基于牛頓歐拉法對(duì)支鏈及平臺(tái)進(jìn)行動(dòng)力學(xué)分析，構(gòu)造6×6方陣，可求解對(duì)Tripod 混聯(lián)機(jī)構(gòu)的動(dòng)力學(xué)逆解。

(2)通過ADAMS仿真分析結(jié)果與MATLAB計(jì)算結(jié)果對(duì)比，運(yùn)動(dòng)學(xué)分析計(jì)算誤差在1%以內(nèi)。仿真結(jié)果與平臺(tái)設(shè)定軌跡基本吻合，證明了運(yùn)動(dòng)學(xué)模型的正確性。簡(jiǎn)化后的理想模型，與實(shí)際模型本身存在一定誤差，動(dòng)力學(xué)仿真結(jié)果與理論值存誤差。在位置驅(qū)動(dòng)模式下驅(qū)動(dòng)力輸出存在波動(dòng)，證明在位置環(huán)控制模式下，加入動(dòng)力學(xué)前饋補(bǔ)償控制的必要性。

(3)通過 TwinCAT 3 開發(fā)Tripod精密平臺(tái)控制系統(tǒng)，實(shí)現(xiàn)平臺(tái)按照預(yù)定軌跡運(yùn)動(dòng)，為Tripod精密平臺(tái)精密運(yùn)動(dòng)控制奠定基礎(chǔ)。