史步海，郭協(xié)濤

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

0 引言

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

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

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

1 EMC2 分析

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

1.1 軟件架構(gòu)

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

1.2 運(yùn)動控制器(EMCMOT)

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

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

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

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

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

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

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

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

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

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

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

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

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

由(1)(2)式有:

創(chuàng)設(shè)問題情境能夠促使學(xué)生積極思考，主動設(shè)計或?qū)ふ医獯鸱桨?，從而解決問題。因此，專業(yè)課教師在課堂教學(xué)中，應(yīng)當(dāng)精心創(chuàng)設(shè)問題情境，引導(dǎo)學(xué)生積極主動地參與課堂學(xué)習(xí)，參與問題設(shè)計、發(fā)現(xiàn)、提出、解決的全過程，引導(dǎo)學(xué)生在發(fā)現(xiàn)和解決實際問題過程中，領(lǐng)悟、總結(jié)出發(fā)現(xiàn)和解決問題的一般方法、思路與技巧，訓(xùn)練學(xué)生解決問題的能力。

由(4)式可解出:

由(2)(5)有:

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

2 S 形加減速控制算法

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

圖3 改進(jìn)后的S 形加減速

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

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

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

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

STEP 1:假設(shè)機(jī)床實際運(yùn)行中加加速段amax剛好能達(dá)到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，此時必定產(chǎn)生勻加減速段，假設(shè)vmax剛好增大至vlim，重新規(guī)劃步驟1 中的amax和vmax。

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

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

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

3 EMC2 中的運(yùn)行及測試

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

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

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

圖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 結(jié)束語

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

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

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

［3］Shuanghui Hao，F(xiàn)ang 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］石川，趙彤，葉佩青，等. 數(shù)控系統(tǒng)S 曲線加減速規(guī)劃研究［J］. 中國機(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］許軍，李華. 開源數(shù)控系統(tǒng)EMC2 的編譯和啟動分析［J］. 現(xiàn)代制造技術(shù)與裝備，2009，183(6):60.