余杰先，張中華，陳凌玲，李久林，溫舒然

（珠海格力智能裝備有限公司，廣東珠海 519000）

0 引言

注塑機(jī)機(jī)械手一般包括單軸手臂機(jī)械手、三軸機(jī)械手、五軸機(jī)械手，由控制系統(tǒng)、驅(qū)動(dòng)系統(tǒng)、執(zhí)行機(jī)構(gòu)等組成[1-3]。執(zhí)行機(jī)構(gòu)有機(jī)械手臂或氣缸以及桁架機(jī)械臂等[4-7]。傳統(tǒng)的機(jī)械手控制系統(tǒng)和驅(qū)動(dòng)系統(tǒng)是分開(kāi)的，兩者之間通過(guò)脈沖模式連接。隨著總線技術(shù)、芯片技術(shù)的不斷發(fā)展，在一個(gè)MCU 里面集成了運(yùn)動(dòng)控制軌跡規(guī)劃和伺服驅(qū)動(dòng)算法，大大減少了線纜的連接，節(jié)省了人工成本，提高了工作效率[8-15]。

注塑機(jī)械手如圖1 所示，其基本的功能就是按照預(yù)先編好的程序執(zhí)行工作流程：在自動(dòng)模式下到達(dá)注塑機(jī)模內(nèi)上方待機(jī)點(diǎn)，等待注塑機(jī)開(kāi)模完成信號(hào)，到達(dá)模內(nèi)取物，取物完成離開(kāi)模內(nèi)，達(dá)到型外放物到流水線上，放物完成機(jī)械手抬上升，到達(dá)模內(nèi)上方等待下一個(gè)開(kāi)模完信號(hào)。如此周期往返執(zhí)行[16-21]。隨著工藝需求的不斷完善，機(jī)械手要記憶每一個(gè)周期的放物位置，為下一個(gè)周期提供基本的位置偏移，這樣就需要一個(gè)XYZ三維的堆疊算法，通過(guò)在示教器界面配置好堆疊的屬性參數(shù)，自動(dòng)運(yùn)行時(shí)自動(dòng)判斷每一個(gè)周期的XYZ坐標(biāo)。注塑機(jī)機(jī)械手一般的運(yùn)動(dòng)包括：?jiǎn)屋S點(diǎn)對(duì)點(diǎn)定長(zhǎng)運(yùn)動(dòng)、多軸點(diǎn)對(duì)點(diǎn)聯(lián)動(dòng)、多軸組合運(yùn)動(dòng)、多軸提前變速變位運(yùn)動(dòng)[21-27]。

圖1 注塑機(jī)械手

本文著重介紹機(jī)械手控制系統(tǒng)基本構(gòu)成、堆疊工藝算法和提前變速運(yùn)動(dòng)算法。

1 機(jī)械手控制系統(tǒng)基本構(gòu)成

本文控制系統(tǒng)為集成控制和驅(qū)動(dòng)為一體，主要包括上位機(jī)示教器、控制模塊、伺服功率模塊?？傮w框圖如圖2所示。

圖2 總體流程

示教器的MCU 使用了TI 公司的AM335X。AM335x微處理器基于ARM Cortex-A8 處理器，在圖像、圖形處理、外設(shè)以及EtherCAT 和PROFIBUS 等工業(yè)接口選項(xiàng)方面得到了增強(qiáng)。該器件支持高級(jí)操作系統(tǒng)（HLOS）。Linux?和Android ?可從德州儀器（TI）免費(fèi)獲取。AM335x 微處理器包含功能框圖中顯示的子系統(tǒng)和以下簡(jiǎn)要說(shuō)明：微處理器單元（MPU）子系統(tǒng)基于ARM Cortex-A8 處理器，PowerVR SGX?圖形加速器子系統(tǒng)提供3D 圖形加速功能以支持顯示和游戲特效。可編程實(shí)時(shí)單元子系統(tǒng)和工業(yè)通信子系統(tǒng)（PRU-ICSS）與ARM內(nèi)核彼此獨(dú)立，允許單獨(dú)操作和計(jì)時(shí)，以實(shí)現(xiàn)更高的效率和靈活性。PRU-ICSS 支持更多外設(shè)接口和EtherCAT、PROFINET、EtherNet/IP、PROFIBUS、Ethernet Powerlink、Sercos 等實(shí)時(shí)協(xié)議。此外，憑借PRU-ICSS 的可編程特性及其對(duì)引腳、事件和所有片上系統(tǒng)（SoC）資源的訪問(wèn)權(quán)限，該子系統(tǒng)可以靈活地實(shí)現(xiàn)快速實(shí)時(shí)響應(yīng)、專(zhuān)用數(shù)據(jù)處理操作以及自定義外設(shè)接口，并減輕SoC 其他處理器內(nèi)核的任務(wù)負(fù)載。

驅(qū)控一體的主MCU 使用了TI 公司的高性能處理器TMS320F28377D。TMS320F28377D基本參數(shù)：雙核32位浮點(diǎn)MCU，主頻200 MHz；兩個(gè)支持ASRAM 和SDRAM的外部存儲(chǔ)器接口（EMIF）；雙6 通道直接存儲(chǔ)器存取（DMA）控制器；多達(dá)169 個(gè)支持輸入濾波的獨(dú)立可編程、復(fù)用通用輸入/輸出（GPIO）引腳；擴(kuò)展外設(shè)中斷控制器（ePIE）；多個(gè)支持外部喚醒的低功耗模式（LPM）；USB 2.0（MAC+PHY）；支持12 引腳3.3 V 兼容通用并行端口（uPP）接口；2 個(gè)控制器局域網(wǎng)（CAN）模塊；3個(gè)高速（最高50 MHz）SPI端口；2個(gè)多通道緩存串行端口（McBSP）；4 個(gè)串行通信接口（SCI/UART）；2 個(gè)I2C接口；4 個(gè)模數(shù)轉(zhuǎn)換器（ADC）；12 個(gè)16 位模式外部通道；24個(gè)12位模式外部通道；8個(gè)具有12位數(shù)模轉(zhuǎn)換器（DAC）參考的窗口化比較器；3 個(gè)12 位緩存DAC 輸出；24條具有增強(qiáng)功能的脈沖調(diào)制器（PWM）通道；16條高分辨率脈寬調(diào)制器（HRPWM）通道；6 個(gè)增強(qiáng)型捕捉（eCAP）模塊；3 個(gè)增強(qiáng)型正交編碼器脈沖（eQEP）模塊；8 條Δ-Σ 濾波器模塊（SDFM）輸入通道；外設(shè)模塊主要包括PWM 模塊，QEP 模塊，SDFM 模塊，SPI 模塊，SCI 模塊，IO 模塊，CNT 計(jì)數(shù)模塊；中斷優(yōu)先級(jí)包括OC中斷XINT1，PWM中斷，IRQ中斷XINT2，PWM同步SPI中斷，SCI中斷采用FIFO模式，中斷優(yōu)先級(jí)從高到低。

2 注塑機(jī)機(jī)械手堆疊算法

當(dāng)前的機(jī)械手從注塑機(jī)中取出產(chǎn)品都是按固定的點(diǎn)位投放，這就還需要工人手動(dòng)將產(chǎn)品按指定排列方式進(jìn)行堆放。本文算法可以使機(jī)械手自動(dòng)以某種設(shè)定的排列方式將成品整齊排列，從而提高生產(chǎn)效率，并且使生產(chǎn)更加安全，更加智能化。

堆疊算法的具體流程為：當(dāng)機(jī)械臂到達(dá)模外的堆疊起點(diǎn)位置，先將Y軸升至原點(diǎn)；然后按順序分別求出第三方向上的目標(biāo)位置、第二方向的目標(biāo)位置、第一方向的目標(biāo)位置；最后按Z-X-Y的順序分3 次單軸運(yùn)動(dòng)到各軸的目標(biāo)位置。堆疊流程如圖3所示。

圖3 堆疊流程

先將Y軸上升至原點(diǎn)，是為了在Z軸和X軸平面上移動(dòng)產(chǎn)品時(shí)不碰撞到之前已經(jīng)排列好的產(chǎn)品。同理，最后步驟中先移動(dòng)Z和X軸，最后才移動(dòng)Y軸也是為了在堆疊本產(chǎn)品時(shí)不會(huì)碰撞到之前已經(jīng)堆疊好的產(chǎn)品。

本文中開(kāi)始和結(jié)束時(shí)的軸運(yùn)動(dòng)步驟是固定的，關(guān)鍵步驟是求出當(dāng)前序號(hào)產(chǎn)品在各軸上的目標(biāo)位置。具體算法為：將產(chǎn)品序號(hào)減1并對(duì)3個(gè)方向上的堆疊個(gè)數(shù)的乘積求余得到結(jié)果temp值，用temp值除以第一和第二方向上堆疊個(gè)數(shù)的乘積并向下取整得到該產(chǎn)品在第三方向上的偏移個(gè)數(shù)，用第三方向的起點(diǎn)位置加上偏移個(gè)數(shù)與堆疊間隔的乘積得到該產(chǎn)品在第三方向上的目標(biāo)位置；將temp 值對(duì)第一方向和第二方向上堆疊個(gè)數(shù)的乘積求余得到新的temp值，用temp值除以第一方向上的堆疊個(gè)數(shù)并向下取整得到該產(chǎn)品在第二方向上的偏移個(gè)數(shù)，用第二方向的起點(diǎn)位置加上偏移個(gè)數(shù)與堆疊間隔的乘積得到該產(chǎn)品在第二方向上的目標(biāo)位置；用temp 值對(duì)第一方向的堆疊個(gè)數(shù)求余得到該產(chǎn)品在第一方向上的偏移個(gè)數(shù)，用第一方向的起點(diǎn)位置加上偏移個(gè)數(shù)與堆疊間隔的乘積得到該產(chǎn)品在第一方向上的目標(biāo)位置。

假設(shè)產(chǎn)品的堆疊順序?yàn)閄-Z-Y，起點(diǎn)位置為x0，y0，z0，在第一方向X軸上堆疊個(gè)數(shù)為nx，堆疊間隔為ix，在第二方向Z軸上堆疊個(gè)數(shù)為nz，堆疊個(gè)數(shù)為iz，在第三方向Y軸上堆疊個(gè)數(shù)為ny，堆疊間隔為iy，產(chǎn)品序號(hào)為n。

首先求得該產(chǎn)品在第三方向Y軸上的目標(biāo)位置Sy：

然后，求得該產(chǎn)品在第二方向Z軸上的目標(biāo)位置Sz：

最后，求得該產(chǎn)品在第一方向X軸上的目標(biāo)位置Sx：

圖4 所示是產(chǎn)品按XZ-Y順序排列的xz平面圖，起點(diǎn)位置為產(chǎn)品序號(hào)1 的位置，在第一方向X軸上的堆疊個(gè)數(shù)為3 個(gè)，堆疊間距為50，第二方向Z軸上的堆疊個(gè)數(shù)為4個(gè)，堆疊間距為40，第三方向Y軸上的堆疊個(gè)數(shù)為5個(gè)，堆疊間隔為60。

圖4 xz平面圖

假設(shè)要求序號(hào)為26 的產(chǎn)品堆放位置。首先，求得該產(chǎn)品在第三方向Y軸上的目標(biāo)位置Sy：

所以，該產(chǎn)品在Y軸的目標(biāo)位置為當(dāng)前XZ平面高2個(gè)Y堆疊間隔距離的XZ平面位置上。

然后，求得該產(chǎn)品在第二方向Z軸上的目標(biāo)位置Sz：

所以該產(chǎn)品在Z軸上的目標(biāo)位置為當(dāng)前XZ平面Z方向上第一列的位置，即Z軸起點(diǎn)位置。

最后，求得該產(chǎn)品在第一方向X軸上的目標(biāo)位置Sx：

所以該產(chǎn)品在X軸上的目標(biāo)位置為當(dāng)前XZ平面上X方向上第二列的位置。所以序號(hào)為26的產(chǎn)品堆放位置為2號(hào)產(chǎn)品位置往Y軸方向偏移2個(gè)Y軸堆疊間距的位置。

3 提前變速運(yùn)動(dòng)算法

傳統(tǒng)的注塑機(jī)行業(yè)，注塑機(jī)鎖模產(chǎn)品成型后，由人工伸手進(jìn)模內(nèi)抓取產(chǎn)品，這不僅效率低，而且風(fēng)險(xiǎn)高。隨著工業(yè)機(jī)器人的發(fā)展，一種可應(yīng)用于注塑機(jī)自動(dòng)抓取的機(jī)械臂控制系統(tǒng)應(yīng)運(yùn)而生，其快速的加減速速度曲線規(guī)劃可以將注塑模內(nèi)產(chǎn)品的抓取周期縮短至2 s，大量應(yīng)用于手機(jī)殼、塑料片的抓取。然而隨著行業(yè)不斷的發(fā)展，注塑機(jī)產(chǎn)品工藝流程復(fù)雜度不斷提高，比如，快餐盒的生產(chǎn)，要求堆疊輕放，則機(jī)械手從注塑機(jī)模內(nèi)抓取產(chǎn)品后，橫行出型外后，Y軸要先快速下降，下降到接近堆放區(qū)域，速度要平滑下降到一個(gè)低速率段，緩慢到達(dá)堆放點(diǎn)，這就需要提前減速的功能。同樣，機(jī)械手在放完產(chǎn)品后，由于像快餐盒這種特殊的產(chǎn)品特性，機(jī)械手Y軸上升的初始階段需要比較低的速率，同時(shí)也要兼顧效率，Y軸上升到一定的區(qū)域后，需要平滑加速到高速率段，這就要引入提前加速的功能。

3.1 提前減速

提前減速的速度曲線如圖5 所示。由圖可知，假設(shè)第n個(gè)采樣周期末的瞬時(shí)速度為V（n），第n個(gè)采樣周期的位置增量為ΔS（n），運(yùn)動(dòng)的總長(zhǎng)度為S，加速度為a，提前減速距離為Sd，第一階段速度為Vobj1，第二階段速度為Vobj2，插補(bǔ)周期為T(mén)s。注塑機(jī)機(jī)械手Y軸從上往下堆疊輕放產(chǎn)品由兩部分組成：加速到勻速段和提前減速到低速段。

圖5 提前減速的速度曲線

（1）第一段速度規(guī)劃

此階段的位移為S1=S-Sd，在加速度期間v（n）=a·Ts·n，勻速期間v（n）=Vobj1=v0，在整個(gè)過(guò)程中：

當(dāng)實(shí)時(shí)S=S1時(shí)，此刻將進(jìn)入提前減速點(diǎn)，時(shí)間點(diǎn)為t0，速度為v0，進(jìn)入第二階段。

（2）第二階段速度規(guī)劃

此階段初速度為v0，位移為S2=S-S1，由減速到低速段、低速勻速段、減速到0 段組成。減速到低速段：t1=（v0-Vobj2）/a；v（n）=v0-a·Ts·n。減速到0 段：t3=Vobj2/a；v（n）=Vobj2-a·（Ts·n-t2）。低速勻速段：t2=S2-0.5×（v0-Vobj2）×（v0-Vobj2）/a；v（n）=Vobj2。

當(dāng)實(shí)時(shí)S=S2，第二階段運(yùn)動(dòng)完成。

3.2 提前加速

提前加速的速度曲線如圖6 所示，其實(shí)現(xiàn)方法和提前減速是一樣的。

圖6 提前加速的速度曲線

4 實(shí)驗(yàn)驗(yàn)證

本算法成功應(yīng)用在自主研發(fā)的注塑機(jī)機(jī)械手控制系統(tǒng)上，在終端客戶處，該系統(tǒng)已經(jīng)能成熟穩(wěn)定運(yùn)行，且能勝任更加復(fù)雜的注塑產(chǎn)品的抓取、堆疊輕放、堆疊輕走等任務(wù)。其中提前減速、提前加速功能，從伺服端抓取的波形圖如圖7～8所示。采用了本文提出的堆疊算法，并采取相應(yīng)的軟硬件控制策略，可以使得注塑模內(nèi)產(chǎn)品的抓取周期縮短至2 s，大量應(yīng)用于手機(jī)殼、塑料片的抓取，極大改善了上述的安全和效率問(wèn)題。

圖7 提前減速速度曲線

圖8 提前加速速度曲線

5 結(jié)束語(yǔ)

隨著現(xiàn)代工業(yè)自動(dòng)化技術(shù)的不斷發(fā)展，不少自動(dòng)化設(shè)備都應(yīng)用到了工業(yè)自動(dòng)化大生產(chǎn)中，其中就包括將機(jī)械臂應(yīng)用于注塑機(jī)的作業(yè)中。本文從注塑機(jī)作業(yè)中產(chǎn)品在模內(nèi)的取放問(wèn)題出發(fā)，以提高生產(chǎn)效率和提高作業(yè)安全為目標(biāo)，分析了傳統(tǒng)作業(yè)和應(yīng)用機(jī)械臂進(jìn)行作業(yè)的發(fā)展歷程，提出了一種更加高效的方法。通過(guò)調(diào)研注塑行業(yè)中的自動(dòng)化流程技術(shù)應(yīng)用發(fā)展歷程，較為明確了解注塑機(jī)械手在注塑自動(dòng)化應(yīng)用的實(shí)際性價(jià)效能，進(jìn)而考慮進(jìn)一步優(yōu)化當(dāng)前注塑機(jī)械手的工藝及相關(guān)算法，以提高注塑工藝的自動(dòng)化程度，降低生產(chǎn)成本，提高生產(chǎn)效率，從而達(dá)到提高生產(chǎn)效益的目的。