盧嘉敏，楊宜民，付根平

(廣東工業(yè)大學(xué)自動(dòng)化學(xué)院，廣東廣州510090)

雙足步行是仿人機(jī)器人實(shí)現(xiàn)其他功能的前提和基礎(chǔ)，是仿人機(jī)器人基本的特征和研究的熱點(diǎn).因?yàn)榉氯藱C(jī)器人具有結(jié)構(gòu)復(fù)雜、高階數(shù)、非線性、強(qiáng)耦合等特點(diǎn)，故采用傳統(tǒng)方法難以對(duì)其進(jìn)行良好的控制，所以必須對(duì)步態(tài)進(jìn)行優(yōu)化.文獻(xiàn)[1]假設(shè)步態(tài)的切換是瞬時(shí)進(jìn)行的，用被動(dòng)沖擊方程對(duì)切換過(guò)程進(jìn)行建模，實(shí)現(xiàn)了五連桿無(wú)足平面雙足機(jī)器人的能量最優(yōu)、力矩最優(yōu)的步行和跑步步態(tài).文獻(xiàn)[2]不考慮側(cè)向運(yùn)動(dòng)的影響，通過(guò)一種力矩最優(yōu)的步態(tài)優(yōu)化方法得到能量最優(yōu)的步態(tài).而文獻(xiàn)[3]在文獻(xiàn)[2]的基礎(chǔ)上通過(guò)增加穩(wěn)定性約束，得到了能耗最小的優(yōu)化步態(tài)，但仍在二維平面內(nèi)研究機(jī)器人的步態(tài)，不適合仿人機(jī)器人在三維空間的步態(tài)優(yōu)化.文獻(xiàn)[4]以關(guān)節(jié)力矩變化最小為目標(biāo)，優(yōu)化出穩(wěn)定的步態(tài)，但該方法過(guò)程較復(fù)雜，只是得到了力矩最小而不是ZMP穩(wěn)定裕度最大的步態(tài).

1 步態(tài)規(guī)劃及其穩(wěn)定性分析

1.1 基于樣條插值的步態(tài)規(guī)劃

仿人機(jī)器人步態(tài)可以分為起步、周期步行和止步3個(gè)階段.3個(gè)階段都要滿足無(wú)奇異位姿、ZMP穩(wěn)定性約束、運(yùn)動(dòng)協(xié)調(diào)性和關(guān)節(jié)角度范圍限制等約束條件[5-9]，故在起步階段需先讓仿人機(jī)器人的膝關(guān)節(jié)彎曲再執(zhí)行起步，起步完成后進(jìn)入周期步行階段，在進(jìn)入止步階段后需在雙足合攏時(shí)把雙膝伸直，恢復(fù)仿人機(jī)器人起步前的直立狀態(tài)，以此完成止步.在徑向和側(cè)向平面內(nèi)把仿人機(jī)器人簡(jiǎn)化成圖1所示的連桿模型，采用三次樣條插值方法[10]規(guī)劃出步態(tài)中的肩關(guān)節(jié)、踝關(guān)節(jié)和髖關(guān)節(jié)的運(yùn)動(dòng)軌跡，即θarm=f(θa，t)，xankle=f(xa，za，t)，xhip=f(dxs，dxe，t)和yhip=f(dys，dym，dye，t)(其中 θarm為手臂的肩關(guān)節(jié)擺動(dòng)角，xankle為踝關(guān)節(jié)的位置，xa和za為各關(guān)鍵時(shí)刻踝關(guān)節(jié)的位置，xhip、yhip為髖關(guān)節(jié)的徑向、側(cè)向位置，dxs、dxe分別為擺動(dòng)腳落地時(shí)和離地時(shí)髖關(guān)節(jié)到支撐腿踝關(guān)節(jié)的距離，dys、dym、dye分別為擺動(dòng)腳落地、離地和抬到最高點(diǎn)時(shí)髖關(guān)節(jié)中心在y軸方向上的坐標(biāo))，然后利用幾何關(guān)系由踝關(guān)節(jié)和髖關(guān)節(jié)的軌跡計(jì)算出膝關(guān)節(jié)的軌跡，最后通過(guò)逆運(yùn)動(dòng)學(xué)方程求得各關(guān)節(jié)的角度軌跡.

圖1 仿人機(jī)器人的連桿模型Fig.1 Link model of humanoid robot

1.2 腿部各關(guān)節(jié)對(duì)步態(tài)穩(wěn)定性的影響分析

仿人機(jī)器人的步態(tài)穩(wěn)定性由ZMP的穩(wěn)定裕度決定.ZMP的穩(wěn)定裕度是指機(jī)器人步行的ZMP值離有效穩(wěn)定區(qū)域邊緣的最小距離，ZMP的穩(wěn)定裕度越大，機(jī)器人的穩(wěn)定性能越好，即ZMP點(diǎn)越接近有效穩(wěn)定區(qū)域中間時(shí)機(jī)器人的步態(tài)越穩(wěn)定.

跟據(jù) ZMP 的計(jì)算公式[11]:

式中:mi為連桿i的質(zhì)量，xi、yi、zi為連桿i的質(zhì)心位置，g為重力加速度.由于ZMP的x分量和y分量類(lèi)似，為了避免重復(fù)，下文只分析ZMP的x分量.把各連桿位姿與關(guān)節(jié)角度的關(guān)系代入式(1)后，得到式(2)所示的ZMP的x分量表達(dá)式:

式中:li、di分別為連桿i的長(zhǎng)度和連桿質(zhì)心到相應(yīng)關(guān)節(jié)的距離，qi為連桿i與z方向的夾角.

由于仿人機(jī)器人的質(zhì)量主要集中在軀干，而髖關(guān)節(jié)是控制上半身軀干位姿的主要關(guān)節(jié)，所以髖關(guān)節(jié)對(duì)ZMP軌跡和機(jī)器人的步態(tài)穩(wěn)定性的影響很大.踝關(guān)節(jié)位姿是規(guī)劃仿人機(jī)器人落腳點(diǎn)、擺動(dòng)腳高度和調(diào)節(jié)機(jī)器人步速的關(guān)鍵關(guān)節(jié)，擺動(dòng)腳高度會(huì)影響到機(jī)器人的雙腳力矩和能耗，步速的快慢會(huì)直接影響到ZMP的變化速率，從而影響機(jī)器人的穩(wěn)定性.膝關(guān)節(jié)是根據(jù)踝關(guān)節(jié)和髖關(guān)節(jié)軌跡通過(guò)幾何關(guān)系計(jì)算出的，故其對(duì)步態(tài)穩(wěn)定性的影響沒(méi)踝關(guān)節(jié)和髖關(guān)節(jié)大.圖2為根據(jù)式(2)計(jì)算所得的踝關(guān)節(jié)、髖關(guān)節(jié)以及膝關(guān)節(jié)對(duì)仿人機(jī)器人步態(tài)ZMP的影響圖，由圖可驗(yàn)證，踝關(guān)節(jié)和髖關(guān)節(jié)對(duì)ZMP的影響較大，對(duì)仿人機(jī)器人的ZMP穩(wěn)定裕度有較大影響，其中踝關(guān)節(jié)和髖關(guān)節(jié)在起步步行階段對(duì)仿人機(jī)器人步態(tài)的穩(wěn)定性影響最為明顯，該影響隨著機(jī)器人步態(tài)進(jìn)入周期步行階段和止步步行階段逐漸減少，而膝關(guān)節(jié)在步態(tài)的起步步行、周期步行和止步步行階段對(duì)仿人機(jī)器人步態(tài)的穩(wěn)定性影響都較小，可忽略不計(jì)，因此，對(duì)髖關(guān)節(jié)和踝關(guān)節(jié)進(jìn)行優(yōu)化可以獲得ZMP穩(wěn)定裕度更大的仿人機(jī)器步態(tài).

圖2 髖、踝和膝關(guān)節(jié)對(duì)機(jī)器人步態(tài)穩(wěn)定性的影響Fig.2 Influence of hip，ankle and knee joints on robot＇s gait stability

2 基于魚(yú)群算法的步態(tài)優(yōu)化設(shè)計(jì)

魚(yú)群算法是一種基于模擬魚(yú)群行為來(lái)尋找全局優(yōu)化的群智能算法[12].該算法對(duì)比傳統(tǒng)的優(yōu)化模式具有分布處理、尋優(yōu)速度較快、適應(yīng)能力強(qiáng)的特點(diǎn)，因此，特別適合用于文中具有多約束條件、多參數(shù)的步態(tài)尋優(yōu).

2.1 魚(yú)群算法

魚(yú)群算法中，人工魚(yú)狀態(tài)表示為向量 Xi=[xi1xi2…xiD]，i=1，2，…，N(N為人工魚(yú)群個(gè)體大小).xiD為第i條人工魚(yú)的第D維向量，每條人工魚(yú)狀態(tài)就是一個(gè)潛在的解，將Xi帶入被優(yōu)化的函數(shù)，根據(jù)函數(shù)值的大小衡量Xi的優(yōu)劣.人工魚(yú)通過(guò)隨機(jī)解進(jìn)行初始化，然后通過(guò)覓食、聚群及追尾等算子來(lái)更新自己，迭代搜尋最優(yōu)解，從而實(shí)現(xiàn)尋優(yōu).其中覓食、聚群、追尾算子和行為選擇具體如下.

1)覓食算子.

當(dāng)人工魚(yú)當(dāng)前狀態(tài)為Xi，在其感知范圍內(nèi)隨機(jī)選擇一個(gè)狀態(tài)Xi，如果發(fā)現(xiàn)Xi對(duì)應(yīng)的函數(shù)值Yj(目標(biāo)函數(shù))更優(yōu)，則向該方向前進(jìn)一步;反之，再重新隨機(jī)選擇狀態(tài)Xi，判斷是否滿足前進(jìn)條件.這樣反復(fù)嘗試try_munber次(覓食行為最大嘗試次數(shù))后，如果仍不滿足前進(jìn)條件，則隨機(jī)移動(dòng)每步步長(zhǎng).

2)聚群算子.

當(dāng)人工魚(yú)當(dāng)前狀態(tài)為 Xi，探索當(dāng)前鄰域內(nèi)di，j=‖Xi－ Xj‖ ＜ Visual(di，j為人工魚(yú)個(gè)體之間的距離，Visual為人工魚(yú)的感知距離)的伙伴數(shù)目nf及中心位置如果Yc/nf＞δYi(δ為擁擠度)，則表明伙伴中心有較多的食物并且不太擁擠，Xi應(yīng)朝伙伴的中心位置方向前進(jìn)一步;否則執(zhí)行覓食算子.

3)追尾算子.

當(dāng)人工魚(yú)當(dāng)前狀態(tài)為 Xi，探索當(dāng)前鄰域內(nèi)di，j＜Visual的伙伴數(shù)目 nf，發(fā)現(xiàn)伙伴中 Xi是伙伴中最優(yōu)的(即Xi對(duì)應(yīng)的Yj為領(lǐng)域內(nèi)最優(yōu)解)，如果Yc/nf＞δYi，則表明伙伴Xi的狀態(tài)具有較高的食物濃度并且其周?chē)惶珦頂D，則朝伙伴Xi的方向前進(jìn)一步，否則執(zhí)行覓食算子.

4)行為選擇

根據(jù)所要解決的問(wèn)題性質(zhì)，對(duì)人工魚(yú)當(dāng)前所處的環(huán)境進(jìn)行評(píng)價(jià)，從而選擇一種行為.如對(duì)于求取極大值的問(wèn)題，最簡(jiǎn)單的評(píng)估方法可以用試探法，就是模擬執(zhí)行聚群、追尾等行為，然后評(píng)價(jià)行動(dòng)后的值，選擇其中的最大者來(lái)實(shí)際執(zhí)行，默認(rèn)的行為方式為覓食算子.

2.2 目標(biāo)函數(shù)的構(gòu)造

仿人機(jī)器人要實(shí)現(xiàn)長(zhǎng)久穩(wěn)定的步行，首先要求其步態(tài)具有較大的穩(wěn)定裕度和較小的能耗，故本文以ZMP穩(wěn)定裕度和整體能耗為參數(shù)共同構(gòu)造目標(biāo)函數(shù).假設(shè)ZMP穩(wěn)定裕度的目標(biāo)函數(shù)為JZ，整體能耗的目標(biāo)函數(shù)為JA，則目標(biāo)函數(shù)J=αJZ－βJA(其中α、β∈[0，1]，是根據(jù)兩者對(duì)仿人機(jī)器人步行時(shí)的比重而設(shè)定，由于在JZ不變的情況下，能量消耗越小J越大，故JA前為減號(hào)).

對(duì)于JZ，由于仿人機(jī)器人步態(tài)是由單腳支撐和雙腳支撐交替作用實(shí)現(xiàn)的，故要對(duì)JZ進(jìn)行分段討論.在步行的單腳支撐期，ZMP必須在支撐腳內(nèi)才能保證其步態(tài)的穩(wěn)定性.假設(shè)在第i個(gè)采樣時(shí)刻，ZMP到腳內(nèi)有效穩(wěn)定區(qū)域的x、y方向邊界的最小距離分別為dx、dy:

式中:a+b=1，且 a/b=Lfoot/Wfoot(Lfoot、Wfoot分別為機(jī)器人腳掌的長(zhǎng)和寬)，則單腳支撐期內(nèi)ZMP的目標(biāo)值函數(shù)為

在雙腳支撐期，ZMP的穩(wěn)定區(qū)域?yàn)殡p腳足底所構(gòu)成的六邊形，如圖3所示.

圖3 雙足支撐期穩(wěn)定區(qū)域Fig.3 The diagram of stability region for double phase

設(shè)在第j個(gè)采樣時(shí)刻ZMP點(diǎn)到區(qū)域邊界的最小距離為ds，則雙腳支撐期內(nèi)ZMP的目標(biāo)值函數(shù)為

而在能耗方面，為保證目標(biāo)函數(shù)值的光滑性，考慮到仿人機(jī)器人的能耗主要來(lái)源于驅(qū)動(dòng)力的損耗，所以采用驅(qū)動(dòng)力矩的函數(shù)來(lái)描述JA，即

式中:τ為關(guān)節(jié)驅(qū)動(dòng)力矩，ωτ為關(guān)節(jié)角速度，u為雙腳支撐期雙腳受到的總力矩，du為雙腳支撐期雙腳受到的總力矩作用于機(jī)器人的力臂長(zhǎng)，TS為單腳支撐期，TD為雙腳支撐期.終上所述，用于優(yōu)化的最終總目標(biāo)函數(shù)為

2.3 基于魚(yú)群算法的步態(tài)優(yōu)化流程

1)初始化各參數(shù)值，設(shè)定人工魚(yú)群規(guī)模N、步長(zhǎng)Step、擁擠度δ、最多嘗試數(shù)try_munber、delta、最大迭代次數(shù)maxgen，并設(shè)置迭代次數(shù)gen=1;

2)在待優(yōu)化步態(tài)參數(shù)的取值范圍內(nèi)初始化魚(yú)群，隨機(jī)得到 X1，X2，…，XN{}，并由其計(jì)算出魚(yú)群中各魚(yú)之間的平均距離作為感知距離Visual，根據(jù)式(7)計(jì)算出各人工魚(yú)的食物濃度J，取最大者Jmax，并保存其狀態(tài);

3)各人工魚(yú)分別執(zhí)行追尾、聚群和覓食算子，并分別評(píng)價(jià)對(duì)應(yīng)的J值，取聚群和追尾算子中的最優(yōu)J值與Jmax比較，如果J優(yōu)于Jmax狀態(tài)，便取代之;

4)判斷結(jié)束條件:根據(jù)迭代次數(shù)maxgen是否滿足條件，若滿足條件則結(jié)束迭代，輸出最優(yōu)解，否則gen自增1，并轉(zhuǎn)3)繼續(xù).

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

本文在MATLAB 7.1環(huán)境下，以直立狀態(tài)的仿人機(jī)器人彎膝起步，接著周期步行直至停止步行并恢復(fù)身體直立狀態(tài)的步態(tài)仿真，以驗(yàn)證本文利用魚(yú)群算法對(duì)步態(tài)參數(shù)優(yōu)化結(jié)果的有效性，同時(shí)將基于魚(yú)群算法的仿人機(jī)器人步態(tài)優(yōu)化(FSASP)與基于遺傳算法的仿人機(jī)器人步態(tài)優(yōu)化(GASP)的尋優(yōu)結(jié)果進(jìn)行比較，說(shuō)明本文方法的優(yōu)越性.仿人機(jī)器人的結(jié)構(gòu)參數(shù)如表1所示.

表1 仿人機(jī)器人各部分參數(shù)Table 1 Configuration parameters of humanoid robot

步態(tài)規(guī)劃方法如前文所述，相應(yīng)參數(shù)為:仿人機(jī)器人雙臂距離0.20 m，兩髖關(guān)節(jié)距離0.10 m，腳掌寬度0.08 m，腳掌高度 0.045 m，下蹲高度0.03 m，抬腳高度 0.01 m，步長(zhǎng) 0.10 m，步行周期 1.2 s，其中單腳支撐期為 0.96 s，雙腳支撐期為 0.24 s，采樣周期為0.01 s.魚(yú)群算法的相關(guān)參數(shù)為:N=4，Step=1，Visual=2，try_munber=50，delta=2，maxgen=100，α =0.8，β =0.2.由于魚(yú)群算法采用隨機(jī)方式初始化魚(yú)群，在重復(fù)運(yùn)行30次魚(yú)群算法，求得的參數(shù)平均值如下:dxs=0.041 m ，dxe=0.33 m ，dys=0.039 m ，dym=0.031 m ，dye=0.009 m，把本文優(yōu)化后的參數(shù)值代入步態(tài)得到的ZMP軌跡如圖4所示，三維步態(tài)棍狀圖如圖5所示，穩(wěn)定裕度結(jié)果如圖6所示，能耗結(jié)果如圖7所示.

圖4 步態(tài)優(yōu)化后的ZMP軌跡Fig.4 The ZMP trajectory after gait optimization

圖5 三維步態(tài)棍狀圖Fig.5 The stick figure of 3-D gait

4 結(jié)束語(yǔ)

本文分析了腿部各關(guān)節(jié)對(duì)仿人機(jī)器人步態(tài)穩(wěn)定性的影響，在徑向和側(cè)向平面內(nèi)選擇對(duì)機(jī)器人步態(tài)穩(wěn)定性影響較大的關(guān)節(jié)位置為尋優(yōu)參數(shù)，以步態(tài)的ZMP穩(wěn)定裕度最大和整體能耗最小為目標(biāo)構(gòu)造目標(biāo)函數(shù)，采用魚(yú)群算法對(duì)基于樣條插值方法規(guī)劃的仿人機(jī)器人步態(tài)進(jìn)行優(yōu)化.仿真結(jié)果表明，該方法優(yōu)化后的步態(tài)ZMP穩(wěn)定裕度大、能耗小、平穩(wěn)流暢，同時(shí)較利用遺傳算法對(duì)步態(tài)進(jìn)行優(yōu)化具有更好的全局搜索能力和更快的尋優(yōu)速度.

圖6 步態(tài)的ZMP穩(wěn)定裕度Fig.6 ZMP Stability margin of gait

圖7 步態(tài)的能耗Fig.7 Energy consumption of gait

從圖4～7可以看出，起步、周期步行和止步3種步態(tài)的ZMP軌跡基本位于支撐區(qū)域中心，ZMP的穩(wěn)定裕度大，步行的功率最小值和最大值分別為4.079 J、125.4 J，平均能耗為 76.2 J，仿人機(jī)器人步行自然、流暢、平穩(wěn)、能耗低，驗(yàn)證了優(yōu)化結(jié)果的有效性.

表2為將FSASP和GASP分別重復(fù)運(yùn)行30次，得到的尋優(yōu)結(jié)果.由表2可知，在對(duì)仿人機(jī)器人步態(tài)優(yōu)化中，本文FSASP相對(duì)于GASP更能脫離局部最優(yōu)收斂到全局最優(yōu)，而且本文FSASP在30次的尋優(yōu)仿真中，尋優(yōu)平均耗時(shí)為6.97 s，而GASP尋優(yōu)平均耗時(shí)為15.40 s，即本文FSASP相對(duì)于GASP尋優(yōu)速度更快.

表2 2種方法30次重復(fù)優(yōu)化的結(jié)果Table 2 30 repeated optimization results of the two methods

[1]CHEVALLEREAU C，AOUSTIN Y.Optimal reference trajectories for walking and running of a biped robot[J].Robotica，2001，19(5):557-569.

[2]BESSONNET G，SEGUIN P，SARDAIN P.A parametric optimization approach to walking pattern synthesis[J].The International Journal of Robotics Research，2005，24(7):523-536.

[3]SHI Z，XU W，ZHONG Y，et al.Optimal sagittal gait with ZMP stabillity during complete walking cycle for humanoid robots[J].Journal of Control Theory and Applications，2007，5(2):133-138.

[4]NGUYEN T P，DAE W K，HAK K K，et al.An optimal control method for biped robot with stable walking gait[C]//2008 2th IEEE-RAS International Conference on Humanoid Robots.Daejeon，USA，2008:211-218.

[5]SHAFII N，KHORSANDIAN A，ABDOLMALEKI A，et al.An optimized gait generator based on Fourier series towards fast and robust biped locomotion involving arms swing[C]//IEEE International Conference on Automation and Logistics.Piscataway，NJ，USA:IEEE，2009:2018-2023.

[6]KAYNOV D，SOUERES P，PIERRO P，et al.A practical decoupled stabilizer for joint-position controlled humanoid robot[C]//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ，USA，2009:3392-3397.

[7]伊強(qiáng)，陳懇，劉莉，等.考慮綜合行走約束的仿人機(jī)器人參數(shù)化3D步態(tài)規(guī)劃方法[J].機(jī)器人，2009，31(4):342-350.YI Qiang，CHEN Ken，LIU Li，et al.3D parametric gait planning of humanoid robot with consideration of comprehensive biped walking constraint[J].Robot，2009，31(4):342-350.

[8]KIM S，KIM C，YOU B.Stable whole-body motion generation for humanoid robots to imitate human motions[C]//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.Saint Louis，USA，2009:2518-2524.

[9]PETER S，JAROSLAV T，ZLATKO F，et al.Development of cognitive capabilities for robot Nao in center for intelligent technologies in Kosice[C]//2011 2nd International Conference on Cognitive Info-communications.Piscataway，NJ，USA，2011:1-5.

[10]HUANG Qiang，YOKOI K，KAJITA S，et al.Planning walking patterns for a biped robot[J].IEEE Transactions on Robotics and Automation，2001，3(17):280-289.

[11]棍田秀司.仿人機(jī)器人[M].北京:清華大學(xué)出版社，2007:87-91.

[12]JIANG Mingyan，WANG Yong.Spread spectrum code estimation by artificial fish swarm algorithm[C]//IEEE International Symposium on IntelligentSignalProcessing，2007，2007:1-6.