本公開(kāi)涉及運(yùn)動(dòng)控制，尤其涉及速度規(guī)劃控制方法及裝置、設(shè)備、存儲(chǔ)介質(zhì)。

背景技術(shù)：

1、在過(guò)去的幾年中，3d打印技術(shù)的快速發(fā)展使得它在許多領(lǐng)域的應(yīng)用變得越來(lái)越廣泛，也迫使3d打印機(jī)本身的性能和功能需要不斷迭代提升。當(dāng)前一些3d打印機(jī)為了平衡打印質(zhì)量和打印機(jī)的速度性能，通常只會(huì)對(duì)其設(shè)計(jì)參數(shù)進(jìn)行優(yōu)化，而不會(huì)優(yōu)化其規(guī)劃算法，這就導(dǎo)致若加加速度(變加速度)和加速度設(shè)置不合理，當(dāng)運(yùn)動(dòng)軌跡方向改變或當(dāng)電機(jī)改變方向時(shí)，快速的運(yùn)動(dòng)將會(huì)導(dǎo)致3d打印機(jī)將產(chǎn)生不可避免的沖擊和振動(dòng)，導(dǎo)致打印出來(lái)的模型表面不光滑、細(xì)節(jié)不清晰甚至出現(xiàn)振動(dòng)和噪音等問(wèn)題，使得最終的打印效果產(chǎn)生重影或回聲等瑕疵。

技術(shù)實(shí)現(xiàn)思路

1、為克服現(xiàn)有技術(shù)中存在的控制目標(biāo)沿復(fù)雜路徑運(yùn)動(dòng)時(shí)，當(dāng)運(yùn)動(dòng)軌跡方向改變或當(dāng)電機(jī)改變方向時(shí)，速度轉(zhuǎn)向不平滑的問(wèn)題，本公開(kāi)提供一種速度規(guī)劃控制方法及裝置、設(shè)備、存儲(chǔ)介質(zhì)。

2、根據(jù)本公開(kāi)實(shí)施例的第一方面，提供一種速度規(guī)劃控制方法，包括：

3、步驟s10：將待執(zhí)行的第一運(yùn)動(dòng)路線中的圓弧段離散成多條線段，得到由依次首尾相接的若干條線段組成的第二運(yùn)動(dòng)路線，記錄各條線段的位置參數(shù)；其中，所述線段的位置參數(shù)至少包括線段的起點(diǎn)坐標(biāo)、終點(diǎn)坐標(biāo)；

4、步驟s20：根據(jù)各條線段的位置參數(shù)和預(yù)先給定的進(jìn)給速度，規(guī)劃相鄰線段的銜接點(diǎn)的銜接速度；

5、步驟s30：基于各條線段的確定位移和相鄰線段銜接點(diǎn)的銜接速度，對(duì)所述第二運(yùn)動(dòng)路線進(jìn)行梯型速度規(guī)劃，得到各條線段的第一運(yùn)動(dòng)控制參數(shù)。

6、在本實(shí)施例中，在規(guī)劃軌跡的運(yùn)動(dòng)參數(shù)前，將圓弧打斷為連續(xù)微小直線段，使規(guī)劃方法具有一般性，打斷后軌跡的平滑度下降必將導(dǎo)致加工路徑方向的改變，為避免運(yùn)動(dòng)速度較快時(shí)，速度方向突變產(chǎn)生的剛性沖擊，通過(guò)根據(jù)各條線段的位置參數(shù)和預(yù)先給定的進(jìn)給速度，規(guī)劃相鄰線段的銜接點(diǎn)的銜接速度，使被控目標(biāo)在到達(dá)銜接點(diǎn)之前將速度降低至合適范圍內(nèi)，實(shí)現(xiàn)運(yùn)動(dòng)的平穩(wěn)過(guò)渡；此外，采用傳統(tǒng)梯形速度規(guī)劃算法，結(jié)合各條線段的確定位移和銜接速度等規(guī)劃出每段的持續(xù)時(shí)間，能夠減小速度方向突變產(chǎn)生的剛性沖擊，顯著提高被控目標(biāo)在運(yùn)動(dòng)過(guò)程中的平滑性和穩(wěn)定性。

7、在一個(gè)實(shí)施例中，在所述步驟s30之后，還包括：

8、步驟s40：對(duì)所述第二運(yùn)動(dòng)路線進(jìn)行s型速度規(guī)劃，得到用于各條線段的第二運(yùn)動(dòng)控制參數(shù)；其中，所述第二運(yùn)動(dòng)控制參數(shù)至少包括：進(jìn)給速度曲線以及每一步需要的控制時(shí)間。

9、在本實(shí)施例中，采用了s型速度規(guī)劃算法對(duì)由線段組成的第二運(yùn)動(dòng)路線進(jìn)行運(yùn)動(dòng)規(guī)劃，相對(duì)于常規(guī)s型速度規(guī)劃算法，計(jì)算更為簡(jiǎn)單，適合在3d打印機(jī)等單片機(jī)控制系統(tǒng)中應(yīng)用，且進(jìn)一步提高了整個(gè)運(yùn)動(dòng)過(guò)程的平滑性。

10、在一個(gè)實(shí)施例中，所述步驟s20，包括：

11、步驟s201：令i＝1，讀取第二運(yùn)動(dòng)路線的第i條線段的位置參數(shù)；

12、步驟s202：判斷當(dāng)前第二運(yùn)動(dòng)路線的第i條線段后是否還有未參與規(guī)劃的線段，若是，則執(zhí)行步驟s203；

13、步驟s203：令i＝i+1，隨后讀取當(dāng)前第二運(yùn)動(dòng)路線的第i條線段的位置參數(shù)，并根據(jù)以下第一公式(1)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段之間的夾角αi-1,i：

14、

15、其中，pi-1(xi-1,yi-1,yi-1)和pi(xi,yi,yi)為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段的起點(diǎn)坐標(biāo)和終點(diǎn)坐標(biāo)，pi(xi,yi,yi)和pi+1(xi+1,yi+1,yi+1)為當(dāng)前第二運(yùn)動(dòng)路線的第i條線段的起點(diǎn)坐標(biāo)和終點(diǎn)坐標(biāo)；

16、步驟s204：根據(jù)第二公式(2)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度，隨后返回執(zhí)行步驟s202；

17、

18、其中，θi-1,i為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接角，ri-1,i為作為中間計(jì)算量的曲率半徑，h為預(yù)設(shè)值，a為用于執(zhí)行所述第一運(yùn)動(dòng)路線的目標(biāo)設(shè)備的向心加速度，a為預(yù)設(shè)值，vi-1,i為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第一銜接速度，vi-1為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段的進(jìn)給速度，vi為預(yù)先設(shè)定的當(dāng)前第二運(yùn)動(dòng)路線的第i條線段的進(jìn)給速度，vm,i為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度。

19、由于現(xiàn)有技術(shù)中人為預(yù)先設(shè)定每條線段的進(jìn)給速度時(shí)，進(jìn)給速度設(shè)置得是否合理主要依賴于經(jīng)驗(yàn)，無(wú)法精確計(jì)算銜接速度，從而存在被控目標(biāo)在不同方向的線段間轉(zhuǎn)向時(shí)，過(guò)渡不平滑的問(wèn)題。在本實(shí)施例中，通過(guò)根據(jù)各條線段的位置參數(shù)和預(yù)先給定的進(jìn)給速度，規(guī)劃相鄰線段的銜接點(diǎn)的銜接速度，該銜接速度能夠使得該銜接點(diǎn)兩側(cè)的運(yùn)動(dòng)近似為半徑很小的圓周運(yùn)動(dòng)，能夠?qū)崿F(xiàn)運(yùn)動(dòng)的平穩(wěn)過(guò)渡。

20、在一個(gè)實(shí)施例中，在步驟s203之后，步驟s204執(zhí)行之前，還包括步驟：

21、步驟s205：根據(jù)預(yù)設(shè)判斷準(zhǔn)則判斷是否需要對(duì)所述第i-1條線段和第i條線段進(jìn)行預(yù)增插補(bǔ)；若是，則執(zhí)行所述步驟s206，否則，則執(zhí)行步驟s204；

22、步驟s206：對(duì)第i-1條線段和第i條線段預(yù)增插補(bǔ)點(diǎn)mi-1、mi，將原來(lái)第二運(yùn)動(dòng)路線中的第i-1條線段和第i條線段規(guī)劃更新為首尾相接的3條線段pi-1mi-1、mi-1mi和mipi+1，并更新第二運(yùn)動(dòng)路線的每條線段的序號(hào)；

23、其中，根據(jù)第三公式(3)計(jì)算預(yù)增插補(bǔ)點(diǎn)的坐標(biāo)和

24、

25、li-1,i為原第i-1條線段和第i條線段的銜接點(diǎn)到預(yù)增差補(bǔ)點(diǎn)的距離，所述為由第i-1條線段的起點(diǎn)pi-1指向第i-1條線段的終點(diǎn)pi的向量，為由第i條線段的起點(diǎn)pi指向第i條線段的終點(diǎn)pi+1的向量，‖pi-1pi‖x、‖pi-1pi‖y、‖pi-1pi‖z分別表示向量在x、y、z軸上的投影長(zhǎng)度，‖pipi+1‖x、‖pipi+1‖y、‖pipi+1‖z分別表示向量在x、y、z軸上的投影長(zhǎng)度。

26、步驟s207：根據(jù)第四公式(4)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段之間的夾角αi-1,i以及第i條線段和第i+1條線段之間的夾角αi,i+1：

27、αi-1,i＝αi,i+1＝αi′-1,i???????????????????(4)其中，αi′-1,i為本次對(duì)第i-1條線段和第i條線段預(yù)增插補(bǔ)前計(jì)算得到的原來(lái)第i-1條線段和第i條線段之間的夾角；

28、步驟s208：根據(jù)第二公式(2)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度；

29、步驟s209：令i＝i+1，隨后根據(jù)第二公式(2)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度，隨后返回執(zhí)行步驟s202。

30、在本實(shí)施例中，與原第二運(yùn)動(dòng)路線相比，通過(guò)對(duì)直線段添加預(yù)增插補(bǔ)點(diǎn)，增大了能給個(gè)運(yùn)動(dòng)路線的銜接次數(shù)、減小了單次銜接點(diǎn)處的夾角，從而能夠減少運(yùn)動(dòng)速度在方向改變時(shí)所帶來(lái)的沖擊，提高了被控運(yùn)動(dòng)目標(biāo)在整個(gè)工作過(guò)程中的平滑穩(wěn)定性。

31、在一個(gè)實(shí)施例中，在所述根據(jù)第二公式(2)計(jì)算當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度之后，所述速度規(guī)劃控制方法還包括：

32、步驟s210：采用單次連續(xù)三段速度前瞻規(guī)劃方法規(guī)劃當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的最終銜接速度；

33、其中，所述步驟s210包括：

34、向前遞推計(jì)算步驟：將第i條線段的輸出速度設(shè)置為0，并根據(jù)第五公式(5)計(jì)算第i條線段的第一輸入速度vi,f應(yīng)滿足的約束條件為：

35、

36、其中，ai為預(yù)先設(shè)定的控制目標(biāo)在第i條線段上運(yùn)動(dòng)時(shí)的加減速度值，si為第i條線段的確定位移，其等于第i條線段的長(zhǎng)度；

37、向后遞推計(jì)算步驟：根據(jù)第六公式(6)計(jì)算第i-1條線段的輸出速度vi-1,bout和第i條線段的第二輸入速度vi,bin應(yīng)滿足的約束條件為：

38、

39、其中，ai-1為預(yù)先設(shè)定的控制目標(biāo)在第i-1條線段上運(yùn)動(dòng)時(shí)的加減速度值，si-1為第i-1條線段的確定位移，其等于第i-1條線段的長(zhǎng)度，vi-1,bin為第i-1條線段的第二輸入速度，且預(yù)設(shè)第1條線段的第二輸入速度v1,bin＝0；

40、最終銜接速度確定步驟：將第i條線段的第一輸入速度vi,f、第i條線段的第二輸入速度vi,bin、第i-1條線段和第i條線段的銜接點(diǎn)的第二銜接速度vm,i中的最小值，作為當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段和第i條線段的銜接點(diǎn)的最終銜接速度vi′。

41、在本實(shí)施例中，采用單次連續(xù)三段速度前瞻規(guī)劃對(duì)第二運(yùn)動(dòng)路線進(jìn)行速度規(guī)劃，并通過(guò)向前和向后遞推原則來(lái)確定銜接點(diǎn)的第二銜接速度，且保證最后一條線段輸出速度為零，能夠無(wú)沖擊地完成每段插補(bǔ)距離。

42、在一個(gè)實(shí)施例中，在所述步驟s210之后，所述速度規(guī)劃控制方法還包括：

43、步驟211：將當(dāng)前第二運(yùn)動(dòng)路線的第i-1條線段的規(guī)劃參數(shù)保存到先進(jìn)先出環(huán)形隊(duì)列的隊(duì)尾中；其中，所述第i-1條線段的規(guī)劃參數(shù)至少包括：第i-1條線段的輸入速度、輸出速度、起點(diǎn)坐標(biāo)、終點(diǎn)坐標(biāo)、進(jìn)給速度；

44、所述步驟s30執(zhí)行之前，還包括：

45、步驟s212：按照先進(jìn)先出原則依次釋放所述先進(jìn)先出環(huán)形隊(duì)列的元素，以作為步驟s30的輸入。

46、在本實(shí)施例中，將規(guī)劃后的參數(shù)保存在先進(jìn)先出環(huán)形隊(duì)列中，在后期梯形規(guī)劃時(shí)，按照先進(jìn)先出原則依次釋放所述先進(jìn)先出環(huán)形隊(duì)列的元素，從而可以減少前期計(jì)算時(shí)間，當(dāng)環(huán)形隊(duì)列有空余時(shí)可以邊計(jì)算第二運(yùn)動(dòng)路線上的后續(xù)線段的規(guī)劃參數(shù)邊進(jìn)行梯形規(guī)劃，縮短整個(gè)方法的執(zhí)行時(shí)間，能夠提高整個(gè)方法的計(jì)算效率。

47、在一個(gè)實(shí)施例中，在所述步驟s30或步驟s40之后，所述速度規(guī)劃控制方法還包括：

48、將所述第一運(yùn)動(dòng)控制參數(shù)或第二運(yùn)動(dòng)控制參數(shù)中的時(shí)間數(shù)據(jù)轉(zhuǎn)化為用于控制目標(biāo)沿第二運(yùn)動(dòng)路線運(yùn)動(dòng)的驅(qū)動(dòng)電機(jī)的驅(qū)動(dòng)數(shù)據(jù)。

49、本實(shí)施例中，將速度規(guī)劃的時(shí)間數(shù)據(jù)轉(zhuǎn)換為電機(jī)的驅(qū)動(dòng)數(shù)據(jù)，即可直接控制驅(qū)動(dòng)電機(jī)控制目標(biāo)運(yùn)動(dòng)，控制簡(jiǎn)單。

50、根據(jù)本公開(kāi)實(shí)施例的第二方面，提供一種速度規(guī)劃控制裝置，包括：

51、離散模塊，用于將待執(zhí)行的第一運(yùn)動(dòng)路線離散成由若干條依次首尾相接的線段組成的第二運(yùn)動(dòng)路線，得到各條線段的位置參數(shù)并記錄；其中，所述線段的位置參數(shù)至少包括線段的起點(diǎn)坐標(biāo)、終點(diǎn)坐標(biāo)；

52、銜接速度規(guī)劃模塊，用于根據(jù)各條線段的位置參數(shù)和預(yù)先給定的進(jìn)給速度，規(guī)劃相鄰線段的銜接點(diǎn)的銜接速度；

53、梯型規(guī)劃模塊，用于基于各條線段的確定位移和相鄰線段銜接點(diǎn)的銜接速度，對(duì)所述第二運(yùn)動(dòng)路線進(jìn)行梯型速度規(guī)劃，得到各條線段的第一運(yùn)動(dòng)控制參數(shù)。

54、根據(jù)本公開(kāi)實(shí)施例的第三方面，提供一種電子設(shè)備，所述電子設(shè)備包括：包括：殼體、處理器、存儲(chǔ)器、電路板和電源電路，其中，電路板安置在殼體圍成的空間內(nèi)部，處理器和存儲(chǔ)器設(shè)置在電路板上；電源電路，用于為上述電子設(shè)備的各個(gè)電路或器件供電；存儲(chǔ)器用于存儲(chǔ)可執(zhí)行程序代碼；處理器通過(guò)讀取存儲(chǔ)器中存儲(chǔ)的可執(zhí)行程序代碼來(lái)運(yùn)行與可執(zhí)行程序代碼對(duì)應(yīng)的程序，用于執(zhí)行前述任一實(shí)施例所述的速度規(guī)劃控制方法。

55、根據(jù)本公開(kāi)實(shí)施例的第四方面，提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)存儲(chǔ)有一個(gè)或者多個(gè)程序，所述一個(gè)或者多個(gè)程序可被一個(gè)或者多個(gè)處理器執(zhí)行，以實(shí)現(xiàn)前述任一實(shí)施例所述的速度規(guī)劃控制方法。

56、應(yīng)當(dāng)理解的是，以上的一般描述和后文的細(xì)節(jié)描述僅是示例性和解釋性的，并不能限制本公開(kāi)。