步進(jìn)電機(jī)三軸聯(lián)動(dòng)的快速加減速算法研究
摘要:加減速控制是數(shù)控系統(tǒng)的關(guān)鍵技術(shù),對(duì)提高數(shù)控系統(tǒng)的精度及速度有重要的意義。提出了一種步進(jìn)電機(jī)三軸聯(lián)動(dòng)的快速加減速算法--動(dòng)態(tài)查表法,該算法結(jié)合DDA
插補(bǔ)算法,可以用普通的單片機(jī)實(shí)現(xiàn)多種加減速曲線的運(yùn)動(dòng)控制,具有運(yùn)算速度快、精度高等優(yōu)點(diǎn)。
1 引言
步進(jìn)電機(jī)具有快速啟停能力強(qiáng)、精度高、轉(zhuǎn)速容易控制的特點(diǎn)。但是,步進(jìn)電機(jī)應(yīng)避免轉(zhuǎn)動(dòng)速率的突變,而且從停止到開(kāi)始轉(zhuǎn)動(dòng)有一個(gè)較低的起動(dòng)頻率,隨后才可以平緩地過(guò)渡到較高的轉(zhuǎn)速。相應(yīng)地在步進(jìn)電機(jī)制動(dòng)時(shí),也應(yīng)該平緩過(guò)渡。如果由于啟動(dòng)和停止控制不當(dāng),步進(jìn)電機(jī)會(huì)出現(xiàn)啟動(dòng)時(shí)抖動(dòng)和停止時(shí)過(guò)沖的現(xiàn)象,從面影響控制系統(tǒng)的控制精度[1][2][3]。為避免這種情況的發(fā)生,要對(duì)步進(jìn)電機(jī)進(jìn)行加減速控制。步進(jìn)電機(jī)加減速控制是數(shù)控系統(tǒng)的關(guān)鍵技術(shù),對(duì)提高數(shù)控系統(tǒng)的精度及速度有重要的意義。加減速控制也得到了廣泛的研究與應(yīng)用。
目前國(guó)內(nèi)外步進(jìn)電機(jī)加減速控制曲線主要包括三種[4]:直線型加減速速度曲線(也稱為梯形曲線)、指數(shù)型加減速曲線、S
型加減速曲線?刂扑惴ㄖ饕胁楸矸ê筒逖a(bǔ)迭代法。查表法的原理是設(shè)置一張時(shí)間間隔表,表中的每一項(xiàng)都對(duì)應(yīng)一個(gè)轉(zhuǎn)速,時(shí)間間隔按一定的規(guī)律變化
[1]。插補(bǔ)迭代法的原理是根據(jù)前一步的基準(zhǔn)點(diǎn)速度和加、減、勻速狀態(tài)來(lái)確定當(dāng)前步的基準(zhǔn)點(diǎn)速度[4][5]。
這兩種控制算法都有各自的適用范圍,查表法適用于單軸的加減速控制,具有算法簡(jiǎn)單、快速的特點(diǎn),但不能適用于多軸聯(lián)動(dòng)。插補(bǔ)迭代算法適用于高精度的控制,算法復(fù)雜,對(duì)處理器的要求較高,一般需要用到DSP
等高檔處理器[4]。本文結(jié)合兩種算法的優(yōu)點(diǎn),設(shè)計(jì)了一種“動(dòng)態(tài)查表法”來(lái)實(shí)現(xiàn)三軸聯(lián)動(dòng)的快速加減速算法。動(dòng)態(tài)查表法即有查表法簡(jiǎn)單、快速的特點(diǎn),又能夠用一般的單片機(jī)來(lái)實(shí)現(xiàn)高精度的多軸聯(lián)動(dòng)加減速控制。
2 動(dòng)態(tài)查表法
根據(jù)線性插補(bǔ)原理,各插補(bǔ)軸的位移與速度比相等。在加減速過(guò)程中,各插補(bǔ)軸的速度、加速度分別與合成的速度、加速度對(duì)應(yīng)成比例[5]。當(dāng)對(duì)合成速度按某種加減速方法進(jìn)行加減速時(shí),各插補(bǔ)軸在保證空間軌跡的同時(shí),也應(yīng)按相同的加減速方法進(jìn)行加減速。查表法具有簡(jiǎn)單、快速的特點(diǎn),但查表法必須事先確定一張加減速的速度表格,不能按比例應(yīng)用到多軸聯(lián)動(dòng)的場(chǎng)合。本文的思路是結(jié)合數(shù)字積分法,在加工某條線段前先按比例快速計(jì)算好相應(yīng)的速度表格,再用查表法來(lái)完成加減速控制,即“動(dòng)態(tài)查表法”。
數(shù)字積分法,也稱 DDA(Digital Differential Analyzer) ,它是建立在數(shù)字積分器基礎(chǔ)上的一種插補(bǔ)算法,其最大特點(diǎn)是易于實(shí)現(xiàn)多坐標(biāo)插補(bǔ)聯(lián)動(dòng)[6]。每一軸設(shè)置一個(gè)累加器,三軸聯(lián)動(dòng)需要設(shè)置三個(gè)累加器,每進(jìn)行一次插補(bǔ),對(duì)三個(gè)累加器分別累加。哪一軸的累加器有溢出則該軸延遲一個(gè)定時(shí)周期進(jìn)給一步(注:本文中的“進(jìn)給一步”代表發(fā)一次脈沖的高電平或低電平,兩次“進(jìn)給一步”代表一個(gè)完整的脈沖)。在實(shí)際應(yīng)用中,會(huì)設(shè)定一個(gè)較大的整數(shù)N,一般是2m,本文設(shè)置的是0x8000,即215。當(dāng)累加超過(guò)該數(shù)時(shí),視為溢出。相應(yīng)的,根據(jù)單片機(jī)的定時(shí)周期,加工速度將折算成兩個(gè)整數(shù)用于實(shí)際的插補(bǔ)運(yùn)算,一個(gè)整數(shù)是n,代表過(guò)n
個(gè)定時(shí)周期進(jìn)給一步,另一個(gè)是dn,代表累加數(shù),當(dāng)dn 累加到達(dá)N 時(shí),延遲一個(gè)定時(shí)周期進(jìn)給一步。則:
V 代表的是指定的加工速度,做加減速控制的加速過(guò)程需要將速度從起跳速度按照一定的曲線加速到V,減速過(guò)程需要將速度從V
再減到起跳速度。
本文設(shè)計(jì)了一系列速度檔位,最低檔位就是起跳速度,然后逐步增加到V,每檔速度同時(shí)對(duì)應(yīng)一個(gè)脈沖數(shù)組,及走過(guò)多少脈沖后切換到另外一個(gè)速度檔位。脈沖與速度構(gòu)成的曲線可以是梯形曲線、指數(shù)型加減速曲線或S
型加減速曲線。每個(gè)速度檔位都按照相同的比例縮小速度V,三軸都按照相同的比例,就滿足了線性插補(bǔ)原理,可以在保證空間軌跡的同時(shí),各軸按照相同的加減速方法進(jìn)行加減速。但計(jì)算速度檔位必須在加工線段前進(jìn)行,而且不能有除法運(yùn)算,否則一般的單片機(jī)難以達(dá)到實(shí)時(shí)性的要求。按一定的比例縮小V
必須用到除法,要避免除法運(yùn)算就只能采用移位運(yùn)算來(lái)代替。由式(1)~(3)可知,k 與V 成反比,而n 和dn 分別對(duì)應(yīng)k 的整數(shù)和小數(shù)部分。假設(shè)要得到V*p/q
對(duì)應(yīng)的n 和dn,其中p、q 都是自然數(shù)且p<q。則k’=k*q/p,要用移位來(lái)代替除法必須p=2m,而且要爭(zhēng)取各速度檔位相對(duì)均勻。
本文設(shè)計(jì)的速度檔位因子分別是:1/16, 1/8, 1/4, 2/5,1/2, 2/3, 4/5, 8/9, 共八個(gè)檔位。對(duì)精度要求高的還可以設(shè)置更多的檔位。確定速度檔位因子后,就可以通過(guò)一系列的移位指令來(lái)計(jì)算V*p/q
對(duì)應(yīng)的n 和dn,以4/5 為例:計(jì)算步驟如下:
n=ctrlDatas[i].n*5;// ctrlDatas[i].n 是給定速度對(duì)應(yīng)的n,*5 也可以用左移位+加法代替
ns[j]=n>>2;//ns[j]是每個(gè)速度檔位對(duì)應(yīng)的n,>>2 代表除以4
dns[j]=(ctrlDatas[i].dn*5)>>2;// dns[j]是每個(gè)速度檔位對(duì)應(yīng)的dn
dns[j]=dns[j]+((n & 0x3)<<13);//將n 的余數(shù)補(bǔ)充到dn
ns[j]+=dns[j]>>15;//將dn 對(duì)應(yīng)的整數(shù)部分補(bǔ)充到n
dns[j]=dns[j] & 0x7FFF;//取余數(shù)
可見(jiàn)通過(guò)一系列的位運(yùn)算和加法運(yùn)算實(shí)現(xiàn)了各速度檔位的計(jì)算,而且精度非常高。實(shí)驗(yàn)證明,常規(guī)的單片機(jī)完全可以達(dá)到實(shí)時(shí)性的要求。
3 動(dòng)態(tài)查表算法描述
動(dòng)態(tài)查表算法有兩部分,一是加工前計(jì)算各檔位速度vc,另一個(gè)是實(shí)時(shí)的加減速控制rtvc。
下面分別描述。
3.1 算法vc,用于加工線段前的預(yù)處理,計(jì)算各軸各檔位速度對(duì)應(yīng)的n 和dn:
。1)求最長(zhǎng)軸所在的索引longestIndex。本文將三軸分別編號(hào)成0,1,2,分別代表XYZ三軸,最長(zhǎng)加工軸代表該軸加工速度最快,以該軸為基準(zhǔn)進(jìn)行加減速,其他軸同步按比例進(jìn)行加減速
。2)如果最長(zhǎng)加工軸的加工脈沖小于系統(tǒng)指定脈沖,以系統(tǒng)指定的低速進(jìn)行加工,清除加減速標(biāo)志,不進(jìn)行加減速控制。算法結(jié)束,否則轉(zhuǎn)第(3)步
。3)如果某軸的速度低于指定速度,該軸不進(jìn)行加減速控制,全部按照指定速度進(jìn)行加工。算法結(jié)束,否則轉(zhuǎn)第(4)步
。4)按照第2 節(jié)的速度檔位劃分及移位算法計(jì)算各軸各檔位速度對(duì)應(yīng)的n 和dn。算法結(jié)束3.2 算法rtvc,用于實(shí)時(shí)加減速控制,根據(jù)發(fā)出脈沖計(jì)數(shù)進(jìn)行相應(yīng)的加減速控制:
(1)如果有加速標(biāo)志,代表還需要進(jìn)行加速控制,轉(zhuǎn)第(2)步,否則轉(zhuǎn)第(5)步
(2)如果最長(zhǎng)加工軸的加工脈沖超過(guò)某檔位對(duì)應(yīng)的脈沖數(shù),轉(zhuǎn)第(3)步,否則轉(zhuǎn)第(5)步
。3)如果達(dá)到最大檔位,代表已經(jīng)完成加速,設(shè)置各軸實(shí)時(shí)n 和dn 為指定正常加工速度V 對(duì)應(yīng)的n 和dn,清除加速標(biāo)志,轉(zhuǎn)第(5)步。否則轉(zhuǎn)第(4)步
。4)設(shè)置各軸實(shí)時(shí)n 和dn 為對(duì)應(yīng)加速檔位速度對(duì)應(yīng)的n 和dn
(5)如果有減速標(biāo)志,代表需要進(jìn)行減速控制,轉(zhuǎn)第(6)步,否則算法結(jié)束
。6)計(jì)算最長(zhǎng)加工軸距離加工終點(diǎn)還有多少脈沖pulse
(7)如果pulse 小于某檔位對(duì)應(yīng)的脈沖數(shù),轉(zhuǎn)第(8)步,否則算法結(jié)束
(8)如果達(dá)到最大檔位,代表已經(jīng)完成減速,清除減速標(biāo)志,算法結(jié)束,否則轉(zhuǎn)第(9)步
(9)設(shè)置各軸實(shí)時(shí)n 和dn 為對(duì)應(yīng)減速檔位速度對(duì)應(yīng)的n 和dn。
算法結(jié)束算法 rtvc 的流程圖如圖1 所示,算法vc 的流程圖省略。
算法 vc 只需要在加工某線段前執(zhí)行一次,而算法rtvc
用于實(shí)時(shí)加減速控制,需要在單片機(jī)的主循環(huán)里面反復(fù)調(diào)用,但由于調(diào)用rtvc 前速度表格已經(jīng)計(jì)算好,只需查表即可,因此算法的實(shí)時(shí)性非常好。
在實(shí)際的加工程序中,除了增加以上兩個(gè)算法之外,還是按照常規(guī)的DDA 算法進(jìn)行積分插補(bǔ),按照常規(guī)判斷加工終點(diǎn)到達(dá)等等,不再贅述。
4 總結(jié)
動(dòng)態(tài)查表法用到的表格有兩個(gè),一個(gè)是切換各檔位速度需要走過(guò)的脈沖,另一個(gè)的各軸各檔位速度對(duì)應(yīng)的n 和dn。后一個(gè)表格是動(dòng)態(tài)計(jì)算生成的。調(diào)整前一個(gè)表格可以實(shí)現(xiàn)不同的加減速曲線,因此動(dòng)態(tài)查表法可以實(shí)現(xiàn)各種加減速曲線,根據(jù)實(shí)際加工情況自由選擇,而不需要增加算法復(fù)雜度。同時(shí),計(jì)算各檔位速度對(duì)應(yīng)的n
和dn 只需要進(jìn)行一系列的位運(yùn)算和加法運(yùn)算,一般的單片機(jī)即可達(dá)到良好的實(shí)時(shí)性。
動(dòng)態(tài)查表法已經(jīng)在數(shù)控點(diǎn)膠機(jī)上得到了很好的應(yīng)用,實(shí)踐表明,用一般的ARM 芯片完全滿足了實(shí)時(shí)性和高精度的要求。動(dòng)態(tài)查表法同樣可以應(yīng)用到其他多軸聯(lián)動(dòng)運(yùn)動(dòng)控制場(chǎng)合。
作者創(chuàng)新點(diǎn):本文設(shè)計(jì)的步進(jìn)電機(jī)三軸聯(lián)動(dòng)快速加減速算法-動(dòng)態(tài)查表法還沒(méi)有文獻(xiàn)記載,屬作者原創(chuàng)。該算法可以實(shí)現(xiàn)不同的步進(jìn)電機(jī)加減速曲線,具有簡(jiǎn)單高效的特點(diǎn),用普通的單片機(jī)即可達(dá)到良好的實(shí)時(shí)性。
杭州浙機(jī)科技有限公司
http://www.whyongqing.cn