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

摘 要：并聯(lián)機(jī)器人具有高動(dòng)態(tài)響應(yīng)性、高剛度、高精度的優(yōu)點(diǎn)，可以廣泛應(yīng)用在煙草企業(yè)異型煙分揀線上以解決效率和成本的問題。本文以Tripod并聯(lián)機(jī)器人為控制對(duì)象，提出了一種基于倍福公司TwinCAT的電控系統(tǒng)解決方案，重點(diǎn)闡述了基于運(yùn)動(dòng)學(xué)模型的機(jī)器人算法的實(shí)現(xiàn)方式。

關(guān)鍵詞：并聯(lián)機(jī)器人;TwinCAT;運(yùn)動(dòng)學(xué)模型;電控系統(tǒng)

面對(duì)煙草行業(yè)亟待解決的效率和成本問題，具有高動(dòng)態(tài)響應(yīng)性、高剛度、高精度的并聯(lián)機(jī)器人在煙草企業(yè)異型煙分揀線上得到廣泛應(yīng)用[1-2]。本文在Tripod并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上[3-4]，對(duì)Tripod并聯(lián)機(jī)器人的電控系統(tǒng)進(jìn)行研究。對(duì)Tripod并聯(lián)機(jī)器人的運(yùn)動(dòng)控制，可基于位置逆解算法對(duì)三個(gè)交流伺服電機(jī)進(jìn)行插補(bǔ)控制[5-6]，且由于并聯(lián)機(jī)構(gòu)是非線性、高耦合的復(fù)雜機(jī)構(gòu)，所以對(duì)電控系統(tǒng)的控制器、伺服驅(qū)動(dòng)系統(tǒng)性能具有更高的要求。

1 硬件設(shè)計(jì)

電控系統(tǒng)的硬件設(shè)計(jì)，從控制器到I/O模塊，再到伺服驅(qū)動(dòng)系統(tǒng)都是通過EtherCAT總線[8]鏈接。

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

倍福公司的TwinCAT3軟件是電控系統(tǒng)的核心部分[7-8]。軟件設(shè)計(jì)總體架構(gòu)，包含三個(gè)模塊：機(jī)器人算法模塊（運(yùn)動(dòng)學(xué)位置正解和逆解）、NC運(yùn)算模塊（軌跡規(guī)劃和伺服控制）、PLC模塊（邏輯控制和運(yùn)動(dòng)控制的主控）。機(jī)器人算法模塊由MTALAB Simulink調(diào)試完成后，通過TE1400組件生成。上位人機(jī)交互界面采用VB開發(fā)，通過TwinCAT3提供的ADS-OCX控件與TwinCAT3進(jìn)行通訊。

Tripod并聯(lián)機(jī)器人運(yùn)動(dòng)控制時(shí)，首先在NC軸配置中新建關(guān)節(jié)軸坐標(biāo)系中的伺服實(shí)軸A1、A2、A3和笛卡爾坐標(biāo)系中的三個(gè)虛軸x、y、z，把伺服實(shí)軸鏈接到伺服驅(qū)動(dòng)器，把虛軸鏈接到PLC。伺服實(shí)軸的編碼器位置反饋值鏈接到正解模塊的輸入，經(jīng)過位置正解運(yùn)算，就可得到末端TCP在笛卡爾空間中的當(dāng)前位置，為運(yùn)動(dòng)控制提供當(dāng)前坐標(biāo)。

在軌跡控制過程中，主要是使用逆解模塊。例如，在PLC程序中調(diào)用TwinCAT3運(yùn)動(dòng)控制庫中的電子凸輪功能塊，當(dāng)功能塊被激活后，基于給定的軌跡要求，NC運(yùn)算核會(huì)規(guī)劃出具體的運(yùn)動(dòng)路徑，每隔2ms的時(shí)間間隔計(jì)算出下一路徑點(diǎn)，把路徑點(diǎn)的笛卡爾空間位置坐標(biāo)? ? ? ? ? ? ? ? ? ? 鏈接到逆解模塊的輸入，實(shí)時(shí)計(jì)算相應(yīng)的關(guān)節(jié)軸位置坐標(biāo)? ? ? ? ? ? ? ? ? ? ? ? ，并通過NC發(fā)送給伺服驅(qū)動(dòng)器?；贜C運(yùn)算核和機(jī)器人算法模塊，每隔2ms，伺服驅(qū)動(dòng)器就會(huì)收到一個(gè)位置數(shù)據(jù)，并控制電機(jī)運(yùn)轉(zhuǎn)到這一位置，當(dāng)循環(huán) 次后，末端TCP就完成了給定軌跡。

2.1機(jī)器人算法模塊設(shè)計(jì)

根據(jù)Tripod并聯(lián)機(jī)器人的位置逆解和位置正解[3-4]，在MATLAB Simulink中使用MATLAB Function函數(shù)編寫算法程序，為機(jī)器人本體的機(jī)械參數(shù)，（x，y，z）和（A1，A2，A3）為對(duì)應(yīng)的TCP笛卡爾空間位置坐標(biāo)和關(guān)節(jié)軸坐標(biāo)，X0設(shè)置關(guān)節(jié)軸坐標(biāo)系的原點(diǎn)偏置，X0確定關(guān)節(jié)軸坐標(biāo)系的正負(fù)方向， 為錯(cuò)誤標(biāo)志。

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

2.2上位人機(jī)交互界面設(shè)計(jì)

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

3 結(jié)論

本文提出了一種基于TwinCAT3的Tripod并聯(lián)機(jī)器人電控系統(tǒng)的解決方案，搭建了硬件設(shè)計(jì)和軟件設(shè)計(jì)框架，詳細(xì)設(shè)計(jì)了機(jī)器人算法模塊，并闡述了運(yùn)動(dòng)控制的實(shí)現(xiàn)方式。最后，根據(jù)人機(jī)交互需求，采用VB完成了上位交互界面的設(shè)計(jì)，為開發(fā)自主開放的機(jī)器人控制系統(tǒng)奠定了堅(jiān)實(shí)基礎(chǔ)。未來，將以本文為基礎(chǔ)，對(duì)機(jī)器人視覺技術(shù)、動(dòng)力學(xué)模型、相關(guān)控制策略[9-10]的綜合應(yīng)用進(jìn)行深入研究，實(shí)現(xiàn)Tripod并聯(lián)機(jī)器人的運(yùn)動(dòng)性能提升和應(yīng)用范圍擴(kuò)展。

參考文獻(xiàn)：

[1]任志剛. 工業(yè)機(jī)器人的發(fā)展現(xiàn)狀及發(fā)展趨勢(shì)[J]. 裝備制造技術(shù)， 2015（3）：166-168.

[2]穆劍橋， 王智斌. 工業(yè)機(jī)器人的發(fā)展及現(xiàn)狀綜述[J]. 電子制作， 2015（18）：81-82.

[3]劉昆. 3-PSS并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[D]. 重慶大學(xué)， 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]顧文昊， 肖武運(yùn)， 張志強(qiáng)， 等. 基于Beckhoff平臺(tái)Delta并聯(lián)機(jī)構(gòu)軌跡規(guī)劃的實(shí)現(xiàn)[J]. 電腦知識(shí)與技術(shù)， 2014， 10（27）： 83-86.

[6]楊中山. 基于IPC的電動(dòng)缸實(shí)驗(yàn)平臺(tái)測(cè)控系統(tǒng)設(shè)計(jì)[D]. 重慶大學(xué)，? 2014.

[7]王進(jìn)， 郭帥， 聶松亮. 基于TwinCAT3的Stewart平臺(tái)控制系統(tǒng)設(shè)計(jì)[J]. 自動(dòng)化博覽， 2015（9）：82-85.

[8]黎妞. 基于EtherCAT的伺服運(yùn)動(dòng)控制系統(tǒng)研究[D]. 武漢科技大學(xué)， 2012.

[9]毛洪國(guó). 基于動(dòng)力學(xué)模型的DELTA機(jī)器人運(yùn)動(dòng)控制研究[D]. 哈爾濱工業(yè)大學(xué)， 2014.

[10]郭超. 高速并聯(lián)機(jī)器人及其控制系統(tǒng)研究[D]. 山東理工大學(xué)， 2014.