基于MATLAB的3-RPS并聯機構控制系統仿真

王愛國，陳健偉

（1.合肥工業大學機械與汽車工程學院，安徽合肥　230009；2.安徽機電職業技術學院，安徽蕪湖　241002；3.江西理工大學機電工程學院，江西贛州　341000）

基于MATLAB的3-RPS并聯機構控制系統仿真

王愛國1，2，陳健偉3

（1.合肥工業大學機械與汽車工程學院，安徽合肥230009；2.安徽機電職業技術學院，安徽蕪湖241002；3.江西理工大學機電工程學院，江西贛州341000）

針對傳統螺旋法求解并聯機構Jacobian矩陣的缺陷，提出了一種基于并聯機構微運動分析的微位移法.該微位移法在求解過程中可避免求解超越方程組的難題，且得到的Jacobian矩陣為非奇異方陣.同時，針對并聯機構傳統控制器軌跡跟蹤精度低的問題，設計了一種基于并聯機構動力學方程和比例切換控制律的滑模變結構控制器（SMC）.首先，建立機構的MATLAB/SimMechanics仿真分析模型，采用微位移法求解并聯機構的Jacobian矩陣，為控制系統提供理論參考輸入值.其次，設計SMC控制器，運用Lyapunov函數證明控制器的穩定性；最后，分別建立機構PID控制和SMC控制系統的MATLAB/Simulink框圖，對其進行仿真分析與對比.結果表明：SMC控制器的軌跡跟蹤精度比PID控制器的精度高，穩態誤差小，且魯棒性強，響應速度快，從而驗證了SMC控制的有效性.關鍵詞：3-RPS并聯機構；微位移法；PID控制；SMC控制；MATLAB仿真

并聯機構的微運動分析是機構運動特性研究的重要組成部分，也是機構誤差分析和控制仿真的基礎［1］.并聯機構具有承載能力強、位置精度高、結構剛度大等優點，其早期的主要研究對象為Stewart平臺［2］.少自由度并聯機構的構型簡單易實現、動力學求解容易，因此少自由度并聯機構的研究越來越普遍.Tsai等人［3］采用螺旋理論的方法求解出3-RPS并聯機構的Jacobian矩陣，并列舉了所有情況的奇異矩陣.但是，得到的Jacobian矩陣不是方陣，不能作為控制系統的參考輸入值.Jacobian矩陣的正確與否直接影響著機構的軌跡跟蹤精度，而軌跡跟蹤精度是并聯機構控制性能的重要指標，因此如何提高并聯機構的軌跡跟蹤精度逐漸成為學者們關注的焦點.朱大昌等人［4］基于廣義Jacobian矩陣，采用模糊PID的控制方法，對3-RPC并聯機構各支鏈位移的軌跡跟蹤誤差進行了研究.Park等人［5］提出了一種基于攝動觀測器的變結構控制方法，探討了機構在不確定項和非線性條件下的穩定性問題. Wu等人［6］將PI控制器與模糊自整定PD控制器相結合，設計了一種混合自整定的PID控制器，解決了控制器軌跡跟蹤的穩定性和收斂性問題.為提高并聯機構的軌跡跟蹤精度，本文基于機構的動力學方程和比例切換控制律，設計了一種以3-RPS并聯機構為研究對象的滑模變結構控制系統.首先，通過Solidworks與MATLAB軟件的關聯插件，將3-RPS并聯機構的Solidworks三維模型轉換為MATLAB/SimMechanics仿真分析模型，采用微位移法求解3-RPS并聯機構的Jacobian矩陣，給控制系統提供參考輸入值.其次，設計滑模變結構控制器（SMC），并證明控制器的穩定性.最后，分別采用PID和SMC控制方式對機構進行仿真分析，對比動平臺的位置變化曲線和軌跡跟蹤誤差曲線，從而證明SMC控制的有效性.

1　3-RPS并聯機構MATLAB建模

3-RPS是具有對稱結構的三自由度并聯機構，其構型簡圖如圖1所示.機構的動、定平臺分別由2個等邊三角形△abc和△ABC組成，動、定平臺之間通過3條支鏈相連接，其中每條支鏈含有1個R副（轉動副）、1個P副（移動副）和1個S副（球副）.定平臺與R副相連，動平臺與S副相連，P副連接著R副和S副.分別建立機構的動、定平臺坐標系o-xyz和O-XYZ，o和O表示動、定平臺的中心.其中，動、定平臺的邊長分別為m和n，驅動桿長為l，支鏈與定平臺的夾角為φi.

SolidWorks軟件具備完善的三維建模功能，而MATLAB軟件在機構運動學和動力學仿真領域具有優越性，通過插件（SimMechanics Link）可以很好地結合SolidWorks和MATLAB軟件.運用Solid-Works建立3-RPS并聯機構的三維模型，將裝配體的.SLDASM文件另存為.XML文件；在MATLAB的命令窗口中輸入mech_import命令，載入剛剛保存的.XML文件；在框圖中添加所需的檢測和驅動模塊，并設置好相應構件的參數，即可建立機構的MATLAB/SimMechanics模型.3-RPS并聯機構的MATLAB簡圖如圖2所示，機構的MATLAB/Sim-Mechanics模型如圖3所示，其支鏈的MATLAB/Sim-Mechanics模型如圖4所示.3-RPS并聯機構各構件的坐標參數值如表1所示.

圖1　3-RPS并聯機構構型簡圖Fig.1 Structure diagram of 3-RPS parallel mechanism

圖2　3-RPS并聯機構MATLAB簡圖Fig.2 MATLAB Diagram of 3-RPS parallel mechanism

圖3　3-RPS并聯機構SimMechanics模型Fig.3 SimMechanics model of 3-RPS parallel mechanism

圖4　3-RPS并聯機構支鏈SimMechanics模型Fig.4 SimMechanics model of 3-RPS parallel mechanism's branched chain

表1　3-RPS并聯機構各構件的坐標參數Table 1 Coordinate parameters of 3-RPS parallel mechanism's each component

2　微運動分析

并聯機構微運動分析的主要目的是得到其輸入輸出的Jacobian矩陣.對于多自由度的并聯機構而言，可采用螺旋法求解Jacobian矩陣.但是，采用螺旋法求解少自由度并聯機構的Jacobian矩陣，得到的Jacobian矩陣往往不是非奇異方陣，且不能避免求解超越方程組的問題.為了避免上述難題，本文提出了一種求解Jacobian矩陣的新方法，即微位移法.微位移法的基本思想為：在機構微運動分析的基礎上，把無窮小量及其高階無窮小量等價成零，將超越方程組簡化，以利于提取機構的輸入與輸出量.當機構的轉角δα，δβ，δγ或者移動距離dx，dy，dz較小時，

其中δv表示δα，δβ，δγ，dx，dy，dz及其高階量.應用微位移法求解得到的Jacobian矩陣為非奇異方陣，可以直接作為并聯機構仿真系統的理論參考輸入值.

假設機構動平臺坐標系的x，y，z軸相對于定平臺分別轉動了δγ，δβ，δα以及平移了dx，dy，dz，根據靜坐標系變換原則，可得到相應的旋轉變換矩陣Rα，β，γ和平移變換矩陣Ttrans.

其中，c（）=cos（），s（）=sin（），下同.

綜上，可得到齊次變換矩陣為

定平臺各鉸鏈中心點A，B，C在O-XYZ中的坐標向量為

動平臺各鉸鏈中心點a，b，c在o-xyz中的坐標向量為

由齊次變換矩陣T，根據公式P′=TP可得動平臺各個鉸鏈中心向量oa，ob，oc旋轉變換后在基坐標O-XYZ中的齊次坐標向量形式，如式（2）所示.由于3-RPS并聯機構實現的是繞x軸、y軸的轉動

和沿z軸的平移，所以將機構繞x軸的轉動δα、繞y軸的轉動δβ和沿z軸的平移dz作為機構的輸出量.根據δα，δβ，dz求解各桿的桿長變化，并將其作為機構的輸入量.

將機構的所有參數寫入MATLAB的m文件中，編寫求解桿長變化量和輸入輸出Jacobian矩陣的程序.通過MATLAB計算各桿長的變化量，可得到Δli與δα，δβ，dz之間的關系，其齊次坐標形式如式（3）所示.

記［Δl1Δl2Δl31］T=Q［δαδβdz1］T，根據Jacobian矩陣的定義［δαδβdz1］T=J［Δl1Δl2Δl31］T，運用微位移法得到的機構輸入輸出Jacobian矩陣J =［Q］-1.將機構的Jacobian矩陣寫入圖5的Leg Trajectory模塊，為控制仿真提供參考輸入.

3　3-RPS并聯機構控制系統設計

3-RPS并聯機構控制系統的總框圖如圖5所示，其中：Controller為系統的控制器；Plant是機構的MATLAB/SimMechanics模型，即圖3所示的系統被控對象；Body Sensor是動平臺傳感器；Scope1和Scope2是示波器，分別顯示動平臺的位置和誤差曲線；Gaussian Noise Generator為外加的高斯噪聲干擾信號，用于驗證控制器的抗干擾性；Leg Trajectory為機構的參考輸入值，其主要作用是根據Jacobian矩陣計算出各桿桿長的變化量.其框圖如圖6所示.其中，EulerXYZ為第2節計算的旋轉變換矩陣，position matrix為位置矩陣，body_pts，pos_base，nominal leg length分別是由機構的尺寸參數組成的矩陣，Desired1為動平臺的期望輸入信號.

圖5　3-RPS并聯機構控制系統總框圖Fig.5 Total block diagram of 3-RPS parallel mechanism's control system

圖6　控制系統參考輸入框圖Fig.6 Block diagram of control system's reference input

3.1傳統PID控制器的設計

PID控制器因其控制算法成熟，已在工程設計中得到了普遍應用.但是，如何確定PID控制器的各個參數是控制器設計的關鍵問題之一.由于經驗湊試法具有較強的適用性，可用于各種控制系統，本文采用經驗湊試法來調整PID控制器的各個參數.3-RPS并聯機構PID控制器MATLAB/Simulink框圖如圖7所示.其中，Integrator為連續時間積分器，Derivative為連續時間微分器.

圖7　PID控制器MATLAB/Simulink框圖Fig.7 MATLAB/Simulink model of PID controller

3.2設計滑模變結構控制器

根據拉格朗日方程，采用獨立坐標描述的3-RPS并聯機構的動力學方程［7］為

2）矩陣M（q）-2Vm（q，）是斜對稱矩陣，即對于任意的向量ξ，有3）定義參數向量Ω，Ω表示M，Vm，F，G的定常

M（q），Vm（q），F）和G（q）滿足線性關系：

M（q）ω+Vm（q）ψ+F）+G（q）+τd=Φ（q，ω，ψ）Ω.

在分析證明SMC控制器的Lyapunov穩定性過程中，需要利用上述性質.

設計滑模變結構控制器（SMC）主要包括設計滑動模態控制律u（x）和切換函數s（x）兩個相對獨立的部分.下面基于3-RPS并聯機構動力學模型，結合比例切換控制律，設計機構的滑模變結構控制器，并根據Lyapunov函數證明SMC控制器的穩定性.

假設系統的理想位置信號為qd（t），誤差信號為e（t）=q（t）-qd（t）.設計的滑模函數為

其中，C=diag（c1，…，cn），ci＞0.選取的Lyapunov函數為V（x）=sTMs，則

其中，ΦΩ0為機構的精確函數矩陣，ω和ψ為控制律大于零的常數，即需要確定的控制參數.由式（4）可知

取ui（x）＞Φ|ΔΩ|max，則˙V≤0.因此，控制系統具有穩定性.根據式（4）的控制律和ui（x）＞Φ|ΔΩ|max的約束條件，SMC控制器的MATLAB/Simulink框圖如圖8所示.其中，Gain1和Gain2分別表示參數ω和ψ的設定值，M表示機構的慣性矩陣，V表示科氏力和離心力矩陣，C1為摩擦矩陣，C2為重力矩陣.

4　仿真分析

設定初始參數：動平臺的質量為1kg，轉動慣量Ixx=11.7kg·m2，Iyy=11.8kg·m2，Izz=20.1 kg·m2，φ1=φ2=φ3=60°，m=50mm，n=100 mm，l=120mm.動平臺的輸入期望參考值：y=0.1sin（3πt），仿真時間為10s.根據經驗湊試法，不斷調整P，I，D的值，Kp=16，Ki=1，Kd=0.2；根據SMC控制器框圖，不斷調整ω和ψ的值，ω=1 540，ψ=53.PID和SMC控制動平臺的位置變化曲線如圖9和圖10所示，軌跡跟蹤曲線如圖11和圖12所示.為驗證PID控制和SMC控制的抗干擾能力，在控制器的輸出端添加一個高斯噪聲干擾信號，則系統的軌跡追蹤曲線如圖13和圖14所示.

圖8　SMC控制器MATLAB/Simulink框圖Fig.8 MATLAB/Simulink model of SMC controller

圖9　PID控制動平臺位置變化曲線Fig.9 Position change curve of moving platform by PID controller

圖10　SMC控制動平臺位置變化曲線Fig.10 Position change curve of moving platform by SMC controller

圖11　PID控制動平臺軌跡跟蹤曲線Fig.11 Trajectory tracking curve of moving platform by PID controller

圖13　添加高斯噪聲后PID控制動平臺軌跡跟蹤曲線Fig.13 Trajectory tracking curve of moving platform by PID controller after adding Gauss noise

圖14　添加高斯噪聲后SMC控制動平臺軌跡跟蹤曲線Fig.14 Trajectory tracking curve of moving platform by SMC controller after adding Gauss noise

對比圖9和圖10可知，在相同的輸入條件下，SMC控制動平臺的位置變化相對于PID更平穩，運動更趨于穩定.對比圖11和圖14，機構在較短的時間內，2種控制方法都出現了一定的波動.PID控制雖有一定的調平能力，但抗干擾能力較差，機構的軌跡跟蹤效果不好.SMC控制的抗干擾能力強，機構的軌跡跟蹤效果相對于PID控制更好.

5　總 結

通過微位移法分析了3-RPS并聯機構的輸入輸出Jacobian矩陣，為機構的控制仿真提供理論的參考輸入值.根據3-RPS并聯機構的MATLAB仿真模型和動力學方程，設計了PID控制器和基于比例切換律的SMC控制器，并對其進行了仿真和對比.結果表明：SMC控制器與PID控制器相比，具有魯棒性強、跟蹤精度高、響應速度快、穩態誤差小等優點，滿足并聯機構高精度的軌跡跟蹤要求，在工程實踐中具有重要的應用價值.

［1］黃真，孔令富，方躍法.并聯機器人機構學理論及控制［M］.北京：機械工業出版社，1997：36-37. HUANG Zhen，KONG Ling-fu，FANG Yue-fa.Theory and control of parallel robot mechanism［M］.Beijing：Mechanical Industry Press，1997：36-37.

［2］STEWART D.A platform with six-degrees-of-freedom［J］.Proceeding of the Institute of Mechanical Engineering，1965，180（5）：371-386.

［3］TSAI L W.Direct kinematic analysis of a 3-RPS parallel manipulator［J］.Mechanical and Machine Theory，2003，38（1）：71-83.

［4］朱大昌，劉云鴻，馮文結.3-RPC型并聯機器人模糊PID控制系統研究［J］.機械傳動，2014，38（2）：114-117.

ZHU Da-chang，LIU Yun-hong，FENG Wen-jie.Research on the fuzzy PID control system of 3-RPC parallel robot［J］.Mechanical Transmission，2014，38（2）：114-117.

［5］PARK Minkyn，LEE Min-cheol，GO Seok-jo.The design of sliding mode controller with perturbation observer for a 6-DOF parallel manipulator［C］//Proceedings of ISIE 2001.Pusan，Korea，March 10-14，2001：1502-1508.

［6］WU Hua-peng，HANDROOS Heikki.Hybrid fuzzy selftunning PID controller for a parallel manipulator［C］//Proceedings of the 5th Word Congress on Intelligent Control and Automation.Hangzhou，China，June 15-19，2004：2545-2550.

［7］楊香蘭.6-DOF并聯機器人動力學建模及模糊變結構控制［D］.河北：燕山大學機械工程學院，2001：28-31.

YANG Xiang-lan.The dynamics modeling and fuzzy variable structure control for 6-DOF parallel robot［D］.Hebei：Yanshan University，College of Mechanical Engineering，2001：28-31.

［8］SHIJING Li，ZUREN Feng，HAO Feng.Variable structure control for 6-6parallel manipulators based on CMAC［C］//Proceedings of 4th World Congress on Intelligent Control and Automation.Shanghai，China，June 15-19，2002：1939-1944.

［9］劉金琨.滑模變結構控制MATLAB仿真［M］.北京：清華大學出版社，2005：25-30.

LIU Jin-kun.MATLAB simulation for sliding mode variable structure control［M］.Beijing：Tsinghua University Press，2005：25-30.

［10］CHENG C，CHEN S H.Design of adaptive variable structure controllers with application to robot manipulators［C］//Proceedings of 5th World Congress on In-telligent Control and Automation.June 15-19，2004：4904-4908.

［11］張立勛.機電系統建模與仿真［M］.哈爾濱：哈爾濱工業大學出版社，2009：140-147.

ZHANG Li-xun.Modeling and simulation of mechanical and electrical systems［M］.Harbin：Harbin Institute of Technology Press，2009：140-147.

［12］朱大昌，李培，顧起華.全柔順并聯機構理論［M］.北京：冶金工業出版社，2013：153-163.

ZHU Da-chang，LI Pei，GU Qi-hua.The theory of compliant parallel mechanism［M］.Beijing：Metallurgical In-dustry Press，2013：153-163.

［13］VICENTE P V，SUGURN A.Dynamic sliding PID control for tracking of robot manipulators：theory and experiments［J］.IEEE Transaction on Robotics and Automation，2003，19（6）：967-976.

Simulation for control system of 3-RPS parallel mechanism based on MATLAB

WANG Ai-guo1，2，CHEN Jian-wei3
（1.School of Mechanical and Automotive Engineering，Hefei University of Technology，Hefei 230009，China；2.Anhui Technical College of Mechanical and Electrical Engineering，Wuhu 241002，China；3.School of Mechanical and Electrical Engineering，Jiangxi University of Science and Technology，Ganzhou 341000，China）

Owing to the defects of traditional spiral method to solve Jacobian matrix of parallel mechanism，a micro-displacement method was put forward based on micro-motion analysis of 3-RPS parallel mechanism.The method could avoid the problem of transcendental equation group in the solving process，and the Jacobian matrix acquired was a non singular matrix.At the same time，aiming at the low trajectory tracking precision of conventional controller of parallel mechanism，the sliding model controller（SMC）was designed by the dynamic equation of 3-RPS parallel mechanism and proportional switching control law.Firstly，the MATLAB/SimMechanics simulation analysis model of 3-RPS parallel mechanism was presented and the Jacobian matrix of 3-RPS parallel manipulator was solved by micro-displacement method，which was the theoretical reference value for control system.Secondly，the SMC controller was designed，then the stability of SMC controller was proven based on the Lyapunov function.Finally，the MATLAB/Simulink model of PID controller and SMC controller were respectively established，and the simulation analysis and comparison were performed.The results show that the SMC controller has higher trajectory tracking precision，smaller steady-state error，faster response speed and better robustness than that of PID controller，so the effectiveness of SMC controller is verified.

3-RPS parallel manipulator；micro-displacement method；PID controller；SMC controller；MATLAB simulation

TP 15

A

1006-754X（2016）02-0172-09

10.3785/j.issn.1006-754X.2016.02.011

2015-12-22.本刊網址·在線期刊：http：//www.journals.zju.edu.cn/gcsjxb

國家自然科學基金資助項目（51175135）；安徽省自然科學基金重點資助項目（KJ2016A138）.

王愛國（1976—），男，安徽合肥人，博士生，副教授，從事控制技術研究，E-mail：wangaiguo_2003@163.com. http：//orcid.org//0000-0003-4621-9051