四足機(jī)器人Trot步態(tài)規(guī)劃與仿真分析

謝楚政，李 光，于權(quán)偉，吳陳成

1 研究背景

移動機(jī)器人可代替人類在一些復(fù)雜工況和惡劣環(huán)境中的工作，例如戰(zhàn)場的援救、地下管道的維修、高層樓房的建設(shè)等。不僅解放了人力、提高人們的生活質(zhì)量、提升幸福感，更是對生產(chǎn)力技術(shù)的一次革命。移動機(jī)器人技術(shù)研究，目前已經(jīng)成為國內(nèi)外科研工作者的熱點(diǎn)研究領(lǐng)域[1]。

根據(jù)移動方式不同，可將機(jī)器人分為輪式機(jī)器人、履帶式機(jī)器人、步行機(jī)器人等。其中，步行機(jī)器人由于其結(jié)構(gòu)模仿自然界中經(jīng)過優(yōu)勝劣汰、自然選擇后生存下來的各種生物結(jié)構(gòu)[2]，所以相比輪式機(jī)器人、履帶式機(jī)器人具有更為優(yōu)異的環(huán)境適應(yīng)能力。設(shè)計性能優(yōu)異的步行機(jī)器人，幾乎可以在各種路況的地面行走[3]。

四足機(jī)器人作為步行機(jī)器人中的一種，與雙足、六足機(jī)器人相比，具有更多優(yōu)勢。雙足機(jī)器人運(yùn)動穩(wěn)定性較差，六足機(jī)器人運(yùn)動速度較慢，而四足機(jī)器人兼具運(yùn)動穩(wěn)定性[4]、快速性[5]，以及較強(qiáng)的負(fù)載能力[6]等優(yōu)勢。因此，本文選擇四足機(jī)器人作為研究目標(biāo)。

目前，一些知名的研究機(jī)構(gòu)已經(jīng)研究出很多性能優(yōu)異的四足機(jī)器人[4-6]。例如：波士頓動力學(xué)工程公司研究開發(fā)了Big dog[7]、Spot 和Spot Mini[8]，麻省理工學(xué)院研發(fā)了MIT Cheetah Mini[9]，瑞士蘇黎世聯(lián)邦理工大學(xué)機(jī)器人系統(tǒng)實(shí)驗(yàn)室向外界展示了其開發(fā)的狗型機(jī)器人ANYmal[10]等。已有對四足機(jī)器人的研究工作，主要集中在結(jié)構(gòu)設(shè)計、軌跡規(guī)劃、控制策略等方面[11]。而軌跡規(guī)劃是機(jī)器人運(yùn)動研究中最關(guān)鍵的環(huán)節(jié)，因此機(jī)器人的使用離不開軌跡規(guī)劃[12]。不管是沒有環(huán)境感知能力的機(jī)器人按照既定的軌跡規(guī)劃進(jìn)行作業(yè)，還是能夠獲取環(huán)境信息、進(jìn)行自律控制的機(jī)器人作業(yè)，都需要提前做好軌跡規(guī)劃[13]。李貽斌等[14]提出了一種Trot 步態(tài)軌跡，并將其在液壓驅(qū)動四足機(jī)器人上實(shí)現(xiàn)。張培強(qiáng)等[15]采用調(diào)整腿長的方式，對四足機(jī)器人Trot 步態(tài)進(jìn)行了優(yōu)化。馬慧姝等[16]采用Kinect 攝像機(jī)采集犬類動物落足點(diǎn)，并用3 次樣條曲線擬合，對步態(tài)進(jìn)行軌跡規(guī)劃。馬宗利等[17]采用調(diào)整質(zhì)心位置的方法，提高了四足機(jī)器人坡面運(yùn)動的穩(wěn)定性。然而以上研究均未考慮四足機(jī)器人足端初始位置對Trot 步態(tài)穩(wěn)定性的影響。

基于此，本研究針對四足機(jī)器人的軌跡規(guī)劃問題展開研究，在運(yùn)動學(xué)分析的基礎(chǔ)上，從足端軌跡優(yōu)化、初始足端位置選擇這兩個方面對四足機(jī)器人的Trot步態(tài)進(jìn)行規(guī)劃，并用Matlab/Simulink 仿真平臺驗(yàn)證此步態(tài)規(guī)劃的合理性。從而實(shí)現(xiàn)四足機(jī)器人以Trot步態(tài)相對穩(wěn)定地行走。

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

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

四足機(jī)器人腿部結(jié)構(gòu)一般分為4 種方式：“全肘式”、“全膝式”、“前膝后肘式”和“前肘后膝式”。為了滿足運(yùn)動需求，一般四足機(jī)器人每條腿至少需要3 個自由度。

本文所研究的四足機(jī)器人采用的是“全肘式”結(jié)構(gòu)，每條腿具有3 個旋轉(zhuǎn)自由度關(guān)節(jié)，結(jié)構(gòu)示意圖如圖1 所示。

圖1 四足機(jī)器人結(jié)構(gòu)示意圖Fig.1 Schematic diagram of quadruped robot structure

2.2 四足機(jī)器人正向運(yùn)動學(xué)方程

采用改進(jìn)D-H 法建立四足機(jī)器人的運(yùn)動學(xué)模型，其示意圖如圖2 所示。

圖2 四足機(jī)器人的運(yùn)動學(xué)模型示意圖Fig.2 Kinematic model of quadruped robots

在圖2 中坐標(biāo)系皆為笛卡爾直角坐標(biāo)系，3 個坐標(biāo)軸下標(biāo)為坐標(biāo)系名稱。坐標(biāo)系{W}為世界坐標(biāo)系，其坐標(biāo)原點(diǎn)為OW，3 個坐標(biāo)軸分別為xW，yW，zW。坐標(biāo)系{B}為機(jī)體坐標(biāo)系，建立在機(jī)體質(zhì)心處，其坐標(biāo)原點(diǎn)為OB，3 個坐標(biāo)軸分別為xB，yB，zB。坐標(biāo)系{0}、{1}、{2}分別表示側(cè)擺、大腿、小腿坐標(biāo)系。坐標(biāo)系{3}為足端坐標(biāo)系。θ1、θ2、θ3分別為3 個關(guān)節(jié)的轉(zhuǎn)動角度。Lx為機(jī)體前腿和后腿的關(guān)節(jié)1 沿xB軸距離，Ly為左右腿關(guān)節(jié)1 沿yB軸距離。四足機(jī)器人前左、前右、后左、后右4 條腿分別用FL、FR、BL、BR 表示。L1、L2、L3代表側(cè)擺、大腿、小腿連桿的長度，其中L1=76 mm，L2=210 mm，L3=210 mm。由于4 條腿坐標(biāo)系的建立方法大致相同，圖中僅標(biāo)出前右腿（FR）的坐標(biāo)系，其D-H 參數(shù)見表1。

表1 四足機(jī)器人前右腿（FR）的D-H 參數(shù)Table 1 FR leg D-H parameters of quadruped robots

根據(jù)表1 所示的D-H 參數(shù)可求得足端正向運(yùn)動學(xué)方程為

2.3 四足機(jī)器人逆向運(yùn)動學(xué)方程

四足機(jī)器人逆向運(yùn)動學(xué)的簡化參數(shù)示意圖如圖3所示。

圖3 FR 腿的逆向運(yùn)動學(xué)參數(shù)示意圖Fig.3 FR leg inverse kinematics parameters of quadruped robots

四足機(jī)器人的逆向運(yùn)動學(xué)求解是軌跡規(guī)劃、運(yùn)動控制以及其他相關(guān)研究的基礎(chǔ)。常用的方法有解析法、幾何法、迭代法等。本研究根據(jù)四足機(jī)器人的運(yùn)動需求，采用幾何法求解逆向運(yùn)動學(xué)方程。同正向運(yùn)動學(xué)方程類似，四足機(jī)器人的4 條腿逆向運(yùn)動學(xué)方程求解選取前右腿（FR）為例，以此說明逆向運(yùn)動學(xué)方程的求解過程，所用到的參數(shù)已在圖3a、b 兩個平面標(biāo)出。

由圖3a 可知，在yBOBzB平面內(nèi)：

如圖3b 所示，在切于關(guān)節(jié)2、關(guān)節(jié)3 的A-A 視圖平面內(nèi)：

由方程（2）～（15）可得到FR 腿逆向運(yùn)動學(xué)方程為

3 四足機(jī)器人軌跡規(guī)劃

四足動物小跑過程中，同時抬起、放下前左、后右腿（或前右、后左腿）每條腿都會在支撐相和擺動相之間來回切換，以此形成對角Trot 步態(tài)，如圖4所示。

圖4 對角步態(tài)示意圖Fig.4 Diagram of diagonal Trot gaits

本研究中，四足機(jī)器人采取Trot 步態(tài)，同時抬起、放下相對的兩條腿，瞬時會有少于3 條腿支撐，沒有穩(wěn)定的支撐域，屬于動步態(tài)的一種。

3.1 足端軌跡規(guī)劃原則及約束

為滿足實(shí)際使用需求，需減小四足機(jī)器人腿部在支撐相和擺動相之間切換時的足端慣性力以及與地面的沖擊力。因此本研究以足端沖擊力為0 的原則，即將足端狀態(tài)定為理想狀態(tài)，與地面接觸時，應(yīng)該滿足足端速度與加速度都為0。以FR 腿為例，建立坐標(biāo)系{D}描述足端位置參數(shù)，如圖5 所示。該圖以關(guān)節(jié)1為坐標(biāo)原點(diǎn)，機(jī)器人前進(jìn)方向?yàn)閤軸，豎直向上為z軸。

圖5 足端位置參數(shù)示意圖Fig.5 Schematic diagram of parameters of position of the extremities

機(jī)器人對角步態(tài)的時序圖如圖6 所示，設(shè)機(jī)器人初始機(jī)體質(zhì)心高度為h。設(shè)步態(tài)周期為T，步長為S，抬腿高度為H，騰空率（理論擺動相占整個運(yùn)動周期比率）為λ。

圖6 對角步態(tài)時序圖Fig.6 Trot gait timing diagram

設(shè)機(jī)器人足端初始位置為（x0，y0，z0）。由于機(jī)器人在y軸方向無運(yùn)動，所以y0=-L1。顯然z軸足端初始位置的絕對值等于初始機(jī)體質(zhì)心高度，即z0=-h。x0值的設(shè)定將對機(jī)器人運(yùn)動的穩(wěn)定性能有較大影響，后續(xù)仿真分析時將對x0值的設(shè)定進(jìn)行詳細(xì)分析。

由于對角步態(tài)向前運(yùn)動時關(guān)節(jié)1 不動，即θ1≡ 0，y(t)≡-L1。只需給出x軸和z軸方向上的約束。根據(jù)上述原則，得到每個周期T內(nèi)，前進(jìn)方向足端約束：

豎直方向足端約束：

3.2 足端軌跡函數(shù)

基于約束方程（17）～（23），擺動相足端軌跡擬采用復(fù)合擺線的形式。參考文獻(xiàn)[14]給出的擺動相足端軌跡為

由足端軌跡求導(dǎo)計算出速度、加速度曲線可知：在z軸方向，t=0，t=λT時，加速度不符合約束方程（19）、（21），所以需要對z軸方向軌跡進(jìn)行優(yōu)化。優(yōu)化后的擺動相足端軌跡函數(shù)如下：

得出擺動相足端軌跡后，根據(jù)約束方程，容易得到支撐相足端軌跡方程為

可根據(jù)實(shí)際情況調(diào)整步態(tài)周期T、步長S、抬腿高度H。實(shí)際使用時，足端軌跡函數(shù)還需加上足端初始位置（x0，y0，z0）的設(shè)定值。

4 Simulink 仿真分析

4.1 三維實(shí)體模型的建立

以優(yōu)寶特“YOBOGO”型四足機(jī)器人為物理模型，根據(jù)其實(shí)際尺寸和質(zhì)量，結(jié)合正向運(yùn)動學(xué)公式（1），在SolidWorks 軟件中建立三維實(shí)體模型，見圖1。用基于SolidWorks 的SW URDF Exporter 插件導(dǎo)出URDF（unified robot description format）文件，該文件是一種機(jī)器人描述文件，在機(jī)器人系統(tǒng)里大量使用。也可用于各種仿真平臺內(nèi)搭建機(jī)器人仿真模型。

4.2 Simulink 仿真模型的建立

將上一步生成的URDF 文件導(dǎo)入Matlab/Simulink仿真軟件中，并將足端軌跡函數(shù)，式（25）～（27），輸入相應(yīng)的Simulink 模塊中，生成足端軌跡信號；并結(jié)合上文中給出的逆向運(yùn)動學(xué)方程式（16）與Simulink 模塊解出的關(guān)節(jié)角度信號，從而對機(jī)器人的實(shí)體模型進(jìn)行仿真控制。

Simulink 仿真運(yùn)動過程如圖7 所示。圖7a 為一個周期T=0.500 s 的運(yùn)動。t=0 s 時為四足機(jī)器人初始狀態(tài)，t=0.125 s 為一組擺動相運(yùn)動狀態(tài)；t=0.250 s 擺動相與支撐相切換；t=0.375 s 為另一組擺動相運(yùn)動狀態(tài)。圖7b 為4 個周期的運(yùn)動。t=0.5, 1.0, 1.5, 2.0 s 分別為4 個周期的初始狀態(tài)。

圖7 Simulink 仿真運(yùn)動過程示意圖Fig.7 Diagram of simulink simulation movement

4.3 仿真條件設(shè)置

為了分別驗(yàn)證四足機(jī)器人足端軌跡、足端初始位置對運(yùn)動穩(wěn)定性的影響，本研究進(jìn)行了如下兩組對比仿真實(shí)驗(yàn)。

實(shí)驗(yàn)一：設(shè)定變量為足端軌跡，選取文獻(xiàn)[14]中給出的足端軌跡函數(shù)為優(yōu)化前軌跡和優(yōu)化后的足端軌跡，以此形成對比。

實(shí)驗(yàn)二：采用優(yōu)化后的足端軌跡函數(shù)，設(shè)定變量為足端初始位置。

四足機(jī)器人由于自身機(jī)體、腿部的重力慣性等因素，當(dāng)進(jìn)行對角步態(tài)行走，抬起對角兩條腿時，會向另一對角傾斜，造成對角步態(tài)兩條腿不能同時著地，從而增大足端與地面沖擊力造成運(yùn)動的不穩(wěn)定性。而初始足端位置的選擇將極大影響對角傾斜方向和大小。所以這里對四足機(jī)器人對角步態(tài)的足端初始位置進(jìn)行仿真分析。四足機(jī)器人向前運(yùn)動時，x軸方向足端初始位置對機(jī)器人穩(wěn)定性影響最大，y軸方向可不動，z軸方向決定的是機(jī)體質(zhì)心高度，需根據(jù)實(shí)際使用需求選取。所以本文主要對x軸方向足端初始位置變化進(jìn)行仿真對比。

實(shí)驗(yàn)中需設(shè)定的參數(shù)有：周期T，步長S，抬腿高度H，騰空率λ，足端初始位置x0、y0、z0。結(jié)合四足機(jī)器人腿部連桿長度參數(shù)：L1=76 mm，L2=210 mm，L3=210 mm。兩組對比實(shí)驗(yàn)中均設(shè)定：周期T=0.5 s，步長S=20 mm，抬腿高度為10 mm，騰空率λ=0.5（即FL、BR 腿運(yùn)動周期與FR、BL 腿運(yùn)動周期相位差為0.5T）。實(shí)驗(yàn)一足端初始位置為x0=-40 mm，y0=-76 mm，z0=-200 mm。實(shí)驗(yàn)二足端初始位置變化范圍為-70～0 mm，每間隔10 mm 取值進(jìn)行仿真，一共仿真8 次，y0=-76 mm，z0=-200 mm。

實(shí)驗(yàn)中四足機(jī)器人先邁FL、BR 腿。每個實(shí)驗(yàn)均仿真5 s，10 個周期。

4.4 仿真結(jié)果分析

兩組對比實(shí)驗(yàn)分別從足端沖擊力F，機(jī)體質(zhì)心位移x、y、z，機(jī)體俯仰角（pitch）φ、滾動角（roll）ω、偏航角（yaw）κ等具體數(shù)值來評價四足機(jī)器人的運(yùn)動穩(wěn)定性能。仿真在0 s 時刻時，機(jī)器人此時是懸空的，距離地面有微小高度，以及機(jī)器人本身慣性所致，前幾個運(yùn)動周期會有不規(guī)律的現(xiàn)象。因此，以下數(shù)據(jù)分析皆不考慮前3 個不規(guī)律周期。

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

在實(shí)驗(yàn)一中，對比的是優(yōu)化前和優(yōu)化后足端軌跡對四足機(jī)器人運(yùn)動穩(wěn)定性的影響。影響結(jié)果如圖8 所示，由圖8 可以看出，優(yōu)化后軌跡的足端沖擊力小于優(yōu)化前軌跡的足端沖擊力。

圖8 優(yōu)化前后實(shí)驗(yàn)一足端沖擊力變化曲線Fig.8 Impact force curves at the extremity of the first experiment of before and after optimization

圖9 為機(jī)體的質(zhì)心位移曲線，從圖9 可以看出，5 s 仿真時間內(nèi)：在x軸方向，優(yōu)化后前進(jìn)更遠(yuǎn)；在y軸方向，優(yōu)化前有較大偏移位移，優(yōu)化后y軸偏移較小，前進(jìn)方向更準(zhǔn)確；在z軸方向，優(yōu)化前后差距不大。

圖9 優(yōu)化前后實(shí)驗(yàn)一機(jī)體質(zhì)心的位移變化曲線Fig.9 Mass center displacement curves of body in the first experiment of before and after optimization

機(jī)體質(zhì)心位移的變化角度如圖10 所示，根據(jù)圖10 歐拉角的變化曲線可以知道，優(yōu)化前俯仰角φ、滾動角ω都比優(yōu)化后的波動大。優(yōu)化后的偏航角κ比優(yōu)化前更小。

圖10 優(yōu)化前后實(shí)驗(yàn)一機(jī)體質(zhì)心歐拉角的變化曲線Fig.10 Mass center Euler angle variation curves of body in the first experiment of before and after optimization

綜上對實(shí)驗(yàn)一優(yōu)化前、后軌跡的各種仿真曲線的分析，可以看出優(yōu)化后足端與地面沖擊力較小，機(jī)體俯仰角、滾動角較小，運(yùn)動較為穩(wěn)定，優(yōu)化后偏航角也變小，前進(jìn)方向更準(zhǔn)確。

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

在實(shí)驗(yàn)二中，對比的是足端初始位置x軸方向的變化對四足機(jī)器人運(yùn)動穩(wěn)定性能的影響。四足機(jī)器人穩(wěn)定向前運(yùn)動時，足端沖擊力F越小越好，x軸方向位移越大越好，y軸方向位移也是更小為宜。故上述3 個評價標(biāo)準(zhǔn)給出其仿真數(shù)值變化的最大值，見表2。分析表2 的數(shù)據(jù)可知，當(dāng)x0=-40 mm 時，足端沖擊力F最小，為19.9 N；x軸方向的位移最大，為0.402 m；y方向的偏移最小，為-1.32×10-2m。

表2 實(shí)驗(yàn)二F、x、y 隨x0變化的最大值Table 2 Maximum change of F, x and y in the second experiment as a function of x0

而z軸方向有地面作為約束，理論位移變化越小越好。同理，俯仰角φ、滾動角ω、偏航角κ亦是變化越小，四足機(jī)器人運(yùn)動越穩(wěn)定。因此，本研究給出上述評價標(biāo)準(zhǔn)的仿真數(shù)值變化的方差，如表3 所示。結(jié)合表2 和表3 可知，當(dāng)x0=-40 mm 時，φ、ω的方差最小；當(dāng)x0=-30 mm 時，z、κ的方差最小。

表3 實(shí)驗(yàn)二中z、φ、ω、κ 的方差Table 3 Variance of z, φ, ω and κ in the second experiment

在四足機(jī)器人的對角步態(tài)仿真過程中，根據(jù)仿真視圖可以看出：當(dāng)x0≥-40 mm 時，四足機(jī)器人有向后傾倒的趨勢，后腿先于前腿著地。x0≤-50 mm時有向前傾倒的趨勢，前腿先于后腿著地。機(jī)器人前腿先著地時明顯不如后腿先著地運(yùn)動穩(wěn)定。結(jié)合上述7 項(xiàng)數(shù)值的對比，可以看出當(dāng)x0=-40 mm 時，各項(xiàng)數(shù)據(jù)都較為理想。由于篇幅限制，這里僅列出x0=-40 mm 以及x0=0 mm 兩次仿真數(shù)值的變化曲線，詳細(xì)對比不同初始足端位置對機(jī)器人穩(wěn)定性的影響。以下將足端初始位置x0=-40 mm 稱為位置一；x0=0 mm 稱為位置二，仿真結(jié)果如圖11～13 所示。

圖11 實(shí)驗(yàn)二不同初始位置下足端沖擊力變化曲線Fig.11 Impact force variation curves at the extremity of the second experiment at different initial positions

圖12 實(shí)驗(yàn)二不同初始位置下機(jī)體質(zhì)心的位移變化曲線Fig.12 Mass center displacement variation curves of body in the second experiment at different initial positions

圖13 實(shí)驗(yàn)二不同初始位置下機(jī)體歐拉角變化曲線Fig.13 Euler angle variation curves of body in the second experiment at different initial positions

由圖11 可知，位置一的足端沖擊力較小。由圖12 可知：x軸方向，位置一的位移較遠(yuǎn)；y軸方向，位置一的波動較?。晃恢枚邢騳軸負(fù)方向位移的趨勢；z軸方向，兩位置波動差距相當(dāng)。由圖13 可知，位置一的角變化較小，較為穩(wěn)定，而位置二俯仰角φ、滾動角ω都波動較大，航偏角κ也有變大趨勢。

綜合實(shí)驗(yàn)二對四足機(jī)器人足端初始選擇的仿真分析可得：當(dāng)x0=-40 mm 時，足端與地面沖擊力較小，機(jī)器人前進(jìn)較遠(yuǎn)，機(jī)體俯仰角、滾動角波動較小，航偏角也較小。因此，此時四足機(jī)器人具有較優(yōu)異的性能，運(yùn)動較為穩(wěn)定，前進(jìn)方向較為準(zhǔn)確。

5 結(jié)語

通過建立四足機(jī)器人正向、逆向運(yùn)動學(xué)模型、基于足端零沖擊原則進(jìn)行足端軌跡規(guī)劃，實(shí)現(xiàn)了四足機(jī)器人對角步態(tài)的運(yùn)動；并且從足端軌跡函數(shù)、足端初始位置兩方面去優(yōu)化四足機(jī)器人的對角步態(tài)；采用Matlab/Simulink 仿真實(shí)驗(yàn)分析足端軌跡、足端初始位置對機(jī)器人運(yùn)動穩(wěn)定性的影響。仿真實(shí)驗(yàn)結(jié)果表明，從足端軌跡、足端初始位置兩方面對機(jī)器人的對角步態(tài)進(jìn)行優(yōu)化規(guī)劃，使得機(jī)器人在運(yùn)動過程中的穩(wěn)定性能有較大的提高，實(shí)現(xiàn)了四足機(jī)器人有較為穩(wěn)定的對角步態(tài)行走。