基于Simulink/SimMechanics的四足機(jī)器人足端軌跡規(guī)劃及動(dòng)態(tài)仿真分析

黃照翔，頡潭成,2*，徐彥偉,2，王亞鋒

（1.河南科技大學(xué) 機(jī)電工程學(xué)院，洛陽(yáng) 471003；2.智能數(shù)控裝備河南省工程實(shí)驗(yàn)室，洛陽(yáng) 471003）

0 引言

四足機(jī)器人具有良好的運(yùn)動(dòng)性能和環(huán)境適應(yīng)性，在復(fù)雜多變的路況能穩(wěn)定運(yùn)動(dòng)，被廣泛應(yīng)用于軍事、救援、環(huán)境探測(cè)等領(lǐng)域[1]，因此對(duì)四足機(jī)器人步態(tài)運(yùn)動(dòng)規(guī)劃，足端軌跡規(guī)劃的研究成為當(dāng)今眾多學(xué)者研究的熱點(diǎn)，而足端軌跡規(guī)劃的好壞直接影響了四足機(jī)器人運(yùn)動(dòng)的平穩(wěn)性，機(jī)構(gòu)的使用壽命等。目前應(yīng)用比較廣泛的足端軌跡規(guī)劃函數(shù)有多項(xiàng)式運(yùn)動(dòng)軌跡、橢圓運(yùn)動(dòng)函數(shù)、擺線運(yùn)動(dòng)函數(shù)等[2]，為了足端軌跡能更好的適應(yīng)環(huán)境變化，很多研究者都會(huì)對(duì)這些函數(shù)進(jìn)行改進(jìn)。例如，日本學(xué)者Yoshihiro SAKAKIBARA[3]等人對(duì)正弦函數(shù)存在的缺點(diǎn)進(jìn)行改進(jìn)，提出了在前進(jìn)方向和豎直方向上的加速度采用正弦函數(shù)的方法；韓國(guó)漢陽(yáng)大學(xué)的Kyeong YongKim[4]等人把橢圓運(yùn)動(dòng)規(guī)律作為足端運(yùn)動(dòng)的軌跡，并且可以通過(guò)改變橢圓的短軸，長(zhǎng)軸以及中心點(diǎn)的位置來(lái)改變橢圓的形狀使足端軌跡適應(yīng)不同的環(huán)境；北京理工大學(xué)的王立鵬提出了一種零沖擊軌跡規(guī)劃方法[5]，該方法采用復(fù)合擺線的方式對(duì)足端進(jìn)行軌跡規(guī)劃，保證了足端位移函數(shù)和速度函數(shù)的連續(xù)性、可導(dǎo)性，使足端在落地時(shí)的速度和加速度為零；山東大學(xué)提出了一種多項(xiàng)式足端運(yùn)動(dòng)軌跡[6]，在擺動(dòng)相和支撐相都采用三次多項(xiàng)式作為軌跡函數(shù)；上海交通大學(xué)的何東青對(duì)修正擺線足端軌跡進(jìn)行了改進(jìn)[7]，將足端擺動(dòng)相的運(yùn)動(dòng)軌跡劃分三個(gè)階段抬腿、跨步、落足，為了使跨步階段的速度恒定，將該階段的加速度設(shè)為零，同時(shí)保證抬腿和跨步兩個(gè)階段的加速度為正弦函數(shù)。為了使四足機(jī)器人在行走過(guò)程中足端運(yùn)動(dòng)更加平穩(wěn)，本文將運(yùn)用改進(jìn)型等速運(yùn)動(dòng)規(guī)律與疊加擺線運(yùn)動(dòng)規(guī)律復(fù)合的方法對(duì)四足機(jī)器人的足端進(jìn)行軌跡規(guī)劃，并在建立的四足機(jī)器人動(dòng)態(tài)模型中分析其有效性和可行性。

1 足端軌跡規(guī)劃

對(duì)四足機(jī)器人的足端軌跡進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃，首先要明白四足機(jī)器人步態(tài)行走的過(guò)程，即四足機(jī)器人各腿之間在擺動(dòng)相和支撐相來(lái)回切換的一個(gè)行進(jìn)過(guò)程。支撐相和擺動(dòng)相是描述單腿運(yùn)動(dòng)的兩個(gè)狀態(tài)[8]，支撐相是指四足機(jī)器人的單腿足端與地面相接觸，承載著機(jī)器人的負(fù)載，并且通過(guò)關(guān)節(jié)相位變化推動(dòng)機(jī)器人的重心在前進(jìn)方向移動(dòng)。擺動(dòng)相是指單腿足端從開(kāi)始離開(kāi)地面邁腿后落地的一個(gè)狀態(tài)，而擺動(dòng)相決定著機(jī)器人在單個(gè)步態(tài)時(shí)抬腿的高度和步距的大小。在擺動(dòng)相和支撐相來(lái)回切換時(shí)，處于擺動(dòng)相的足端在落地時(shí)會(huì)與地面產(chǎn)生一定的沖擊造成機(jī)體的振動(dòng)，所以對(duì)足端進(jìn)行軌跡規(guī)劃主要是對(duì)擺動(dòng)相進(jìn)行無(wú)沖擊的軌跡規(guī)劃設(shè)計(jì)[9]。對(duì)支撐相進(jìn)行軌跡規(guī)劃時(shí)我們應(yīng)盡量使機(jī)身與地面保持水平，避免軀干起伏，為了同時(shí)能夠保證足端與地面之間沒(méi)有相對(duì)滑動(dòng)的趨勢(shì)，將足端軌跡采用勻速直線運(yùn)動(dòng)[10]，從而完成機(jī)體的向前移動(dòng)。四足機(jī)器人單腿在一個(gè)步態(tài)周期內(nèi)的足端軌跡規(guī)劃運(yùn)動(dòng)過(guò)程如圖1所示。

圖1 單腿足端軌跡運(yùn)動(dòng)過(guò)程圖

1.1 改進(jìn)型等速運(yùn)動(dòng)規(guī)律規(guī)劃

改進(jìn)型等速運(yùn)動(dòng)規(guī)律是一種組合型運(yùn)動(dòng)規(guī)律，就是幾種不同運(yùn)動(dòng)規(guī)律組合在一起而設(shè)計(jì)出的運(yùn)動(dòng)規(guī)律，組合時(shí)不同的曲線在拼接處必須保持連續(xù)，即拼接處的位移，速度，加速度要相同。改進(jìn)型等速運(yùn)動(dòng)規(guī)律是在等速運(yùn)動(dòng)規(guī)律的基礎(chǔ)上，為了保證等速運(yùn)動(dòng)的同時(shí)速度又不會(huì)產(chǎn)生突變，將正弦運(yùn)動(dòng)規(guī)律與等速運(yùn)動(dòng)規(guī)律組合而成。這種方法通常是在等速運(yùn)動(dòng)曲線的1/4周期和3/4周期處拼接半個(gè)周期的正弦加速度運(yùn)動(dòng)曲線，這樣可以使兩個(gè)曲線在拼接處的加速度為零，防止速度和位移產(chǎn)生突變，這種組合方式是一種無(wú)剛性和柔性沖擊的運(yùn)動(dòng)規(guī)律?；诖宋覀兛梢缘贸龈倪M(jìn)型等速運(yùn)動(dòng)規(guī)律的位移曲線；

有邊界條件t=0時(shí)S=0且t=T時(shí)S=S0，可以求得待定系數(shù)A的值：

式中t∈[0，T]，T為運(yùn)動(dòng)周期，為運(yùn)動(dòng)行程，有式(1)～式(3)可以得出改進(jìn)型等加速運(yùn)動(dòng)規(guī)律的位移曲線圖，如圖2所示。速度曲線圖，如圖3所示。加速度曲線圖，如圖4所示。

圖2 位移曲線圖

圖3 速度曲線圖

圖4 加速度曲線圖

1.2 疊加擺線運(yùn)動(dòng)規(guī)律規(guī)劃

疊加擺線運(yùn)動(dòng)規(guī)律是將兩條不同周期的擺線運(yùn)動(dòng)進(jìn)行疊加形成的運(yùn)動(dòng)規(guī)律，疊加形成的擺線與擺線運(yùn)動(dòng)規(guī)律相比速度曲線中間段為等速，運(yùn)動(dòng)更加平穩(wěn)，加減速的運(yùn)動(dòng)效率更高，并且沒(méi)有剛性和柔性沖擊。擺線運(yùn)動(dòng)規(guī)律的位移曲線為S0(t)，則位移如式(4)所示：

設(shè)兩條相加的擺線位移曲線為：S1(t)，S2(t)。

疊加擺線運(yùn)動(dòng)規(guī)律位移曲線為S(t)則：

式(7)中t∈[0，T]，T為運(yùn)動(dòng)周期，a，b為兩條疊加擺線的系數(shù)，有式(7)可得疊加后的擺線位移曲線圖，如圖5所示；速度曲線圖，如圖6所示；加速度曲線圖，如圖7所示。

圖5 疊加擺線位移曲線圖

圖6 疊加擺線速度曲線圖

圖7 疊加擺線速度曲線圖

1.3 復(fù)合運(yùn)動(dòng)軌跡規(guī)劃

四足機(jī)器人的行走軌跡由單腿足端擺動(dòng)相軌跡和支撐相軌跡組成，對(duì)足端軌跡實(shí)現(xiàn)規(guī)劃我們首先要確定軌跡的起始點(diǎn)，最高點(diǎn)，落足點(diǎn)的信息，然后運(yùn)用復(fù)合運(yùn)動(dòng)合成軌跡。為了使機(jī)器人行走穩(wěn)定在水平方向上使用改進(jìn)型等速運(yùn)動(dòng)規(guī)律，運(yùn)動(dòng)周期為T(mén)，在豎直方向上的抬腿階段和落地階段使用疊加擺線運(yùn)動(dòng)規(guī)律，運(yùn)動(dòng)周期分別為T(mén)/2。由改進(jìn)型等速運(yùn)動(dòng)規(guī)律和疊加擺線運(yùn)動(dòng)規(guī)律復(fù)合形成足端擺動(dòng)相的運(yùn)動(dòng)軌跡，以勻速直線運(yùn)動(dòng)作為足端支撐相運(yùn)動(dòng)軌跡，足端軌跡如圖8所示。

圖8 足端軌跡位移曲線圖

2 動(dòng)態(tài)仿真

為了驗(yàn)證本文足端軌跡規(guī)劃方法的可行性，在MATLAB軟件的Simulink中建立四足機(jī)器人的動(dòng)態(tài)仿真模型。四足機(jī)器人模型實(shí)現(xiàn)動(dòng)態(tài)行走目前主要依賴于運(yùn)動(dòng)學(xué)關(guān)系的實(shí)現(xiàn)[11]，其次是機(jī)器人足端與地面接觸力的模型設(shè)計(jì)。所以首先要建立單腿的運(yùn)動(dòng)學(xué)模型，通過(guò)幾何解析法推導(dǎo)正逆運(yùn)動(dòng)學(xué)方程，然后將規(guī)劃好的足端軌跡坐標(biāo)函數(shù)通過(guò)逆運(yùn)動(dòng)學(xué)方程得出每個(gè)位置坐標(biāo)點(diǎn)對(duì)應(yīng)的髖關(guān)節(jié)和膝關(guān)節(jié)的關(guān)節(jié)角度變量，將這些關(guān)節(jié)角度變量作為仿真模型的驅(qū)動(dòng)變量實(shí)現(xiàn)足端軌跡行走，軌跡仿真模塊如圖9所示。足端與地面接觸力模型的建立采用External Force and Torque模塊，該模塊代表一個(gè)外部機(jī)構(gòu)施加在剛體上的一般力和力矩，而地面對(duì)足端的接觸力是一種阻尼力其大小與足端位移速度以及阻尼系數(shù)相關(guān)，足端與地面接觸力模型如圖10所示。

圖9 軌跡模塊仿真圖

圖10 足端與地面接觸力模型仿真圖

在動(dòng)態(tài)模型建立的基礎(chǔ)上，以四足機(jī)器人的Trot步態(tài)為例，分別對(duì)改進(jìn)型等速運(yùn)動(dòng)規(guī)律與疊加擺線運(yùn)動(dòng)規(guī)律復(fù)合形成的足端運(yùn)動(dòng)軌跡和復(fù)合擺線足端運(yùn)動(dòng)軌跡進(jìn)行仿真，對(duì)比分析這兩種足端軌跡在直角坐標(biāo)空間和關(guān)節(jié)空間的運(yùn)動(dòng)特性，動(dòng)態(tài)仿真模型如圖11所示。

圖11 四足機(jī)器人動(dòng)態(tài)仿真圖

2.1 直角坐標(biāo)空間足端軌跡運(yùn)動(dòng)仿真

由仿真得到四足機(jī)器人在直角坐標(biāo)空間下的運(yùn)動(dòng)曲線，在基于改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合而成的軌跡規(guī)劃方法下，其足端速度，加速度曲線，如圖12(a)、圖13(a)所示。在基于復(fù)合擺線運(yùn)動(dòng)規(guī)律的軌跡規(guī)劃方法下，其足端速度，加速度，如圖12(b)、圖13(b)所示。

圖12 速度曲線

圖13 加速度曲線

2.2 關(guān)節(jié)坐標(biāo)空間足端軌跡運(yùn)動(dòng)仿真

通過(guò)仿真得到四足機(jī)器人在關(guān)節(jié)坐標(biāo)空間的運(yùn)動(dòng)曲線，在基于改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合而成的軌跡規(guī)劃方法下，其髖關(guān)節(jié)和膝關(guān)節(jié)的角度變化曲線如圖14(a)所示，角速度曲線如圖15(a)所示，角加速度曲線如圖16(a)所示。在基于復(fù)合擺線運(yùn)動(dòng)規(guī)律的軌跡規(guī)劃方法下，其髖關(guān)節(jié)和膝關(guān)節(jié)的角度變化曲線如圖14(b)所示，角速度曲線如圖15(b)所示，角加速度曲線如圖16(b)所示。

圖14 關(guān)節(jié)角位移曲線圖

圖15 關(guān)節(jié)角速度曲線圖

圖16 關(guān)節(jié)角加速度曲線圖

3 對(duì)比分析

由圖12足端速度曲線可知，兩種復(fù)合運(yùn)動(dòng)軌跡的速度曲線都相對(duì)平滑，自然，沒(méi)有尖點(diǎn)出現(xiàn)。而改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的速度曲線與復(fù)合擺線形成的速度曲線相比，其速度峰值有所降低，速度變化的波動(dòng)范圍有所下降，而且在水平方向和豎直方向上都有較長(zhǎng)的勻速運(yùn)動(dòng)階段，在前進(jìn)和抬腿方向運(yùn)動(dòng)更加平穩(wěn)。由圖13足端加速度曲線可知，兩條復(fù)合運(yùn)動(dòng)軌跡的加速度曲線在峰值上并無(wú)明顯的變化，但改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的加速度曲線與復(fù)合擺線形成的加速度曲線相比，其加減速時(shí)間明顯縮短，加減速過(guò)程更快，運(yùn)動(dòng)效率較高。有表一可得，復(fù)合運(yùn)動(dòng)軌跡在水平方向和豎直方向的速度峰值均減少了30%左右?？傮w來(lái)說(shuō)基于改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的足端軌跡在水平與豎直方向都能長(zhǎng)時(shí)間保持勻速，而且速度峰值有所降低，加減速效率更高，運(yùn)行較平穩(wěn)。

由圖14關(guān)節(jié)角位移曲線可知，兩種軌跡規(guī)劃方法下的角位移都相對(duì)平滑、自然，無(wú)劇烈波動(dòng)現(xiàn)象，關(guān)節(jié)角位移峰值無(wú)明顯差別。由圖15關(guān)節(jié)角速度曲線得，改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的角速度運(yùn)動(dòng)曲線與復(fù)合擺線的角速度運(yùn)動(dòng)曲線相比，峰值得到降低，而且峰值處變化比較平緩，無(wú)突變現(xiàn)象。由圖16關(guān)節(jié)角加速度曲線得，兩種軌跡的膝關(guān)節(jié)角加速度曲線走勢(shì)基本一樣，但改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的髖關(guān)節(jié)角加速度運(yùn)動(dòng)曲線與復(fù)合擺線的髖關(guān)節(jié)角加速度運(yùn)動(dòng)曲線相比，減速過(guò)程中角加速度峰值較大，減速過(guò)程較快。有表二可得，復(fù)合運(yùn)動(dòng)角速度降低13%左右，角加速度增加43%左右。總的來(lái)說(shuō)基于改進(jìn)型等加速運(yùn)動(dòng)與疊加擺線運(yùn)動(dòng)復(fù)合形成的軌跡與復(fù)合擺線軌跡規(guī)劃相比，在關(guān)節(jié)空間，其角速度曲線變化平穩(wěn)，加減速過(guò)程加快。

表1 直角坐標(biāo)空間運(yùn)動(dòng)狀態(tài)對(duì)比

表2 關(guān)節(jié)坐標(biāo)空間運(yùn)動(dòng)狀態(tài)對(duì)比

4 結(jié)語(yǔ)

本文利用改進(jìn)型等加速運(yùn)動(dòng)規(guī)律與疊加擺線運(yùn)動(dòng)規(guī)律復(fù)合運(yùn)動(dòng)對(duì)四足機(jī)器人足端軌跡進(jìn)行規(guī)劃，并在MATLAB的Simulink中建立四足機(jī)器人的動(dòng)態(tài)仿真模型，分析四足機(jī)器人足端在直角坐標(biāo)空間的軌跡曲線，以及膝關(guān)節(jié)和髖關(guān)節(jié)下的角度變化曲線。研究結(jié)果表明：1）改進(jìn)型等加速運(yùn)動(dòng)規(guī)律與疊加擺線運(yùn)動(dòng)規(guī)律復(fù)合形成的足端軌跡曲線在豎直方向與水平方向都有一段勻速運(yùn)動(dòng)階段，而且落地速度為零，使得整體運(yùn)動(dòng)更加平穩(wěn)。2）膝關(guān)節(jié)和髖關(guān)節(jié)的角度，角速度，角加速度變化曲線都相對(duì)光滑、連續(xù)、無(wú)尖點(diǎn)出現(xiàn)減小了運(yùn)動(dòng)過(guò)程中的振動(dòng)。