本發(fā)明屬于機器人控制技術(shù)相關(guān)，更具體地，涉及一種足式機械臂機器人的步態(tài)切換控制方法和系統(tǒng)。

背景技術(shù)：

1、足式機器人可以依靠離散落足點實現(xiàn)機體運動，相比輪式機器人在環(huán)境適應(yīng)性方面具有顯著優(yōu)勢。安裝在四足機器人上的機械臂，除了對物體進行操作外，還可以輔助機器人平衡以提高其動態(tài)運動性能。在某些特定情況下，如空間受限環(huán)境或腿部關(guān)節(jié)損壞時，四足機器人需要依靠兩條腿來維持平衡和移動能力。

2、然而，如何協(xié)調(diào)四足機器人和機械臂的運動使機器人實現(xiàn)雙足運動是一個具有挑戰(zhàn)性的問題，具體表現(xiàn)在如何控制足式機械臂機器人進入雙足站立狀態(tài)以及維持雙足站立狀態(tài)的平衡，現(xiàn)有技術(shù)中缺乏針對四足機械臂機器人的步態(tài)切換控制方法。

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

1、針對現(xiàn)有技術(shù)的以上缺陷或改進需求，本發(fā)明提供了一種足式機械臂機器人的步態(tài)切換控制方法和系統(tǒng)，實現(xiàn)四足機械臂機器人四足步態(tài)與雙足步態(tài)間的切換。

2、為實現(xiàn)上述目的，按照本發(fā)明的一個方面，提供了一種足式機械臂機器人的步態(tài)切換控制方法，該方法包括下列步驟：

3、s1分別計算足式機械臂機器人當(dāng)前步態(tài)以及待切換步態(tài)的零力矩點的位置；

4、s2依據(jù)當(dāng)前步態(tài)和待切換步態(tài)的零力矩點的位置和線速度以及從當(dāng)前步態(tài)運動到待切換步態(tài)的時間，規(guī)劃足式機械臂機器人的零力矩點從當(dāng)前步態(tài)運動到待切換步態(tài)的運動軌跡，該運動軌跡作為零力矩點參考軌跡；

5、s3使用預(yù)測控制算法控制足式機械臂機器人按照零力矩點參考軌跡進行運動，實時計算實際運動軌跡和零力矩點參考軌跡之間的跟蹤誤差，利用該跟蹤誤差計算所述預(yù)測控制算法的損失函數(shù)，根據(jù)損失函數(shù)實時調(diào)整所述預(yù)測控制算法的控制參數(shù)，同時實時輸出狀態(tài)軌跡和輸入軌跡，以此實現(xiàn)足式機械臂機器人的步態(tài)切換；所述預(yù)測控制算法以零力矩點參考軌跡為輸入，狀態(tài)軌跡和輸入軌跡為輸出。

6、進一步優(yōu)選地，步驟s3中得到的損失函數(shù)表達(dá)式為：

7、

8、其中，x和u分別代表狀態(tài)軌跡和輸入軌跡，p(x,t)是機器人的零力矩點在當(dāng)前時刻t下的位置，p(t)ref為零力矩點參考軌跡，正定矩陣q和半正定矩陣r是跟蹤參考狀態(tài)軌跡和輸入軌跡的權(quán)重矩陣，矩陣qp是跟蹤零力矩點參考軌跡p(t)ref的權(quán)重矩陣。

9、進一步優(yōu)選地，步驟s3中，預(yù)測控制算法需滿足如下約束條件，浮動基動力學(xué)約束：

10、

11、其中，代表足式機械臂質(zhì)心處的線動量和角動量，代表廣義坐標(biāo)，包含機器人基坐標(biāo)系在慣性系下的六維坐標(biāo)qbase，qleg代表四足機器人的腿部關(guān)節(jié)角度，qarm代表機械臂的關(guān)節(jié)角度，ab和aj代表質(zhì)心動量矩陣acom中的機身對應(yīng)部分以及關(guān)節(jié)對應(yīng)部分，fci和分別代表由質(zhì)心指向接觸點的向量以及地面對機器人的三維接觸力。

12、在足式機器人接觸項時，足端點與地面間需服從無滑動約束：

13、

14、在非接觸項，足端點擺動軌跡需沿著地面的法向量并且跟蹤參考軌跡v*(t)，當(dāng)處于非接觸項時接觸力為零：

15、

16、fci＝0

17、在足端觸地時，接觸力需滿足摩擦錐約束：

18、

19、其中，μ為地面摩擦系數(shù)，和分別代表地面接觸力在三個方向上的分量。

20、進一步優(yōu)選地，在步驟s3中，所述狀態(tài)軌跡包括足式機械臂機器人在當(dāng)前狀態(tài)下機械臂質(zhì)心處的線動量和角動量、腿部的關(guān)節(jié)角度，機械臂的關(guān)節(jié)角度；所述輸入軌跡包括地面對足式機械臂機器人的三維接觸力、腿部的關(guān)節(jié)速度和機械臂的關(guān)節(jié)速度。

21、進一步優(yōu)選地，通過分層二次規(guī)劃重新分配機器人的接觸力和關(guān)節(jié)力矩，跟蹤優(yōu)化的軌跡，滿足動態(tài)約束和目標(biāo)跟蹤任務(wù)。

22、動態(tài)約束任務(wù)包括浮動基座動態(tài)約束、關(guān)節(jié)力矩限制約束、接觸足無滑動約束和摩擦圓錐約束：

23、

24、目標(biāo)跟蹤任務(wù)包括擺動足跟蹤、浮動基座、機械臂關(guān)節(jié)指令和接觸足力跟蹤任務(wù)：

25、

26、其中jci代表在接觸點ci處的浮動基雅克比，和分別代表足端擺動軌跡跟蹤的剛度和阻尼系數(shù)，和分別代表由優(yōu)化的足端關(guān)節(jié)角度計算的足端期望位置和速度，和分別代表足端真實的位置和速度，和代表機械臂末端軌跡跟蹤的剛度和阻尼系數(shù)，和代表機械臂當(dāng)前的關(guān)節(jié)角度和關(guān)節(jié)速度。

27、進一步優(yōu)選地，步驟s1中零力矩點位置的具體計算方法如下：

28、

29、其中xcog，ycog和zcog代表機器人重心在慣性系下各個方向的坐標(biāo)，px，py和pz是零力矩點三維坐標(biāo)的符號表達(dá)。

30、進一步優(yōu)選地，使用貝塞爾曲線擬合生成零力矩點參考軌跡，生成零力矩點參考軌跡使用公式如下：

31、p(τ)＝(1-τ)3a+3(1-τ)2τc+3(1-τ)τ2d+τ3b

32、

33、t＝τ·ttraj

34、其中，控制點a和b代指零力矩點的初始和最終的二維坐標(biāo)，控制點c和d分別沿著零力矩點初始速度方向和最終速度方向和代表初始速度值和最終速度值，參數(shù)τ∈[0,1]代表了從初始點p1到最終點p2的整個過程參數(shù)；為貝塞爾曲線添加時間標(biāo)度：t＝τ·ttraj；其中，ttraj代表零力矩點軌跡從初始點p1到終點p2的持續(xù)總時間。

35、按照本發(fā)明的第二個方面，提供了一種實現(xiàn)上述任一項步態(tài)切換控制方法的足式機械臂機器人的步態(tài)切換控制系統(tǒng)，所述步態(tài)切換控制系統(tǒng)包括：

36、計算模塊，所述計算模塊用于計算足式機械臂機器人的零力矩點位置；

37、零力矩點規(guī)劃模塊，所述零力矩點規(guī)劃模塊用于規(guī)劃足式機械臂機器人從當(dāng)前步態(tài)運動到待切換步態(tài)的運動零力矩點參考軌跡；

38、預(yù)測控制模塊，所述預(yù)測控制模塊用于構(gòu)建以零力矩點參考軌跡為輸入，狀態(tài)軌跡和輸入軌跡為輸出的預(yù)測控制算法，并利用該預(yù)測控制算法控制所述足式機械臂機器人運動，并實時輸出狀態(tài)軌跡和輸入軌跡，控制足式機械臂機器人的步態(tài)切換。

39、總體而言，通過本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，具備下列有益效果：

40、本發(fā)明考慮機械臂動力學(xué)對機器人整機運動性能的影響，提出了一種基于零力矩點軌跡規(guī)劃的足式機械臂機器人的步態(tài)切換控制方法和系統(tǒng)，輔助足式機器人完成雙足步態(tài)切換，可實現(xiàn)四足機器人穩(wěn)定的雙足步態(tài)切換以及行走。

41、本發(fā)明使用基于質(zhì)心動力學(xué)的非線性模型預(yù)測控制算法對零力矩點參考軌跡進行跟蹤并將跟蹤誤差值添加到模型預(yù)測控制中的損失函數(shù)，通過結(jié)合相應(yīng)的約束條件，包括浮動基動力學(xué)約束、無滑動約束、接觸力約束和摩擦錐約束來優(yōu)化狀態(tài)軌跡以及輸入軌跡，以保證步態(tài)切換的成功，保持足式機械臂機器人的平衡性和穩(wěn)定性。本發(fā)明提出的控制框架可以拓展到多種步態(tài)運動，以便應(yīng)對機器人關(guān)節(jié)損壞等極端場景。

技術(shù)特征：

1.一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，包括以下步驟：

2.如權(quán)利要求1所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，步驟s3中得到的損失函數(shù)表達(dá)式為：

3.如權(quán)利要求1所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，步驟s3中的預(yù)測控制算法的約束條件包括浮動基動力學(xué)約束、無滑動約束、接觸力約束、摩擦錐約束。

4.如權(quán)利要求3所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，所述浮動基動力學(xué)約束表達(dá)式為：

5.如權(quán)利要求1所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，在步驟s3中，所述狀態(tài)軌跡包括足式機械臂機器人在當(dāng)前狀態(tài)下機械臂質(zhì)心處的線動量和角動量、腿部的關(guān)節(jié)角度，機械臂的關(guān)節(jié)角度；所述輸入軌跡包括地面對足式機械臂機器人的三維接觸力、腿部的關(guān)節(jié)速度和機械臂的關(guān)節(jié)速度。

6.如權(quán)利要求1所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，步驟s1中零力矩點的位置的計算方法如下：

7.如權(quán)利要求1所述的一種足式機械臂機器人的步態(tài)切換控制方法，其特征在于，步驟s2的具體實現(xiàn)方法為使用貝塞爾曲線擬合生成零力矩點參考軌跡，零力矩點參考軌跡如下：

8.一種足式機械臂機器人的步態(tài)切換控制系統(tǒng)，其特征在于，用于實現(xiàn)上述權(quán)利要求1-6任一項所述的步態(tài)切換控制方法，所述步態(tài)切換控制系統(tǒng)包括：

技術(shù)總結(jié)
本發(fā)明屬于機器人控制技術(shù)領(lǐng)域，更具體地，涉及一種足式機械臂機器人的步態(tài)切換控制方法和系統(tǒng)，該方法包括下列步驟：計算足式機械臂機器人當(dāng)前步態(tài)以及待切換步態(tài)的零力矩點的位置；規(guī)劃足式機械臂機器人的零力矩點參考軌跡；使用模型預(yù)測控制算法控制足式機械臂機器人按照零力矩點參考軌跡進行運動，并根據(jù)損失函數(shù)實時調(diào)整所述預(yù)測控制算法的控制參數(shù)，同時實時輸出狀態(tài)軌跡和輸入軌跡，以此實現(xiàn)足式機械臂機器人的步態(tài)切換。本發(fā)明提供的足式機械臂機器人的步態(tài)切換控制方法，充分考慮了機械臂動力學(xué)對機器人整機運動性能的影響，可以實現(xiàn)足式機器人的步態(tài)切換控制。

技術(shù)研發(fā)人員：羅欣,趙傳林
受保護的技術(shù)使用者：華中科技大學(xué)
技術(shù)研發(fā)日：
技術(shù)公布日：2024/12/19