基于TwinCAT的Tripod并聯機器人電控系統研究

高俊東 劉雨佳 林忠文 余彬煬

摘 要：并聯機器人具有高動態響應性、高剛度、高精度的優點，可以廣泛應用在煙草企業異型煙分揀線上以解決效率和成本的問題。本文以Tripod并聯機器人為控制對象，提出了一種基于倍福公司TwinCAT的電控系統解決方案，重點闡述了基于運動學模型的機器人算法的實現方式。

關鍵詞：并聯機器人;TwinCAT;運動學模型;電控系統

面對煙草行業亟待解決的效率和成本問題，具有高動態響應性、高剛度、高精度的并聯機器人在煙草企業異型煙分揀線上得到廣泛應用[1-2]。本文在Tripod并聯機器人運動學分析的基礎上[3-4]，對Tripod并聯機器人的電控系統進行研究。對Tripod并聯機器人的運動控制，可基于位置逆解算法對三個交流伺服電機進行插補控制[5-6]，且由于并聯機構是非線性、高耦合的復雜機構，所以對電控系統的控制器、伺服驅動系統性能具有更高的要求。

1 硬件設計

電控系統的硬件設計，從控制器到I/O模塊，再到伺服驅動系統都是通過EtherCAT總線[8]鏈接。

2 軟件設計

倍福公司的TwinCAT3軟件是電控系統的核心部分[7-8]。軟件設計總體架構，包含三個模塊：機器人算法模塊（運動學位置正解和逆解）、NC運算模塊（軌跡規劃和伺服控制）、PLC模塊（邏輯控制和運動控制的主控）。機器人算法模塊由MTALAB Simulink調試完成后，通過TE1400組件生成。上位人機交互界面采用VB開發，通過TwinCAT3提供的ADS-OCX控件與TwinCAT3進行通訊。

Tripod并聯機器人運動控制時，首先在NC軸配置中新建關節軸坐標系中的伺服實軸A1、A2、A3和笛卡爾坐標系中的三個虛軸x、y、z，把伺服實軸鏈接到伺服驅動器，把虛軸鏈接到PLC。伺服實軸的編碼器位置反饋值鏈接到正解模塊的輸入，經過位置正解運算，就可得到末端TCP在笛卡爾空間中的當前位置，為運動控制提供當前坐標。

在軌跡控制過程中，主要是使用逆解模塊。例如，在PLC程序中調用TwinCAT3運動控制庫中的電子凸輪功能塊，當功能塊被激活后，基于給定的軌跡要求，NC運算核會規劃出具體的運動路徑，每隔2ms的時間間隔計算出下一路徑點，把路徑點的笛卡爾空間位置坐標? ? ? ? ? ? ? ? ? ? 鏈接到逆解模塊的輸入，實時計算相應的關節軸位置坐標? ? ? ? ? ? ? ? ? ? ? ? ，并通過NC發送給伺服驅動器。基于NC運算核和機器人算法模塊，每隔2ms，伺服驅動器就會收到一個位置數據，并控制電機運轉到這一位置，當循環 次后，末端TCP就完成了給定軌跡。

2.1機器人算法模塊設計

根據Tripod并聯機器人的位置逆解和位置正解[3-4]，在MATLAB Simulink中使用MATLAB Function函數編寫算法程序，為機器人本體的機械參數，（x，y，z）和（A1，A2，A3）為對應的TCP笛卡爾空間位置坐標和關節軸坐標，X0設置關節軸坐標系的原點偏置，X0確定關節軸坐標系的正負方向， 為錯誤標志。

假設TCP當前位置（x，y，z）=（50，150，785）、X0=401.7mm、X0=1，比較MATLAB和TwinCAT3中位置逆解結果：（A1，A2，A3）=（-12.86，-93.37，17.38），證明機器人算法模塊的實現方式是正確的。在實際應用中，將（A1，A2，A3）與NC伺服實軸鏈接，（x，y，z）與NC虛軸鏈接，其余的輸入輸出與PLC相關變量鏈接，就可以將機器人算法模塊進行靈活調用，實現Tripod并聯機器人的運動控制。

2.2上位人機交互界面設計

本文使用VB進行上位人機交互界面設計，并通過ADS-OCX控件[8]與TwinCAT3進行通訊。

3 結論

本文提出了一種基于TwinCAT3的Tripod并聯機器人電控系統的解決方案，搭建了硬件設計和軟件設計框架，詳細設計了機器人算法模塊，并闡述了運動控制的實現方式。最后，根據人機交互需求，采用VB完成了上位交互界面的設計，為開發自主開放的機器人控制系統奠定了堅實基礎。未來，將以本文為基礎，對機器人視覺技術、動力學模型、相關控制策略[9-10]的綜合應用進行深入研究，實現Tripod并聯機器人的運動性能提升和應用范圍擴展。

參考文獻：

[1]任志剛. 工業機器人的發展現狀及發展趨勢[J]. 裝備制造技術， 2015（3）：166-168.

[2]穆劍橋， 王智斌. 工業機器人的發展及現狀綜述[J]. 電子制作， 2015（18）：81-82.

[3]劉昆. 3-PSS并聯機器人運動學分析及仿真[D]. 重慶大學， 2015.

[4]Jingjun Zhang. A Method for Obtaining Direct and Inverse Pose Solutions to Delta Parallel? Robot Based on ADAMS[C]. IEEE International Conference on Mechatronics and Automation， 2009：1332-1336.

[5]顧文昊， 肖武運， 張志強， 等. 基于Beckhoff平臺Delta并聯機構軌跡規劃的實現[J]. 電腦知識與技術， 2014， 10（27）： 83-86.

[6]楊中山. 基于IPC的電動缸實驗平臺測控系統設計[D]. 重慶大學，? 2014.

[7]王進， 郭帥， 聶松亮. 基于TwinCAT3的Stewart平臺控制系統設計[J]. 自動化博覽， 2015（9）：82-85.

[8]黎妞. 基于EtherCAT的伺服運動控制系統研究[D]. 武漢科技大學， 2012.

[9]毛洪國. 基于動力學模型的DELTA機器人運動控制研究[D]. 哈爾濱工業大學， 2014.

[10]郭超. 高速并聯機器人及其控制系統研究[D]. 山東理工大學， 2014.