3-RPS并聯(lián)機(jī)器人動(dòng)力學(xué)分析及控制

梁 超，高宏力，彭志文，文 剛

（西南交通大學(xué) 機(jī)械工程學(xué)院，四川 成都 610031）

1 引言

目前，并聯(lián)機(jī)器人的機(jī)構(gòu)剛性、承載能力、運(yùn)動(dòng)控制性能均優(yōu)于串聯(lián)機(jī)器人，故其廣泛應(yīng)用于醫(yī)療器械、武器、并聯(lián)床等行業(yè)。因此，對(duì)并聯(lián)機(jī)器人的研究是很有必要的[1]。但并聯(lián)機(jī)器人具有較強(qiáng)的非線性及耦合性的特點(diǎn)，且機(jī)構(gòu)越復(fù)雜，自由度越高其耦合性越突出，對(duì)其實(shí)現(xiàn)高精度控制也越困難[2]。采用的3-RPS并聯(lián)機(jī)器人是在Stewart六自由度并聯(lián)機(jī)器人的基礎(chǔ)上發(fā)展而來(lái)的[3]。具有三個(gè)自由度，故其機(jī)構(gòu)的耦合性弱于Stewart，可適用于某些對(duì)控制精度要求較高又不需要高自由度的場(chǎng)合[4]。涉及的3-RPS并聯(lián)機(jī)構(gòu)分別由三組轉(zhuǎn)動(dòng)副、移動(dòng)副、球面副并聯(lián)于上下平臺(tái)而構(gòu)成的[5]。通過改變?nèi)齻€(gè)移動(dòng)副的位移使上平臺(tái)產(chǎn)生上下移動(dòng)、左右搖擺、前后俯仰三個(gè)自由度。

并聯(lián)機(jī)器人具有復(fù)雜的數(shù)學(xué)計(jì)算性，其機(jī)構(gòu)多為高自由度、高非線性、高度耦合的特點(diǎn)[6]。目前，主要應(yīng)用于并聯(lián)機(jī)器人的建模方法有Lagrange法、Newton-Euler法、Kane法、Gauss法及虛功原理法等[7]。采用Newton-Euler法建立機(jī)構(gòu)的逆動(dòng)力學(xué)模型，并利用Simulink及Simmechanics對(duì)模型的準(zhǔn)確性進(jìn)行了驗(yàn)證。

由于并聯(lián)機(jī)器人具有很強(qiáng)的非線性及存在未建模誤差，故對(duì)其實(shí)現(xiàn)高精度控制較為困難。當(dāng)前廣泛應(yīng)用于并聯(lián)機(jī)器人控制算法的有計(jì)算力矩控制等[8]，但由于沒有考慮到系統(tǒng)的未建模誤差導(dǎo)致系統(tǒng)的控制精度不高?；贚yapunov理論采用不確定性魯棒控制方法，充分考慮系統(tǒng)的不確定性，基于系統(tǒng)定義的標(biāo)稱模型設(shè)計(jì)系統(tǒng)的計(jì)算力矩控制方法作為鎮(zhèn)定標(biāo)稱系統(tǒng)，基于Lyapunov函數(shù)設(shè)計(jì)系統(tǒng)的魯棒補(bǔ)償控制器以補(bǔ)償系統(tǒng)不確定性及為建模誤差引起的系統(tǒng)控制誤差。使用Matlab/Simulink軟件對(duì)所設(shè)計(jì)的魯棒控制系統(tǒng)進(jìn)行仿真，并與傳統(tǒng)的計(jì)算力矩進(jìn)行了對(duì)比試驗(yàn)，結(jié)果證明魯棒控制的控制精度優(yōu)于計(jì)算力矩控制。

2 動(dòng)力學(xué)分析

3-RPS的機(jī)構(gòu)簡(jiǎn)圖，如圖1所示。其下平臺(tái)A1A2A3位于半徑為R的圓內(nèi)接等邊三角形的三個(gè)頂點(diǎn)，通過轉(zhuǎn)動(dòng)副與移動(dòng)副相連，且轉(zhuǎn)動(dòng)副的軸線與圓相切。上平臺(tái)B1B2B3位于半徑為r的圓內(nèi)接等邊三角形的三個(gè)頂點(diǎn)，通過球面副與移動(dòng)副相連。機(jī)構(gòu)動(dòng)作時(shí)，通過驅(qū)動(dòng)移動(dòng)副的主驅(qū)動(dòng)關(guān)節(jié)實(shí)現(xiàn)上平臺(tái)產(chǎn)生上下位移z、左右搖擺α、前后俯仰β三個(gè)自由度。

圖1 3-RPS機(jī)構(gòu)簡(jiǎn)圖Fig.1 3-RPS Mechanism

2.1 動(dòng)力學(xué)模型的建立

分別以上下平臺(tái)的圓心P，O為原點(diǎn)建立動(dòng)坐標(biāo)系P-xyz和定坐標(biāo)系O-XYZ。以向量為Z軸的方向向量為X軸方向，Y軸由右手定則確定。同理確定動(dòng)坐標(biāo)系P-xyz。以Ai為原點(diǎn)建支鏈坐標(biāo)系 A-xyz向量方向?yàn)閦i軸方向，轉(zhuǎn)動(dòng)副的軸線為yi軸。

iiii假設(shè)機(jī)構(gòu)的各個(gè)構(gòu)件為剛性體，且忽略各轉(zhuǎn)動(dòng)副及移動(dòng)副間的摩擦阻力。利用Newton-Euler方法構(gòu)建系統(tǒng)的逆動(dòng)力學(xué)模型[9]；

聯(lián)立式（1）～式（3）代入式（4）可得系統(tǒng)的動(dòng)力學(xué)方程：

式中：mp、m2—上平臺(tái)及活塞桿的質(zhì)量和定坐標(biāo)系下的加速度，g=（0，0，-g）c為重力加速度；F、N—平臺(tái)載荷及合外力矩；θi—移動(dòng)副與動(dòng)坐標(biāo)系Z軸的夾角—直支鏈坐標(biāo)系相對(duì)于定坐標(biāo)系、動(dòng)坐標(biāo)系的旋轉(zhuǎn)矩陣—各球面副中心在上平臺(tái)的位置向量—上平臺(tái)在動(dòng)坐標(biāo)系下的合力矩；Ai—在支鏈坐標(biāo)系下的合力矩—支鏈 i關(guān)于 Ai的合角動(dòng)量；M（q）—正定慣性矩陣柯氏及向心力矩陣的非線性耦合項(xiàng)；G（q）—重力矩陣，q=（z，α，β）T。

2.2 機(jī)構(gòu)逆動(dòng)力學(xué)仿真模型

Simulink具有邏輯結(jié)構(gòu)清晰、仿真效率高、結(jié)構(gòu)準(zhǔn)確且非常適合于動(dòng)態(tài)系統(tǒng)的建模與分析。故通過使用Simulink來(lái)構(gòu)建系統(tǒng)的子函數(shù)模塊以及利用Simulink中便利的矩陣運(yùn)算來(lái)實(shí)現(xiàn)系統(tǒng)逆動(dòng)力學(xué)方程程序的編寫，方便實(shí)現(xiàn)系統(tǒng)仿真運(yùn)行的高效性及準(zhǔn)確性，為進(jìn)一步系統(tǒng)控制仿真奠定基礎(chǔ)。其逆動(dòng)力學(xué)Simulink模型，如圖2所示。Simmechanics是基于Simulink的機(jī)械系統(tǒng)建模軟件，通過約束、驅(qū)動(dòng)其中的關(guān)節(jié)及移動(dòng)副等，可實(shí)現(xiàn)機(jī)構(gòu)按照預(yù)先設(shè)定的軌跡進(jìn)行運(yùn)動(dòng)，且Simmechanics中提供了大量的傳感器模塊，為機(jī)構(gòu)的運(yùn)行監(jiān)測(cè)及反饋控制提供了便利。系統(tǒng)Simmechanics建模示意圖，如圖3所示。

圖2 Simulink系統(tǒng)逆動(dòng)力學(xué)模型Fig.2 Inverse Dynamics Model of Simulink System

圖3 系統(tǒng)Simmechanics建模Fig.3 System Simmechanics Modeling

3 魯棒控制器設(shè)計(jì)

由式（5）可知并聯(lián)機(jī)器人是一類具有高耦合性及非線性較強(qiáng)的系統(tǒng)，易受到環(huán)境因素及模型不準(zhǔn)確的影響。在傳統(tǒng)的計(jì)算力矩控制器中通過系統(tǒng)反饋的速度、加速度與輸入的理想速度、加速度之間的差值再基于系統(tǒng)的標(biāo)稱模型PD調(diào)節(jié)系統(tǒng)的質(zhì)量矩陣分項(xiàng)。其忽略了系統(tǒng)本身模型的不確定性及非線性因素?；贚yapunov函數(shù)設(shè)計(jì)了一種魯棒控制方法，通過在計(jì)算力矩控制的基礎(chǔ)上增加一項(xiàng)魯棒控制器，用以抵消由于系統(tǒng)模型的不確定性及非線性而引起的系統(tǒng)穩(wěn)態(tài)誤差，有效的改善了系統(tǒng)的控制精度，通過仿真結(jié)果證明了本系統(tǒng)的有效性。

由式（5）考慮具有外部干擾f∈Rn的的動(dòng)力學(xué)方程：

具有如下性質(zhì)：

性質(zhì)1對(duì)于正定矩陣M（q）及其逆矩陣M-1（q）對(duì)?q∈Rn一致有界。

性質(zhì)2動(dòng)力系統(tǒng)可通過如下方程表示：

式中：α—系統(tǒng)參數(shù)的一個(gè) m 維向量，Y（q，q˙，q¨）—n×m 的矩陣。設(shè)計(jì)的魯棒控制律為[10]：

由式（8）可知，令：

將式（2）、式（7）、式（8）代入式（6）可得：

在常見的魯棒控制律被應(yīng)用于機(jī)器人控制的文獻(xiàn)中，廣義誤差項(xiàng)常被定義為x=［e，e˙］T，其所定義的誤差方程為2n階，而采用的魯棒控制律設(shè)計(jì)所定義的誤差方程γ~為n階。

由式（10），選取k2滿足Lyapunov方程

式中：P—正定矩陣，且Q=QT≥0。構(gòu)造系統(tǒng)的Lyapunov函數(shù)為：

則對(duì)其時(shí)間求導(dǎo)，將（10）式代入可得：

設(shè)非線性控制律μ：

由式（11）及系統(tǒng)不確定項(xiàng)φ得：（φ+μ）＜0；

故：V˙（t，x）＜-xTQx＜0

若系統(tǒng)滿足Lyapunov方程，選取k1使得Re［λi（-k1）］＜0，i=1，2，…，n。

因此，由Lyapunov穩(wěn)定性定理可知采用魯棒控制律（11）可使系統(tǒng)全局穩(wěn)定。

4 系統(tǒng)仿真算例

對(duì)3-RPS并聯(lián)機(jī)器人的跟蹤誤差進(jìn)行測(cè)量，以檢驗(yàn)所設(shè)計(jì)的基于計(jì)算力矩鎮(zhèn)定標(biāo)稱模型的魯棒控制算法的控制性能。因此，通過使用Matlab軟件對(duì)文中所設(shè)計(jì)的控制系統(tǒng)進(jìn)行了動(dòng)力學(xué)控制系統(tǒng)仿真。系統(tǒng)的物理參數(shù)，如表1所示。

表1 系統(tǒng)物理參數(shù)Tab.1 System Physical Parameters

根據(jù)系統(tǒng)物理參數(shù)模型，且符合一般性，設(shè)系統(tǒng)的輸入軌跡q 為：q=［0.6+0.1sin（2t），0.15sin（t），0.1sin（t）］T。

根據(jù)（6）式所建立的動(dòng)力學(xué)模型，采用2.2節(jié)中所建立的Simulink及 Simmechanics系統(tǒng)仿真模型，采用式（8）、式（11）基于計(jì)算力矩鎮(zhèn)定控制的魯棒控制律，設(shè)計(jì)系統(tǒng)的控制算法。并設(shè)計(jì)了系統(tǒng)的計(jì)算力矩S控制器。對(duì)比了計(jì)算力矩和所設(shè)計(jì)的控制算法對(duì)理想軌跡的跟蹤精度。其仿真結(jié)果，如圖4所示。

圖4 Z方向的跟蹤軌跡Fig.4 Tracking Trajectory of Z Direction

由圖4可知，系統(tǒng)在采用計(jì)算力矩控制時(shí)，在Z方向的最大誤差精度為2.13%，在α及β角方向的跟蹤誤差分別為1.67%、1.15%；采用所設(shè)計(jì)的魯棒控制律，在Z方向的最大誤差精度為0.47%，在α及β角方向的跟蹤誤差分別為1.46%、0.92%，如圖5、圖6所示。因此基于計(jì)算力矩標(biāo)稱鎮(zhèn)定的魯棒性控制系統(tǒng)，在計(jì)算力矩的基礎(chǔ)上進(jìn)一步提高了系統(tǒng)的控制精度。尤其是其在Z方向的跟蹤誤差具有較為顯著的改善，因?yàn)楫?dāng)系統(tǒng)在Z方向移動(dòng)時(shí)，通過直接檢測(cè)上平臺(tái)的位移信號(hào)值即可，對(duì)Z方向的誤差的敏感度較高故在較小位移誤差的條件下仍能達(dá)到較高的控制精度。而上平臺(tái)檢測(cè)到的角度是通過各個(gè)活塞桿間位移差來(lái)實(shí)現(xiàn)，其對(duì)角度誤差的敏感度較低。

圖5 α角跟蹤軌跡Fig.5 α Angular Tracking

圖6 β角跟蹤軌跡Fig.6 β Angular Tracking

5 結(jié)論

運(yùn)用Newton-Euler方法構(gòu)建對(duì)3-RPS并聯(lián)機(jī)器人的逆動(dòng)力學(xué)模型，并對(duì)其進(jìn)行逆動(dòng)力學(xué)控制，比較了當(dāng)前常用于并聯(lián)機(jī)器人的計(jì)算力矩控制策略與基于計(jì)算力矩標(biāo)稱鎮(zhèn)定的魯棒控制策略。經(jīng)仿真驗(yàn)證得，所采用的基于計(jì)算力矩標(biāo)稱鎮(zhèn)定的魯棒控制策略，所設(shè)計(jì)的魯棒控制器對(duì)于控制3-RPS并聯(lián)機(jī)器人的穩(wěn)定誤差精度優(yōu)于計(jì)算力矩控制；基于計(jì)算力矩策略的魯棒控制模型充分考慮了系統(tǒng)動(dòng)力學(xué)標(biāo)稱模型而引起的系統(tǒng)未建模誤差，優(yōu)化了系統(tǒng)動(dòng)力學(xué)模型；在檢測(cè)上平臺(tái)的位姿時(shí)改進(jìn)了檢測(cè)方法：通過對(duì)3-RPS并聯(lián)機(jī)器人上平臺(tái)位姿進(jìn)行直接檢測(cè)，而非采用傳統(tǒng)的通過檢測(cè)移動(dòng)副的位移換算而得到系統(tǒng)平臺(tái)的位姿，有利于對(duì)平臺(tái)的位姿進(jìn)行監(jiān)測(cè)控制。