基于高階s型運動軌跡的位置與速度控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于精密運動控制領(lǐng)域。
【背景技術(shù)】
[0002] 在精密數(shù)控加工系統(tǒng)、機器人系統(tǒng)及精密工作臺系統(tǒng)等精密運動控制系統(tǒng)中，為 了避免各軸產(chǎn)生的沖擊、失步、超程和振蕩，W保證在高加速度、高速度的情況下，各運動機 構(gòu)的平穩(wěn)定位，常需要進(jìn)行加減速控制，常用的方法有兩種，一種是梯形加速度運動軌跡， 一種是S型運動軌跡。梯形加速度運動軌跡計算量小、易于實現(xiàn)，但是由于加速度軌跡不平 滑，存在突變，易激起系統(tǒng)振蕩，對系統(tǒng)造成沖擊，因此在實際工程中很少應(yīng)用。S型運動軌 跡能夠保證加速度曲線的連續(xù)變化，而且階數(shù)越高，軌跡越平滑，對系統(tǒng)的沖擊越小。3階、 4階、5階S曲線在實際工程中得到了大量應(yīng)用。
[0003] S曲線的階數(shù)越高，軌跡規(guī)劃算法越復(fù)雜。傳統(tǒng)的軌跡規(guī)劃算法往往針對給定的 行程、速度、加速度等參數(shù)值進(jìn)行設(shè)計，當(dāng)參數(shù)值發(fā)生改變時，需重新修改算法程序。為了在 任意給定行程、速度、加速度等參數(shù)值的情況下，不需重新更改算法程序而進(jìn)行運動軌跡規(guī) 劃，一種方法是求出各階曲線關(guān)于行程、速度、加速度等參數(shù)的分段解析表達(dá)式，然后將表 達(dá)式寫成函數(shù)，行程、速度、加速度等參數(shù)作為形參，運種方法程序設(shè)計非常簡單，但是運動 軌跡解析表達(dá)式的求解非常復(fù)雜，易出錯，S曲線階數(shù)越高越復(fù)雜，3階S曲線有7個分段， 4階S曲線有15個分段，對于更高階的S曲線，運種方法是幾乎不被采用的。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是為了解決利用傳統(tǒng)的軌跡規(guī)劃算法的位置與速度控制方法的可 移植性差的問題，本發(fā)明提供一種基于高階S型運動軌跡的位置與速度控制方法。
[0005] 本發(fā)明的基于高階S型運動軌跡的位置與速度控制方法，所述高階S型運動軌跡 為N階S曲線；所述控制方法包括如下步驟：
[000引步驟一：待規(guī)劃的N階S曲線共有2^個分段點，撕i為各階S曲線限定值，表示位 置的第n階導(dǎo)數(shù)最大值，n= 0,1，，N;根據(jù)第N階S曲線的限定值巧f"，求取第N階S曲線 在各分段點的值SW比]，k= 0,1，2,，;
[0007] 步驟二：求取第k個分段點對應(yīng)的時間值T比]和第n階S曲線在第k個分段點的 值S(n)比]，k= 0，l，2，，2N-l，n= 0，1，，N-1 ;
[0008] 步驟S:對確定T比]和SW比]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型運動軌 跡；
[0009] 步驟四：根據(jù)規(guī)劃后的N階S型運動軌跡，利用位置的采樣值、位置的第1階導(dǎo)數(shù) 的采樣值分別對位置與速度進(jìn)行控制。
[0010]所述步驟一中，根據(jù)第N階S曲線的限定值巧，求取第N階S曲線在各分段點的 值SW比]的方法為：
[0011]
[0012] S(N)[1]=0;
[0013]根據(jù)S(N)[2ii+i] =H)*S(N)[i];j= 2，. . .，N，i= 0，1，. . .，2!1，確定當(dāng)k= 2，， 2^-1時的SW比]。
[0014] 所述步驟二中，求取第k個分段點對應(yīng)的時間值T比]和第n階S曲線在第k個分 段點的值SW比]的方法為：
[001 引（1)當(dāng)k= 0 時：
[0016] S(n)[0] = 0,n= 0，1，. . .，N-1 ;
[0017] T[0] = 0 ；
[001 引 似當(dāng)k= 1，2,. . .jN-l時：
[0019]首先，求取T比]的值：
[0022] T[3] =T[l]+T[2]；
[0023]k> 4 時，
[0024]根據(jù)
和T[2W] =T[2]巧山，j= 2，， N-1，i=l，，2.i-l，獲得T比];
[002引根據(jù)獲得的T比]、T比-1]和SW比-1]，獲得第n階S曲線在第k個分段點的值SW比]:
[0026]
[0027]其中，t=T比]。
[0028] 所述步驟S中，對確定T比]和SW比]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型 運動軌跡的方法為：
[0029]根據(jù)采樣頻率f，求取各階S曲線在第1 (1 = 1，2,...）個采樣點的值：
[003引當(dāng)巧竭《J冶rf透茲-1;!時，首先從T[o]至T[2M]判斷如屬的分段區(qū)間，對于其 所
[0033]屬的區(qū)間段
各階S曲線在f處的采樣值為：
[0036] 當(dāng)1 = 1至1 =T巧W-1]xf+1，共T巧W-1]xf+1個采樣點的值均求取完成后，N階 S型運動軌跡規(guī)劃完成。
[0037] 本發(fā)明的有益效果在于，本發(fā)明針對給定行程、速度、加速度及加速度各階導(dǎo)數(shù)限 定值的情況下，提出了一種高階S型運動軌跡規(guī)劃算法，該算法可移植性強，可應(yīng)用于任意 階S曲線的軌跡規(guī)劃，具有較強的使用價值。
【附圖說明】
[0038] 圖1為【具體實施方式】一所述的控制方法的流程示意圖。
[003引圖2為4階S曲線的示意圖，Sm、Vm、Am、Jm和Dm所標(biāo)注的5條曲線依次表示位置、 速度、加速度、加速度一階導(dǎo)數(shù)、加速度二階導(dǎo)數(shù)的軌跡。
【具體實施方式】
[0040]結(jié)合圖1和圖2說明本實施方式，本實施方式所述的基于高階S型運動軌跡的位 置與速度控制方法，本實施方式高階S型運動軌跡為4階S曲線，4階S曲線位置、速度（位 置一階導(dǎo)數(shù)）、加速度（位置二階導(dǎo)數(shù)）、加速度一階導(dǎo)數(shù)（位置=階導(dǎo)數(shù)）和加速度二階 導(dǎo)數(shù)（位置四階導(dǎo)數(shù)）的限定值分別為式。巧f.)、K，，議！i)、(貨1)、人，(巧門和公
[00川 4階S曲線共有24= 16個分段點，設(shè)第k個分段點對應(yīng)的時間值為T比]，各階曲 線在第k個分段點的值分別為S比]、V比]、A比]、J比]和D比]，k= 0，1，2，，15 ;
[0042] 所述方法包括如下步驟：
[004引步驟一：根據(jù)加速度二階導(dǎo)數(shù)A,(識'），求取D比]:
[0044] D[0]=Dm;
[0045] D[l]=0；
[0046]根據(jù)S(N)[2"+i] = H)*S(N)[i];其中j= 2，...，N，i =0，1，...，2"，確定D= 化，0, -Dm, 0, -Dm, 0,Dm, 0, -Dm, 0,Dm, 0,Dm, 0, -Dm, 0};
[0047]步驟二：T比]、S比]、V比]、A比]、J比]，k= 0，1，2，，15:
[0048] (1)當(dāng)k= 0時：
[0049]T[0] =S[0] =V[0] =A[0] =J[0] = 0；
[0050]似當(dāng)k= 1，2, . . .，15 時：
[0051] 首先，求取T比]的值：
[0052] T[0] = 0 ;
[005引T山=Jm/Dm;
[0054]T巧]=Am/Jm;
[005引T閒=T凹巧山；
[005引TW] =T閒+ (Vm-2V[3]) /Am;
[0057] T閒=TW]巧山；
[0058]T[6] =T[4]巧巧];
[005引T[7]=TW]巧巧];
[0060]T閒=T[7] + (Sm-2S[7])Am;
[006。T腳=T閒巧山；
[0062] T[10] =T巧]+T[2];
[006引T[山=T閒巧閒；
[0064] T[12] =T閒巧[4];
[006引T[13]=T閒巧巧];
[0066]T[14] =T閒巧[6];
[0067]T[15] =T閒巧[7];
[006引根據(jù)獲得的T比]、T比-1]和SW比-1]，獲得第n階S曲線在第k個分段點的值SW比]:
[0069]
[0070]其中，t=T比];
[0071] 步驟S:對確定T比]和SW比]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型運動軌 跡：
[0072] 根據(jù)采樣頻率f，求取各階S曲線在第1 (1=1，2,...）個采樣點的值：
[0073]當(dāng)y<r腳時，d=j=a=v=s=0;
[0074]當(dāng)y〉r[巧時，d=j=a = V = 0, s =Sm;
[0075] 當(dāng)巧0! < ^ < 了[2"-非時，首先從T[0]至T巧w-u判斷^所屬的分段區(qū)間，對于其 所屬的區(qū)間段f運約域巧衾木銷，各階S曲線在I處的采樣值為：
[0076]d=D比];
[0081] 當(dāng)1 = 1至1 =T[1引Xf+1，共T[1引Xf+1個采樣點的值均求取完成后，4階S 型運動軌跡規(guī)劃完成，如圖2所示；
[0082] 步驟四：根據(jù)規(guī)劃后的N階S型運動軌跡，利用位置的采樣值與位置的第1階導(dǎo)數(shù) 的采樣值分別對位置與速度進(jìn)行控制。
【主權(quán)項】
1. 基于高階S型運動軌跡的位置與速度控制方法，其特征在于，所述高階S型運動軌跡 為N階S曲線；所述控制方法包括如下步驟： 步驟一：待規(guī)劃的N階S曲線共有乂個分段點，筆Γ*為各階S曲線限定值，表示位置的 第η階導(dǎo)數(shù)最大值，η = 0, 1，，N ;根據(jù)第N階S曲線的限定值，:求取第N階S曲線在各 分段點的值 S(N) [k]，k = 0, 1，2,，2Ν-1 ; 步驟二：求取第k個分段點對應(yīng)的時間值T[k]和第η階S曲線在第k個分段點的值 S(n) [k]，k = 0, 1，2,，2Ν-1，η = 0, 1，，N-I ; 步驟三：對確定T[k]和S(n) [k]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型運動軌跡； 步驟四：根據(jù)規(guī)劃后的N階S型運動軌跡，利用位置的采樣值、位置的第1階導(dǎo)數(shù)的采 樣值分別對位置與速度進(jìn)行控制。2. 根據(jù)權(quán)利要求1所述的基于高階S型運動軌跡的位置與速度控制方法，其特征在于， 所述步驟一中，根據(jù)第N階S曲線的限定值，求取第N階S曲線在各分段點的值S (N) [k] 的方法為：S(N)[1] = 0 ; 根據(jù) S(N)[2.jl+i] = (_l)*S(N)[i] ;j = 2，...，N，i = 0，1，...，2"，確定當(dāng)1^ = 2，，2[1 時的 S(N)[k]。3. 根據(jù)權(quán)利要求2所述的基于高階S型運動軌跡的位置與速度控制方法，其特征在于， 所述步驟二中，求取第k個分段點對應(yīng)的時間值T [k]和第η階S曲線在第k個分段點的值 S(n) [k]的方法為： (1) 當(dāng) k = 0 時： S(n) [0] = 0, η = 0,1，· · ·，N-1 ; T[0] = 0 ； (2) 當(dāng) k = 1，2, · · ·，2N-1 時： 首先，求取T [k]的值：T[3] = T[l]+T[2]； k彡4時， 根據(jù)；和了⑵+丨]=!^]』]+!^]，」=]，，. 1，i = 1，，2Μ，獲得 T[k]; 根據(jù)獲得的T [k]、T [k-1]和S(n) [k-1]，獲得第η階S曲線在第k個分段點的值S(n) [k]:其中，t = T[k]。4.根據(jù)權(quán)利要求3所述的基于高階S型運動軌跡的位置與速度控制方法，其特征在于， 所述步驟三中，對確定T[k]和S(n) [k]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型運動軌跡 的方法為： 根據(jù)采樣頻率f，求取各階S曲線在第1(1 = 1，2，...）個采樣點的值：，首先從T[0]至T[2N-1]判斷$所屬的分段區(qū)間，對于其所 屬的區(qū)間g各階S曲線在§處的采樣值為：當(dāng)I = 1至I = T[2N-l]Xf+l，共T[2N-l]Xf+l個采樣點的值均求取完成后，N階S型 運動軌跡規(guī)劃完成。
【專利摘要】基于高階S型運動軌跡的位置與速度控制方法，屬于精密運動控制領(lǐng)域。為了解決利用傳統(tǒng)的軌跡規(guī)劃算法的位置與速度控制方法的可移植性差的問題。所述控制方法包括：一：根據(jù)第N階S曲線的限定值求取第N階S曲線在各分段點的值S(N)[k]；二：求取第k個分段點對應(yīng)的時間值T[k]和第n階S曲線在第k個分段點的值S(n)[k]；三：對確定T[k]和S(n)[k]的S曲線進(jìn)行采樣，獲得規(guī)劃后的N階S型運動軌跡；四：根據(jù)規(guī)劃后的N階S型運動軌跡，利用位置的采樣值、位置的第1階導(dǎo)數(shù)的采樣值分別對位置與速度進(jìn)行控制。本發(fā)明用于運動機構(gòu)位置與速度的控制。
【IPC分類】G05B19/19
【公開號】CN105182906
【申請?zhí)枴緾N201510615768
【發(fā)明人】陳興林, 宋法質(zhì), 王一光, 董岳, 劉楊, 陳震宇, 張常江, 萬勇利, 劉洋, 何良辰
【申請人】哈爾濱工業(yè)大學(xué)
【公開日】2015年12月23日
【申請日】2015年9月24日