本發明涉(she)及(ji)機(ji)械臂控制領域，具體涉(she)及(ji)一種(zhong)多關節同步(bu)的s型速度曲線軌跡規劃方法及(ji)設備。

背景技術：

1、在機械臂的(de)運動過(guo)程中，要使其在視覺上呈(cheng)現同步性，就需要考慮多關(guan)節(jie)的(de)同步問題，即如何(he)保(bao)證機械臂的(de)各(ge)個關(guan)節(jie)同時(shi)啟(qi)動，同時(shi)停止，并按照期望轉動不同的(de)角度或者(zhe)是移動不同的(de)距離。

2、使用(yong)拋物線擬合的(de)(de)線性速度(du)曲線能保證機(ji)(ji)械(xie)臂的(de)(de)各個(ge)關節的(de)(de)速度(du)是連續變(bian)化的(de)(de)，計(ji)算也相對簡(jian)單(dan)，因(yin)此得到了相對廣泛的(de)(de)應用(yong)。但是，這種規劃方法(fa)無法(fa)保證加速度(du)不(bu)突(tu)(tu)變(bian)，而突(tu)(tu)變(bian)的(de)(de)加速度(du)會降低驅動裝置的(de)(de)使用(yong)壽命與(yu)機(ji)(ji)械(xie)臂的(de)(de)定位精度(du)。

3、s型(xing)速(su)度曲線的軌(gui)跡規(gui)劃方法(fa)彌補了(le)這個缺點(dian)，保證(zheng)了(le)加(jia)速(su)度的連續性，但是由(you)于其計(ji)算復雜(za)，因此(ci)以往關(guan)于這種軌(gui)跡規(gui)劃方法(fa)的研究都是針對單(dan)個關(guan)節(jie)的，在多關(guan)節(jie)同步(bu)方面的應用存在一(yi)定(ding)的挑戰。

技術實現思路

1、有(you)鑒于此，本發明一方(fang)(fang)面提供一種多關節(jie)同(tong)步的s型(xing)速度(du)曲線軌跡規劃(hua)方(fang)(fang)法，包括：

2、獲取機械臂的(de)各個(ge)關節的(de)期望位(wei)移、最(zui)大(da)速(su)度(du)絕(jue)對值、最(zui)大(da)加(jia)速(su)度(du)絕(jue)對值和(he)固定的(de)加(jia)加(jia)速(su)度(du)絕(jue)對值；

3、根據所述最(zui)大加(jia)速度(du)絕對值和加(jia)加(jia)速度(du)絕對值計算速度(du)變化量；

4、根(gen)據所(suo)述最大(da)速度(du)(du)絕對值、最大(da)加速度(du)(du)絕對值和速度(du)(du)變化量設置(zhi)各(ge)個關節對應(ying)的s型速度(du)(du)曲線軌跡中(zhong)的多個運(yun)動時間(jian)段；

5、根據各個關(guan)節的(de)所述多個運(yun)動(dong)時間(jian)段計算各個關(guan)節的(de)總運(yun)動(dong)時間(jian)，并將其中(zhong)最大的(de)總運(yun)動(dong)時間(jian)作為參(can)考標準；

6、根據所述參考標(biao)準降低(di)除最(zui)(zui)大(da)總運動時(shi)間外的(de)剩余(yu)各個關節的(de)最(zui)(zui)大(da)速度，得到最(zui)(zui)小可用速度；

7、根(gen)據所述最(zui)小可用速(su)度(du)確(que)定是否(fou)改變最(zui)大加(jia)速(su)度(du)絕對值，并根(gen)據結果利用速(su)度(du)變化(hua)量、加(jia)加(jia)速(su)度(du)時間和參考標準計算(suan)臨界運(yun)動距離(li)；

8、根據最(zui)小可用速度(du)、速度(du)變(bian)化(hua)量和最(zui)大加速度(du)絕對值計算勻加速的時間；

9、根據最(zui)(zui)小可用(yong)速(su)度(du)、加(jia)(jia)加(jia)(jia)速(su)度(du)時間、參考標準、最(zui)(zui)大加(jia)(jia)速(su)度(du)絕(jue)對值、加(jia)(jia)加(jia)(jia)速(su)度(du)絕(jue)對值和(he)勻加(jia)(jia)速(su)的(de)時間計算(suan)剩(sheng)余各個關節的(de)實際移動距離；

10、根據臨界運(yun)(yun)動(dong)距離(li)與剩余各(ge)個關(guan)節(jie)的實際移(yi)動(dong)距離(li)的關(guan)系(xi)確定剩余各(ge)個關(guan)節(jie)的運(yun)(yun)動(dong)時(shi)間段(duan)；

11、若，則根據剩余(yu)各個關(guan)節(jie)的實際移動距(ju)離(li)設(she)置剩余(yu)各個關(guan)節(jie)的運動時間段為

12、，

13、，

14、，

15、其中(zhong)，是單個關(guan)節到達最大(da)速度的(de)時間。

16、可(ke)選地，若，則根據加加速度(du)絕對值和最大加速度(du)計(ji)算(suan)最大速度(du)；

17、根(gen)據參考標準、加(jia)(jia)加(jia)(jia)速度(du)絕對值、最大加(jia)(jia)速度(du)和最大速度(du)計算(suan)修正(zheng)后的(de)(de)剩(sheng)余各(ge)個關節的(de)(de)實際移動距離；

18、根據修正后的(de)(de)剩余各(ge)個關節(jie)的(de)(de)實際移動距離設置(zhi)剩余各(ge)個關節(jie)的(de)(de)運動時間段為

19、，

20、，

21、，

22、其中，

23、，

24、，

25、，

26、。

27、可選地，，

28、。

29、可選地，利用(yong)如下方式得到(dao)最小可用(yong)速度：

30、。

31、可選地(di)，利用如下公式計算(suan)剩(sheng)余各個(ge)關節的實際移動距離：

32、。

33、可選地，根據(ju)所述(shu)最小可用速(su)度確(que)定是(shi)否(fou)改變最大加速(su)度絕(jue)對(dui)值，包括(kuo)：

34、判斷最小(xiao)可(ke)用速度與的大小(xiao)；

35、若，則(ze)確定最小可用速度不(bu)改變最大加速度絕對值；

36、若，則(ze)確定(ding)最(zui)(zui)小可用速(su)度改變最(zui)(zui)大加速(su)度絕對(dui)值，并(bing)計算改變后的最(zui)(zui)大加速(su)度值。

37、可選地，利用如下公式計算最大速度：

38、。

39、可選地，根據所述最大速(su)度絕對值(zhi)、最大加(jia)速(su)度絕對值(zhi)和速(su)度變化量設(she)置各(ge)個(ge)關節對應的(de)s型速(su)度曲線軌跡中的(de)多(duo)個(ge)運動時間段，包括：

40、判(pan)斷最大速度絕(jue)對值與的(de)大小；

41、若(ruo)，則確定各個關(guan)節移(yi)動到目標(biao)位(wei)置處(chu)的移(yi)動距離(li)為期望(wang)位(wei)移(yi)；

42、判斷所(suo)述移動距離(li)是否大(da)于等于二倍(bei)的加速度段的移動距離(li)；

43、若(ruo)≥2，則各個運動時間(jian)段分別設(she)置為(wei)

44、，

45、，

46、；

47、其中，。

48、可選地，還包(bao)括：

49、若，則利(li)用如下公式調(diao)整(zheng)所述最大(da)加速度絕對值：

50、；

51、其(qi)中，為調整(zheng)后的最大(da)加(jia)速度絕對(dui)值。

52、可(ke)選地，還包括：

53、若＜2，則判斷(duan)最小(xiao)移動距離(li)是否小(xiao)于等于所(suo)述實際移動距離(li)；

54、若(ruo)≤，則各個運動(dong)時間段分別設(she)置為

55、，

56、，

57、；

58、若(ruo)＞，則(ze)各個運動時間段(duan)分(fen)別設置為

59、，

60、。

61、可選地(di)，利(li)用如下公式(shi)計算所述最小移動距離：

62、。

63、本發明(ming)第二(er)方面提供了一種多(duo)關(guan)節同(tong)步(bu)的s型速度曲(qu)線(xian)軌(gui)跡規劃(hua)設備，該設備包(bao)括：處理器以(yi)及與所(suo)述(shu)(shu)(shu)(shu)處理器連接的存(cun)儲(chu)器；其中，所(suo)述(shu)(shu)(shu)(shu)存(cun)儲(chu)器存(cun)儲(chu)有可(ke)被(bei)所(suo)述(shu)(shu)(shu)(shu)處理器執行的指(zhi)令，所(suo)述(shu)(shu)(shu)(shu)指(zhi)令被(bei)所(suo)述(shu)(shu)(shu)(shu)處理器執行，以(yi)使所(suo)述(shu)(shu)(shu)(shu)處理器執行上述(shu)(shu)(shu)(shu)的多(duo)關(guan)節同(tong)步(bu)的s型速度曲(qu)線(xian)軌(gui)跡規劃(hua)方法。

64、本發明(ming)首(shou)先通(tong)(tong)過(guo)(guo)規劃(hua)機(ji)(ji)械(xie)臂各(ge)個(ge)(ge)(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)的(de)(de)(de)運(yun)(yun)動(dong)時(shi)(shi)間(jian)段，基于期望位移、最(zui)(zui)(zui)大速(su)度和加(jia)速(su)度等(deng)參(can)數(shu)，為每個(ge)(ge)(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)設定初步(bu)的(de)(de)(de)運(yun)(yun)動(dong)時(shi)(shi)間(jian)，通(tong)(tong)過(guo)(guo)計算(suan)每個(ge)(ge)(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)在各(ge)個(ge)(ge)(ge)運(yun)(yun)動(dong)時(shi)(shi)間(jian)段的(de)(de)(de)總(zong)運(yun)(yun)動(dong)時(shi)(shi)間(jian)，確定最(zui)(zui)(zui)長(chang)的(de)(de)(de)總(zong)運(yun)(yun)動(dong)時(shi)(shi)間(jian)作為參(can)考(kao)標(biao)準，確定整個(ge)(ge)(ge)機(ji)(ji)械(xie)臂的(de)(de)(de)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)運(yun)(yun)動(dong)的(de)(de)(de)時(shi)(shi)間(jian)框架(jia)，確保(bao)所有關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)能夠同(tong)步(bu)完(wan)成預定任(ren)務。隨后，根據最(zui)(zui)(zui)長(chang)的(de)(de)(de)總(zong)運(yun)(yun)動(dong)時(shi)(shi)間(jian)對其他短時(shi)(shi)間(jian)的(de)(de)(de)各(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)進行重新規劃(hua)，根據最(zui)(zui)(zui)長(chang)總(zong)運(yun)(yun)動(dong)時(shi)(shi)間(jian)確定參(can)考(kao)標(biao)準，降低其余關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)的(de)(de)(de)最(zui)(zui)(zui)大速(su)度，從(cong)而(er)保(bao)證其余各(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)的(de)(de)(de)運(yun)(yun)動(dong)時(shi)(shi)間(jian)與最(zui)(zui)(zui)長(chang)時(shi)(shi)間(jian)相等(deng)，確保(bao)各(ge)個(ge)(ge)(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)在整個(ge)(ge)(ge)運(yun)(yun)動(dong)過(guo)(guo)程(cheng)中能夠同(tong)步(bu)協調(diao)(diao)。通(tong)(tong)過(guo)(guo)這(zhe)種調(diao)(diao)整，優化了各(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)的(de)(de)(de)運(yun)(yun)動(dong)效(xiao)率，使得機(ji)(ji)械(xie)臂的(de)(de)(de)各(ge)關(guan)(guan)(guan)(guan)(guan)(guan)節(jie)(jie)能夠在規定時(shi)(shi)間(jian)內高效(xiao)、精確地(di)完(wan)成預定任(ren)務。

技術特征：

1.一種多關節同步的s型速度曲線軌跡規劃方法(fa)，其特征在于，包括：

2.根據權利要求1所述(shu)的方(fang)法，其(qi)特(te)征在于，

3.根據權(quan)利要求1所述的方法，其特征在于：

4.根據權利要求1所述(shu)的(de)方(fang)法，利用(yong)如下方(fang)式得(de)到(dao)最小可用(yong)速度：

5.根據權利要求1所述的方(fang)法，其(qi)特(te)征在于，利用如下公式計算剩(sheng)余各個關節的實際(ji)移動距離(li)：

6.根據(ju)權利要求1所述(shu)(shu)的方(fang)法(fa)，其特(te)征(zheng)在于，根據(ju)所述(shu)(shu)最小可用速(su)度(du)確定是否改(gai)變最大(da)加速(su)度(du)絕(jue)對值(zhi)，包括：

7.根據權利要求2所述(shu)的(de)方法，其特征在(zai)于(yu)，利用如下公式(shi)計算最大速(su)度：

8.根據(ju)權利要(yao)求1所(suo)述的方法(fa)，其特征在于，根據(ju)所(suo)述最大速度(du)(du)絕對值、最大加速度(du)(du)絕對值和速度(du)(du)變化量設置各個關節對應的s型速度(du)(du)曲線軌跡中(zhong)的多(duo)個運動時間段，包括：

9.根據權利(li)要(yao)求8所(suo)述的方(fang)法，其特(te)征在于(yu)，還(huan)包括(kuo)：

10.根據權利要求8所述(shu)的方法，其特(te)征(zheng)在于，還包(bao)括(kuo)：

11.根據權利要(yao)求10所述(shu)(shu)的方法，其特征在于，利用如下公式計算(suan)所述(shu)(shu)最小移動距離：

12.一種多關節(jie)同步(bu)的(de)s型速度曲線(xian)軌跡(ji)規(gui)劃設備，其特征在于，包括：處理(li)器以(yi)及與所(suo)(suo)述(shu)處理(li)器連接的(de)存儲(chu)器；其中(zhong)，所(suo)(suo)述(shu)存儲(chu)器存儲(chu)有可被所(suo)(suo)述(shu)處理(li)器執(zhi)行的(de)指令，所(suo)(suo)述(shu)指令被所(suo)(suo)述(shu)處理(li)器執(zhi)行，以(yi)使所(suo)(suo)述(shu)處理(li)器執(zhi)行如權利(li)要求1-11中(zhong)任意(yi)一項(xiang)所(suo)(suo)述(shu)的(de)多關節(jie)同步(bu)的(de)s型速度曲線(xian)軌跡(ji)規(gui)劃方法(fa)。

技術總結
本發明提供一種多關節同步的S型速度曲線軌跡規劃方法及設備。應用于機械臂控制領域。該方法包括：根據最大速度絕對值、最大加速度絕對值和速度變化量設置各個關節對應的S型速度曲線軌跡中的多個運動時間段；計算各個關節的總運動時間，將最大的總運動時間作為參考標準；根據參考標準降低除最大總運動時間外的剩余各個關節的最大速度；根據最小可用速度確定是否改變最大加速度絕對值，計算臨界運動距離；計算勻加速的時間；計算剩余各個關節的實際移動距離；根據臨界運動距離與剩余各個關節的實際移動距離的關系確定剩余各個關節的運動時間段。本發明實現了機械臂的各關節能夠在規定時間內高效、精確地完成預定任務。

技術研發人員：張昊任,陳向前,徐坤
受保護的技術使用者：真健康（廣東橫琴）醫療科技有限公司
技術研發日：
技術公布日：2024/10/10