王旭浩,張 華

(浙江理工大學(xué) 機(jī)械與自動(dòng)控制學(xué)院,杭州 310018)

數(shù)控系統(tǒng)通常采用連續(xù)直線或圓弧對(duì)加工路徑進(jìn)行逼近來完成插補(bǔ).因多軌跡段的銜接處存在過渡拐角尖銳的特點(diǎn),所以對(duì)離散小線段加工時(shí),采用首位速度為零的加減速控制方式,但該方法使整個(gè)插補(bǔ)過程中的速度方向連續(xù)突變,即驅(qū)動(dòng)軸的頻繁啟停會(huì)影響運(yùn)動(dòng)定位的精度.針對(duì)以上問題,張曉輝等[1]建立拐角曲線過渡模型,保證軌跡銜接處的平滑性,該算法使加工效率得到有效的提升,但不能保證加工精度;Fan 等[2]采用了基于樣條曲線插補(bǔ)的軌跡規(guī)劃算法,此算法參數(shù)約束少且計(jì)算速度快,但存在軌跡形狀隨速度變化而不可控的缺點(diǎn).許健等[3]提出一種基于有限項(xiàng)正弦級(jí)數(shù)的新位置規(guī)劃算法來提高路徑過渡的速度,但速度比例參數(shù)過大會(huì)使得過渡速度很小,不利于保持速度相對(duì)穩(wěn)定.關(guān)于速度規(guī)劃技術(shù)在插補(bǔ)算法中的應(yīng)用,Hu 等[4]采用梯形加減速控制前瞻的進(jìn)給速度,但存在速度和加速度曲線過渡不平滑的問題;Cao 等[5]將多路徑段細(xì)分為直線段和圓弧小線段,并對(duì)直線和圓弧軌跡的銜接處進(jìn)行建模,由軌跡的位移約束條件規(guī)劃銜接點(diǎn)的最優(yōu)速度,但前瞻處理速度時(shí),采用對(duì)稱S 形加減速控制算法,該算法在規(guī)劃初末速度不同的路徑段時(shí)缺乏靈活性;冷洪濱等[6]采用三次多項(xiàng)式曲線代替復(fù)雜的S 形曲線進(jìn)行速度規(guī)劃,在加減速過程中實(shí)現(xiàn)了加加速度的連續(xù),但加加速度在加減速的始末位置仍然存在突變的情況.為了進(jìn)一步減少系統(tǒng)的柔性沖擊,王允森等[7]采用了四次多項(xiàng)式加減速算法、寧培志等[8]采用五次多項(xiàng)式加減速算法,但上述算法需要大量的時(shí)間進(jìn)行參數(shù)的計(jì)算,實(shí)現(xiàn)較復(fù)雜;史中權(quán)等[9]提出了一種加速度自適應(yīng)調(diào)整的前瞻處理算法,該算法在加速度的規(guī)劃上更具靈活性,但在軌跡的拐角處未考慮平滑過渡的問題.

基于以上研究現(xiàn)狀,提出了一種多軌跡段平滑過渡的前瞻插補(bǔ)算法.該算法通過建立圓弧過渡模型進(jìn)行相鄰軌跡段的銜接,并基于非對(duì)稱S 形加減速算法進(jìn)行前瞻速度規(guī)劃.本文基于速度曲線擬合構(gòu)造了一種新型柔性加減速算法,并基于此算法對(duì)插補(bǔ)運(yùn)算中的歸一化因子進(jìn)行求解,使加速度過渡更加平穩(wěn).最后將算法在Matlab 端和伺服系統(tǒng)中均進(jìn)行驗(yàn)證,結(jié)果表明,該算法可以使多軌跡段平滑過渡,有效優(yōu)化電機(jī)運(yùn)動(dòng)的平穩(wěn)性,提高末端執(zhí)行器的精度和效率.

1 相鄰軌跡段的圓弧過渡模型

對(duì)于連續(xù)軌跡而言,任意兩直線段的連接處選用建立圓弧模型來進(jìn)行拐角的平滑過渡處理[10].該算法可以根據(jù)特征點(diǎn)的坐標(biāo)和半徑調(diào)節(jié)等參數(shù)規(guī)劃出過渡圓弧的圓心坐標(biāo)、圓弧轉(zhuǎn)接點(diǎn)坐標(biāo)和拐角等參數(shù).

如圖1 所示,Qn,vn,rn(n=1,2,3,···)(下同)分別為初始設(shè)定的路徑目標(biāo)點(diǎn)、速度和過渡圓弧半徑;QTn,vTn,On分別是圓弧轉(zhuǎn)接點(diǎn)、轉(zhuǎn)接點(diǎn)速度和圓心;Kn為兩圓弧轉(zhuǎn)接點(diǎn)連線的中點(diǎn);αn和ln分別為拐角大小和角平分線;dn為分割后的小路徑段的長(zhǎng)度.

圖1 相鄰軌跡段的圓弧轉(zhuǎn)接模型

由Q0,Q1,Q2構(gòu)成的路徑段轉(zhuǎn)接模型為例進(jìn)行求解,設(shè)Q1坐標(biāo)為(x1,y1,z1),拐角α1存在如下函數(shù)關(guān)系:

由幾何關(guān)系可得:

由上式可計(jì)算得出QT1的坐標(biāo)(xT1,yT1,zT1)為:

同理可以求得QT2的坐標(biāo)值.由Q0和QT1的坐標(biāo)可求得直線長(zhǎng)度d1的值,因K1為QT1QT2的中點(diǎn),可得:

由式(5)可求得K1點(diǎn)坐標(biāo).由圖1 中的幾何關(guān)系可得:

將式(6)帶入式(7)可求得O1點(diǎn)坐標(biāo).圓弧轉(zhuǎn)接模型的所有參數(shù)均求解完畢,剩余轉(zhuǎn)接模型可以此類推.

2 新型柔性加減速控制算法

傳統(tǒng)的余弦加減速算法通過余弦函數(shù)來計(jì)算出加速、勻速和減速段的執(zhí)行時(shí)間和位移,該算法采用的余弦函數(shù)的計(jì)算量較直線函數(shù)要大,并沒有充分利用三角函數(shù)加減速算法和直線形加減速算法在軌跡規(guī)劃階段計(jì)算結(jié)果一致的特點(diǎn)來簡(jiǎn)化計(jì)算.對(duì)此提出一種新型柔加減速控制算法,在軌跡規(guī)劃時(shí),將直線形加減速算法代替余弦算法對(duì)加速段、勻速段和減速段的執(zhí)行時(shí)間進(jìn)行求解,即在插補(bǔ)控制時(shí),將當(dāng)前插補(bǔ)周期帶入余弦加減速函數(shù)中進(jìn)行曲線擬合[11].此算法能使速度和加速度的過渡更加連續(xù)平滑,并減少傳統(tǒng)余弦加減速算法的計(jì)算量.

2.1 計(jì)算加減速段的執(zhí)行時(shí)間

由圖2 知,vs為初速度,vm為勻速運(yùn)行時(shí)的速度,ve為末速度,設(shè)a 和d 分別為加速段和減速段的加速度.加速段時(shí)間Ta和運(yùn)行位移Sa可分別表示為:

圖2 直線形加減速算法速度曲線

減速段的時(shí)間Td和運(yùn)行位移Sd可分別表示為:

勻速段的運(yùn)行位移Sm和執(zhí)行時(shí)間Tm可分別表示為:

由圖2 可知:

2.2 構(gòu)造各速度段的位移函數(shù)

由余弦加減速算法構(gòu)造加速段的速度時(shí)間函數(shù):

上式中,t /Ta∈[0,1],ka和kb的取值分別為:

因三角函數(shù)無限可導(dǎo),所以聯(lián)立式(12)和(13)連續(xù)求導(dǎo)得到的加速度和加加速度均連續(xù)且有界,從而提升系統(tǒng)的平穩(wěn)性.各速度段位移函數(shù)S(t)如下：

將式(8),(9)和式(10)的計(jì)算結(jié)果,帶入到(14)式中,擬合出各階段對(duì)應(yīng)的新位移函數(shù):

對(duì)式(15)中的各段位移函數(shù)S(t)分別求導(dǎo)即可得出系統(tǒng)對(duì)應(yīng)不同階段的速度函數(shù):

同理,對(duì)速度函數(shù)v(t)求導(dǎo)可得系統(tǒng)各段的加速度函數(shù):

系統(tǒng)的穩(wěn)定性分析:取式(15)～(17)中加速段的位移、速度和加速度函數(shù)構(gòu)造一個(gè)非線性系統(tǒng):取其李雅普諾夫函數(shù)V 為:

可知V 正定.沿系統(tǒng)的任意軌跡:令V 對(duì)t 求導(dǎo):

顯然上式是負(fù)定的,由李雅普諾夫穩(wěn)定性定理可知:該系統(tǒng)是漸進(jìn)穩(wěn)定的.

由式(15),對(duì)各個(gè)階段位移函數(shù)積分可得:

將上式與式(8)～(10)對(duì)比可知,式(20)的結(jié)果與直線形加減速算法中的位移公式相同.這就很大程度上簡(jiǎn)化了速度前瞻和軌跡規(guī)劃時(shí)的運(yùn)算量.

2.3 基于Matlab 的仿真對(duì)比

(1)在Matlab 程序中設(shè)定同一組參數(shù),即位移為50 mm,初速度為0 mm/s,末速度為8 mm/s,最大速度為40 mm/s,最大加速度為110 mm/s2.對(duì)傳統(tǒng)余弦算法和新型柔性算法的運(yùn)算效率進(jìn)行仿真對(duì)比.得到的運(yùn)動(dòng)學(xué)曲線如圖3,各速度段執(zhí)行時(shí)間如表1.

圖3 余弦算法與新型柔性算法的速度曲線

表1 兩種算法各速度段的運(yùn)算時(shí)間(單位:s)

由圖3 和表1 所示的仿真結(jié)果表明:同一組參數(shù)約束下,新型柔性算法運(yùn)算總時(shí)間更短,可有效減小原余弦加減速算法的運(yùn)算量,提升運(yùn)算的實(shí)時(shí)性.

(2)對(duì)新型柔性加減速算法、直線形加減速算法及傳統(tǒng)7 段S 形加減速算法進(jìn)行仿真,得到的運(yùn)動(dòng)學(xué)曲線對(duì)比如圖4 所示.

由圖4 可知:相較于直線形加減速算法,新型柔性加減速算法的速度過渡更加平滑;相較于傳統(tǒng)7 段S 形加減速算法,新型算法的加速度過渡的平滑性得到有效改善,算法構(gòu)造的過程更簡(jiǎn)潔;新型加減速算法的速度曲線根據(jù)給定的參數(shù)約束條件不同而呈現(xiàn)非對(duì)稱的狀態(tài),滿足多路徑段規(guī)劃時(shí)的外界約束要求.

圖4 3 種加減速算法的運(yùn)動(dòng)學(xué)曲線對(duì)比圖

3 前瞻插補(bǔ)算法設(shè)計(jì)

前瞻控制技術(shù)的兩個(gè)關(guān)鍵點(diǎn)是加減速控制和連續(xù)軌跡段間的銜接速度處理[12,13],算法設(shè)計(jì)如下.

3.1 加減速控制算法

軌跡規(guī)劃中的速度控制部分由非對(duì)稱S 形加減速算法[14,15]進(jìn)行規(guī)劃,該算法的運(yùn)動(dòng)學(xué)曲線如圖5 所示.

圖5 非對(duì)稱S 形加減速算法的運(yùn)動(dòng)學(xué)曲線

由圖5 所示的運(yùn)動(dòng)學(xué)曲線可知,時(shí)間ta～tg將整個(gè)速度曲線分為加加速、勻加速、減加速、勻速、減減速、勻減速和加減速7 個(gè)階段;v0,vmax,v1分別為初速度,設(shè)定最大速度和末速度;amax和amin分別是加速度的最大值和最小值;jmax和jmin分別代表加加速度的最大值和最小值.下節(jié)將按照非對(duì)稱S 形加減速算法對(duì)連續(xù)軌跡的細(xì)分小線段進(jìn)行速度的規(guī)劃.

3.2 軌跡銜接速度處理

以圖1 中Q0Q1QT3構(gòu)成的拐角為例,由非對(duì)稱S 形加減速控制方法將軌跡段中的直線段Q0QT1和QT2QT3進(jìn)行加減速規(guī)劃,對(duì)銜接圓弧QT1QT2做勻速處理.為了使末端執(zhí)行器高效率運(yùn)行,將圓弧段的速度規(guī)劃為相鄰直線段的可達(dá)最大速度vlim.具體如下:

若滿足:

則加速段的時(shí)間Tsa可以求為:

否則按下式求解:

若滿足:

則減速段的時(shí)間Tsd按下式求解:

否則按下式求解:

勻速段時(shí)間Tsv可求解為:

上式中,q0和q1分別代表初末位置;若Tsv＞0,即存在勻速段,則系統(tǒng)的限制最大速度vlim取vmax;若Tsv≤0,則vlim＜vmax.

加速直線段Q0QT1的速度與時(shí)間如下函數(shù)關(guān)系:

減速直線段QT2QT3的速度時(shí)間存在以下函數(shù)關(guān)系:

將兩直線段中間的銜接圓弧QT1QT2的速度規(guī)劃為vlim,至此,連續(xù)軌跡段間的銜接速度規(guī)劃完畢.

3.3 位置插補(bǔ)

以Q0QT1段為例,設(shè)起始點(diǎn)Q0的坐標(biāo)為(x1,y1,z1).中間插值點(diǎn)的位置坐標(biāo)(x,y,z)可以表示為:

式中,λ 為歸一化因子,(△x,△y,△z)代表首末位置間的增量.故插補(bǔ)問題轉(zhuǎn)變?yōu)閷?duì)歸一化因子λ 的求解.傳統(tǒng)的插補(bǔ)算法基于拋物線過渡的方式對(duì)歸一化因子進(jìn)行求解,但規(guī)劃得出的速度和加速度曲線的并不連續(xù),這會(huì)加劇系統(tǒng)振蕩.本文將采用新型柔性速度規(guī)劃算法來進(jìn)行歸一化參數(shù)的求解.

為簡(jiǎn)化算法,根據(jù)第2 節(jié)內(nèi)容將新型柔性加減速算法的速度做對(duì)稱處理.設(shè)加速階段的時(shí)間為T1,位移為S1,勻速階段末端的時(shí)間為T2,位移為S2,總的運(yùn)動(dòng)時(shí)間為Tsum,總位移為Ssum.設(shè)末端執(zhí)行器在勻速運(yùn)動(dòng)時(shí)的線速度為v2,基于直線形加減速算法求解各段速度的時(shí)間時(shí),設(shè)加速度和減速度的值為a_n,則可以計(jì)算出加速階段的時(shí)間和位移分別為:

總時(shí)間Tsum為:

將位移、時(shí)間和加速度參數(shù)進(jìn)行歸一化處理:

上式中,S1λ,T1λ,T2λ,aλ分別是S1,T1,T2,a_n的歸一化參數(shù),結(jié)合新型柔性型速度規(guī)劃算法的位移公式,對(duì)歸一化因子λ 進(jìn)行求解:

上式中,t 代表當(dāng)前時(shí)刻,且t=i/N,(0≤t≤1),i=1,2,3,···,N,其中N 代表總的插值點(diǎn)數(shù),通過下式來計(jì)算：

式中,Pn是插值參數(shù),用于增加插值點(diǎn)數(shù);X 和Y 在直線插補(bǔ)中分別表示末端執(zhí)行器的總位移和線速度;在圓弧插補(bǔ)中分別表示末端執(zhí)行器的圓心角和角速度.每個(gè)插值點(diǎn)的時(shí)間值,都有一個(gè)λ 與之對(duì)應(yīng),且0≤λ≤1,結(jié)合式(30)即可得到各個(gè)插值點(diǎn)的位置坐標(biāo).

3.4 插補(bǔ)算法流程

插補(bǔ)算法流程如圖6.

圖6 插補(bǔ)算法流程圖

4 實(shí)驗(yàn)測(cè)試與結(jié)果分析

4.1 基于Matlab 的多段軌跡過渡實(shí)驗(yàn)

設(shè)定目標(biāo)曲線是由9 條線段組成的連續(xù)軌跡,給定如表2 的位置坐標(biāo)、過渡圓弧段的速度參數(shù)及其半徑參數(shù);設(shè)定系統(tǒng)允許的最大加速度amax=600 mm/s2,最大加加速度jmax=3000 mm/s3,插補(bǔ)周期為0.002 s,將以上參數(shù)初始化并進(jìn)行空間插補(bǔ)運(yùn)算,對(duì)比傳統(tǒng)算法和平滑過渡前瞻算法得到的軌跡效果及運(yùn)動(dòng)學(xué)曲線.

表2 實(shí)驗(yàn)一給定實(shí)驗(yàn)參數(shù)

方法一:傳統(tǒng)算法采用多軌跡段初末速度為零的加減速控制方式.由方法一規(guī)劃的空間軌跡效果如圖7 所示;其的運(yùn)動(dòng)學(xué)曲線如圖8 所示.

圖7 傳統(tǒng)算法規(guī)劃的空間軌跡

圖8 傳統(tǒng)算法規(guī)劃的速度和加速度曲線

由圖8 中的速度曲線可知,傳統(tǒng)的控制方式存在頻繁啟停的問題,容易引起振動(dòng)并對(duì)運(yùn)動(dòng)軸造成沖擊.基于方法一計(jì)算得到的總執(zhí)行時(shí)間為5.2636 s.

方法二:采用多軌跡段平滑過渡的前瞻算法,過渡圓弧半徑設(shè)為r=3 mm,其余參數(shù)同方法一.由方法二規(guī)劃的空間軌跡效果如圖9 所示;其運(yùn)行學(xué)曲線如圖10 所示.

圖9 平滑過渡前瞻算法規(guī)劃的空間軌跡

圖10 平滑過渡的前瞻算法規(guī)劃的運(yùn)動(dòng)學(xué)曲線

對(duì)比圖7 和圖9 可知,由方法二規(guī)劃得到的軌跡輪廓過渡的更加平滑;對(duì)比圖8 和圖10 可知,速度前瞻插補(bǔ)算法將過渡圓弧的速度規(guī)劃為相鄰直線段能達(dá)到的最大速度,提升了末端執(zhí)行器速度的連續(xù)性和平穩(wěn)性;基于方法二計(jì)算得到的總執(zhí)行時(shí)間為3.4877 s,相較于方法一,該算法將執(zhí)行效率提升近33.8%.

4.2 基于工控機(jī)的測(cè)試與分析

實(shí)驗(yàn)平臺(tái)(如圖11)的上位機(jī)采用研華的工控機(jī),處理器是intel(R)Core(TM)i5-4460,CPU 頻率是3.20 GHz,系統(tǒng)內(nèi)存為8 GB.上位機(jī)軟件TwinCAT 通過EtherCAT總線實(shí)現(xiàn)TwinCAT 和伺服驅(qū)動(dòng)器的實(shí)時(shí)通信,并可以完成人機(jī)界面的搭建.按表3 將規(guī)劃后的位置坐標(biāo)下發(fā)給伺服驅(qū)動(dòng)器,速度和加速度數(shù)據(jù)由機(jī)械手末端電機(jī)的編碼器進(jìn)行采集,并在上位機(jī)界面中實(shí)時(shí)顯示.

在上位機(jī)軟件中進(jìn)行程序編寫和設(shè)定參數(shù).設(shè)定最大加速度amax=600 mm/s2,最大加加速度jmax=3000 mm/s3,插補(bǔ)周期為0.002 s.實(shí)驗(yàn)通過限制拐點(diǎn)處不同的最大速度來檢驗(yàn)加減速控制的平穩(wěn)性,并分析過渡圓弧的半徑參數(shù)對(duì)于插補(bǔ)效率的影響.由編碼器采集的三組半徑參數(shù)對(duì)應(yīng)的數(shù)據(jù)繪制而成的速度和加速度曲線分別如圖12 和圖13 所示.

表3 初始化參數(shù)

由圖13 可知,在設(shè)定最大加速度為600 mm/s2時(shí),電機(jī)的最大加速度為378 mm/s2,表明本文前瞻算法在速度的約束下,對(duì)加速度做出了自適應(yīng)的調(diào)整,證明了算法的可行性;由圖12 和圖13 可知,當(dāng)拐角半徑參數(shù)設(shè)定為1.6 mm 時(shí),系統(tǒng)的運(yùn)行時(shí)間為6.9251 s;當(dāng)半徑設(shè)定為4.2 mm 時(shí),系統(tǒng)的運(yùn)行時(shí)間為6.8409 s;當(dāng)半徑設(shè)定為7.3 mm 時(shí),系統(tǒng)的執(zhí)行時(shí)間為6.7273 s,系統(tǒng)的執(zhí)行時(shí)間隨設(shè)定半徑參數(shù)的變化而變化.

圖11 實(shí)驗(yàn)平臺(tái)示意圖

圖12 規(guī)劃的速度曲線

圖13 規(guī)劃的加速度曲線

本實(shí)驗(yàn)結(jié)果表明:

(1)在不同的速度約束下,前瞻插補(bǔ)算法可以自適應(yīng)調(diào)整速度和加速度,保證了速度過渡的平穩(wěn)性;

(2)因多軌跡段之間靠圓弧銜接,所以每段軌跡規(guī)劃的初末速度不必減小為零,即不存在電機(jī)頻繁啟停的問題,有效的提升了加工效率,驗(yàn)證了該前瞻算法的對(duì)每小段軌跡銜接速度處理的有效性和優(yōu)越性;

(3)本文算法對(duì)參數(shù)設(shè)定進(jìn)行了簡(jiǎn)化,用戶可在上位機(jī)界面設(shè)定半徑參數(shù)對(duì)目標(biāo)軌跡做理想的修形處理,也可根據(jù)插補(bǔ)效率隨過渡圓弧半徑參數(shù)的不同而不同的特點(diǎn),設(shè)定最優(yōu)的半徑參數(shù)來使系統(tǒng)的加工效率最大化;給定的速度和加速度參數(shù)可作為算法應(yīng)用硬件平臺(tái)的最大限制速度,進(jìn)而防止軌跡形狀因速度變化而出現(xiàn)不可控的缺點(diǎn).

5 結(jié)論

多軌跡段平滑過渡的前瞻插補(bǔ)算法能夠使插補(bǔ)輪廓曲線更加圓滑,有效解決了傳統(tǒng)多軌跡段初末速度為零的加減速控制方式帶來的頻繁啟停問題,并且用戶可以調(diào)節(jié)半徑參數(shù)對(duì)軌跡進(jìn)行修形處理;該算法采用的速度前瞻技術(shù)可以規(guī)劃出多軌跡段銜接圓弧的最優(yōu)速度,并實(shí)時(shí)計(jì)算下一插補(bǔ)周期的執(zhí)行速度,使末端執(zhí)行器的運(yùn)動(dòng)具有連續(xù)性,有助于提高機(jī)床的實(shí)際運(yùn)行效率;在插補(bǔ)算法的設(shè)計(jì)中,相較于基于拋物線過渡和傳統(tǒng)余弦過渡的插補(bǔ)算法,基于新型柔性加減速控制算法的空間插補(bǔ)算法對(duì)于速度和加速度的連續(xù)性均有穩(wěn)定的提升,適用于精度要求較高的場(chǎng)合.