基于TwinCAT3的Stewart平臺控制系統設計

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

基于TwinCAT3的Stewart平臺控制系統設計

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

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

Stewart平臺；TwinCAT；SIMULINK；PC控制

1 引言

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

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

建模仿真技術與自動化技術相結合是機器人設計的趨勢。本文以Stewart平臺為例，基于Windows平臺的TwinCAT3軟件和MATLAB/SIMULINK軟件完成了Stewart平臺控制系統的研發，實現了運動學算法的編寫和對平臺的實時控制。

圖1 平臺實物圖

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

2 運動學分析

并聯機器人機構運動學分析包括正運動學分析和逆運動學分析兩個方面。在求解前首先建立坐標系。

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

圖2 Stewart 平臺坐標系定義

2.1 位置逆解

并聯機構的運動學逆解是從工作坐標反求關節坐標，有唯一解。在平臺的實際應用中，通過路徑規劃算法每個周期給定的是平臺的六個空間姿態參數X，Y，Z，α，β，γ。此過程的實質是在平臺運動中，輸入給定的位姿，控制伺服電機使電動缸到達所需長度。

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

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

旋轉變換矩陣T=

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

同理可得L2…L6。[4]

2.2 位置正解

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

圖3 運動學正解算法流程圖

3 軟件設計

3.1 MATLAB編程和生成TcCOM Object

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

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

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

3.2 TwinCAT軟件配置和編程

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

圖5 在TwinCAT中導入正、逆解功能塊

由于Stewart平臺具有笛卡爾坐標系的六個自由度并且需要控制六個關節電機。在倍福的TwinCAT軟件的NC中首先添加A1到A6六個關節坐標系軸并鏈接到驅動器和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位姿作為輸入變量，輸出變量則作為電動缸的給定位置通過外部給定功能塊發給NC再發給驅動器。

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

圖6 系統框圖

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

圖7 HMI界面

3.3 硬件平臺

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

圖8 硬件鏈接拓撲結構圖

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

4 仿真和測試

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

圖9 運行效果圖

5 結語

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

[1] 黃真, 孔令富, 方躍法. 并聯機器人機構學理論及控制[M]. 北京：機械工業出版社, 1997: 46 - 64.

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

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

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

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

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

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

[8] 趙新華, 彭商賢. 一種分析并聯機器人位置正解的高效算法[J]. 天津大學學報. 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-），男，河南駐馬店人，上海大學在職研究生，現任德國倍福自動化工程師，主要從事機器人應用研究。

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

聶松亮（1989-），男，碩士研究生，現就讀于上海大學。