久久久91-久久久91精品国产一区二区-久久久91精品国产一区二区三区-久久久999国产精品-久久久999久久久精品

作者：德國倍福自動(dòng)化有限公司 王進(jìn)，王建成，孔惠增

摘要：目前國內(nèi)的平行四邊形機(jī)器人主要用于碼垛，其結(jié)構(gòu)主要有混聯(lián)式絲杠機(jī)構(gòu)和雙平行四邊形旋轉(zhuǎn)軸式機(jī)構(gòu)，本文通過對這兩種四邊形機(jī)構(gòu)的運(yùn)動(dòng)學(xué)分析，推導(dǎo)出四邊形機(jī)器人的正逆解算法。再利用倍福TwinCAT軟件的實(shí)時(shí)運(yùn)算核運(yùn)行正逆解算法，把軌跡規(guī)劃功能塊產(chǎn)生的笛卡爾坐標(biāo)系位置值實(shí)時(shí)轉(zhuǎn)化成關(guān)節(jié)坐標(biāo)系位置值并利用外部給定功能通過EtherCAT總線實(shí)時(shí)地對電機(jī)和機(jī)器人進(jìn)行控制。本文還通過對碼垛機(jī)器人末端軌跡的分析利用TwinCAT軟件的NCI插補(bǔ)功能開發(fā)了軌跡規(guī)劃抓放功能塊。結(jié)果顯示通過對兩種機(jī)器人結(jié)構(gòu)的分析以及在TwinCAT平臺的軟件編程實(shí)現(xiàn)了碼垛機(jī)器人的控制。

關(guān)鍵詞: 碼垛機(jī)器人；TwinCAT；NCI插補(bǔ)；抓放功能；PC控制

Abstract: At present, there are two types of mechanism of the domestic palletizing robots, i.e., the mixed gear and rack mechanism and the double parallel quadrilateral rotating axis mechanism. Through the kinematic analysis of the two palletizing mechanisms, this paper derives the positive and inverse algorithm of the palletizing robot. TwinCAT is implemented to solve the inverse algorithm in real time. Then the Descartes coordinate system generated by the trajectory planning function block is transformed into the position value of the joint coordinate system. At the same time, the motor and the robot are controlled by the EtherCAT bus by the motion control function block. By analyzing the terminal trajectory of palletizing robot, a trajectory planning and grasping function block is developed based on the NCI function of TwinCAT software. By analyzing the structure of the two types of robots and the software and hardware development of the TwinCAT platform,results show that the control of palletizing robot is realized.

Key words: Robot; TwinCAT; Pick and place; PC Control
1　引言

平行四邊形機(jī)器人主要用于碼垛行業(yè)，碼垛機(jī)器人主要將包裝袋或者箱體等按照一定次序碼到托盤上，以便實(shí)現(xiàn)物料的搬運(yùn)、裝卸、存儲、運(yùn)輸?shù)任锪骰顒?dòng)。具有工作效率高，可靠性好，柔性好的特點(diǎn)，在糧油食品飼料化工等行業(yè)得到廣泛應(yīng)用。

圖1 雙平行四邊形旋轉(zhuǎn)軸式機(jī)器人

圖2 混聯(lián)式絲杠碼垛機(jī)器人實(shí)物圖

混聯(lián)式絲杠碼垛機(jī)器人是一種四自由度混聯(lián)機(jī)構(gòu)，它結(jié)構(gòu)簡單，價(jià)格低廉，性能優(yōu)越，廣泛應(yīng)用于工業(yè)搬運(yùn)和物流生產(chǎn)線，但受限于絲杠或齒條傳動(dòng)機(jī)構(gòu)對負(fù)載的限制，難以用于重載機(jī)器人。雙平行四邊形旋轉(zhuǎn)軸式機(jī)構(gòu)主要用于重載搬運(yùn)機(jī)器人，其特點(diǎn)是重復(fù)精度高、運(yùn)行速度快、負(fù)載能力大、可靠性高等特點(diǎn)。但是相對于輕載機(jī)器人由于負(fù)載自重比其在本體設(shè)計(jì)和高動(dòng)態(tài)性能實(shí)現(xiàn)方面難度較大^[1]。

本文對這兩種機(jī)構(gòu)進(jìn)行了簡單分析得到這兩種機(jī)構(gòu)的正逆解的算法，重點(diǎn)對如何在TwinCAT中實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制設(shè)計(jì)和運(yùn)動(dòng)軌跡規(guī)劃進(jìn)行了論述，并在實(shí)際應(yīng)用中通過監(jiān)測該機(jī)器人運(yùn)行的位置、速度以及加速度曲線對機(jī)器人的性能進(jìn)行分析。

2 機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析

2.1 混聯(lián)式絲杠機(jī)構(gòu)分析

圖3 混聯(lián)式絲杠碼垛機(jī)器人幾何示意圖

混聯(lián)式絲杠機(jī)器人有四個(gè)關(guān)節(jié)運(yùn)動(dòng)：底座旋轉(zhuǎn)（繞Z軸）、水平運(yùn)動(dòng)（X軸方向）、垂直上下運(yùn)動(dòng)（Z軸上下）和末端旋轉(zhuǎn)運(yùn)動(dòng)。底座電機(jī)和末端旋轉(zhuǎn)電機(jī)通過減速機(jī)驅(qū)動(dòng)底座和機(jī)械手末端旋轉(zhuǎn)。水平絲杠電機(jī)和垂直絲杠電機(jī)通過同步帶輪帶動(dòng)水平絲杠導(dǎo)軌和垂直絲杠導(dǎo)軌運(yùn)動(dòng)。其導(dǎo)軌上的滑塊帶動(dòng)前大臂和后大臂沿各自導(dǎo)軌做水平方向運(yùn)動(dòng)和垂直方向運(yùn)動(dòng)。

如圖3所示，現(xiàn)設(shè)定一個(gè)固定坐標(biāo)系O-xy，各關(guān)節(jié)坐標(biāo)為A（0，zA）、C（xc，0）、B（xB，zB）、D（xD，zD）、E(xE，zE)、F（xF，zF），根據(jù)機(jī)構(gòu)分析^[2]當(dāng)滿足桿長條件：ad=bc時(shí)，可大大簡化主傳動(dòng)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)求解。這將使計(jì)算量減小，便于機(jī)器人運(yùn)動(dòng)控制設(shè)計(jì)。這時(shí)機(jī)械手的位置正解為：

對機(jī)構(gòu)做逆運(yùn)動(dòng)學(xué)分析，可得逆解公式如下，其中θ為碼垛機(jī)器人在搬運(yùn)過程中箱子的角度。

2.2　雙平行四邊形旋轉(zhuǎn)軸式機(jī)器人機(jī)構(gòu)分析

圖4 雙平行四邊形旋轉(zhuǎn)軸式碼垛機(jī)器人幾何示意圖

雙平行四邊形機(jī)構(gòu)如圖4所示，由兩組平行四邊形組成。其中紅色所示的主平行四邊形確定末端點(diǎn)位置，綠色的輔助平行四邊形使末端執(zhí)行器與水平面保持一個(gè)固定的姿態(tài)。驅(qū)動(dòng)主平行四邊形的電機(jī)被對稱安裝在轉(zhuǎn)座上，其分別標(biāo)為軸2、軸3，對應(yīng)關(guān)節(jié)角度為θ2、 3 ；底座由一個(gè)電機(jī)驅(qū)動(dòng)，記為軸1，對應(yīng)關(guān)節(jié)角度為θ1，前三個(gè)關(guān)節(jié)確定末端點(diǎn)的位置。在手爪處加裝一個(gè)電機(jī)，轉(zhuǎn)角記為θ4，用以對所操作工件進(jìn)行旋轉(zhuǎn)。根據(jù)機(jī)構(gòu)幾何關(guān)系，可得末端執(zhí)行器位置正解如下：^[1]

（3）式中x、y、z、θz為機(jī)器人末端在世界坐標(biāo)系中的坐標(biāo)和角度；

θ1、θ2、θ3、θ4、θ5 為機(jī)器人各關(guān)節(jié)角度。

由正解方程，可得逆解方程如下：

（5）

（6）

（7）θ4=θ2-θ1

（8）θ5=π-θ3

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

3.1　PLC編程

根據(jù)上文的運(yùn)動(dòng)學(xué)分析可以在TwinCAT中寫出機(jī)器人的正逆解函數(shù)功能塊，圖5是以混聯(lián)式絲杠機(jī)構(gòu)為例寫出的正逆解函數(shù)。

圖5 混聯(lián)式絲杠機(jī)構(gòu)正逆解函數(shù)

本文所設(shè)計(jì)的兩種四邊形機(jī)器人在笛卡爾坐標(biāo)系中一共有四個(gè)自由度并且需要控制四個(gè)關(guān)節(jié)電機(jī)。在倍福的TwinCAT軟件的NC中首先添加A1到A4四個(gè)關(guān)節(jié)坐標(biāo)系軸并鏈接到驅(qū)動(dòng)器和PLC，為了方便對笛卡爾坐標(biāo)系軸操作再添加X、Y、Z、C四個(gè)虛擬軸并鏈接到PLC。對于正解是把四個(gè)伺服電機(jī)A1到A4的角度或者位置作為輸入變量賦值給正解功能塊，經(jīng)過正解運(yùn)算輸出X、Y、Z、C的位置值作為機(jī)器人在笛卡爾坐標(biāo)系中的位置。而逆解功能塊將給定的X、Y、Z、C的位置和角度作為輸入變量，通過逆解運(yùn)算得到電機(jī)的角度值，通過外部給定功能塊發(fā)給NC再發(fā)給伺服驅(qū)動(dòng)器。^[3]

在PLC程序中，可以直接對笛卡爾坐標(biāo)系中的X、Y 、Z 、C四個(gè)虛擬軸進(jìn)行控制。調(diào)用TwinCAT軟件的運(yùn)動(dòng)控制庫中的PTP、NCI等運(yùn)動(dòng)控制功能，可以方便地讓機(jī)器人移動(dòng)到某一位姿或者進(jìn)行連續(xù)循環(huán)運(yùn)動(dòng)。軟件的具體編程步驟如圖6所示的軟件流程圖。由于NC系統(tǒng)的路徑規(guī)劃功能強(qiáng)大，這些位置數(shù)據(jù)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)都是連續(xù)的從而保證了伺服驅(qū)動(dòng)器的連續(xù)穩(wěn)定運(yùn)行。

圖6 軟件流程圖

倍福的TwinCAT軟件自帶了HMI功能，界面可用于調(diào)試和用戶操作，如圖7所示。

圖7 HMI界面

3.2　軌跡規(guī)劃

NCI軟件是倍福TwinCAT自帶的插補(bǔ)運(yùn)動(dòng)控制軟件，支持三軸插補(bǔ)以及五個(gè)輔助軸的運(yùn)動(dòng)，可以實(shí)現(xiàn)空間中的直線插補(bǔ)、圓弧插補(bǔ)、樣條曲線等軌跡插補(bǔ)動(dòng)作。碼垛機(jī)器人有三個(gè)插補(bǔ)軸以及一個(gè)旋轉(zhuǎn)軸，選擇NCI做軌跡規(guī)劃能滿足碼垛機(jī)器人的動(dòng)作要求，同時(shí)減少了開發(fā)難度。

圖8 NCI和G代碼編程

四邊形碼垛機(jī)器人抓放動(dòng)作通常是一個(gè)門型曲線，由三條平滑過渡的曲線組成，如圖9所示。其在NCI中的G代碼編程如圖8所示，其中#setparamVertexSmoothing(R5)#是設(shè)定平滑過渡類型和過渡半徑R5；

G01 Z=R2 F=R4 M80是從放置點(diǎn)運(yùn)行到放置點(diǎn)上方，F(xiàn)=R4是指運(yùn)行速度。Z=R2目標(biāo)位置的Z軸坐標(biāo)，所有R變量都可以在PLC中寫入。M80是通過NCI的M函數(shù)來控制碼垛機(jī)器人夾手關(guān)閉的變量；

G01 X=R10 Y=R11 Z=R12 Q1=R13 F=R14M8 1是從放置點(diǎn)上方運(yùn)行到抓取點(diǎn)上方，X=R10Y=R11 Z=R12 Q1=R13是運(yùn)行目標(biāo)位置，F(xiàn)=R14是運(yùn)行速度，M81是通過NCI的M函數(shù)來控制碼垛機(jī)器人夾手打開的變量；

G01 Z=R22 F=R24 是從抓取點(diǎn)上方運(yùn)行到抓取點(diǎn)。

所有的R參數(shù)都可以在PLC程序或者HMI中賦值。

圖9 抓放曲線

4　運(yùn)行和調(diào)試

本文以混聯(lián)式絲杠四邊形機(jī)器人為例進(jìn)行運(yùn)行測試，先移動(dòng)到抓取位置（300,500,-300），再從當(dāng)前位置走門型曲線運(yùn)行到放置位置（0,-500,-400），其中Z軸最高提升到絕對高度0，首先可以在TwinCAT的HMI中進(jìn)行示教和位置設(shè)置，如圖10所示。

圖10 示教點(diǎn)設(shè)置

然后可以在PLC程序中通過NCI的R變量讀寫功能把這些示教好的位置傳遞到NCI中，運(yùn)行軌跡曲線如圖11第一欄；X和Y以及Z軸的速度曲線如下圖第二欄；X、Y、Z、A1、A2、A3軸的位置曲線如下圖第三欄。

從TwinCAT自帶的ScopeView示波器軟件采集到的軌跡和速度位置數(shù)據(jù)可以看出，通過NCI編程可以直接對碼垛機(jī)器人的笛卡爾坐標(biāo)系軸進(jìn)行操作，關(guān)節(jié)坐標(biāo)系軸會(huì)按照逆解功能塊所得到的角度值自動(dòng)運(yùn)行，實(shí)現(xiàn)了混聯(lián)式絲杠四邊形機(jī)器人的末端軌跡控制。對于雙平行四邊形旋轉(zhuǎn)軸式機(jī)器人，按照所分析好的運(yùn)動(dòng)學(xué)算法，編寫相應(yīng)的正逆解功能塊采用同樣的方法也能實(shí)現(xiàn)相應(yīng)的控制。

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

5　結(jié)論

本文主要介紹了利用倍福TwinCAT軟件編程實(shí)現(xiàn)四邊形碼垛機(jī)器人控制系統(tǒng)設(shè)計(jì)和實(shí)現(xiàn)的方法。首先介紹了兩種常用碼垛機(jī)器人運(yùn)動(dòng)學(xué)的正解和逆解算法，然后重點(diǎn)論述了對于本文的機(jī)器人正逆解算法怎么在TwinCAT軟件中進(jìn)行了編程、并給出了機(jī)器人控制的程序流程圖，還介紹了一種利用倍福NCI插補(bǔ)軟件實(shí)現(xiàn)機(jī)器人空間抓放軌跡的方法，最后在倍福TwinCAT軟件中進(jìn)行了PLC程序和操作界面編程，并結(jié)合硬件進(jìn)行了實(shí)際運(yùn)行和調(diào)試。運(yùn)行結(jié)果表明本機(jī)器人軟件設(shè)計(jì)可以對四邊形碼垛機(jī)器人進(jìn)行笛卡爾坐標(biāo)系軸點(diǎn)動(dòng)操作、抓放軌跡操作等各種控制，也可以單獨(dú)對關(guān)節(jié)坐標(biāo)系軸操作。從而實(shí)現(xiàn)了在TwinCAT軟件中對四邊形碼垛機(jī)器人的設(shè)計(jì)和控制，驗(yàn)證了運(yùn)動(dòng)學(xué)算法的正確性以及使用TwinCAT設(shè)計(jì)機(jī)器人的優(yōu)越性，也為其他機(jī)器人模型在TwinCAT軟件中的開發(fā)和程序編寫提供了示范。

作者簡介：

王進(jìn)（1985-），男，河南駐馬店人，現(xiàn)任德國倍福自動(dòng)化有限公司運(yùn)動(dòng)控制產(chǎn)品經(jīng)理，主要從事機(jī)器人和運(yùn)動(dòng)控制方面應(yīng)用研發(fā)。

王建成（1975-），男，安徽黃山人，現(xiàn)任德國倍福自動(dòng)化有限公司系統(tǒng)應(yīng)用部經(jīng)理。

孔惠增（1989-），男，河南南陽人，現(xiàn)任德國倍福自動(dòng)化有限公司系統(tǒng)應(yīng)用部應(yīng)用工程師。

參考文獻(xiàn)：

[1] 游瑋. 雙平行四邊形高速重載搬運(yùn)機(jī)器人動(dòng)態(tài)設(shè)計(jì)與控制研究[D]. 哈爾濱工業(yè)大學(xué), 2011.

[2] 張志強(qiáng), 臧冀原. 混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J]. 機(jī)械設(shè)計(jì), 2010, 11 (11) ：47 - 51.

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

摘自《自動(dòng)化博覽》2018年9月刊