上海大學(xué)智能制造及機器人重點實驗室，德國倍福自動化有限公司 王進上海大學(xué)智能制造及機器人重點實驗室 郭帥，聶松亮

基于TwinCAT3的Stewart平臺控制系統(tǒng)設(shè)計

上海大學(xué)智能制造及機器人重點實驗室，德國倍福自動化有限公司 王進上海大學(xué)智能制造及機器人重點實驗室 郭帥，聶松亮

基于倍福TwinCAT3軟件, 本文詳細闡述了使用倍福PC控制技術(shù)完成Stewart平臺的控制系統(tǒng)的設(shè)計流程。首先推導(dǎo)了六自由度并聯(lián)平臺的正、逆解算法。其次利用MATLAB/SIMULINK軟件中的M函數(shù)生成正、逆解模塊。再次，通過TwinCAT3的TE1400組件將SIMULINK代碼生成器的目標配置為TwinCAT，生成可以集成到TwinCAT3環(huán)境的SIMULINK語言的TcCOM模型。最后在TwinCAT3軟件中加載TcCOM模型，利用TwinCAT軟件自帶的運動控制功能塊和NC軌跡生成器通過EtherCAT總線實時地對電機和平臺進行控制。結(jié)果顯示TwinCAT3軟件與MATLAB結(jié)合使用給機器人控制系統(tǒng)設(shè)計帶來的顯著優(yōu)越性。

Stewart平臺；TwinCAT；SIMULINK；PC控制

1 引言

并聯(lián)機器人剛度大，慣量低，精度高，可廣泛應(yīng)用于工業(yè)、航空、軍事、娛樂等領(lǐng)域[1]。Stewart平臺具有俯仰、橫滾、偏航、升降、縱向平移和側(cè)向平移六個自由度。由于其獨特的結(jié)構(gòu)，在工業(yè)領(lǐng)域，與機床技術(shù)相結(jié)合，相比傳統(tǒng)機床具有剛度好、精度高、高速和高加速度等特點。在運動仿真領(lǐng)域，通過在活動平臺上安置座艙，配合虛擬現(xiàn)實等技術(shù)手段可以開發(fā)各類運動模擬器，用于模擬飛機飛行、艦船航行以及車輛行駛。在測量測試領(lǐng)域，可以為多種設(shè)備提供模擬測試平臺。在空間技術(shù)領(lǐng)域，可以研制開發(fā)空間飛行器對接機構(gòu)。當(dāng)前，六自由度并聯(lián)機構(gòu)動力學(xué)和運動學(xué)算法日漸成熟。但是怎么把仿真算法模型高效率地放在運動控制器運行是個新的難題。MATLAB具有強大的數(shù)學(xué)運算能力，同時SIMULINK標準庫又提供了大量的圖形化功能塊，減少了工程人員建立機器人模型所需要的時間。但是MATLAB軟件不具有實時控制自動化硬件的能力。而傳統(tǒng)的PLC雖然可以控制I/O模塊和伺服電機，但是缺少像MATLAB那樣方便強大的矩陣運算能力和SIMULINK庫中大量的算法功能塊。

倍福TwinCAT3軟件是基于Windows軟硬件平臺的實時控制軟件[2]，它通過內(nèi)部對Windows底層的更改，實現(xiàn)了對I/O模塊和驅(qū)動器的實時控制。該軟件采用Visual Studio的Shell，支持符合IEC61131的PLC語言及C++的混合編程，另外該軟件通過TcCom Object功能可以導(dǎo)入MATLAB/SIMULINK生成的模塊。該軟件的PLC有豐富的運動控制功能庫可用于軌跡規(guī)劃和對伺服軸進行控制。同時采用倍福的百兆EtherCAT總線可以方便靈活實時地對伺服驅(qū)動器進行控制。

建模仿真技術(shù)與自動化技術(shù)相結(jié)合是機器人設(shè)計的趨勢。本文以Stewart平臺為例，基于Windows平臺的TwinCAT3軟件和MATLAB/SIMULINK軟件完成了Stewart平臺控制系統(tǒng)的研發(fā)，實現(xiàn)了運動學(xué)算法的編寫和對平臺的實時控制。

圖1 平臺實物圖

Stewart運動平臺（如圖1所示）是由六個電動缸通過萬向節(jié)鉸鏈（或球鉸鏈）將上、下兩個平臺連接而成。下平臺固定在基礎(chǔ)上，借助六個電動缸的伸縮運動，完成上平臺在三維空間六個自由度（X，Y，Z，α，β，γ）的運動，從而模擬出各種空間運動姿態(tài)。六個電動缸采用倍福的基于EtheCAT總線的絕對值編碼器伺服電機，可以通過網(wǎng)線直接連接到倍福的PC控制器?？刂破鞑捎帽陡X2020嵌入式控制器，CPU為賽揚1.4G單核處理器，內(nèi)裝Win7E系統(tǒng)可以方便地兼容其他Windows平臺的軟件。

2 運動學(xué)分析

并聯(lián)機器人機構(gòu)運動學(xué)分析包括正運動學(xué)分析和逆運動學(xué)分析兩個方面。在求解前首先建立坐標系。

如圖2所示，在下平臺建立靜止坐標系O-XYZ，其中X軸平行于鉸點A4、A5所在直線。在上平臺建立動坐標系o-xyz。上平臺的運動可分解為X、Y、Z方向的平移，以及繞其坐標軸的旋轉(zhuǎn)α，β，γ。下平臺各鉸點Ai的坐標為（XAi，YAi，ZAi），上平臺各鉸點ai的動坐標為（xai，yai，zai）靜坐標為（Xai，Yai，Zai）與其對應(yīng)的相連的電動缸長度為 (i=1，……，6)

圖2 Stewart 平臺坐標系定義

2.1 位置逆解

并聯(lián)機構(gòu)的運動學(xué)逆解是從工作坐標反求關(guān)節(jié)坐標，有唯一解。在平臺的實際應(yīng)用中，通過路徑規(guī)劃算法每個周期給定的是平臺的六個空間姿態(tài)參數(shù)X，Y，Z，α，β，γ。此過程的實質(zhì)是在平臺運動中，輸入給定的位姿，控制伺服電機使電動缸到達所需長度。

我們以1號電動缸為例,（Xa1，Ya1，Za1）和（XA1，YA1，ZA1）分別是上、下鉸點在靜止坐標系中的坐標，（xa1，ya1，za1）為上鉸點在運動坐標系中的坐標，（X，Y，Z）為上平臺中心在靜止坐標系中的坐標，其關(guān)系可如下表達

其中T是旋轉(zhuǎn)變換矩陣，T=c×b×a。其中c表示繞z軸旋轉(zhuǎn)γ，b表示繞y軸旋轉(zhuǎn)β，a表示繞x軸旋轉(zhuǎn)α[3]，分別如下所示：

旋轉(zhuǎn)變換矩陣T=

將矩陣T代入公式（1），即可求出（Xa1, Ya1, Za1），從而可以求出電動缸的長度L1為：

同理可得L2…L6。[4]

2.2 位置正解

已知機構(gòu)輸入件的位置，求解機構(gòu)輸出件的位置和姿態(tài)稱為機構(gòu)的位置正解。不同于串聯(lián)機器人，六自由度運動平臺的正解非常復(fù)雜，并且存在多解。其求解方法大致可分為代數(shù)解析法[5、6]和數(shù)值法[7、8]。解析法的研究集中在尋找全部解，而數(shù)值法的研究注重提高求解效率以滿足實時性的要求，本文采用數(shù)值法求正解。六自由度平臺的逆解求解過程上一節(jié)中已經(jīng)推導(dǎo)，為了簡化正解的求解過程，本文使用牛頓迭代法。利用逆解算法迭代電動缸桿長采用逐次逼近數(shù)值法來求位置正解。由于本平臺電機采用絕對值多圈編碼器，所以上電后電機編碼器的位置反饋可以轉(zhuǎn)化為電動缸的桿長，再經(jīng)過位置正解求出平臺的空間姿態(tài)。圖3是正解算法的流程圖。

圖3 運動學(xué)正解算法流程圖

3 軟件設(shè)計

3.1 MATLAB編程和生成TcCOM Object

首先利用MATLAB強大的矩陣運算和數(shù)學(xué)處理能力，把上文敘述的正逆解算法，采用M函數(shù)做成函數(shù)功能塊，在SIMULINK中調(diào)用這個功能塊并添加輸入輸出接口，如圖4所示。

圖4 在MATLAB/SIMULINK中編寫正、逆解功能塊

倍福TwinCAT3軟件的TE1400組件是一個SIMULINK代碼編譯器，可以把SIMULINK中執(zhí)行的圖形化功能塊生成可以實時運行的C或者C++的代碼。在一臺安裝有MATLAB的電腦上安裝TE1400后把SIMULINK代碼生成器的目標配置為TwinCAT，就可以把SIMULINK中的正逆解工程文件生成可以集成到TwinCAT3環(huán)境的具有輸入輸出變量的SIMULINK語言的TcCOM模型。

3.2 TwinCAT軟件配置和編程

首先在TwinCAT軟件的TcCOM Object中調(diào)用上文中生成的正逆解功能塊。可以看到加載的Lpt-backward和Lpt-forward功能塊的圖形與變量都與SIMULINK中一致。如圖5所示。

圖5 在TwinCAT中導(dǎo)入正、逆解功能塊

由于Stewart平臺具有笛卡爾坐標系的六個自由度并且需要控制六個關(guān)節(jié)電機。在倍福的TwinCAT軟件的NC中首先添加A1到A6六個關(guān)節(jié)坐標系軸并鏈接到驅(qū)動器和PLC，再添加X、Y、Z、A、B、C（X、Y、Z、α、β、γ）六個笛卡爾坐標系軸鏈接到PLC。對于正解將六個電動缸的伺服電機A1到A6的位置作為輸入變量鏈接到正解功能塊，功能塊輸出X、Y、Z、A、B、C的值作為平臺在笛卡爾坐標系中的位置。而逆解功能塊將給定的X、Y、Z、A、B、C位姿作為輸入變量，輸出變量則作為電動缸的給定位置通過外部給定功能塊發(fā)給NC再發(fā)給驅(qū)動器。

在PLC程序中，直接對笛卡爾坐標系中的X、Y、Z、A、B、C六個虛擬軸進行控制。調(diào)用TwinCAT軟件的運動控制庫中的點動、定位、凸輪等功能塊，可以方便地讓機器人移動到某一位姿或者進行連續(xù)循環(huán)運動。其原理是觸發(fā)功能塊后TwinCAT軟件的NC運算核會自動按照樣條曲線規(guī)劃出運動路徑，并以2ms的周期間隔計算出下一目標位置，我們將這一位置發(fā)送給機器人運行模塊，模塊會實時地逆解出電機的角度，再通過外部給定發(fā)送給伺服驅(qū)動器。通過NC運算核和機器人運算核，伺服驅(qū)動器每2ms會收到一個位置數(shù)據(jù)，并讓電機運轉(zhuǎn)到這一位置。由于NC的路徑規(guī)劃功能強大，這些位置數(shù)據(jù)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)都是連續(xù)的，從而保證了伺服驅(qū)動器的連續(xù)穩(wěn)定運行，如圖6所示。

圖6 系統(tǒng)框圖

另外還可以通過高級語言或者第三方軟件編輯預(yù)定軌跡，按照2ms的時間間隔發(fā)送給機器人運行程序，讓機器人按照預(yù)定軌跡完成一連串動作。在調(diào)試階段有時候需要不通過機器人正逆解而直接對A1到A6電機進行單獨操作，所以在PLC程序中還增加了對實軸電機的單獨操作功能。倍福的TwinCAT軟件可以通過ADS通信和C#開發(fā)的HMI軟件進行數(shù)據(jù)交換，HMI界面如圖7所示。

圖7 HMI界面

3.3 硬件平臺

如圖8拓撲結(jié)構(gòu)所示，在工控機的網(wǎng)口通過網(wǎng)線直接連接倍福輸入輸出I/O模塊以及六個伺服驅(qū)動器AX5101。輸入模塊EL1008用作啟停按鈕和限位開關(guān)，輸出模塊el2008用作報警等輸出。

圖8 硬件鏈接拓撲結(jié)構(gòu)圖

倍福的EtherCAT總線是一種高性能的以太網(wǎng)總線，與100個伺服軸的通訊只需100μs 。在此期間，可以向所有軸提供給定位置和速度以及控制字，并接收它們的實際位置和速度以及狀態(tài)字。分布式時鐘技術(shù)保證了這些軸之間的同步時間偏差小于1μs 。從硬件上滿足了Stewart平臺的控制要求。

4 仿真和測試

為了驗證算法和編程的正確性，首先將正逆解算法在MATLAB軟件中進行編程，對于正解算法和逆解算法都能得到與輸入變量對應(yīng)的輸出變量。然后將算法移植到TwinCAT軟件中，利用TwinCAT軟件的可以不帶硬件運行的功能進行測試。最后把TwinCAT軟件的軸變量與硬件電機驅(qū)動器相鏈接，運行后可以看到Stewart運動平臺可以按照預(yù)定的軌跡進行正常運行，運行效果如圖9所示。藍色為給定X軸位置，紅色為X軸實際位置。

圖9 運行效果圖

5 結(jié)語

本文主要介紹了利用TwinCAT3和MATLAB軟件設(shè)計Stewart運動平臺的方法。首先研究了運動平臺的正解和逆解算法，然后對于本文的正逆解算法在MATLAB軟件中進行了編程和仿真并利用TE1400組件導(dǎo)入到TwinCAT3，最后在倍福TwinCAT3軟件平臺進行了接口和操作界面編程并在基于PC和EtherCAT總線的硬件平臺進行了實際運行。運行結(jié)果表明本設(shè)計可以對Stewart平臺進行定位操作、按預(yù)置軌跡操作、反饋位姿信息等各種操作。從而驗證了運動學(xué)算法的正確性以及使用TwinCAT3與MATLAB軟件編寫程序的優(yōu)越性，提高了機器人設(shè)計中仿真和軟件編程的效率。

[1] 黃真, 孔令富, 方躍法. 并聯(lián)機器人機構(gòu)學(xué)理論及控制[M]. 北京：機械工業(yè)出版社, 1997: 46 - 64.

[2] http: //www. beckhoff. com. cn/english/[EB/CD].

[3] 劉極峰. 機器人技術(shù)基礎(chǔ)[M]. 北京: 高等教育出版社, 2006: 63 - 67.

[4] 楊永立. 六自由度運動平臺的仿真研究[J]. 工程機械文摘, 2009, 1.

[5] 梁崇高, 榮輝. 一種Stewart平臺機械手位移正解[J]. 機械工程學(xué)報, 1991, 27 (2): 26 - 30.

[6] 趙鐵石, 黃真. 一種新型四自由度并聯(lián)平臺就夠及其位置分析[J]. 機械科學(xué)與技術(shù), 2000, 19 (6): 927 - 929.

[7] 劉安心, 楊廷利. 求一般6 - SPS并聯(lián)機器人結(jié)構(gòu)的全部位置正解[J]. 機械科學(xué)與技術(shù), 1996, 15 (4): 543 - 546.

[8] 趙新華, 彭商賢. 一種分析并聯(lián)機器人位置正解的高效算法[J]. 天津大學(xué)學(xué)報. 2000, 33 (2): 134 - 137.

Design of Control System for Stewart Platform based on Beckhoff Twincat3

Based on BECKHOFF software in TwinCAT3, this paper describes the process of control system design using BECKHOFF PC technology for Stewart platform in detail. First, the direct and inverse solution algorithms of the 6-degrees parallel connection platform are developed. Second, the direct and inverse solution modules are generated through the M functions in MATLAB/ SIMULINK. Third, the target of SIMULINK code generator is configured to TwinCAT via TE1400 in TwinCAT3 and the TcCOM model in SIMULINK language can be integrated to TwinCAT3 environment. Finally, the TcCOM model is loaded into TwinCAT3. Using TwinCAT motion control function blocks and NC trajectory generator, the motors and platform can be controlled in real time via EtherCAT bus, which realizes the desired functions. Results show that Twinca3 combined with MATLAB improves the efficiency for control system design.

Stewart platform; TwinCAT; SIMULINK; PC control

王進（1985-），男，河南駐馬店人，上海大學(xué)在職研究生，現(xiàn)任德國倍福自動化工程師，主要從事機器人應(yīng)用研究。

郭帥（1970-），男，安徽蚌埠人，博士，副研究員，現(xiàn)就職于德國倍福自動化有限公司，主要研究方向為機器人、張力控制。

聶松亮（1989-），男，碩士研究生，現(xiàn)就讀于上海大學(xué)。