基于EMC2 的S 形加減速控制算法*

史步海，郭協(xié)濤

(華南理工大學 自動化科學與工程學院，廣州 510640)

0 引言

數控加工正朝著高速高效方向發(fā)展，高速加工要求機床各運動軸能在極短的時間內達到高速運行狀態(tài)并實現高速準停。目前國內經濟型CNC 系統(tǒng)大都采用T 形加減速和指數加減速控制方法，國內外一些高檔的數控系統(tǒng)則開始采用S 形加減速控制方法［1］。

EMC2 是開源的CNC 系統(tǒng)軟件，自帶了T 形加減速控制算法，算法簡單，但是啟動和結束時存在加速度突變，對機床造成沖擊。傳統(tǒng)S 形加減速控制算法能較好的克服傳統(tǒng)加減速方法的缺點，但算法相對來說比較復雜，實現起來有難度，而且加速度曲線是梯形的，加加速度存在突變。

T 形和傳統(tǒng)的S 形加減速控制算法已不能滿足高速機床及現代化裝備加工的需要，研究新的算法是非常必要的。為使加速度曲線更加平滑，加加速度沒有突變，Jianren SUN 等在文獻［5］中提出三角函數的平方的正弦曲線加減速算法。為減少s 形加減速控制算法在條件判斷和計算方面的復雜性，Shuanghui Hao［3］等提出了等價T 形加減速的概念。本文在對EMC2 各個模塊分析的基礎上，針對其軌跡規(guī)劃的原理與加減速控制算法相關程序進行深入的分析，并在此基礎上提出了新的改進算法，并將改進后的新型S 形加減速控制應用到其中。

1 EMC2 分析

美國國家標準和技術研究所主持的增強型運動控制器(Enhanced Machine Controller project，EMC)項目是在RTLinux 操作系統(tǒng)上建立的軟件數控系統(tǒng)，EMC2 是在原有EMC 軟件的基礎上加入了許多新的特性和功能。

1.1 軟件架構

整個EMC2 軟件系統(tǒng)由四個主要模塊組成:運動控制器(EMCMOT)運行在實時環(huán)境中，執(zhí)行軌跡規(guī)劃、正逆運動學運算，計算出電機控制系統(tǒng)的期望輸出;I/O 控制器(EMCIO)主要處理緊急啟停、冷卻裝置等其他輔助性輸入輸出控制;人機界面(GUI)為用戶提供友好的操作平臺;任務控制器(EMCTASK)是系統(tǒng)樞紐，他從用戶界面取得操作信息，然后將其分解為對運動控制器和I/O 控制器的操作，并且監(jiān)控運動控制器和I/O 控制器完成實際的操作;并且監(jiān)控運動控制器和I/O 控制器完成實際操作。這種模塊化、層次化的軟件設計結構使得開發(fā)式數控系統(tǒng)易于擴展，模塊化的軟件架構能夠滿足各種類型機床的控制［2］。

1.2 運動控制器(EMCMOT)

運動控制器模塊是數控系統(tǒng)的核心，主要完成刀具與工件間的相對運動控制，包括對軸位置采樣、計算軌跡值、插補以及計算給伺服的輸出值等控制。

運動控制器程序完成與伺服控制所有相關工作，其程序位于源碼Src/Emc/Motion 中的motion. c。在motion. c 中主要有init_hal_io、函數、init_comm_buffers、init_thread()等函數。init_hal_io 函數初始化HAL 引腳和參數，使得從實時控制模塊傳來的數據能夠接收和使用;init_comm_buffers 函數分配并初始化與EMC 用戶空間通訊的命令、狀態(tài)、和錯誤信息;函數init_thread()創(chuàng)建實時線程，并在所創(chuàng)建的線程上加載函數，主要創(chuàng)建了Controller 和CommandHandler，其中Controller 線程主要負責軌跡規(guī)劃和電機伺服控制，CommanHandler 線程用來處理任務層發(fā)下來的針對Motion 模塊的控制命令。Controller 線程的主要代碼位于源碼Src/Emc/Motion 下的control. c 中，其中的emcmotController()函數是伺服周期里的主循環(huán)函數，所有的狀態(tài)邏輯計算與軌跡計算都從這里調用。emcmotController()函數依次調用process_inputs()，do_forward()，get_pos_cmd()等函數。其中get_pos_cmd()函數最為重要，用以產生軌跡的位置點。在自由(free)模式下get_pos_cmd()工作在關節(jié)空間，每個關節(jié)單獨規(guī)劃，每個關節(jié)有一個簡單的“軌跡規(guī)劃器”，可實現單軸的點動和回零，其程序直接位于自由模式下對應的程序段中。聯(lián)動(coordinated)模式下get_pos_cmd()主要調用軌跡規(guī)劃器(trajectory planer)文件和三次樣條曲線文件的函數根據輸入(MDI、G 代碼)完成笛卡爾空間的直線、圓弧、攻牙插補，可實現多軸的空間運動功能。首先調用tpRuncycle 的得到插補的下個位置坐標存放在emcmotDebug－ ＞queue 隊列中，從隊列中取出進行逆運動學變換計算得到關節(jié)空間各軸坐標存放在positions 里面，則各軸坐標為三次樣條曲線的第四點，最后調用cubicInterpolate 函數進行三次樣條插補得到各軸新的位置坐標。

位于源碼Src/Emc/Kinematics 下軌跡規(guī)劃器程序文件tc.c 是軌跡點控制器，用以建立和維護軌跡點隊列結構體TC_QUEUE_STRUCT，而文件tp. c 是軌跡規(guī)劃器，主要維護一個大的結構體TP_STRUCT，所有與軌跡規(guī)劃相關的參數都在里面有所體現，TP_STRUCT 里面的隊列為tc.c 里面的軌跡點隊列，tp.c主要運算在tpRunCycle()函數中完成。tpRunCycle()中主要是調用tcRuncycle()函數求出當前運行的軌跡段tc 和下一軌跡段nexttc 運行到達的具體位置，tc 和nexttc 都是TC_STRUCT 類型的結構體。有速度融合要求時在tpRunCycle()會計算出融合速度。

運動控制器在EMC2 中被實現成一個在實時周期運行的HAL 模塊，提供了6 條線性軸外加3 條旋轉軸總共9 軸的控制維度。它周期地執(zhí)行軌跡規(guī)劃，計算出下個周期各個運動軸需要執(zhí)行的位移量，從軌跡規(guī)劃的方式上，運動控制器模塊采取了2 級插補的軌跡規(guī)劃方式，即粗插補和精插補。粗插補的計算周期通常被配置為精插補的10 倍，主要執(zhí)行梯形速度軌跡插補。精插補主要負責將精插補的數據通過機器人坐標轉換成機器關節(jié)空間后，對每個電機軸進行更平滑的3 次樣條曲線插補，以控制每個實際電機軸更平滑的運轉。

1.3 軌跡規(guī)劃器工作原理

當EMC2 工作在自由模式下，速度從零開始到零結束，速度以最大加速度增加至速度上限之后以最大速度勻速運行直到開始減速，減速時同樣以加速時的加速度進行。這種運動有兩種情況:

(1)如果有勻速段，速度加速至期望的進給速度之后進入勻速段，最后減速至0。

(2)如果沒有勻速段，速度加速至走完一半的路徑，剩下的另一半路徑的運動過程中速度減至0。

圖1 軌跡規(guī)劃的位移、速度和加速度

圖1 中的位移軌跡表示花了4500 個軌跡規(guī)劃周期(EMC2 的軌跡規(guī)劃周期是0.001s)時間剛好走完10 的位移量;速度軌跡表明軌跡規(guī)劃器的工作原理，首先計算出期望速度(虛線表示)，期望速度要盡可能大而且以最大加速度減速至終點時剛好為0，這樣保證了在最短的時間內走完路徑，而實際速度(實線表示)受最大速度和最大加速度的限制，一開始從零以最大加速度加速，到達最大速度時以最大速度勻速運行，當實際速度與期望速度相等時，便以最大加速度減速至終點停止運行，這個時間段內期望速度和實際速度的曲線是重疊的;加速度軌跡表示為實際運行過程中的加速度變化，加速度在加速段為最大值，勻速段為0，減速度為反方向的最大值［6］。

圖2 軌跡規(guī)劃速度與實際速度

假設T 為終點位置，P 為當前位置，vs為新的軌跡規(guī)劃周期中的速度，tm為運行的總時間，am為最大允許加速度，vc為當前速度，ts為軌跡規(guī)劃周期。

如圖2 所示，速度曲線與速度軸和時間軸所圍成的面積就是總位移，也就是需要走完的路徑長度:

由(1)(2)式有:

創(chuàng)設問題情境能夠促使學生積極思考，主動設計或尋找解答方案，從而解決問題。因此，專業(yè)課教師在課堂教學中，應當精心創(chuàng)設問題情境，引導學生積極主動地參與課堂學習，參與問題設計、發(fā)現、提出、解決的全過程，引導學生在發(fā)現和解決實際問題過程中，領悟、總結出發(fā)現和解決問題的一般方法、思路與技巧，訓練學生解決問題的能力。

由(4)式可解出:

由(2)(5)有:

這等價于EMC2 源碼的函數tcRunCycle 中的位移和速度公式。

2 S 形加減速控制算法

傳統(tǒng)的S 形加減速控制算法，其基本思想為:將加減速過程細分為7 個階段:加加速段，勻加速段，減加速段，勻速段，加減速段，勻減速段，減減速段，通過漸變控制各段的加速度使其加減速過程的速度按S 形曲線的形式變化，從而達到減少沖擊的目的［4］。為使加速度曲線更加平滑，加加速度沒有突變，需對傳統(tǒng)的S 形加減速控制算法進行改進。圖3為改進后的S 曲線加減速全過程中的位置、速度、加速度以及加加速度曲線。加速過程包含加加速度段、勻加速段、減加速段，減速過程包含加減速段、勻減速段、減減速段;加速和減速之間通過勻速運動銜接，整個過程分為7 段。式中，amax為機床允許的最大加速度;jmax為機床允許的最大加加速度。

圖3 改進后的S 形加減速

為減少S 形加減速控制算法在條件判斷和計算方面的復雜性，可利用等價T 形加減速的概念，并對S 曲線加速段進行規(guī)劃。

圖4 等價T 形的S 形加減速

S 形加減速控制算法速度曲線的加速段和減速段、加速度曲線的加加速段和減加速段、加速段曲線的加減速段和減減速段分別是對稱的，充分利用其對稱性可大大簡化速度規(guī)劃過程。S 形加減速控制算法的加速和減速過程的平均加速度為圖4 中直線AD 和直線EH 的斜率，所以整個速度曲線可以等價為梯形ADEH 的梯形加減速曲線［2］。

jerk、alim、vlim分別為機床允許的最大加加速度、最大加速度、最大速度，amax、vmax分別為機床實際運行過程中所能達到的最大加速度和最大速度，aequ為規(guī)劃加減速過程等價加速度，sref為規(guī)劃路徑的長度，s為機床實際需要運行的路徑長度。由對稱性知:

STEP 1:假設機床實際運行中加加速段amax剛好能達到alim，并且沒有勻加減速段和勻速段。

STEP 2:如果s ＜sref，vlim＞vmax，即s 不夠大以致加速度沒有足夠的時間來增大至alim，即amax＜alim，令s=2amax，步驟1 中的amax和vmax需要重新規(guī)劃。

STEP 3:如果步驟2 中vlim＜ vmax，即vmax超過vlim，amax和vmax需要重新規(guī)劃，令vmax=amax* t1此時有勻速段。

STEP 4:如果s ＞sref，vlim＜vmax，那么步驟1 中的amax和vmax需要重新規(guī)劃，方法與步驟3 相同，此時有勻速段。

STEP 5:如果s ＞sre，vlim＞vmax，即s 足夠長，以致amax有足夠的時間來增大至alim，并且vmax沒有超過vlim，此時必定產生勻加減速段，假設vmax剛好增大至vlim，重新規(guī)劃步驟1 中的amax和vmax。

STEP 6:在步驟5 中如果s ＜sref，s 不夠長以致vmax無法增大至vlim，重新規(guī)劃，勻加減速段減小。

STEP 7:在步驟5 中如果s ＞sref，s 足夠長以致vmax能夠增大至vlim而且產生勻速段，重新規(guī)劃t2，aeuq，vmax。

圖5 改進后的S 形加減速算法規(guī)劃流程圖

3 EMC2 中的運行及測試

EMC2 中采用的插補計算方法是數據采樣插補法，也稱為時間分割法，它尤其適合于閉環(huán)和半閉環(huán)以直流或交流電機為執(zhí)行機構的位置采樣控制系統(tǒng)。這種方法是把加工一段直線或圓弧的整段時間細分為許多相等的時間間隔，稱為單位時間間隔(或插補周期)。每經過一個單位時間間隔就進行一次插補計算，算出在這一時間間隔內各坐標軸的進給量，邊計算，邊加工，直至加工終點［2］。

EMC2 本身沒有定義S 形加減速中需要用到的最大加加速度這個變量，通過修改源碼在配置文件ini 文件中增加最大加加速度的配置，其中單軸的最大加加速度用于自由模式下，全局的軌跡的最大加加速度用于聯(lián)動模式下。

將完善后的等價T 形的S 形加減速算法應用到EMC2 上，修改相應的源碼，在修改后的源文件中編譯安裝EMC2。在實驗平臺上的測試結果如下各圖所示。

圖6 原系統(tǒng)在單軸模式下速度曲線

圖7 改造后系統(tǒng)在單軸模式下的速度曲線

圖8 原系統(tǒng)在聯(lián)動模式下的速度曲線

圖9 改造后系統(tǒng)在聯(lián)動模式下的速度曲線

圖10 原系統(tǒng)在聯(lián)動模式下的加速度曲線

圖11 改造后系統(tǒng)在聯(lián)動模式下的加速度曲線

4 結束語

針對EMC2 系統(tǒng)中自帶的T 形加減速控制算法啟動和結束時存在加速度突變以及傳統(tǒng)的S 形加減速控制算法存在加加速度突變的問題，文中通過提出一種新的S 形加減速控制算法，對EMC2 系統(tǒng)主要模塊進行改造和升級，并在實驗樣機平臺上進行實際測試。實驗證明，機床伺服系統(tǒng)速度和加速度曲線更加平滑，運行穩(wěn)定，滿足高速加工要求的柔性加減速特性，能實現高速啟停而又減少對機床的沖擊，并且新算法規(guī)劃簡單便于應用。此方法為數控加工的加減速控制提供了一種新的方法和探索。

［1］商允舜. CNC 數控系統(tǒng)加減速控制方法研究與實現［D］. 浙江:浙江大學機械與能源工程學院，2006.

［2］吳晶. 基于EMC2 的工業(yè)機器人研究［D］. 廣州:華南理工大學自動化科學與工程學院，2011.

［3］Shuanghui Hao，Fang Song，Jie Liu，Minghui Hao.An applied CNC acceleration and deceleration control algorithm research［C］. Proceedings of 2008 IEEE International Conference on Mechatronics and Automation.IEEE，2008:404－407.

［4］石川，趙彤，葉佩青，等. 數控系統(tǒng)S 曲線加減速規(guī)劃研究［J］. 中國機械工程，2007，18(12):1421－1422.

［5］Tomislav Staroveski，Danko Brezak，Toma udilisk，Dubravko Majetic.Implementation of a Linux-Based CNC open control system.12thInternational Scientific Conference on Production Engineering.Zagreb:Croatian Association of Production Engineering，2009:210－213.

［6］Simple tp notes.http://wiki.linuxcnc.org/cgi-bin/wiki.pl?Simple_Tp_Notes.

［7］Jianren SUN，Chibing HU.Research on simulation of acceleration and deceleration algorithrithm with trigonometric function squared sine shape curve.2010 Third International Conference on Information and Computing.

［8］The EMC Tean.User Manual V2.5.2012.http://www.linuxcnc.org/index.php/English/documentation.

［9］ The EMC Tean. Integrators Manual V2.5.2012. http://www.linuxcnc.org/index.php/English/documentation.

［10］Kim Doang Nguyen，I-Ming Chen，Teck-Chew Ng.Planning algorithms for S-curve Trajectories［J］.IEEE.2007

［11］許軍，李華. 開源數控系統(tǒng)EMC2 的編譯和啟動分析［J］. 現代制造技術與裝備，2009，183(6):60.