本發(fā)明涉及機(jī)器人運(yùn)行軌跡算法領(lǐng)域，具體地說，尤其涉及一種多軸時(shí)間同步的s型軌跡規(guī)劃算法。

背景技術(shù)：

1、目前，工業(yè)機(jī)器人被廣泛應(yīng)用于自動(dòng)化生產(chǎn)行業(yè)，并在其中擔(dān)任著重要的角色。機(jī)器人需要按照控制器的指令來完成特定的運(yùn)動(dòng)。不同的控制策略將會(huì)導(dǎo)致機(jī)器人有不同的運(yùn)動(dòng)特性，而機(jī)器人的軌跡規(guī)劃算法正是機(jī)器人運(yùn)動(dòng)控制策略的核心算法，其目的是確定機(jī)器人的位置、速度、加速度隨時(shí)間的變化規(guī)律。優(yōu)秀的軌跡規(guī)劃算法可以減少機(jī)器人本體的震動(dòng)，實(shí)現(xiàn)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)。不同的軌跡規(guī)劃算法會(huì)影響關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)路徑、時(shí)間和趨勢(shì)，從而導(dǎo)致機(jī)器人運(yùn)行性能的差異，并對(duì)其精度和壽命產(chǎn)生不同程度的影響。

2、機(jī)器人關(guān)節(jié)的運(yùn)動(dòng)控制算法主要包括梯形曲線、多項(xiàng)式軌跡規(guī)劃和s型曲線軌跡規(guī)劃等。梯形曲線和多項(xiàng)式軌跡規(guī)劃的加速度通常是不連續(xù)或是不平滑的，這在實(shí)際應(yīng)用中會(huì)引發(fā)機(jī)械沖擊，從而導(dǎo)致設(shè)備機(jī)械結(jié)構(gòu)件的磨損。因此，在關(guān)節(jié)機(jī)器人中，s型軌跡規(guī)劃由于具有能夠平滑的控制速度和加速度的變化，可以在不犧牲系統(tǒng)性能的情況下實(shí)現(xiàn)高效的軌跡規(guī)劃，被廣泛應(yīng)用于工業(yè)中，以減少對(duì)機(jī)械結(jié)構(gòu)的沖擊，提升機(jī)器人精度并延長(zhǎng)其使用壽命。

3、然而，上述軌跡規(guī)劃方法大多適用于單電機(jī)情況下的軌跡控制。在關(guān)節(jié)機(jī)器人系統(tǒng)中，多個(gè)軸需要協(xié)同運(yùn)動(dòng)。當(dāng)各軸的運(yùn)動(dòng)時(shí)間不同步時(shí)，機(jī)械臂末端的運(yùn)動(dòng)軌跡可能偏離預(yù)定路徑，從而降低運(yùn)動(dòng)精度。這種誤差在精密制造、裝配和加工等應(yīng)用場(chǎng)景中，會(huì)顯著降低其產(chǎn)品質(zhì)量，使得產(chǎn)品達(dá)不到預(yù)期的設(shè)計(jì)要求。此外，運(yùn)動(dòng)時(shí)間不同步還可能導(dǎo)致各軸之間的速度和加速度不協(xié)調(diào)，從而使運(yùn)動(dòng)不平滑，并承受額外的力和扭矩，導(dǎo)致機(jī)械臂振動(dòng)和抖動(dòng)，影響其工作穩(wěn)定性和工作效率，并增加機(jī)械磨損。在需要力控制的應(yīng)用中，如裝配和表面打磨，軸運(yùn)動(dòng)不同步會(huì)導(dǎo)致力的分布不均勻，進(jìn)而影響實(shí)際工作效果。因此，為了減少對(duì)系統(tǒng)的沖擊和振動(dòng)，同時(shí)充分利用電機(jī)的伺服性能，關(guān)節(jié)機(jī)器人在運(yùn)動(dòng)控制時(shí)需要對(duì)六個(gè)軸進(jìn)行運(yùn)動(dòng)時(shí)間同步。

4、現(xiàn)有的機(jī)器人軌跡時(shí)間同步方法存在多種缺陷：

5、直接規(guī)劃法，在給定的約束條件下保持設(shè)定的加速度，以最高速度運(yùn)行并盡可能延長(zhǎng)勻速時(shí)間，從而充分發(fā)揮電機(jī)性能。然而，這種方法難以在不同的軌跡規(guī)劃算法之間通用，梯形同步規(guī)劃推導(dǎo)簡(jiǎn)單、計(jì)算效率高，直接規(guī)劃法復(fù)雜度低。但因?yàn)閟型規(guī)劃的復(fù)雜性過高，直接規(guī)劃法無法應(yīng)用于s型軌跡規(guī)劃中；

6、廣義速度法，通過將多個(gè)位移合并為一個(gè)向量進(jìn)行運(yùn)算，之后再分解，但該方法容易導(dǎo)致軸的速度和加速度超出限制；

7、雙樣條法，盡管可以實(shí)現(xiàn)多軸同步，但大多采用相同的節(jié)點(diǎn)矢量進(jìn)行路徑擬合，沒有考慮運(yùn)動(dòng)學(xué)相關(guān)的約束，并且實(shí)現(xiàn)過程復(fù)雜。

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

1、本發(fā)明的目的，在于提供一種多軸時(shí)間同步的s型軌跡規(guī)劃算法，其通過對(duì)原始軌跡進(jìn)行適當(dāng)修改，不僅實(shí)現(xiàn)了多軸運(yùn)動(dòng)時(shí)間的同步，還解決了因運(yùn)動(dòng)系統(tǒng)動(dòng)力學(xué)飽和限制可能導(dǎo)致的機(jī)器人速度、加速度或電機(jī)扭矩超過電機(jī)允許范圍的問題。

2、本發(fā)明是通過以下技術(shù)方案實(shí)現(xiàn)的：

3、一種多軸時(shí)間同步的s型軌跡規(guī)劃算法，包括如下步驟，

4、s1、采用分段式s型軌跡規(guī)劃方法對(duì)每個(gè)軸進(jìn)行獨(dú)立的軌跡規(guī)劃，包括以下步驟：

5、1)判斷是否能達(dá)到最大加速度；

6、2)判斷是否有勻速段；

7、3)判斷加速段和減速段能否到達(dá)最大加減速度；

8、s2、6軸運(yùn)動(dòng)時(shí)間調(diào)整：將其余5軸新的運(yùn)動(dòng)時(shí)間設(shè)定為最長(zhǎng)運(yùn)動(dòng)軸時(shí)間；

9、s3、利用步驟s2中重新定義的各個(gè)軸參數(shù)，將6軸時(shí)間同步縮放，關(guān)節(jié)運(yùn)動(dòng)軌跡重規(guī)劃；

10、s4、根據(jù)求得的參數(shù)，計(jì)算任意軌跡的位置。

11、進(jìn)一步地，步驟s1中還包括以下步驟：整個(gè)運(yùn)動(dòng)過程分為加加速段、勻加速段、減加速段、勻速段、加減速段、勻減速段、減減速段；設(shè)q0為初始位置，q1為終止位置，v0為初始速度，v1為終止速度，vmax為最大速度，amax為最大加速度，jmax為最大加加速度，tj1為加加速段時(shí)間，ta為加速段時(shí)間，tv為勻速段時(shí)間，tj2為減加速段時(shí)間，td為減速段時(shí)間；

12、1)判斷是否能達(dá)到最大加速度包括如下步驟：

13、設(shè)剛好能夠到達(dá)最大速度vmax，但無勻加速段，v0′是加加速段結(jié)束時(shí)的速度，此時(shí)可得加加速段：

14、

15、a為減加速段任意時(shí)刻的加速度，可得減加速段：

16、

17、根據(jù)式1.1和式1.2可得如果則表示加速度到達(dá)最大值，會(huì)導(dǎo)致加速階段的速度超出最大速度限制，此時(shí)需要對(duì)amax進(jìn)行調(diào)整，過程如下：

18、

19、如果則表示能達(dá)到最大加速度，有勻加速段，勻加速段的時(shí)間為tj1勻，

20、求得參數(shù)如下：

21、

22、s為運(yùn)動(dòng)的總路徑長(zhǎng)度，計(jì)算勻速段時(shí)間步驟如下：

23、

24、td的求解過程與ta相似，同理可求出軌跡規(guī)劃的減速階段的各個(gè)參數(shù)；

25、2)判斷是否有勻速段包括如下步驟：

26、當(dāng)tv>0時(shí)，表示有勻速段，運(yùn)動(dòng)規(guī)劃總時(shí)間t＝ta+tv+td，將此時(shí)的狀態(tài)定義為狀態(tài)1，state＝1，該軌跡為7段式軌跡，記錄此狀態(tài)后續(xù)多軸時(shí)間同步規(guī)劃時(shí)會(huì)使用；

27、當(dāng)tv<0時(shí)，則表示無勻速段，且達(dá)不到最大速度；設(shè)最大加速度和最大減速度均可達(dá)到，此時(shí)的軌跡有加加速段、勻加速段、加減速段、減加速段、勻減速段、減減速段；

28、

29、根據(jù)式1.4和式1.6可計(jì)算出此時(shí)vmax的值如下：

30、

31、將vmax帶入到ta、td的計(jì)算公式中，即可求得兩者的值；

32、3)判斷加速段和減速段能否到達(dá)最大加減速度包括如下步驟：

33、在步驟2)中已經(jīng)求出ta、td的值，如果ta<0則表示沒有加速段，只有減速段，將此時(shí)的狀態(tài)定義為狀態(tài)2，state＝2，該軌跡為3段式軌跡；如果td<0則表示沒有減速段，只有加速段，將此時(shí)的狀態(tài)定義為狀態(tài)3，state＝3，該軌跡為3段式軌跡；如果ta>＝2×tj1&&td>＝2×tj2，將此時(shí)的狀態(tài)定義為狀態(tài)4，state＝4，該軌跡無勻速段；當(dāng)ta<2×tj1||td<2×tj2時(shí)，則表示加速段或者減速段無法達(dá)到最大加速度，需要調(diào)整最大加速度值，使其能滿足條件，采用牛頓迭代法降低最大加速度值，重新計(jì)算vmax、ta、td，直到其滿足ta>＝2×tj1&&td>＝2×tj2。

34、進(jìn)一步地，步驟s2具體包括：以最長(zhǎng)運(yùn)動(dòng)軸時(shí)間tmax為基準(zhǔn)，將其余5軸新的運(yùn)動(dòng)時(shí)間tn設(shè)定為該時(shí)間，將各個(gè)軸的ta，td，tv，tj1，tj2，vmax，amax，jmax，q1，q0傳給步驟s3。

35、進(jìn)一步地，步驟s3還包括：通過步驟s1計(jì)算出各段的運(yùn)動(dòng)時(shí)間和總時(shí)間，將新的運(yùn)動(dòng)時(shí)間作為重規(guī)劃的時(shí)間，使用縮放因子對(duì)加速段、勻速段、減速段分別進(jìn)行縮放，步驟s1中在進(jìn)行單軸規(guī)劃時(shí)，已將軌跡分為了4種狀態(tài)，在縮放時(shí)要分別進(jìn)行考慮；

36、狀態(tài)1為7段式s型軌跡，設(shè)縮放因子(tn>＝t),將各個(gè)段的時(shí)間分別乘上縮放因子λ,得到新的加速段時(shí)間tan、勻速段時(shí)間tvn、減速段時(shí)間tdn，此時(shí)最大速度設(shè)置為vmaxn，分別計(jì)算縮放前和縮放后的運(yùn)動(dòng)距離s，如式1.8所示：

37、

38、根據(jù)式1.8，解得vmaxn的值如式1.9所示：

39、

40、求出新的vmaxn后，需要求出勻加速段時(shí)間tj1n和最大加速度amaxn，v′0為加加速段末尾的速度，v″′0為勻加速段末尾的速度，v″′0為減加速段末尾的速度；

41、

42、根據(jù)式1.9和式1.10可以解得tj1n得值如下：

43、

44、此時(shí)，tj1n取負(fù)值；

45、以求得狀態(tài)1的所有軌跡重規(guī)劃的參數(shù)，如式1.12所示：

46、

47、狀態(tài)4為無勻速段軌跡，時(shí)間縮放計(jì)算方法與狀態(tài)1相似，公式tvn和vmaxn計(jì)算公式不同，在運(yùn)算時(shí)去掉勻速段，求得狀態(tài)4參數(shù)如式1.13所示：

48、

49、狀態(tài)2為減速段，

50、

51、通過式1.14解出當(dāng)前的jmax和t2的值；

52、狀態(tài)3為加速段，t1為勻加速段的時(shí)間，t2為減加速段時(shí)間，

53、

54、通過式1.15解出當(dāng)前的jmax和t2的值。

55、進(jìn)一步地，將該軌跡的曲線圖和各段軌跡的參數(shù)帶入位置方程求得任意時(shí)刻的位置。

56、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：

57、1.首先，對(duì)六個(gè)軸分別進(jìn)行s型軌跡規(guī)劃，計(jì)算每個(gè)獨(dú)立關(guān)節(jié)的s曲線數(shù)據(jù)；然后，以運(yùn)動(dòng)時(shí)間最長(zhǎng)的軸作為基準(zhǔn)，采用時(shí)間縮放法對(duì)其他各軸的運(yùn)動(dòng)曲線進(jìn)行調(diào)整；在此過程中，還基于動(dòng)力學(xué)限制，調(diào)整各關(guān)節(jié)的最大速度和最大加速度，以實(shí)現(xiàn)多軸同步軌跡規(guī)劃，使其速度和加速度不會(huì)超出動(dòng)力學(xué)約束的限制，有效提高了多軸運(yùn)動(dòng)的協(xié)調(diào)性和機(jī)器人整體的運(yùn)行精度。

58、2.不僅實(shí)現(xiàn)了多軸運(yùn)動(dòng)時(shí)間的同步，還解決了因運(yùn)動(dòng)系統(tǒng)動(dòng)力學(xué)飽和限制可能導(dǎo)致的機(jī)器人速度、加速度或電機(jī)扭矩超過電機(jī)允許范圍的問題。