本發(fā)明涉及機(jī)器人控制，尤其涉及一種基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法。

背景技術(shù)：

1、現(xiàn)有的無(wú)人機(jī)有多種類型，其中具有多個(gè)關(guān)節(jié)的運(yùn)動(dòng)機(jī)器人廣泛用于地面探索和處理工作，而這些工作的環(huán)境往往具有較復(fù)雜的空間布局，運(yùn)動(dòng)路徑上不可避免地會(huì)存在障礙，因此對(duì)于運(yùn)動(dòng)機(jī)器人來(lái)說(shuō)，運(yùn)動(dòng)規(guī)劃的功能是至關(guān)重要的。

2、機(jī)器人的運(yùn)動(dòng)軌跡規(guī)劃有多種形式，常見(jiàn)的有基于關(guān)節(jié)空間的軌跡規(guī)劃和基于笛卡爾坐標(biāo)系的軌跡規(guī)劃。但是，對(duì)于具有多個(gè)關(guān)節(jié)的運(yùn)動(dòng)機(jī)器人來(lái)說(shuō)，其每個(gè)關(guān)節(jié)(可稱為機(jī)器人的足)的運(yùn)動(dòng)不同，不同足的落腳點(diǎn)決定了機(jī)器人的行進(jìn)姿態(tài)。然而，上述基于關(guān)節(jié)空間和笛卡爾坐標(biāo)系的軌跡規(guī)劃方法雖然可以控制關(guān)節(jié)角度的變換速度，但是無(wú)法預(yù)測(cè)機(jī)器人足端落點(diǎn)的位置，導(dǎo)致多足機(jī)器人的運(yùn)動(dòng)規(guī)劃存在一定局限性。

3、因此，有必要提出一種新的步態(tài)規(guī)劃方法來(lái)解決上述問(wèn)題。

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

1、本發(fā)明旨在解決現(xiàn)有多足機(jī)器人步態(tài)規(guī)劃不能預(yù)測(cè)足端落點(diǎn)位置的技術(shù)問(wèn)題。

2、為解決上述技術(shù)問(wèn)題，本發(fā)明提供一種基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，包括以下步驟：

3、s1、選取機(jī)器人運(yùn)動(dòng)空間中可構(gòu)成三角形的軌跡點(diǎn)，所述軌跡點(diǎn)包括第一軌跡點(diǎn)a、第二軌跡點(diǎn)b和第三軌跡點(diǎn)c，其中，所述機(jī)器人的步態(tài)規(guī)劃為將其落腳點(diǎn)從所述第一軌跡點(diǎn)a經(jīng)過(guò)所述第二軌跡點(diǎn)b運(yùn)動(dòng)至所述第三軌跡點(diǎn)c；

4、s2、根據(jù)所述軌跡點(diǎn)計(jì)算對(duì)應(yīng)的圓弧軌跡圓心以及圓弧軌跡半徑；

5、s3、根據(jù)所述軌跡點(diǎn)、所述圓弧軌跡圓心以及所述圓弧軌跡半徑構(gòu)建圓弧軌跡平面方程；

6、s4、將經(jīng)過(guò)所述軌跡點(diǎn)的圓弧軌跡劃分為多個(gè)分段，得到多個(gè)采樣點(diǎn)；

7、s5、分別計(jì)算每一采樣點(diǎn)在所述機(jī)器人運(yùn)動(dòng)空間中的坐標(biāo)，并將坐標(biāo)代入所述圓弧軌跡平面方程進(jìn)行運(yùn)動(dòng)逆解，得到所述機(jī)器人在所述采樣點(diǎn)時(shí)的運(yùn)動(dòng)角度；

8、s6、基于所有所述采樣點(diǎn)對(duì)應(yīng)的所述運(yùn)動(dòng)角度，得到所述機(jī)器人的步態(tài)規(guī)劃結(jié)果。

9、更進(jìn)一步地，所述定義所述第一軌跡點(diǎn)a、所述第二軌跡點(diǎn)b、所述第三軌跡點(diǎn)c的坐標(biāo)分別為a(x1，y1，z1)，b(x2，y2，z2)，c(x3，y3，z3)，所述圓弧軌跡圓心為o(x，y，z)，其滿足以下關(guān)系式：

10、(x-x1)2+(y-y1)2+(z-z1)2＝(x-x2)2+(y-y2)2+(z-z2)2；

11、(x-x2)2+(y-y2)2+(z-z2)2＝(x-x3)2+(y-y3)2+(z-z3)2。

12、更進(jìn)一步地，所述圓弧軌跡平面方程滿足以下關(guān)系式：

13、ax+b+cz＝1；

14、其中：

15、

16、更進(jìn)一步地，步驟s5具體為：

17、定義所述采樣點(diǎn)的總數(shù)為n，初始迭代值i＝1，采樣點(diǎn)i與所述第一軌跡點(diǎn)a之間的夾角θ滿足以下關(guān)系式：

18、

19、所述采樣點(diǎn)i與所述第一軌跡點(diǎn)a在所述圓弧軌跡中的旋轉(zhuǎn)軸ω滿足以下關(guān)系式：

20、

21、將所述旋轉(zhuǎn)軸ω歸一化，并構(gòu)造四元數(shù)q：

22、

23、其中，x、y、z是旋轉(zhuǎn)軸的單位向量；其中，所述四元數(shù)的實(shí)部a和虛部b、c、d滿足以下關(guān)系式：

24、

25、基于所述旋轉(zhuǎn)軸和所述四元數(shù)構(gòu)建旋轉(zhuǎn)矩陣r：

26、

27、將所述旋轉(zhuǎn)矩陣r左乘采樣點(diǎn)i對(duì)應(yīng)的向量，得到采樣點(diǎn)i的坐標(biāo)，將采樣點(diǎn)i的坐標(biāo)代入所述圓弧軌跡平面方程進(jìn)行運(yùn)動(dòng)逆解，得到所述機(jī)器人在采樣點(diǎn)i時(shí)的運(yùn)動(dòng)角度；

28、使i＝i+1，重復(fù)上述步驟，直到i>n，得到所有采樣點(diǎn)的所述運(yùn)動(dòng)角度。

29、更進(jìn)一步地，構(gòu)建所述旋轉(zhuǎn)矩陣r時(shí)，將所述四元數(shù)的虛部b、c、d的符號(hào)全部取反。

30、更進(jìn)一步地，所述第一軌跡點(diǎn)a與所述圓弧軌跡圓心o、所述第三軌跡點(diǎn)與所述圓弧軌跡圓心o之間的向量夾角θac滿足以下關(guān)系式：

31、

32、本發(fā)明所達(dá)到的有益效果，在于提出了一種能夠預(yù)測(cè)機(jī)器人落腳點(diǎn)的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，該方法在進(jìn)行步態(tài)規(guī)劃過(guò)程中只需要選取三個(gè)軌跡點(diǎn)，然后通過(guò)軌跡點(diǎn)求解出機(jī)器人圓弧軌跡的圓心以及半徑的大小，通過(guò)圓心以及半徑求解出圓弧所在平面的方程，再通過(guò)插補(bǔ)的方法，在圓弧上截取適當(dāng)?shù)狞c(diǎn)，代入機(jī)器人運(yùn)動(dòng)學(xué)逆解方程，進(jìn)而求解出機(jī)器人關(guān)節(jié)在圓弧路徑上移動(dòng)的角度，該方法使得機(jī)器人的運(yùn)動(dòng)軌跡是可預(yù)見(jiàn)的，從而保證機(jī)器人控制的穩(wěn)定性。

技術(shù)特征：

1.一種基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，包括以下步驟：

2.根據(jù)權(quán)利要求1所述的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，所述定義所述第一軌跡點(diǎn)a、所述第二軌跡點(diǎn)b、所述第三軌跡點(diǎn)c的坐標(biāo)分別為a(x1，y1，z1)，b(x2，y2，z2)，c(x3，y3，z3)，所述圓弧軌跡圓心為o(x，y，z)，其滿足以下關(guān)系式：

3.根據(jù)權(quán)利要求2所述的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，所述圓弧軌跡平面方程滿足以下關(guān)系式：

4.根據(jù)權(quán)利要求3所述的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，步驟s5具體為：

5.根據(jù)權(quán)利要求4所述的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，構(gòu)建所述旋轉(zhuǎn)矩陣r時(shí)，將所述四元數(shù)的虛部b、c、d的符號(hào)全部取反。

6.根據(jù)權(quán)利要求4所述的基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，其特征在于，所述第一軌跡點(diǎn)a與所述圓弧軌跡圓心o、所述第三軌跡點(diǎn)與所述圓弧軌跡圓心o之間的向量夾角θac滿足以下關(guān)系式：

技術(shù)總結(jié)
本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，尤其涉及一種基于圓弧軌跡的機(jī)器人穩(wěn)定步態(tài)規(guī)劃方法，該方法包括以下步驟：選取機(jī)器人運(yùn)動(dòng)空間中可構(gòu)成三角形的軌跡點(diǎn)；根據(jù)所述軌跡點(diǎn)計(jì)算對(duì)應(yīng)的圓弧軌跡圓心以及圓弧軌跡半徑；根據(jù)所述軌跡點(diǎn)、所述圓弧軌跡圓心以及所述圓弧軌跡半徑構(gòu)建圓弧軌跡平面方程；將經(jīng)過(guò)所述軌跡點(diǎn)的圓弧軌跡劃分為多個(gè)分段，得到多個(gè)采樣點(diǎn)；分別計(jì)算每一采樣點(diǎn)在所述機(jī)器人運(yùn)動(dòng)空間中的坐標(biāo)，并將坐標(biāo)代入所述圓弧軌跡平面方程進(jìn)行運(yùn)動(dòng)逆解，得到所述機(jī)器人在所述采樣點(diǎn)時(shí)的運(yùn)動(dòng)角度；基于所有所述采樣點(diǎn)對(duì)應(yīng)的所述運(yùn)動(dòng)角度，得到所述機(jī)器人的步態(tài)規(guī)劃結(jié)果。本發(fā)明使得機(jī)器人的運(yùn)動(dòng)軌跡是可預(yù)見(jiàn)的，從而保證機(jī)器人控制的穩(wěn)定性。

技術(shù)研發(fā)人員：鄧嘉宏,繆文南,郭明鑫,何富華,梁梓壕,王子林
受保護(hù)的技術(shù)使用者：廣州城市理工學(xué)院
技術(shù)研發(fā)日：
技術(shù)公布日：2025/1/28