基于Bezier 曲線的四足機(jī)器人Trot 步態(tài)優(yōu)化

薛晨慷，李 光，易 靜，譚薪興

（湖南工業(yè)大學(xué) 機(jī)械工程學(xué)院，湖南 株洲 412007）

1 研究背景

近年來，移動式機(jī)器人受到了社會各界的關(guān)注，越來越多的科研工作者對其進(jìn)行研究[1]。當(dāng)前，移動式機(jī)器人主要分為履帶式、輪式、足式、爬行或蠕動式等[2]。履帶式、輪式機(jī)器人對地形有嚴(yán)格要求，一般要在較平坦、無障礙物的地面工作，而足式機(jī)器人運(yùn)動更靈活，可在復(fù)雜崎嶇路面上行走，如沙地等[3]，可被應(yīng)用于資源探查、軍事運(yùn)輸[4]等方面。

足式機(jī)器人包括單足、雙足、四足和六足機(jī)器人等[5]。與單足、雙足機(jī)器人相比，四足機(jī)器人的運(yùn)動穩(wěn)定性更好、負(fù)載能力更強(qiáng)、運(yùn)動靈活性更佳[6]；與六足機(jī)器人相比，則四足機(jī)器人結(jié)構(gòu)簡單[7]、成本較低，且具備更好的機(jī)動性?；诖?，本研究選取四足機(jī)器人為研究對象。

目前，對四足機(jī)器人的研究主要集中在結(jié)構(gòu)設(shè)計、運(yùn)動規(guī)劃、算法控制3 個方面[8]。其中運(yùn)動規(guī)劃在四足機(jī)器人中起著至關(guān)重要的作用，可分為路徑規(guī)劃和軌跡規(guī)劃兩個部分。合理規(guī)劃足端軌跡有助于減小機(jī)器人觸地時的沖擊，減少對機(jī)體的傷害；而且足端軌跡對機(jī)器人的穩(wěn)定性有著非常大的影響。

關(guān)于足端軌跡規(guī)劃的方法，田俊等[9]提出了一種基于五次多項(xiàng)式的靜步態(tài)足端軌跡，五次多項(xiàng)式規(guī)劃足端軌跡能夠保證加速度連續(xù)過度，缺點(diǎn)在于加速度處于不斷加速減速的過程中，容易出現(xiàn)加速度過大的問題。馬慧姝等[10]人利用相機(jī)拾取生物犬步行時的足端數(shù)據(jù)點(diǎn)，再通過三次B 樣條曲線擬合的方法形成足端軌跡，但規(guī)劃的軌跡曲線不經(jīng)過擬合數(shù)據(jù)的原始點(diǎn)。Liu C.H.等[11]利用直線軌跡來規(guī)劃四足機(jī)器人的足端軌跡，而直線軌跡的缺點(diǎn)在于抬腿和落地的瞬間速度以及加速度不為0。也有學(xué)者利用復(fù)合擺線[12]、橢圓曲線[13]等規(guī)劃四足機(jī)器人的足端軌跡；利用函數(shù)公式規(guī)劃足端軌跡的優(yōu)點(diǎn)在于方法簡單，但是會限制機(jī)器人足端軌跡的形狀。

綜上所述，本研究旨在遵守質(zhì)心穩(wěn)定的原則，結(jié)合四足機(jī)器人足端軌跡約束條件，利用Bezier 曲線規(guī)劃四足機(jī)器人的足端軌跡，并在仿真平臺上驗(yàn)證此步態(tài)的可行性；然后，將所得到的結(jié)果與復(fù)合擺線規(guī)劃足端軌跡法得到的結(jié)果進(jìn)行比較，以此驗(yàn)證Bezier曲線軌跡優(yōu)化的可行性與有效性，以期為機(jī)器人足端軌跡的研究提供理論參考。

2 四足機(jī)器人結(jié)構(gòu)及運(yùn)動學(xué)模型

2.1 四足機(jī)器人結(jié)構(gòu)

四足機(jī)器人的結(jié)構(gòu)是根據(jù)四足哺乳動物的身體結(jié)構(gòu)，并基于仿生學(xué)原理設(shè)計而成的。四足機(jī)器人主要由機(jī)身和4 條腿組成，根據(jù)腿部結(jié)構(gòu)的不同可劃分為全膝式、全肘式、外膝肘式和內(nèi)膝肘式機(jī)器人。本研究采用的四足機(jī)器人為全肘式結(jié)構(gòu)，每條腿有側(cè)擺關(guān)節(jié)、髖關(guān)節(jié)和膝關(guān)節(jié)3 個關(guān)節(jié)，共3 個自由度，其中側(cè)擺關(guān)節(jié)可以實(shí)現(xiàn)機(jī)器人的側(cè)向運(yùn)動，而髖關(guān)節(jié)和膝關(guān)節(jié)組合可以實(shí)現(xiàn)機(jī)器人的前后向運(yùn)動以及俯仰運(yùn)動。

2.2 四足機(jī)器人運(yùn)動學(xué)模型

以四足機(jī)器人右前腿為例，采用標(biāo)準(zhǔn)D-H 法建立坐標(biāo)系，其運(yùn)動學(xué)模型如圖1 所示。

圖1 中，坐標(biāo)系O{XW,YW,ZW}為世界坐標(biāo)系；坐標(biāo)系B{X,Y,Z}為機(jī)身坐標(biāo)系，原點(diǎn)在機(jī)器人軀干的質(zhì)心處，X軸正方向?yàn)闄C(jī)器人前進(jìn)的方向，Z軸正方向?yàn)樨Q直朝上，Y軸的正方向根據(jù)右手定則確定。坐標(biāo)系{0},{1},{2},{3}分別為側(cè)擺關(guān)節(jié)、髖關(guān)節(jié)、膝關(guān)節(jié)以及足端的坐標(biāo)系；θ1,θ2,θ3分別為3 個關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)角；L1,L2,L3分別為側(cè)擺、大腿、小腿3 根連桿的長度。

2.2.1 正運(yùn)動學(xué)

以四足機(jī)器人的右前腿為例，依據(jù)所建立的坐標(biāo)系，推導(dǎo)出機(jī)器人的運(yùn)動學(xué)方程，其D-H 參數(shù)如表1 所示。

表1 右前腿D-H 參數(shù)表Table 1 D-H parameter table of right front legs

表1 中，ai為連桿長度，αi為關(guān)節(jié)扭角，di為關(guān)節(jié)距離，θi為關(guān)節(jié)轉(zhuǎn)角。

機(jī)器人矩陣變換的通式為

于是得到如下各關(guān)節(jié)之間的變換矩陣：

以側(cè)擺關(guān)節(jié)處坐標(biāo)系為基座標(biāo)，則足端對基座標(biāo)的關(guān)系可以表示為

將表1 各參數(shù)帶入式（5）中得到：

最終可以確定機(jī)器人足端正運(yùn)動學(xué)方程為

2.2.2 逆運(yùn)動學(xué)

如果機(jī)器人末端的位姿已經(jīng)給出，要求其各關(guān)節(jié)的值，則需運(yùn)動反解。逆運(yùn)動學(xué)主要有代數(shù)法和幾何法兩種，本文采用代數(shù)法求逆運(yùn)動學(xué)解。

1）求θ3。對式（7）進(jìn)行平方和，得

令矩陣方程中對應(yīng)元素相等，可得到：

利用式中第三個等式條件，同時令

經(jīng)三角代換得

3）求θ2。利用式（13）中的兩個等式條件，可以得到式（17）：

聯(lián)立式（9）（10）（11）（17），計算可得

3 四足機(jī)器人足端軌跡規(guī)劃與優(yōu)化

根據(jù)抬腿順序的不同，四足機(jī)器人步態(tài)可分為對稱步態(tài)和不對稱步態(tài)[14]。對稱步態(tài)中，對角步態(tài)即Trot 步態(tài)，因其穩(wěn)定性好、速度大[15]、重心偏移量小等優(yōu)點(diǎn)成為最常見的研究步態(tài)。因此本研究采用Trot 步態(tài)進(jìn)行研究，即行走過程中同時抬起、落下右前和左后腿（或左前和右后腿）。Trot 步態(tài)的時序圖如圖2 所示。

3.1 足端軌跡規(guī)劃

根據(jù)參考文獻(xiàn)[16]，可以得到復(fù)合擺線軌跡擺動相的方程為

式中：s為步長；h為抬腿高度；T為步態(tài)周期；λ為騰空率，即擺動相占整個步態(tài)周期的比例。

復(fù)合擺線規(guī)劃的足端軌跡，其水平和豎直方向上速度、加速度分量在初始及結(jié)束的瞬間都為0。因此利用五次Bezier 曲線對復(fù)合擺線足端軌跡進(jìn)行擬合。

Bezier 曲線于1962 年，由法國工程師Pierre Bezier 發(fā)明，并被成功應(yīng)用于汽車車體工業(yè)設(shè)計，且將其廣泛應(yīng)用[17]?，F(xiàn)如今Bezier 曲線主要用于Photoshop 等計算機(jī)繪圖軟件中曲線的繪制。Bezier曲線由線段與節(jié)點(diǎn)組成，可以依據(jù)起始點(diǎn)、控制點(diǎn)、終止點(diǎn)的坐標(biāo)繪制出一條光滑的曲線[18]。如圖3 所示為根據(jù)4 個點(diǎn)繪制的Bezier 曲線。圖中控制點(diǎn)之間的連線為Bezier 曲線的控制多邊形，生成的光滑曲線則為Bezier 曲線。

通過Bezier 曲線擬合得到的擺動相軌跡方程為

式中：i=0,1,…,5；Pxi為第i點(diǎn)的橫坐標(biāo)，取值為0,0,0.5s,0.5s,s,s；Pzi為第i點(diǎn)的縱坐標(biāo)，取值為0,0.65h,1.3h,1.3h,0.65h,0。

設(shè)置步長s=20 mm，抬腿高度h=10 mm。

綜上可得，復(fù)合擺線規(guī)劃的足端軌跡和五次Bezier 曲線的足端軌跡擬合曲線如圖4 所示。

結(jié)合參考文獻(xiàn)[19]的研究可知，當(dāng)足端軌跡與地面的底角為鈍角時，沖擊力更小，機(jī)器人行進(jìn)更加穩(wěn)定。因此在五次Bezier 曲線的基礎(chǔ)上對軌跡進(jìn)行修改，使其滿足抬腿與落地時足端軌跡與地面的底角大于90°。為實(shí)現(xiàn)這一條件，需要在Bezier 曲線的起始點(diǎn)和終止點(diǎn)處額外添加3 個控制點(diǎn)，用該方法形成的Bezier 曲線最終共需12 個點(diǎn)控制。

首先對Z軸坐標(biāo)進(jìn)行重新規(guī)劃，原五次Bezier曲線中最高點(diǎn)的縱坐標(biāo)為1.3h，現(xiàn)將0～1.3h重新劃分為6 等分。預(yù)設(shè)足端軌跡與地面的夾角為135°，并根據(jù)Px0=0,Pz0=0，Pz1=0.26h，通過斜率公式計算得到Px1=-0.13s，令Px2=Px1=-0.13s。用同樣的方法可得到Px9=Px10=1.13s，最終得到的所有控制點(diǎn)的坐標(biāo)如表2 所示。

表2 初始控制點(diǎn)坐標(biāo)Table 2 Initial control point coordinates

依據(jù)表2 中數(shù)據(jù)，最終得到一個周期內(nèi)擺動相的足端軌跡方程，為

足端軌跡曲線如圖5 所示。

對軌跡進(jìn)行求導(dǎo)，可以得到其速度與加速度。由于Bezier 曲線控制點(diǎn)的坐標(biāo)是常數(shù)，并且與變量t無關(guān)，因此對函數(shù)的求導(dǎo)轉(zhuǎn)化為對伯恩斯坦多項(xiàng)式Bn,i(t)的求導(dǎo)：

最終計算得到軌跡的速度、加速度方程分別為

水平和豎直方向上的速度、加速度分量曲線圖像分別為圖6、圖7 所示。

觀察圖5、圖6、圖7 中的曲線，發(fā)現(xiàn)形成的軌跡存在以下問題：

1）實(shí)際抬腿高度會超出預(yù)設(shè)的抬腿高度10%；

2）水平方向的速度和加速度在t=0 時刻過大；

3）水平方向上的速度和加速度在一個擺動相的時間內(nèi)振幅過大。

針對以上問題，依據(jù)Trot 步態(tài)約束條件對控制點(diǎn)的坐標(biāo)進(jìn)行優(yōu)化。

3.2 足端軌跡優(yōu)化

Trot 步態(tài)約束條件如下：

水平方向?yàn)?/p>

豎直方向?yàn)?/p>

針對速度：通過改變3 個控制點(diǎn)的橫坐標(biāo)使兩個一階插值點(diǎn)為0，讓速度等于0；

針對加速度：通過改變4 個控制點(diǎn)的橫坐標(biāo)使其兩個二階插值點(diǎn)為0，讓加速度等于0；

針對抬腿高度：通過減小各控制點(diǎn)的縱坐標(biāo)，使抬腿高度滿足要求。

依據(jù)上述條件，經(jīng)過多組數(shù)據(jù)對比，最終得到一組最優(yōu)控制的點(diǎn)坐標(biāo)，如表3 所示。

表3 最優(yōu)控制點(diǎn)坐標(biāo)Table 3 Optimal control point coordinates

優(yōu)化后一個周期內(nèi)擺動相軌跡函數(shù)為

一個周期內(nèi)擺動相的足端軌跡如圖8 所示。

其速度與加速度圖像如圖9 和圖10 所示。

4 實(shí)驗(yàn)仿真與結(jié)果分析

4.1 仿真平臺搭建

本文采用Solidworks 軟件，以“YOBOGO”四足機(jī)器人為模型，建立四足機(jī)器人的虛擬樣機(jī)。四足機(jī)器人的結(jié)構(gòu)參數(shù)如表4 所示。

表4 四足機(jī)器人結(jié)構(gòu)參數(shù)表Table 4 Structural parameters of quadruped robots

將模型導(dǎo)入Matlab/Simulink 中，結(jié)合實(shí)際情況，設(shè)置機(jī)器人的材料為鋁合金，地面的接觸剛度為1×104N/m，接觸阻尼為1×102N/m，靜摩擦系數(shù)為0.9，動摩擦系數(shù)為0.75。之后通過Simulink 模塊并結(jié)合編寫的程序，搭建如圖11 所示仿真平臺，并進(jìn)行仿真實(shí)驗(yàn)。

4.2 仿真實(shí)驗(yàn)結(jié)果分析

4.2.1 實(shí)驗(yàn)一結(jié)果分析

實(shí)驗(yàn)一將復(fù)合擺線規(guī)劃的足端軌跡與Bezier 曲線規(guī)劃的足端軌跡進(jìn)行對比。仿真過程中，除足端軌跡函數(shù)外，保證機(jī)器人的初始位置、步態(tài)周期T、占空比、步長s及抬腿高度h都完全相同。仿真結(jié)果如圖12、13 所示，分別從機(jī)體前進(jìn)距離、偏航距離、質(zhì)心波動范圍、足端的沖擊力等方面判斷機(jī)器人穩(wěn)定性。又因機(jī)器人初始時刻距離地面有微小高度，落地期間產(chǎn)生的數(shù)據(jù)不準(zhǔn)確，因此，為保證數(shù)據(jù)的有效性，對前兩個周期即1 s 內(nèi)的數(shù)據(jù)不予采用。

由圖12 可看出，10 s 仿真時間內(nèi)，無論是x軸方向的前進(jìn)距離，y軸方向的偏航距離，以及z軸方向的波動范圍，都有較大改善。由圖13 所示不同步態(tài)下沖擊力對比曲線可看出，Bezier 曲線規(guī)劃的軌跡下，足端與地面接觸時的沖擊力更小。

4.2.2 實(shí)驗(yàn)二結(jié)果分析

實(shí)驗(yàn)二通過改變四足機(jī)器人步長s和抬腿高度h，對照步長s=20 mm，抬腿高度h=10 mm 時的沖擊力和質(zhì)心位移，找到足端軌跡最優(yōu)步長和抬腿高度。設(shè)置步長范圍為20～40 mm，間距為5 mm；抬腿高度區(qū)間為7～15 mm，間距為2 mm。每兩組進(jìn)行一次對比實(shí)驗(yàn)，選擇數(shù)據(jù)更好的一組進(jìn)入下一組對比實(shí)驗(yàn)，仿真時間設(shè)置為20 s。仿真結(jié)果見圖14、15。

由圖14 和15 可看出，h=9 mm，s=25 mm 時，機(jī)器人行走距離更遠(yuǎn)、速度更快，偏航距離及偏航角度更小；質(zhì)心的上下波動范圍以及足端沖擊力幾乎沒變化。因此，此組步長及抬腿高度最佳。

5 結(jié)語

本研究應(yīng)用Bezier 曲線規(guī)劃四足機(jī)器人的足端軌跡。首先，采用五次Bezier 曲線對復(fù)合擺線規(guī)劃的足端軌跡進(jìn)行擬合，并依據(jù)足端軌跡規(guī)劃約束條件，使足端沖擊力最小的規(guī)劃方法，對曲線的控制點(diǎn)進(jìn)行增加和修改，最終得到Bezier 曲線規(guī)劃四足機(jī)器人的足端軌跡方程。其次，在Matlab/Simulink 軟件中建立機(jī)器人仿真平臺，計算并建立機(jī)器人的正逆運(yùn)動學(xué)模型；從機(jī)器人的前進(jìn)距離、偏航距離、質(zhì)心波動程度及落地時足端對地面的沖擊力4 個方面，與復(fù)合擺線規(guī)劃的足端軌跡進(jìn)行對比，驗(yàn)證了Bezier曲線規(guī)劃下的足端軌跡效率更高，機(jī)器人運(yùn)動也更穩(wěn)定。最后，在Bezier 曲線規(guī)劃的足端軌跡下對抬腿高度及步長之間的關(guān)系進(jìn)行研究，找到了抬腿高度與步長組合的最優(yōu)解，完成對四足機(jī)器人行走的最優(yōu)規(guī)劃。結(jié)果表明：采用Bezier 曲線規(guī)劃足端軌跡，能較好地優(yōu)化機(jī)器人的運(yùn)動效率及穩(wěn)定性。