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

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

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

0 引言

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

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

圖1 注塑機械手

本文著重介紹機械手控制系統基本構成、堆疊工藝算法和提前變速運動算法。

1 機械手控制系統基本構成

本文控制系統為集成控制和驅動為一體，主要包括上位機示教器、控制模塊、伺服功率模塊。總體框圖如圖2所示。

圖2 總體流程

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

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

2 注塑機機械手堆疊算法

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

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

圖3 堆疊流程

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

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

假設產品的堆疊順序為X-Z-Y，起點位置為x0，y0，z0，在第一方向X軸上堆疊個數為nx，堆疊間隔為ix，在第二方向Z軸上堆疊個數為nz，堆疊個數為iz，在第三方向Y軸上堆疊個數為ny，堆疊間隔為iy，產品序號為n。

首先求得該產品在第三方向Y軸上的目標位置Sy：

然后，求得該產品在第二方向Z軸上的目標位置Sz：

最后，求得該產品在第一方向X軸上的目標位置Sx：

圖4 所示是產品按XZ-Y順序排列的xz平面圖，起點位置為產品序號1 的位置，在第一方向X軸上的堆疊個數為3 個，堆疊間距為50，第二方向Z軸上的堆疊個數為4個，堆疊間距為40，第三方向Y軸上的堆疊個數為5個，堆疊間隔為60。

圖4 xz平面圖

假設要求序號為26 的產品堆放位置。首先，求得該產品在第三方向Y軸上的目標位置Sy：

所以，該產品在Y軸的目標位置為當前XZ平面高2個Y堆疊間隔距離的XZ平面位置上。

然后，求得該產品在第二方向Z軸上的目標位置Sz：

所以該產品在Z軸上的目標位置為當前XZ平面Z方向上第一列的位置，即Z軸起點位置。

最后，求得該產品在第一方向X軸上的目標位置Sx：

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

3 提前變速運動算法

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

3.1 提前減速

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

圖5 提前減速的速度曲線

（1）第一段速度規劃

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

當實時S=S1時，此刻將進入提前減速點，時間點為t0，速度為v0，進入第二階段。

（2）第二階段速度規劃

此階段初速度為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。

當實時S=S2，第二階段運動完成。

3.2 提前加速

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

圖6 提前加速的速度曲線

4 實驗驗證

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

圖7 提前減速速度曲線

圖8 提前加速速度曲線

5 結束語

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