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

上海大學(xué)智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室，德國(guó)倍福自動(dòng)化有限公司 王進(jìn)上海大學(xué)智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室 郭帥，聶松亮

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

上海大學(xué)智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室，德國(guó)倍福自動(dòng)化有限公司 王進(jìn)上海大學(xué)智能制造及機(jī)器人重點(diǎn)實(shí)驗(yàn)室 郭帥，聶松亮

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

Stewart平臺(tái)；TwinCAT；SIMULINK；PC控制

1 引言

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

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

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

圖1 平臺(tái)實(shí)物圖

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

2 運(yùn)動(dòng)學(xué)分析

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

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

圖2 Stewart 平臺(tái)坐標(biāo)系定義

2.1 位置逆解

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

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

其中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），從而可以求出電動(dòng)缸的長(zhǎng)度L1為：

同理可得L2…L6。[4]

2.2 位置正解

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

圖3 運(yùn)動(dòng)學(xué)正解算法流程圖

3 軟件設(shè)計(jì)

3.1 MATLAB編程和生成TcCOM Object

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

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

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

3.2 TwinCAT軟件配置和編程

首先在TwinCAT軟件的TcCOM Object中調(diào)用上文中生成的正逆解功能塊?？梢钥吹郊虞d的Lpt-backward和Lpt-forward功能塊的圖形與變量都與SIMULINK中一致。如圖5所示。

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

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

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

圖6 系統(tǒng)框圖

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

圖7 HMI界面

3.3 硬件平臺(tái)

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

圖8 硬件鏈接拓?fù)浣Y(jié)構(gòu)圖

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

4 仿真和測(cè)試

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

圖9 運(yùn)行效果圖

5 結(jié)語(yǔ)

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

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

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

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

[4] 楊永立. 六自由度運(yùn)動(dòng)平臺(tái)的仿真研究[J]. 工程機(jī)械文摘, 2009, 1.

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

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

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

[8] 趙新華, 彭商賢. 一種分析并聯(lián)機(jī)器人位置正解的高效算法[J]. 天津大學(xué)學(xué)報(bào). 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

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

郭帥（1970-），男，安徽蚌埠人，博士，副研究員，現(xiàn)就職于德國(guó)倍福自動(dòng)化有限公司，主要研究方向?yàn)闄C(jī)器人、張力控制。

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