新型速度控制算法及新型脈沖控制算法
【專利摘要】本發(fā)明公開了新型速度控制算法及新型脈沖控制算法，在進行軌跡處理中，包括用戶設(shè)定的速度參數(shù)在上位機進行解析步驟；將軌跡劃分成微線段步驟；基于微線段的速度前瞻步驟和逐線段輸出步驟；在所述基于微線段的速度前瞻步驟與逐線段輸出步驟之間的處理過程中，在對每一條微線段處理時，采用分周期控制速度，實時改變當前速度，本發(fā)明解決現(xiàn)有技術(shù)到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整所存在的技術(shù)不足之處，而提供新型速度控制算法及新型脈沖控制算法，在處理中，運動曲線不是預先定好的，而是可隨時調(diào)速的，基于微線段的速度前瞻和逐線段輸出之間的控制方式上，可隨時調(diào)整速度，以適應多種變形軌跡場合。
【專利說明】新型速度控制算法及新型脈沖控制算法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及自動控制【技術(shù)領(lǐng)域】，具體的說，是新型速度控制算法及新型脈沖控制算法。

【背景技術(shù)】
[0002]現(xiàn)有的軌跡運動中，在進行軌跡處理中，包括用戶設(shè)定的速度參數(shù)在上位機進行解析步驟；將軌跡劃分成微線段步驟；基于微線段的速度前瞻步驟和逐線段輸出步驟；而在基于微線段的速度前瞻步驟和逐線段輸出步驟的處理上，基于微線段的速度前瞻之后到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整。
[0003]如圖1所示，軌跡由三條線段組成，以往的算法是先進行速度前瞻，目的是:
(1)根據(jù)初始A點速度，結(jié)束C點速度，和限制的最高速度，以及脈沖數(shù)，計算出B點C點的速度，這牽涉到B點C點方向及矢量角，由前瞻算法決定；
(2)根據(jù)計算出的A點速度B點速度以及每一段的脈沖數(shù)，計算出當前段的降速點，這樣保證每一段的速度曲線為T形或S形。
[0004]因為算法在程序運行前期就將所有的速度及減速點設(shè)置好，之后板卡將按照既定的參數(shù)運行至軌跡終止。如果想要在運動過程中改變當前速度或最大速度，將是很困難的事情。
[0005]在原有的算法中，速度前瞻決定了整條曲線的所有拐點及每條微線段的最高速和減速點，因此當數(shù)據(jù)設(shè)置好之后，不能再修改，只能按此參數(shù)運行完之后才能開始下一條曲線，比較機械，更適用于比較專用的場合。
[0006]目前已有的實現(xiàn)脈沖的技術(shù)是由DSP+FPGA實現(xiàn)，DSP計算軌跡前瞻，對FPGA進行預置參數(shù)，F(xiàn)PGA根據(jù)參數(shù)完成整條軌跡的輸出，中途調(diào)整參數(shù)不易。


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

[0007]本發(fā)明的目的在于解決現(xiàn)有技術(shù)到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整所存在的技術(shù)不足之處，而提供新型速度控制算法及新型脈沖控制算法，在處理中，運動曲線不是預先定好的，而是可隨時調(diào)速的，基于微線段的速度前瞻和逐線段輸出之間的控制方式上，可隨時調(diào)整速度，以適應多種變形軌跡場合。
[0008]本發(fā)明通過下述技術(shù)方案實現(xiàn):新型速度控制算法，在進行軌跡處理中，包括用戶設(shè)定的速度參數(shù)在上位機進行解析步驟；將軌跡劃分成微線段步驟；基于微線段的速度前瞻步驟和逐線段輸出步驟；在所述基于微線段的速度前瞻步驟與逐線段輸出步驟之間的處理過程中，在對每一條微線段處理時，采用分周期控制速度，實時改變當前速度。
[0009]進一步的，為更好的實現(xiàn)本發(fā)明，所述在對每一條微線段處理時的具體步驟為:前瞻設(shè)定一條微線段的起點速度和終點速度，依據(jù)終點速度為基準值，在此條微線段上運動過程中，通過微線段剩余距離的返算量，實時調(diào)節(jié)當前速度，使當前速度最終到達終點處與基準值相同。
[0010]進一步的，為更好的實現(xiàn)本發(fā)明，在所述采用分周期控制速度處理中，采用等周期控制速度，所述微線段處理時采用Ims周期控制。
[0011]新型脈沖控制算法，用于完成新型速度控制算法的脈沖控制算法，具體分為下述步驟:
(1)設(shè)定單位時間內(nèi)運行速度以及在該速度下走的脈沖數(shù)；
(2)按照設(shè)定發(fā)出第一個脈沖串，并開始計時；
(3)在第一個控制周期結(jié)束時刻，DSP寫入新的脈沖串參數(shù)命令字，同時FPGA讀取此參數(shù)；
(4)上一個單位時間內(nèi)最后一個脈沖結(jié)束時，立即使用新的脈沖串，此時進入下一個單位計時階段；
(5)反復運行步驟(4)，直至一條軌跡運行結(jié)束。
[0012]進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的控制周期為0.5?2ms。
[0013]進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的的脈沖占空比為50%。
[0014]進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的控制周期為1ms。
[0015]本發(fā)明與現(xiàn)有技術(shù)相比，具有以下優(yōu)點及有益效果:
(I)本發(fā)明解決現(xiàn)有技術(shù)到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整所存在的技術(shù)不足之處，而提供新型速度控制算法及新型脈沖控制算法，在處理中，運動曲線不是預先定好的，而是可隨時調(diào)速的，基于微線段的速度前瞻和逐線段輸出之間的控制方式上，可隨時調(diào)整速度，以適應多種變形軌跡場合。
[0016](2)本發(fā)明在新型分周期控制中，采用等周期控制，微線段之間的銜接速度也是由速度前瞻計算好的，在具體每條微線段中，分周期控制速度，可以實時改變速度，即運動曲線不是預先定好的，而是可隨時調(diào)速的，更符合一般運動控制卡的概念。
[0017](3)本發(fā)明基于等周期的控制更通用，更靈活，適于在更復雜的控制領(lǐng)域。
[0018](4)本發(fā)明每個周期內(nèi)不須計算所有剩余軌跡的參數(shù)，而只需預先計算隨后一小段軌跡，就能保證軌跡的勻滑。
[0019](5)本發(fā)明采用控制周期為1ms，能有效提高控制精度。

【專利附圖】

【附圖說明】
[0020]圖1為現(xiàn)有技術(shù)處理的一條軌跡示意圖。
[0021]圖2為本發(fā)明進行處理時的一條軌跡進行微線段的示意圖。
[0022]圖3為本發(fā)明處理一條微線段時的表現(xiàn)圖。
[0023]圖4為本發(fā)明的脈沖運行圖。

【具體實施方式】
[0024]本 申請人:自認為【技術(shù)領(lǐng)域】內(nèi)技術(shù)員結(jié)合現(xiàn)有公知技術(shù)，并根據(jù)本申請文件所公開的內(nèi)容即可實現(xiàn)本發(fā)明。
[0025]下面結(jié)合實施例對本發(fā)明作進一步地詳細說明，但本發(fā)明的實施方式不限于此。
[0026]實施例1:
新型速度控制算法，在進行軌跡處理中，包括用戶設(shè)定的速度參數(shù)在上位機進行解析步驟；將軌跡劃分成微線段步驟；基于微線段的速度前瞻步驟和逐線段輸出步驟；在所述基于微線段的速度前瞻步驟與逐線段輸出步驟之間的處理過程中，在對每一條微線段處理時，采用分周期控制速度，實時改變當前速度，解決現(xiàn)有技術(shù)到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整所存在的技術(shù)不足之處，而提供新型速度控制算法及新型脈沖控制算法，在處理中，運動曲線不是預先定好的，而是可隨時調(diào)速的，基于微線段的速度前瞻和逐線段輸出之間的控制方式上，可隨時調(diào)整速度，以適應多種變形軌跡場合。
[0027]實施例2:
本實施例是在上述實施例的基礎(chǔ)上進一步優(yōu)化，進一步的，為更好的實現(xiàn)本發(fā)明，所述在對每一條微線段處理時的具體步驟為:前瞻設(shè)定一條微線段的起點速度和終點速度，依據(jù)終點速度為基準值，在此條微線段上運動過程中，通過微線段剩余距離的返算量，實時調(diào)節(jié)當前速度，使當前速度最終到達終點處與基準值相同；在新型分周期控制中，采用等周期控制，微線段之間的銜接速度也是由速度前瞻計算好的，在具體每條微線段中，分周期控制速度，可以實時改變速度，即運動曲線不是預先定好的，而是可隨時調(diào)速的，更符合一般運動控制卡的概念。
[0028]實施例3:
本實施例在實施例1的基礎(chǔ)上進一步優(yōu)化，進一步的，為更好的實現(xiàn)本發(fā)明，在所述采用分周期控制速度處理中，采用等周期控制速度，所述微線段處理時采用Ims周期控制；基于等周期的控制更通用，更靈活，適于在更復雜的控制領(lǐng)域。
[0029]實施例4:
新型脈沖控制算法，用于完成新型速度控制算法的脈沖控制算法，具體分為下述步驟:
(O設(shè)定單位時間內(nèi)運行速度以及在該速度下走的脈沖數(shù)；
(2)按照設(shè)定發(fā)出第一個脈沖串，并開始計時；
(3)在第一個控制周期結(jié)束時刻，DSP寫入新的脈沖串參數(shù)命令字，同時FPGA讀取此參數(shù)；
(4)上一個單位時間內(nèi)最后一個脈沖結(jié)束時，立即使用新的脈沖串，此時進入下一個單位計時階段；
(5)反復運行步驟(4)，直至一條軌跡運行結(jié)束。
[0030]實施例5:
本實施例是在上述實施例的基礎(chǔ)上進一步優(yōu)化，進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的控制周期為0.5?2ms。
[0031]實施例6:
本實施例在實施例4的基礎(chǔ)上進一步優(yōu)化，進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的的脈沖占空比為50%。
[0032]實施例7:
本實施例在實施例4的基礎(chǔ)上進一步優(yōu)化，進一步的，為更好的實現(xiàn)本發(fā)明，所述分周期控制的控制周期為1ms。
[0033]實施例8: 本實施例綜合上述所有實施例而給出的一種優(yōu)化的實施方式，如圖2、圖3所示，新型等周期控制中，微線段之間的銜接速度(B點、C點)也是由速度前瞻計算好的，但在具體每條微線段中，分周期控制速度，可以實時改變速度。
[0034]這樣，在每一周期返算時需判斷在該微線段終點速度能不能達到預先前瞻好的節(jié)點速度(如圖2所示，此時的節(jié)點為B點和C點)，每個微線段中間的速度可以調(diào)整，主要是要保證到達B點和C點的速度，因為速度前瞻的目的就是根據(jù)軌跡拐點的矢量角度計算好B點和C點的速度，保證在運動軌跡的拐點方向改變時速度能夠平滑，不至于產(chǎn)生太大沖擊，而起始速度A點速度和終點速度D點速度是預先用戶設(shè)定的，而在微線段內(nèi)如線段AB，起點速度指A點速度，終點速度指B點速度，以往的算法已經(jīng)預算好了 AB線段上以及以后每條線段的每一點的速速，硬件輸出就行，改變起來不便，新型算法在AB線段上進行周期(L)等分，每一點的速度可以調(diào)整，在調(diào)整當前速度的時候要去返算調(diào)整后能不能在B點達到前瞻好的速度，其他線段也一樣，因為是I毫秒改變一次速度，再返算一次，控制更靈活。
[0035]每個周期內(nèi)不須計算所有剩余軌跡的參數(shù)，而只需預先計算隨后一小段軌跡，只要保證在B點、C點到達前瞻的速度，就能保證軌跡的勻滑。例如在AB線段上運行，當前改變速度時，假如速度增加，如果無限增加的話，在臨近B點時減速，以預定加速度減速是怎么也減不到前瞻的B點速度的，所以增加速度的時候要返算能不能降速至前瞻好的B點速度。
[0036]如圖3所示，A點為一條微線段的起始點，B點為微線段的結(jié)束點，兩點速度都是前瞻算好的，運動就是要保證在這些點的速度符合前瞻速度，而微線段之內(nèi)速度可以按T形變化，升速-勻速-減速，也可以按S形變化。以T形為例，以往的算法規(guī)定了 AB線段內(nèi)的最大速度和加速度，從A點以加速度上升,升至最大速度時勻速運動,達到減速點時再以加速度下降，這樣可以保證兩點的速度，如圖3中實線所示，這條運動曲線對于以往的算法是固定的，不能調(diào)整，新型算法中如虛線所示，在M點改變速度，則計算剩余的位移能不能滿足達到B點速度要求，比如升速，速度最大升至N，因為再升的話加速度下降時速度不能降至B點設(shè)定的速度，這就是改變-返算的過程，每I毫秒進行一次。
[0037]當實時改變速度后，數(shù)據(jù)返算量較小，這樣在運動過程中調(diào)速會很方便；更易于速度控制，也更符合一般運動控制卡的概念，即運動曲線不是預先定好的，而是可隨時調(diào)速的，在分周期的控制速度中，基于等周期的控制更通用，更靈活，適于在更復雜的控制領(lǐng)域，比如機器人，而以往的更專用，適用固定軌跡的場合；比如在機器人控制中，即便能前瞻算好線段兩端點的速度，也不能固定這條線段的軌跡，要隨時去檢測，去調(diào)整，這樣只有在周期性的控制中才能做到。現(xiàn)有的技術(shù)實時性更好更好，但效率沒有舊有的高，因為舊有方式算好后就由硬件固定輸出，而現(xiàn)在要每個周期返算剩余路徑速度；每個周期內(nèi)不須計算所有剩余軌跡的參數(shù)，而只需預先計算隨后一小段軌跡，就能保證軌跡的勻滑。
[0038]實施例9:
本實施例是在上述9個實施例的基礎(chǔ)上，當分周期控制的控制周期為Ims時的【具體實施方式】，微線段的起點速度和終點速度已經(jīng)由速度前瞻算好，而升降速的過程是變化的，如果速度和加速度一樣，可以實現(xiàn)跟以往算法一樣的效果，但在每一周期會返算當前微線段剩余路徑，從而對當前速度進行微調(diào)，且用戶可以隨時調(diào)整，調(diào)整時只需要返算當前微線段剩余路徑即可，在實施例9已有表述，在此不再贅述。
[0039]如圖4所示，新型脈沖控制算法:
I)、設(shè)定單位時間(Ti)內(nèi)運行速度以及在該速度下走的脈沖數(shù):用戶要設(shè)定運行速度，加速度，總脈沖數(shù)個數(shù)，控制周期為1ms，運行速度的單位為每分鐘多少個脈沖；那么在勻速運動時，根據(jù)速度(用戶設(shè)定)和時間(I毫秒)相乘即得到在當前周期我要運行多少個脈沖；由于單位時間內(nèi)的脈沖數(shù)就是運行速度，在這單位時間內(nèi)的脈沖速度是一樣的，即脈沖的頻率是一定的；但每個單位時間不是剛好運動整數(shù)個脈沖，那就需要在下一個單位時間內(nèi)多退少補，所以每個單位時間內(nèi)的速度和脈沖數(shù)有可能是變化的；勻速運動時，根據(jù)速度V和我們固定的周期即可得到單位時間的脈沖數(shù)，而變速運動時結(jié)合加速度dV來判斷，加速度的單位為每分鐘變化多少個脈沖，通過與毫秒之間的換算，即可得到下一周期該多發(fā)或少發(fā)多少個脈沖。設(shè)定脈沖指設(shè)定脈沖的個數(shù):由個數(shù)和時間(lms)，即可得到速度為脈沖數(shù)/分鐘，也就是脈沖頻率，跟用戶設(shè)定速度單位一致；由于脈沖為數(shù)字信號，占空比為50%，幅度值由外圍電路決定。
[0040]2)、按照設(shè)定發(fā)出脈沖串1，并開始計時；
3 )、在設(shè)定的單位時間結(jié)束時，第一個控制周期結(jié)束時刻，即在A時刻，DSP寫入新的脈沖串(脈沖串2)參數(shù)命令字，同時FPGA讀取此參數(shù)，A點即I毫秒定時點，DSP根據(jù)FPGA返回的位置值計算下一個I毫秒的速度，在此點并寫入FPGA，F(xiàn)PGA只有在以上一個周期的速度值運行完當前的完整脈沖后，才取用這個速度值，進行當前周期的輸出；
4)、上一個單位時間內(nèi)最后一個脈沖結(jié)束時(B時刻)，立即使用脈沖串2，此時進入Ti+Ι時刻，在Ti+1時刻的起點即為B，A時刻為控制周期端點，在此刻就更新下一周期的數(shù)據(jù)，但此時脈沖串I尚未走完，需要等到這個脈沖串結(jié)束后，才用新數(shù)據(jù)參與運算，也就是如此才會導致有的周期多運行一個脈沖，有的周期少運行一個脈沖。
[0041]5)、以此類推直到運動結(jié)束。
[0042]DSP與FPGA之間進行速度或脈沖控制及返算原理如下:
在軌跡確定的運動中:
勻速運動時，根據(jù)用戶設(shè)定速度V，設(shè)定距離(即脈沖數(shù))S，則每個控制周期(I毫秒)內(nèi)的運動應該是固定的，比如為M。在運動開始時，DSP發(fā)速度參數(shù)V給FPGA，F(xiàn)PGA以速度V從硬件輸出速度波形，以后每個控制周期(I毫秒)，DSP會檢測FPGA當前發(fā)的脈沖個數(shù)，即已走距離。由于脈沖個數(shù)不可能是小數(shù)，則在每I毫秒的檢測點上，運動的距離不一定為整數(shù)，假設(shè)第一個I毫秒后，DSP檢測到FPGA實際運動的距離為M-1，則當前下發(fā)的速度參數(shù)為(M+l)/lms，即在下一個I毫秒內(nèi)將少走的一個脈沖補上；這樣，每個I毫秒內(nèi)的速度可能是變化的，但最終的速度、時間和距離不變；
在變速運動時，DSP根據(jù)加速度計算得出當前速度值，以及在當前點以后I毫秒內(nèi)的平局速度，以及在I毫秒內(nèi)需要發(fā)送的脈沖總數(shù)給FPGA，F(xiàn)PGA即以此速度和脈沖數(shù)從硬件發(fā)出相關(guān)波形，I毫秒后DSP再檢測FPGA實際下發(fā)的距離，再根據(jù)此時的平均速度及應發(fā)的脈沖數(shù)(多退少補，即若檢測到實際發(fā)出脈沖數(shù)比上一條指令的少，則在當前指令上加上差值，反之類似)。
[0043]若在運動中速度進行調(diào)整，以T形運動為例，每次I毫秒檢測當前速度及剩余距離SI，根據(jù)當前速度V，終點速度Vs和加速度A計算平均速度和時間，平均速度Va=(當前速度V+終點速度Vs)/2，時間T=(當前速度-終點速度)/A。若Va*T〈Sl，表示當前速度可繼續(xù)升速或者已經(jīng)進入設(shè)定最高速保持勻速運動，直到Va*T>=Sl時進入減速狀態(tài),開始減速。
[0044]在勻速運動過程中每個單位時間內(nèi)的速度和脈沖數(shù)有可能是變化的，而變速過程中每個單位時間內(nèi)的速度和脈沖數(shù)是變化的。
[0045]本發(fā)明解決現(xiàn)有技術(shù)到逐線段輸出，采用固定的利用FPGA硬件進行輸出，不能進行實時調(diào)整所存在的技術(shù)不足之處，而提供新型速度控制算法及新型脈沖控制算法，在處理中，運動曲線不是預先定好的，而是可隨時調(diào)速的，基于微線段的速度前瞻和逐線段輸出之間的控制方式上，可隨時調(diào)整速度，以適應多種變形軌跡場合。
[0046]以上所述，僅是本發(fā)明的較佳實施例，并非對本發(fā)明做任何形式上的限制，凡是依據(jù)本發(fā)明的技術(shù)實質(zhì)對以上實施例所作的任何簡單修改、等同變化，均落入本發(fā)明的保護范圍之內(nèi)。
【權(quán)利要求】
1.新型速度控制算法，其特征在于:在進行軌跡處理中，包括用戶設(shè)定的速度參數(shù)在上位機進行解析步驟；將軌跡劃分成微線段步驟；基于微線段的速度前瞻步驟和逐線段輸出步驟；在所述基于微線段的速度前瞻步驟與逐線段輸出步驟之間的處理過程中，在對每一條微線段處理時，采用分周期控制速度，實時改變當前速度。
2.根據(jù)權(quán)利要求1所述的新型速度控制算法，其特征在于:所述在對每一條微線段處理時的具體步驟為:前瞻設(shè)定一條微線段的起點速度和終點速度，依據(jù)終點速度為基準值，在此條微線段上運動過程中，通過微線段剩余距離的返算量，實時調(diào)節(jié)當前速度，使當前速度最終到達終點處與基準值相同。
3.根據(jù)權(quán)利要求1所述的新型速度控制算法，其特征在于:在所述采用分周期控制速度處理中，采用等周期控制速度，所述微線段處理時采用Ims周期控制。
4.新型脈沖控制算法，其特征在于:用于完成新型速度控制算法的脈沖控制算法，具體分為下述步驟: 設(shè)定單位時間內(nèi)運行速度以及在該速度下走的脈沖數(shù)； 按照設(shè)定發(fā)出第一個脈沖串，并開始計時； 在第一個控制周期結(jié)束時刻，DSP寫入新的脈沖串參數(shù)命令字，同時FPGA讀取此參數(shù)； 上一個單位時間內(nèi)最后一個脈沖結(jié)束時，立即使用新的脈沖串，此時進入下一個單位計時階段； 反復運行步驟(4 )，直至一條軌跡運行結(jié)束。
5.根據(jù)權(quán)利要求4所述的新型脈沖控制算法，其特征在于:所述分周期控制的控制周期為0.5?2ms。
6.根據(jù)權(quán)利要求4所述的新型脈沖控制算法，其特征在于:所述分周期控制的的脈沖占空比為50%。
7.根據(jù)權(quán)利要求4所述的新型脈沖控制算法，其特征在于:所述分周期控制的控制周期為1ms。
【文檔編號】G05B19/042GK104298151SQ201410501119
【公開日】2015年1月21日 申請日期:2014年9月26日 優(yōu)先權(quán)日:2014年9月26日
【發(fā)明者】鄒爽 申請人:成都樂創(chuàng)自動化技術(shù)股份有限公司