一種注塑機械手堆疊算法及其運動控制策略

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

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

0 引言

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

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

圖1 注塑機械手

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

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

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

圖2 總體流程

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

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

2 注塑機機械手堆疊算法

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

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

圖3 堆疊流程

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

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

假設(shè)產(chǎn)品的堆疊順序為X-Z-Y，起點位置為x0，y0，z0，在第一方向X軸上堆疊個數(shù)為nx，堆疊間隔為ix，在第二方向Z軸上堆疊個數(shù)為nz，堆疊個數(shù)為iz，在第三方向Y軸上堆疊個數(shù)為ny，堆疊間隔為iy，產(chǎn)品序號為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平面圖，起點位置為產(chǎn)品序號1 的位置，在第一方向X軸上的堆疊個數(shù)為3 個，堆疊間距為50，第二方向Z軸上的堆疊個數(shù)為4個，堆疊間距為40，第三方向Y軸上的堆疊個數(shù)為5個，堆疊間隔為60。

圖4 xz平面圖

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

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

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

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

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

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

3 提前變速運動算法

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

3.1 提前減速

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

圖5 提前減速的速度曲線

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

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

當(dāng)實時S=S1時，此刻將進(jì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)實時S=S2，第二階段運動完成。

3.2 提前加速

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

圖6 提前加速的速度曲線

4 實驗驗證

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

圖7 提前減速速度曲線

圖8 提前加速速度曲線

5 結(jié)束語

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