基于魚群算法的仿人機器人步態(tài)優(yōu)化

盧嘉敏，楊宜民，付根平

(廣東工業(yè)大學自動化學院，廣東廣州510090)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 魚群算法

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

1)覓食算子.

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

2)聚群算子.

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

3)追尾算子.

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

4)行為選擇

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

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

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

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

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

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

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

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

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

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

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

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

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

3)各人工魚分別執(zhí)行追尾、聚群和覓食算子，并分別評價對應(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 仿真實驗及結(jié)果分析

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

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

步態(tài)規(guī)劃方法如前文所述，相應(yīng)參數(shù)為:仿人機器人雙臂距離0.20 m，兩髖關(guān)節(jié)距離0.10 m，腳掌寬度0.08 m，腳掌高度 0.045 m，下蹲高度0.03 m，抬腳高度 0.01 m，步長 0.10 m，步行周期 1.2 s，其中單腳支撐期為 0.96 s，雙腳支撐期為 0.24 s，采樣周期為0.01 s.魚群算法的相關(guān)參數(shù)為:N=4，Step=1，Visual=2，try_munber=50，delta=2，maxgen=100，α =0.8，β =0.2.由于魚群算法采用隨機方式初始化魚群，在重復(fù)運行30次魚群算法，求得的參數(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é)束語

本文分析了腿部各關(guān)節(jié)對仿人機器人步態(tài)穩(wěn)定性的影響，在徑向和側(cè)向平面內(nèi)選擇對機器人步態(tài)穩(wěn)定性影響較大的關(guān)節(jié)位置為尋優(yōu)參數(shù)，以步態(tài)的ZMP穩(wěn)定裕度最大和整體能耗最小為目標構(gòu)造目標函數(shù)，采用魚群算法對基于樣條插值方法規(guī)劃的仿人機器人步態(tài)進行優(yōu)化.仿真結(jié)果表明，該方法優(yōu)化后的步態(tài)ZMP穩(wěn)定裕度大、能耗小、平穩(wěn)流暢，同時較利用遺傳算法對步態(tài)進行優(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，仿人機器人步行自然、流暢、平穩(wěn)、能耗低，驗證了優(yōu)化結(jié)果的有效性.

表2為將FSASP和GASP分別重復(fù)運行30次，得到的尋優(yōu)結(jié)果.由表2可知，在對仿人機器人步態(tài)優(yōu)化中，本文FSASP相對于GASP更能脫離局部最優(yōu)收斂到全局最優(yōu)，而且本文FSASP在30次的尋優(yōu)仿真中，尋優(yōu)平均耗時為6.97 s，而GASP尋優(yōu)平均耗時為15.40 s，即本文FSASP相對于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]伊強，陳懇，劉莉，等.考慮綜合行走約束的仿人機器人參數(shù)化3D步態(tài)規(guī)劃方法[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]棍田秀司.仿人機器人[M].北京:清華大學出版社，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.