本技術(shù)涉及機器人控制，尤其涉及一種針灸機器人彎曲矯正控制方法、裝置、設(shè)備及存儲介質(zhì)。

背景技術(shù)：

1、針刺前，針灸機器人從初始位姿(位置和姿態(tài))運動到預(yù)備針刺的目標位姿需要考慮一系列嚴格的環(huán)境約束條件，然而，相關(guān)技術(shù)對于針灸機器人通過軌跡優(yōu)化生成從初始位置移動到目標位置之間的軌跡，該軌跡不確定，穩(wěn)定性較差；針刺過程中，針受到外作用力容易彎曲，會降低針刺的準確性。

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

1、本技術(shù)實施例的主要目的在于提出一種針灸機器人彎曲矯正控制方法、裝置、設(shè)備及存儲介質(zhì)，以提高針灸機器人的軌跡穩(wěn)定并降低針刺過程中針發(fā)生彎曲的概率。

2、為實現(xiàn)上述目的，本技術(shù)實施例的一方面提出了一種針灸機器人彎曲矯正控制方法，所述方法包括以下步驟：

3、根據(jù)針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)確定所述針灸機器人末端針尖的位置作為初始位置；確定待針刺的位置作為目標位置；

4、對所述初始位置到所述目標位置之間的路徑規(guī)劃多段折線軌跡，根據(jù)多段所述折線軌跡控制所述針灸機器人移動；

5、根據(jù)所述針灸機器人處于所述目標位置時的關(guān)節(jié)角數(shù)據(jù)確定所述針尖的位置及針頭對應(yīng)的位置，并獲取針受到的外作用力和針的長度；

6、根據(jù)所述針尖當(dāng)前位置、所述針頭當(dāng)前位置、所述外作用力以及所述針的長度計算得到針的彎曲角度；

7、根據(jù)所述彎曲角度控制所述針灸機器人進行針刺。

8、在一些實施例中，所述根據(jù)針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)確定所述針灸機器人末端針尖的位置作為初始位置，包括以下步驟：

9、對所述針灸機器人建立運動學(xué)方程；

10、根據(jù)所述運動學(xué)方程確定所述針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)；

11、從所述針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)中確定所述針尖的位置作為所述初始位置；

12、所述運動學(xué)方程為：

13、0tn(q1,q2,l,qn)＝0t11t2ln-1tn；

14、其中，q＝[q1,q2,l,qn]t為所述針灸機器人的關(guān)節(jié)角，{0}～{n}分別為對應(yīng)的關(guān)節(jié)坐標系，{n}的原點位于所述針尖處；itj表示關(guān)節(jié)坐標系{j}相對于關(guān)節(jié)坐標系{i}的齊次變換矩陣。

15、在一些實施例中，所述確定待針刺的位置作為目標位置，包括以下步驟：

16、根據(jù)所述運動學(xué)方程建立所述針灸機器人的速度級運動學(xué)方程；

17、所述速度級運動學(xué)方程為：

18、

19、其中，為所述針灸機器人的末端位姿，為的導(dǎo)數(shù)，為的導(dǎo)數(shù)，表示關(guān)節(jié)角q的導(dǎo)數(shù)；jn為關(guān)節(jié)空間到操作空間的雅可比矩陣；0pn＝[px,py,pz]t為所述針灸機器人末端在關(guān)節(jié)坐標系{0}下的三維位置矢量，px，py，pz分別表示x、y、z軸的坐標分量；為所述針灸機器人末端以歐拉角表示的姿態(tài)，α、β、γ分別表示歐拉角中的roll、pitch、yaw分量；

20、根據(jù)所述速度級運動學(xué)方程確定所述針灸機器人的關(guān)節(jié)角速度方程為：

21、

22、其中，為所述針灸機器人的廣義逆矩陣；

23、當(dāng)設(shè)置于所述針灸機器人末端的局部相機檢測到所述目標位置時，確定所述目標位置在局部相機坐標系中的局部坐標；根據(jù)所述局部坐標和所述速度級運動學(xué)方程確定所述針灸機器人的末端位姿作為所述目標位置的位姿；

24、所述局部相機坐標系下所述針灸機器人的末端位姿為：

25、

26、其中，為k時刻所述針灸機器人的末端位姿，為所述局部相機坐標系{lc}相對于關(guān)節(jié)坐標系{0}的齊次變換矩陣，為所述局部坐標，為k時刻所述針灸機器人的末端姿態(tài)；

27、當(dāng)所述局部相機未檢測到所述目標位置時，確定所述目標位置在全局相機坐標系中的全局坐標；根據(jù)所述全局坐標和所述速度級運動學(xué)方程確定所述針灸機器人的末端位姿，并將得到的末端位姿代入到所述關(guān)節(jié)角速度方程，再對代入結(jié)果求積分，積分結(jié)果作為所述全局相機坐標系下所述目標位置的位姿；

28、所述全局相機坐標系下所述針灸機器人的末端位姿為：

29、

30、其中，為k時刻所述針灸機器人的末端位姿，為所述全局相機坐標系{gc}相對于關(guān)節(jié)坐標系{0}的齊次變換矩陣，為所述全局坐標。

31、在一些實施例中，所述對所述初始位置到所述目標位置之間的路徑規(guī)劃多段折線軌跡，包括以下步驟：

32、對所述針灸機器人建立k時刻的離散關(guān)節(jié)空間模型；

33、所述離散關(guān)節(jié)空間模型為：

34、qk+1＝aqk+buk；

35、其中，表示所述針灸機器人的關(guān)節(jié)角，表示單位狀態(tài)矩陣，表示控制矩陣，ts表示離散時間間隔，u表示所述針灸機器人的關(guān)節(jié)角速度，u為控制量，

36、所述離散關(guān)節(jié)空間模型的增廣動態(tài)方程為：

37、

38、其中，為預(yù)設(shè)的參考關(guān)節(jié)角，為目標關(guān)節(jié)角，為k時刻實際關(guān)節(jié)角與參考關(guān)節(jié)角之間的跟蹤誤差，所述離散關(guān)節(jié)空間模型的狀態(tài)量為

39、基于所述離散關(guān)節(jié)空間模型建立分段常值的跟蹤模型；

40、所述跟蹤模型為：

41、

42、其中，lv為第一目標函數(shù)，為定義在視野n上的增量控制序列，和為正定權(quán)重矩陣，為凸多邊形約束集，表示最大輸出允許集；

43、所述跟蹤模型的約束條件為：

44、tsji(qi)uk+i-sdi＝0；

45、其中，uk+i表示第i個視野對應(yīng)的控制量，ji表示第i個視野對應(yīng)的雅可比矩陣，sdi為該時刻對應(yīng)的目標軌跡跟蹤值；

46、包含所述約束條件的所述跟蹤模型為：

47、

48、s.t.vel-con；

49、其中，lsp為第二目標函數(shù)，w為軌跡跟蹤權(quán)重；

50、利用包含所述約束條件的所述跟蹤模型對所述初始位置到所述目標位置之間的路徑規(guī)劃多段折線軌跡。

51、在一些實施例中，所述根據(jù)多段所述折線軌跡控制所述針灸機器人移動，包括以下步驟：

52、規(guī)劃多段所述折線軌跡的第一期望軌跡；

53、更新包含所述約束條件的所述跟蹤模型得到第一跟蹤模型；

54、將各段所述第一期望軌跡代入到所述第一跟蹤模型，并控制所述針灸機器人移動；

55、所述第一跟蹤模型為：

56、

57、其中，lsr為第三目標函數(shù)，jtk為n自由度的所述針灸機器人的雅可比矩陣，wr為第一軌跡跟蹤權(quán)重，jrxy表示與roll和pitch自由度對應(yīng)的子雅可比矩陣；qn,qn-1表示末端2自由度的針灸執(zhí)行機構(gòu)的關(guān)節(jié)角；sd1為所述第一期望軌跡；

58、

59、其中，p0為所述初始位置，o為所述目標位置，w為直角拐點，所述初始位置到所述目標位置之間的路徑為p0→w→o；λ為規(guī)劃步長，n為控制視野，ρ為正整數(shù)，c為p0→w或w→o段的規(guī)劃次數(shù)，∈為可調(diào)整的正數(shù)；當(dāng)||p-w||2<∈時，將p0→w的軌跡切換到w→o。

60、在一些實施例中，所述根據(jù)所述針尖當(dāng)前位置、所述針頭當(dāng)前位置、所述外作用力以及所述針的長度計算得到針的彎曲角度，包括以下步驟：

61、將所述目標位置作為球心，將所述針的長度作為半徑建立球面；

62、在所述球面上確定預(yù)期位置，并根據(jù)所述針尖當(dāng)前位置、所述針頭當(dāng)前位置、所述外作用力以及所述半徑確定所述預(yù)期位置的坐標；

63、根據(jù)所述針尖當(dāng)前位置的坐標、所述針頭當(dāng)前位置的坐標以及所述預(yù)期位置的坐標并利用反余弦函數(shù)計算得到針的所述彎曲角度。

64、在一些實施例中，所述根據(jù)所述彎曲角度控制所述針灸機器人進行針刺，包括以下步驟：

65、根據(jù)所述彎曲角度規(guī)劃所述針尖下一時刻的期望位置；

66、根據(jù)所述期望位置生成第二期望軌跡；

67、

68、其中，sd2為所述第二期望軌跡，m為所述針尖下一時刻的期望位置；p′為所述針尖的當(dāng)前位置；0pm為所述針尖下一時刻的期望位置相對所述針尖的坐標系的坐標；0pp′為所述針尖的當(dāng)前位置相對所述針尖的坐標系的坐標；

69、更新包含所述約束條件的所述跟蹤模型得到第二跟蹤模型；

70、所述第二跟蹤模型為：

71、

72、其中，lar為第四目標函數(shù)；jt′k為所述針頭的坐標系{e}對應(yīng)的線速度雅可比矩陣，wa為第二軌跡跟蹤權(quán)重；

73、將所述第二期望軌跡代入到所述第二跟蹤模型，并控制所述針灸機器人進行針刺。

74、為實現(xiàn)上述目的，本技術(shù)實施例的另一方面提出了一種針灸機器人彎曲矯正控制裝置，所述裝置包括：

75、位置確定單元，用于根據(jù)針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)確定所述針灸機器人末端針尖的位置作為初始位置；確定待針刺的位置作為目標位置；

76、第一控制單元，用于對所述初始位置到所述目標位置之間的路徑規(guī)劃多段折線軌跡，根據(jù)多段所述折線軌跡控制所述針灸機器人移動；

77、數(shù)據(jù)確定單元，用于根據(jù)所述針灸機器人處于所述目標位置時的關(guān)節(jié)角數(shù)據(jù)確定所述針尖的位置及針頭對應(yīng)的位置，并獲取針受到的外作用力和針的長度；

78、彎曲確定單元，用于根據(jù)所述針尖當(dāng)前位置、所述針頭當(dāng)前位置、所述外作用力以及所述針的長度計算得到針的彎曲角度；

79、第二控制單元，用于根據(jù)所述彎曲角度控制所述針灸機器人進行針刺。

80、為實現(xiàn)上述目的，本技術(shù)實施例的另一方面提出了一種電子設(shè)備，所述電子設(shè)備包括存儲器和處理器，所述存儲器存儲有計算機程序，所述處理器執(zhí)行所述計算機程序時實現(xiàn)上述的方法。

81、為實現(xiàn)上述目的，本技術(shù)實施例的另一方面提出了一種計算機可讀存儲介質(zhì)，所述計算機可讀存儲介質(zhì)存儲有計算機程序，所述計算機程序被處理器執(zhí)行時實現(xiàn)上述的方法。

82、本技術(shù)實施例至少包括以下有益效果：

83、本實施例可以根據(jù)針灸機器人當(dāng)前的關(guān)節(jié)角數(shù)據(jù)確定針灸機器人末端針尖的位置作為初始位置；確定待針刺的位置作為目標位置；對初始位置到目標位置之間的路徑規(guī)劃多段折線軌跡，根據(jù)多段折線軌跡控制針灸機器人移動；根據(jù)針灸機器人處于目標位置時的關(guān)節(jié)角數(shù)據(jù)確定針尖的位置及針頭對應(yīng)的位置，并獲取針受到的外作用力和針的長度；根據(jù)針尖當(dāng)前位置、針頭當(dāng)前位置、外作用力以及針的長度計算得到針的彎曲角度；根據(jù)彎曲角度控制針灸機器人進行針刺。通過多段折線軌跡可相對固定初始位置到目標位置之間的路徑，降低規(guī)劃軌跡的不確定性并提高針灸機器人的移動穩(wěn)定性；結(jié)合針的位置、針受到的外作用力、針的長度以及針的彎曲角度，可矯正針刺過程針的發(fā)生彎曲，從而提高針刺準確性。