3-RPS并聯(lián)機(jī)構(gòu)SMC控制系統(tǒng)仿真

朱城偉，朱大昌，陳健偉，張榮興

（江西理工大學(xué) 機(jī)電工程學(xué)院，江西 贛州 341000）

1 引言

并聯(lián)機(jī)構(gòu)由于其結(jié)構(gòu)穩(wěn)定，承載能力強(qiáng)，動態(tài)響應(yīng)快和良好的位置精度等優(yōu)點(diǎn)得到機(jī)器人領(lǐng)域的關(guān)注[1]，少自由度并聯(lián)機(jī)構(gòu)運(yùn)動學(xué)、動力學(xué)研究相對簡單，具有很強(qiáng)的靈活性，制造相對容易，在各個領(lǐng)域中更具有應(yīng)用潛力，因而成為國內(nèi)外學(xué)者研究較多的一類機(jī)構(gòu)[2]。在少自由度并聯(lián)機(jī)構(gòu)中，目前研究最多的主要是3自由度并聯(lián)機(jī)構(gòu)，由于并聯(lián)機(jī)構(gòu)的Jacobian矩陣存在超越方程組，難以對其進(jìn)行求解，通常是由螺旋理論求得Jacobian矩陣，但所得為方陣緣故難以對其進(jìn)行控制仿真。采用矢量法以及并聯(lián)機(jī)構(gòu)微運(yùn)動進(jìn)行求解并聯(lián)機(jī)構(gòu)的Jacobian矩陣，可以避免在求解Jacobian矩陣所出現(xiàn)的超越方程的問題，從而得到并聯(lián)機(jī)構(gòu)的輸入-輸出連續(xù)映射方程組。

在并聯(lián)機(jī)構(gòu)在控制領(lǐng)域中，隨著計(jì)算機(jī)技術(shù)的發(fā)展，人們利用人工智能的方法將數(shù)學(xué)模型和操作經(jīng)驗(yàn)存入計(jì)算機(jī)建立控制系統(tǒng)模型，以便對整個機(jī)械系統(tǒng)進(jìn)行控制[3]。然而控制方法是多樣的，比如PID控制，模糊控制，神經(jīng)網(wǎng)絡(luò)控制等。但由于被控對象具有非線性，耦合性以及不確定性的特性，傳統(tǒng)的控制器很難得到良好的控制。文獻(xiàn)[4]提出用帶有重力補(bǔ)償?shù)腜ID控制器對六自由度并聯(lián)機(jī)器人進(jìn)行控制，來消除機(jī)構(gòu)的穩(wěn)態(tài)誤差提高控制精度。文獻(xiàn)[5]分析6-PTRT并聯(lián)機(jī)構(gòu)的耦合特征，基于同步誤差和系統(tǒng)的穩(wěn)定性設(shè)計(jì)了光滑滑模控制算法，該方法可以抗拒并聯(lián)機(jī)構(gòu)支鏈之間的耦合效應(yīng)，實(shí)現(xiàn)并聯(lián)機(jī)構(gòu)每個分支的協(xié)調(diào)，同步和更精確的運(yùn)動。對于并聯(lián)機(jī)構(gòu)控制方面，大部分是基于6自由度，然而少自由度并聯(lián)機(jī)構(gòu)的控制研究較少。滑模變結(jié)構(gòu)控制本質(zhì)上具有良好的魯棒性，瞬態(tài)響應(yīng)速度快，對參數(shù)和外部擾動變化不靈敏。因此，滑模變結(jié)構(gòu)控制被視為許多系統(tǒng)控制的有效途徑，如不確定非線性系統(tǒng)，離散時(shí)間非線性系統(tǒng)與廣義隨機(jī)混雜系統(tǒng)[5]。由于柔順并聯(lián)機(jī)構(gòu)是一個封閉式高度非線性耦合系統(tǒng)，其動力學(xué)模型的數(shù)學(xué)表達(dá)式十分復(fù)雜[6]。建立相對應(yīng)的數(shù)學(xué)模型，工作量大，不易于實(shí)現(xiàn)，通過Matlab中的SimMechanics對系統(tǒng)進(jìn)行建模，其優(yōu)點(diǎn)可為多體動力機(jī)械系統(tǒng)及其控制系統(tǒng)系統(tǒng)了直觀有效的建模分析手段[7]。采用SimMechanics Link接口，建立基于SolidWorks與SimMechanics的并聯(lián)機(jī)構(gòu)模型，并通過3-RPS并聯(lián)機(jī)構(gòu)的動力學(xué)方程，設(shè)計(jì)SMC控制器的切換函數(shù)與控制律，分別使用PID控制器與SMC控制器對并聯(lián)機(jī)構(gòu)進(jìn)行仿真，對比并聯(lián)機(jī)構(gòu)動平臺的軌跡跟蹤誤差曲線，證明所設(shè)計(jì)的滑模變結(jié)構(gòu)控制的有效性。

2 3-RPS并聯(lián)機(jī)構(gòu)微運(yùn)動分析

3-RPS并聯(lián)機(jī)構(gòu)由動平臺、定平臺和三條支鏈構(gòu)成，如圖1所示。支鏈包含轉(zhuǎn)動副、移動副和球副。其中轉(zhuǎn)動副連接定平臺各個頂點(diǎn)，球副連接動平臺各個頂點(diǎn)，轉(zhuǎn)動副和球副之間由移動副連接。在定平臺建立基坐標(biāo)系O-xyz，同時(shí)在動平臺上建立其動坐標(biāo)系P-uvw，ΔA1A2A3為等邊三角形，其邊長為a，ΔB1B2B3為等邊三角形，其邊長為b，AiBi的長度為li。

圖1 3-RPS并聯(lián)機(jī)構(gòu)Fig.1 3-RPS Parallel Mechanism

假定A1B1與OA1的夾角為φ1，A2B2與OA2的夾角為φ2，A3B3與OA3的夾角為φ3，AiBi基于基坐標(biāo)系的坐標(biāo)為：

根據(jù)3-RPS并聯(lián)機(jī)構(gòu)具有兩轉(zhuǎn)動一平移的運(yùn)動特性，假定3-RPS并聯(lián)機(jī)構(gòu)的動平臺繞著下x軸轉(zhuǎn)動α角，繞著y軸轉(zhuǎn)動β角，沿著z軸移動距離d。

則繞x軸轉(zhuǎn)動α角的轉(zhuǎn)動矩陣為：

繞著y軸轉(zhuǎn)動β角的轉(zhuǎn)動矩陣為：

沿著z軸移動距離d移動矩陣為：

根據(jù)：R（α，β，d）=R（x，α）R（y，β）R（z，d）

可以得到3-RPS的旋轉(zhuǎn)矩陣：

式中：s（）=sin（），c（）=cos（）下同。

由于3-RPS并聯(lián)機(jī)構(gòu)具有繞著x軸與y軸的轉(zhuǎn)動和沿著z軸移動三個自由度，所以姿態(tài)反解是已知繞著x軸的轉(zhuǎn)角α、繞著y軸的轉(zhuǎn)角β以及z軸的移動距離d，求解各個桿長的變化。

由于并聯(lián)機(jī)構(gòu)采用微運(yùn)動求解Jacobian矩陣，可以避免超越方程在矩陣中出現(xiàn)，同時(shí)求解得到的Jacobian矩陣屬于非奇異矩陣，可用作并聯(lián)機(jī)構(gòu)的理論輸入?yún)⒖贾怠τ诓⒙?lián)機(jī)構(gòu)的微運(yùn)動，動平臺的位移應(yīng)屬于納米級。故在進(jìn)行計(jì)算的同時(shí)，有關(guān)于α、β、d的高階次項(xiàng)趨近于無窮小。

綜上所述：

3 3-RPS并聯(lián)機(jī)構(gòu)動力學(xué)建模

求解3-RPS并聯(lián)機(jī)構(gòu)的動能和勢能，可以把并聯(lián)機(jī)構(gòu)分為動平臺和支鏈兩個部分[10]。可分別求出3-RPS并聯(lián)機(jī)構(gòu)動平臺以及三條支鏈的動能及勢能，然后再求出整體的動能及勢能。

3-RPS 并聯(lián)機(jī)構(gòu)動平臺的位姿參數(shù)為：q=［0，0，z，0，β，α］

根據(jù)并聯(lián)機(jī)構(gòu)動平臺的位姿參數(shù)可以求出動平臺的動能為：

式中：mplat—動平臺質(zhì)量；Ii—動平臺的轉(zhuǎn)動慣量。

動平臺的勢能為：Pplat=［0，0，mplat，g，0，0］Tq

圖2 驅(qū)動桿簡圖Fig.2 Diagram of Drive Rod

求解3-RPS并聯(lián)機(jī)構(gòu)3條支鏈的動能以及勢能，可以把支鏈分為固定部分以及移動部分，如圖6所示。支鏈的總長為li，固定部分的中心點(diǎn)為G1，與Ai的距離為e1，質(zhì)量為m1，移動部分的中心點(diǎn)為G2，與Bi的距離為e2，質(zhì)量為m2。

對此求導(dǎo)可得出支鏈的單位速度矢量為：

求出各個支鏈的位移矢量為：

根據(jù)拉格朗日運(yùn)動學(xué)機(jī)械人動態(tài)方程可以寫為：

4 建立并聯(lián)機(jī)構(gòu)的模擬框圖

4.1 結(jié)構(gòu)模型的建立

在Matlab中的SimMechanics中建立并聯(lián)機(jī)構(gòu)的模擬框圖，由于3-RPS并聯(lián)機(jī)構(gòu)是由定平臺、動平臺以及三條鉸鏈構(gòu)成，其中三條鉸鏈由轉(zhuǎn)動副、移動副、球副組成的。可以使用SimMechanics中的剛體和運(yùn)動副模塊來描述各個物體之間的相互運(yùn)動關(guān)系，如圖3所示。

圖3 3-RPS并聯(lián)機(jī)構(gòu)結(jié)構(gòu)模型Fig.3 Structure Model of 3-RPS Parallel Mechanism

4.2 數(shù)學(xué)模型的建立

根據(jù)求出的雅可比矩陣，我們可以在Matlab中建立3-RPS并聯(lián)機(jī)構(gòu)的數(shù)學(xué)模型，因?yàn)?-RPS并聯(lián)機(jī)構(gòu)具有兩轉(zhuǎn)動一平移，則它的轉(zhuǎn)動的變換矩陣依據(jù) R（α，β）=R（x，α）R（y，β）可求得，而3-RPS并聯(lián)機(jī)構(gòu)的移動變換矩陣則為式（1）。

將之前求得的轉(zhuǎn)動旋轉(zhuǎn)矩陣與移動變換矩陣帶入EulerXYZ中，同時(shí)在Desired1中給定3-RPS并聯(lián)機(jī)構(gòu)的輸入信號，position matrix為位置矩陣，leg length為初始桿長矩陣，body_pts為并聯(lián)機(jī)構(gòu)動平臺坐標(biāo)矩陣，pos_base則為定平臺坐標(biāo)矩陣。

圖4 3-RPS并聯(lián)機(jī)構(gòu)數(shù)學(xué)模型Fig.4 Mathematical Model of 3-RPS Parallel Mechanism

5 控制器的設(shè)計(jì)

5.1 PID控制器設(shè)計(jì)

PID控制器是一種廣泛使用的工業(yè)控制器，，使用控制誤差比例，積分和微分作用形成控制器的輸出，并且這些組件的線性組合可以折衷系統(tǒng)響應(yīng)速度和穩(wěn)定性性的性能[9]。采用試湊法對PID控制器參數(shù)進(jìn)行調(diào)整，使3-RPS并聯(lián)機(jī)構(gòu)達(dá)到最佳的運(yùn)動軌跡，同樣這也是最常用的方法之一，3-RPS并聯(lián)機(jī)構(gòu)PID控制器框圖，如圖5所示。其中，Integrator為連續(xù)時(shí)間積分，Gain為增益。

圖5 PID控制系統(tǒng)框圖Fig.5 Block Diagram of PID Control System

5.2 基于比例切換函數(shù)滑模變結(jié)構(gòu)控制器設(shè)計(jì)

設(shè) 3-RPS并聯(lián)機(jī)構(gòu)動平臺的期望位姿為qd（zd，αd，βd），系統(tǒng)的誤差信號為e=q-qd，設(shè)計(jì)切換函數(shù)為：s=ce+e˙

式中：c=diag（c1，c2，c3），其中 c1，c2，c3均為可調(diào)參數(shù)。

由于 M（q）為正定對稱陣，且矩陣函數(shù)M˙（q）+2C（q，q˙）對于任意 q，q˙，具有斜對稱性，可得：

根據(jù)Lyapunov穩(wěn)定性定理，可見所設(shè)計(jì)控制算法穩(wěn)定，設(shè)計(jì)SMC控制器的Simulink框圖。3-RPS的SMC控制器總圖，如圖6所示。

圖6 SMC控制總圖Fig.6 General of SMC Control

6 控制仿真結(jié)果

為了驗(yàn)證所設(shè)計(jì)的控制器的有效性，在Matlab的Simulink中進(jìn)行仿真。用兩種控制方法對3-RPS并聯(lián)機(jī)構(gòu)進(jìn)行控制仿真，分別得出兩種控制對于3-RPS的三自由度位移軌跡誤差曲線，3-PRS與定平臺相連的桿長為60mm，與動平臺相連的桿長為60mm，兩個桿長的可移動距離為40mm，三條支鏈與定平臺的角度φ1=φ2=φ3=60°，定平臺的邊長為120mm，動平臺的邊長為60mm。

通過不斷調(diào)節(jié)PID參數(shù)使得3-PRS并聯(lián)機(jī)構(gòu)控制達(dá)到最佳效果，此時(shí)PID控制器的參數(shù)為kp=48，kI=48，kD=48。不斷調(diào)整SMC控制器模型中的ω和φ的值，直到控制曲線趨于穩(wěn)定且無明顯抖振現(xiàn)象時(shí)，此時(shí)ω=714，ψ=163，并通過在Simulink仿真中得出驅(qū)動桿的實(shí)際位移與理論位移的誤差變化。

PID控制器與滑模變結(jié)構(gòu)控制器對于3-RPS并聯(lián)機(jī)構(gòu)三條支鏈的位移軌跡誤差曲線，如圖7～圖9所示。其中，虛ωΨ代表PID控制誤差曲線，實(shí)線代表基于比例控制的SMC控制誤差曲線。由圖可知，清楚的反映了PID控制和SMC控制實(shí)際位移和理論位移的誤差大小，SMC控制與PID控制相比的誤差小，并且兩者趨近于穩(wěn)定誤差的時(shí)間，SMC控制比PID控制的時(shí)間更短。

圖7 支鏈A1B1位移誤差Fig.7 Displacement Error of Branched Chain A1B1

圖8 支鏈A2B2位移誤差Fig.8 Displacement Error of Branched Chain A2B2

圖9 支鏈A3B3位移誤差Fig.9 Displacement Error of Branched Chain A3B3

7 結(jié)論

通過微運(yùn)動建立3-RPS并聯(lián)機(jī)構(gòu)的輸入-輸出Jacobian矩陣，為機(jī)構(gòu)運(yùn)動學(xué)和控制仿真分析奠定了理論基礎(chǔ)。并根據(jù)3-RPS并聯(lián)機(jī)構(gòu)的三維模型和動力學(xué)方程，設(shè)計(jì)了PID控制器和基于比例控制的SMC控制器，通過在Simulink中進(jìn)行建模與仿真的結(jié)果表明，滑模變結(jié)構(gòu)控制優(yōu)于傳統(tǒng)的PID控制，采用SMC控制器控制3-RPS并聯(lián)機(jī)構(gòu)調(diào)整時(shí)間短，誤差小，并且響應(yīng)速度快。控制效果好，基于比例切換函數(shù)的SMC控制器對于并聯(lián)機(jī)構(gòu)的控制是有效的。