雙足機(jī)器人節(jié)能步態(tài)規(guī)劃算法

盧志強(qiáng)，侯媛彬，孟 蕓，周福娜

(1.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054；2.河南大學(xué) 計(jì)算與信息工程學(xué)院，河南 開(kāi)封 475004)

0 引 言

雙足機(jī)器人具有類(lèi)人的結(jié)構(gòu)和外觀，可以很好地適應(yīng)人類(lèi)的生活環(huán)境，是替代人類(lèi)從事如煤礦救援等危險(xiǎn)性工作的理想機(jī)器人[1]。人們期待雙足機(jī)器人能夠以類(lèi)人的方式在現(xiàn)實(shí)環(huán)境中穩(wěn)定可靠地行走[2]，甚至是奔跑[3]。雖然人類(lèi)可以相對(duì)輕松地執(zhí)行各種運(yùn)動(dòng)，但將人類(lèi)的行為轉(zhuǎn)化為雙足機(jī)器人動(dòng)態(tài)運(yùn)動(dòng)仍然是一項(xiàng)具有挑戰(zhàn)性的任務(wù)。近40 a來(lái)，國(guó)內(nèi)外研究機(jī)構(gòu)與學(xué)者在雙足機(jī)器人領(lǐng)域進(jìn)行了許多理論[4]和實(shí)驗(yàn)研究[5]，并取得顯著的發(fā)展，如HRP-4可以將車(chē)輛行駛在道路中央[6]，ATLAS能跨越障礙、攀爬樓梯等[7]。

雙足行走問(wèn)題是雙足機(jī)器人運(yùn)動(dòng)研究中最重要且最根本的問(wèn)題，按照研究思想的不同，提出雙足行走方法可分為3類(lèi)：其一是以人類(lèi)行走特征為參考的步態(tài)生成方法。雙足機(jī)器人設(shè)計(jì)的原型是人類(lèi)本身，很多研究人員使用高速光學(xué)運(yùn)動(dòng)捕捉系統(tǒng)獲取人體運(yùn)動(dòng)的軌跡，分析人類(lèi)的運(yùn)動(dòng)特征，并將這些特征應(yīng)用到仿人機(jī)器人的運(yùn)動(dòng)中。BOUTIN等捕捉人體全身運(yùn)動(dòng)軌跡，生成雙足機(jī)器人步行模式，將該方法成功應(yīng)用于HRP-2和HOAP-3這2款機(jī)器人的行走[8]。吳偉國(guó)等采集人體髖、腿、踝的運(yùn)動(dòng)樣本，構(gòu)建機(jī)器人仿人步行樣本庫(kù)，實(shí)現(xiàn)機(jī)器人與人跟隨步行[9]。其二是基于中樞模式發(fā)生器(central pattern generator，CPG)的步態(tài)生成方法。人類(lèi)的雙足運(yùn)動(dòng)具有周期節(jié)律性的特征，CPG是能夠產(chǎn)生節(jié)律運(yùn)動(dòng)的神經(jīng)環(huán)路網(wǎng)絡(luò)，模仿CPG產(chǎn)生節(jié)律信號(hào)，可控制機(jī)器人步行運(yùn)動(dòng)。劉成菊等使用腳底壓力和腰部加速度作為CPG的驅(qū)動(dòng)，實(shí)現(xiàn)了機(jī)器人NAO對(duì)不同地形的自適應(yīng)行走[10]。JUANG等使用連續(xù)多目標(biāo)蟻群優(yōu)化的全連接遞歸神經(jīng)網(wǎng)絡(luò)構(gòu)成CPG，解決NAO的多目標(biāo)步態(tài)生成問(wèn)題[11]。其三是基于雙足機(jī)器人簡(jiǎn)化模型的步態(tài)生成方法。雙足機(jī)器人一般有幾十個(gè)自由度且沒(méi)有固定的基座，導(dǎo)致其動(dòng)力學(xué)系統(tǒng)非常復(fù)雜?？紤]到系統(tǒng)復(fù)雜度和計(jì)算復(fù)雜度，利用完整的動(dòng)力學(xué)系統(tǒng)模型難以實(shí)現(xiàn)雙足機(jī)器人的步行運(yùn)動(dòng)，簡(jiǎn)化動(dòng)力學(xué)系統(tǒng)便成為實(shí)現(xiàn)雙足機(jī)器人步行運(yùn)動(dòng)很好且容易實(shí)現(xiàn)的方案。HONG等將仿人機(jī)器人簡(jiǎn)化成一個(gè)單質(zhì)點(diǎn)和可伸縮不計(jì)質(zhì)量的腿構(gòu)成的線性倒立擺模型[12]，SHAFII等將雙足機(jī)器人簡(jiǎn)化成一個(gè)不計(jì)質(zhì)量桌子與將全部質(zhì)量集中在一輛在桌面上行駛小車(chē)構(gòu)成的桌子-小車(chē)模型[13]，SHIMMYO等在單質(zhì)點(diǎn)倒立擺模型的基礎(chǔ)上，進(jìn)一步考慮了支撐腿和擺動(dòng)腿質(zhì)量，構(gòu)成了三質(zhì)心倒立擺模型[14]。

目前，已有的雙足機(jī)器人結(jié)構(gòu)復(fù)雜程度還遠(yuǎn)遜于人類(lèi)，這決定了以人類(lèi)特征為參考和基于中樞模式發(fā)生器的步態(tài)生成方法有其局限性，研究人員更傾向于采用雙足機(jī)器人簡(jiǎn)化模型的方法。簡(jiǎn)化模型的精度決定了控制效果的精度，LUO等在三質(zhì)心模型中，采用加入角動(dòng)量的模型預(yù)測(cè)控制，獲得了比單質(zhì)心模型或一般三質(zhì)心模型高的零力矩點(diǎn)尋跡精度[15]。另一方面，減少機(jī)器人控制中的約束條件能獲得更優(yōu)的控制效果，SHIN等允許機(jī)器人身體垂直運(yùn)動(dòng)，提高了行走速度、降低了驅(qū)動(dòng)電機(jī)的能耗[16]。若允許機(jī)器人身體有更多自由度的運(yùn)動(dòng)，可以獲得更好的控制效果。約束條件的減少，則意味著機(jī)器人控制中參數(shù)空間的增大，用人工智能的方法尋找最優(yōu)解將是一個(gè)有意義的問(wèn)題，WRIGHT等綜述了機(jī)器人運(yùn)動(dòng)控制中的常用智能算法[17]。

結(jié)合當(dāng)前雙足機(jī)器人行走步態(tài)規(guī)劃領(lǐng)域的研究成果和存在的不足，文中提出一種兼顧雙足機(jī)器人行走穩(wěn)定性的節(jié)能步態(tài)規(guī)劃算法，解決雙足機(jī)器人行走的步態(tài)規(guī)劃與控制問(wèn)題。

1 問(wèn)題陳述

1.1 系統(tǒng)結(jié)構(gòu)

文中使用的雙足機(jī)器人，具有雙臂和雙腿，可以模仿人體的行走運(yùn)動(dòng)，機(jī)器人的每條腿有5個(gè)自由度，分別為髖部轉(zhuǎn)角q1/q6，q2/q7，膝蓋轉(zhuǎn)角q3/q8，腳踝轉(zhuǎn)角q4/q9和q5/q10，設(shè)關(guān)節(jié)向量q=[q1,q2,…,q10]T。機(jī)器人前、側(cè)和垂直方向，分別被定義為X軸、Y軸和Z軸方向。世界坐標(biāo)系的原點(diǎn)定位于機(jī)器人直立時(shí)的兩腳中點(diǎn)，機(jī)器人模型如圖1所示。雙足機(jī)器人每個(gè)腳底分別安裝4片F(xiàn)SR壓力傳感器，用來(lái)測(cè)量腳底作用力的分布，表1列出了機(jī)器人身體及右腿的質(zhì)量及長(zhǎng)度，機(jī)器人左腿的參數(shù)相同。

表1 雙足機(jī)器人的基本參數(shù)

圖1 實(shí)驗(yàn)機(jī)器人的連桿結(jié)構(gòu)

擬議的雙足機(jī)器人步態(tài)行走基于以下假設(shè)。

1)上身在任何時(shí)候都保持直立的姿態(tài)。人的軀干俯仰角度一般在3°以內(nèi)[18]，多數(shù)相關(guān)研究表明，這個(gè)假設(shè)是可以接受的[8-10，12-16]。

2)雙腳總是與地面平行。常見(jiàn)的多數(shù)仿人機(jī)器人沒(méi)有腳趾，在抬腳和落腳時(shí)，不能發(fā)揮改善驅(qū)動(dòng)性能的作用[19]。

1.2 ZMP方程

在諸多雙足機(jī)器人的動(dòng)態(tài)穩(wěn)定性指標(biāo)中，使用最為廣泛的是零力矩點(diǎn)(zero moment point，ZMP)概念，ZMP是地面上由重力和慣性產(chǎn)生的總力矩水平分量為零的點(diǎn)[20]。設(shè)雙足機(jī)器人的5個(gè)質(zhì)量點(diǎn)分別為身體b，左腿lu，左足ld，右腿ru,右足rd，組成集合Pc={b,lu,ld,ru,rd}，位置為{ri|i∈Pc}，其中b與髖中心點(diǎn)的距離為hb。同時(shí)，設(shè)左髖關(guān)節(jié)lh，左膝關(guān)節(jié)lk，左踝關(guān)節(jié)lf,右髖關(guān)節(jié)rh，右膝關(guān)節(jié)rk，右踝關(guān)節(jié)rf，組成集合Pr={lh,lk,lf,rh,rk,rf}，位置為{rj|j∈Pr}。機(jī)器人的位置示意如圖2所示。

圖2 雙足機(jī)器人的五質(zhì)心模型

在ZMP位置rZMP=[xZMP,yZMP，0]T處，地面作用力的合力F產(chǎn)生的力矩MZMP，與由于各質(zhì)心的重力和慣性施加于機(jī)器人身體的合力矩M，滿足

n×MZMP=n×(M-rZMP×F)=0

(1)

式中

(2)

(3)

(4)

1.3 有效ZMP區(qū)域

雙足機(jī)器人的建模中不可避免的存在建模誤差，HONG等將ZMP固定在支撐腳的中心，以實(shí)現(xiàn)最穩(wěn)定的行走[12]，但這并不是高效的方法。在支撐腳的區(qū)域內(nèi)，劃出一些邊緣區(qū)域，用來(lái)補(bǔ)償建模誤差[16]，并使ZMP位于支撐腳中部的有效ZMP區(qū)域(allowable ZMP region，AZR)方法，是一種在克服誤差與高效行走之間權(quán)衡的較好方法。設(shè)足長(zhǎng)lfl、足寬lfw的雙足機(jī)器人，步行時(shí)步幅為s，雙足Y軸距離是w，AZR示意如圖3所示。文中分別用ηl=lal/lfl和ηw=law/lfw表示腳長(zhǎng)方向和腳寬方向的容限，后續(xù)步態(tài)規(guī)劃算法中，ηl取固定值，并約定η=ηw。以左腳在前，右腳在后為例，在第1個(gè)DSP中，AZR是由點(diǎn)r1,r2,r3,r4,r5,r6構(gòu)成的六邊形區(qū)域；在SSP時(shí)，AZR是由點(diǎn)r4,r5,r6,r7構(gòu)成的長(zhǎng)方形；在第2個(gè)DSP中，AZR是由點(diǎn)r5,r6,r7,r8,r9,r10構(gòu)成的六邊形。

圖3 雙足機(jī)器人的AZR

1.4 能耗指標(biāo)函數(shù)

依賴電池提供能源的雙足機(jī)器人，能耗E包括運(yùn)動(dòng)能耗Em和非運(yùn)動(dòng)能耗Ea這2部分[22]。Em用于維持電機(jī)的運(yùn)行，是系統(tǒng)能耗的主要部分，其值為電機(jī)瞬時(shí)電壓向量um(t)和電流向量im(t)對(duì)時(shí)間t的積分。Ea用于傳感器、控制器和內(nèi)部電路運(yùn)行，其功率Pa在一段時(shí)間內(nèi)是相對(duì)穩(wěn)定的，可以表示為時(shí)間t的線性函數(shù)。即

(5)

(6)

1.5 問(wèn)題定義

基于前面的分析，文中的節(jié)能步態(tài)規(guī)劃問(wèn)題可表述為

1)給定雙足機(jī)器人步長(zhǎng)和AZR，允許身體在行走中三維運(yùn)動(dòng)，滿足1.1中假設(shè)條件，確定最小化能耗指標(biāo)函數(shù)E的步態(tài)軌跡。

2)已知運(yùn)動(dòng)距離，確定包含開(kāi)始步、中間步和停止步的完整步態(tài)軌跡，并利用AZR，提高雙足機(jī)器人行走的魯棒性。

2 步態(tài)規(guī)劃算法

2.1 算法概述

文中提出的步態(tài)規(guī)劃算法分為步態(tài)參數(shù)優(yōu)化(gait parameter optimization，GPO)算法和步態(tài)合成(gait synthesis，GSYN)算法2部分，如圖4所示。

圖4 步態(tài)規(guī)劃算法示意

1)GPO算法從步長(zhǎng)集合S和AZR容限集合H選取元素si和ηi，在si傅里葉級(jí)數(shù)表示空間中布網(wǎng)運(yùn)算，獲得符合ηi要求的種子集合Pi，在Pi中元素pi的鄰域，依據(jù)文中1.4中的算法計(jì)算能耗指標(biāo)函數(shù)，按照最優(yōu)梯度法迭代運(yùn)算，獲得最小能耗指標(biāo)函數(shù)的電機(jī)優(yōu)化角度序列(步態(tài)軌跡)gi={qi(n)|N=1,2,…,N}，存入數(shù)據(jù)庫(kù)。

GPO算法運(yùn)算時(shí)間較長(zhǎng)，算法宜于離線運(yùn)行，對(duì)S和H完成運(yùn)算后，將離線數(shù)據(jù)庫(kù)中的結(jié)果，復(fù)制到在線數(shù)據(jù)庫(kù)，GSYN計(jì)算中可實(shí)時(shí)調(diào)用。

2.2 GPO算法

GPO算法用于生成步態(tài)控制的電機(jī)角度序列，是文中算法的核心。按照雙足機(jī)器人行走的精度要求，從最小步長(zhǎng)至最大步長(zhǎng)離散取值，確定S中的元素si。H從值域[0,1]選取若干等級(jí)，對(duì)應(yīng)不同的魯棒性要求的ηi。GPO算法從S和H依次選取元素si和ηi，進(jìn)行布網(wǎng)梯度優(yōu)化計(jì)算，以獲得滿足最小化E的gi。

2.2.1 布網(wǎng)計(jì)算方法

所謂布網(wǎng)計(jì)算，是將多維參數(shù)取值按照一定間隙離散化為網(wǎng)格，使構(gòu)成的參數(shù)空間網(wǎng)格化，進(jìn)而計(jì)算各個(gè)網(wǎng)格交點(diǎn)函數(shù)值的過(guò)程。若機(jī)器人步長(zhǎng)為si，身體b進(jìn)行滿足1.1中假設(shè)1的三維周期運(yùn)動(dòng)，位置rb(n)=[xb(n)yb(n)zb(n)]T可用有限階的傅里葉級(jí)數(shù)表示，即

(7)

式中rb(n)用參數(shù){ak|k=1,2,…,18}表示；N為步態(tài)周期，ω0=2π/N。若ak∈[dk,uk]按照間隙μk取值為Ak={dk,dk+μk,dk+2μk,…,uk},k=1,2,…,18。則布網(wǎng)形成的交點(diǎn)集合U為(Ak|K=1,2,…,18)的笛卡爾積，即U=A1×A2×…×A18。

另一方面，若設(shè)機(jī)器人運(yùn)動(dòng)起始為左腿在前，右腿在后的雙支撐階段，隨后右腿擺動(dòng)向前，且右腳rf起始位置為[0 0 0]T，并設(shè)運(yùn)動(dòng)滿足1.1中假設(shè)3，則位置rrf(n)=[xrf(n)yrf(n)zrf(n)]T運(yùn)動(dòng)軌跡為

(8)

式中w為步寬，cm;hf為步高,cm;N1=N/16，N2=7N/16，N3=3N/8。機(jī)器人的雙足運(yùn)動(dòng)具有對(duì)稱性，左腳軌跡rlf(n)為rrf(n)時(shí)移N/2，并在Y方向?qū)ΨQ取值。

HERZOG等論述了已知rb(n)和rrf(n)求解雙足機(jī)器人關(guān)節(jié)角度q(n)的逆動(dòng)力學(xué)方法[24]。布網(wǎng)空間U一般具有較大的元素?cái)?shù)目，考慮rb(n)具有的運(yùn)動(dòng)特性，可排除rb(n)在U中的一些不合理取值，從而減少需要的逆動(dòng)力學(xué)計(jì)算數(shù)量。常見(jiàn)的rb(n)特性有如下特點(diǎn)。

1)xb(n)在步態(tài)周期內(nèi)單調(diào)增。

2)yb(n)前半個(gè)周期為凸函數(shù)，后半個(gè)周期為凹函數(shù)，且在半個(gè)周期內(nèi)無(wú)拐點(diǎn)。

3)zb(n)在前、后半個(gè)周期內(nèi)均為先單調(diào)增，后單調(diào)減。

2.2.2 梯度尋優(yōu)算法

算法2.1 梯度尋優(yōu)算法

給定Pi，δλ，AZR，

forλ=1,2,…,Λ do

重復(fù)進(jìn)行

end for

2.3 GSYN算法

給定運(yùn)動(dòng)距離d，規(guī)劃S*={sj|j=1,2,…,c}包括2個(gè)起始步s1和s2，2個(gè)停止步sc-2和sc-1，多個(gè)循環(huán)步sm，m=3,4,…,c-3。其中s1=sc-1，s2=sc-2，循環(huán)步sm具有相等的距離。起始步和停止步的總長(zhǎng)db控制在1～2sm，即sm

(9)

yMIN=min{c0(yZMP(n)-yAZR(n))|n=1,2,…,N}

(10)

按照增量PI控制算法，可計(jì)算Δηj

(11)

式中kP,kI分別為比例、積分系數(shù)。

(12)

3 仿真與實(shí)驗(yàn)分析

表2 梯度尋優(yōu)中的

圖5 能耗指標(biāo)函數(shù)收斂曲線

為對(duì)比算法性能，以機(jī)器人髖高度固定[15]和髖高度按余弦波形垂直變化[16]，其他條件與文中算法相同，設(shè)置算法二和算法三，經(jīng)過(guò)優(yōu)化計(jì)算后，其E分別為280.4和252.4 mJ，并標(biāo)識(shí)于圖5中。設(shè)機(jī)器人步態(tài)以DSP起始，單步周期N=16，DSP比率σ=0.25，3種算法的單步ZMP軌跡如圖6所示，其中文中算法采用能耗E=231.5 mJ時(shí)的步態(tài)數(shù)據(jù)。

圖6 ZMP數(shù)據(jù)分析

表3 ZMP軌跡分析

若取d=90 cm，sm=12 cm，按照式(9)計(jì)算得c=10，s1=2 cm,s2=7 cm，則機(jī)器人運(yùn)動(dòng)的步長(zhǎng)S*={2,7,12,12,12,12,12,12,7,2}。在機(jī)器人運(yùn)動(dòng)的每步均以ηj=5/6從在線數(shù)據(jù)庫(kù)中取得gj=[q1q2…q16]，j=1,2,…,10，控制機(jī)器人的關(guān)節(jié)運(yùn)動(dòng)，對(duì)應(yīng)的雙足機(jī)器人步態(tài)運(yùn)動(dòng)的動(dòng)態(tài)仿真如圖7所示。

圖7 機(jī)器人步行的動(dòng)態(tài)仿真

依照前述仿真數(shù)據(jù)，實(shí)測(cè)機(jī)器人行走試驗(yàn)如圖8所示。試驗(yàn)中，設(shè)定行走距離90 cm，在第1步為右腳支撐，設(shè)定SSP右腳yAZR=-1.5 cm，初始η1=1/3，在行走中，檢測(cè)到y(tǒng)MIN=1.53 cm，經(jīng)過(guò)式(11)PI及邏輯運(yùn)算，得Δη1=-1/3，按式(12)取η2=2/3。其后，經(jīng)過(guò)第2，3步的校正后，η值穩(wěn)定在5/6。若控制中選用η1=1/3的控制方法，行走sm=12 cm時(shí)，E=276.3 mJ。而采用文中的GSYN算法，調(diào)整η=5/6后，行走12 cm的E=231.5 mJ，其值下降了16.21%。

圖8 機(jī)器人行走的試驗(yàn)

4 結(jié) 論

1)電機(jī)驅(qū)動(dòng)的機(jī)器人具有質(zhì)量分散的特點(diǎn)，建立多質(zhì)心機(jī)器人模型可較好的描述機(jī)器人物理特性。在機(jī)器人多質(zhì)心模型的基礎(chǔ)上，建立了以負(fù)荷轉(zhuǎn)矩為主要參數(shù)的能耗函數(shù)，能在仿真中精確而方便地計(jì)算機(jī)器人運(yùn)動(dòng)的能耗指標(biāo)。

2)GPO算法中的對(duì)運(yùn)動(dòng)空間網(wǎng)格化的過(guò)程中，要求單個(gè)網(wǎng)孔內(nèi)不出現(xiàn)多個(gè)峰值，選擇合適的網(wǎng)孔間隙，此方法適用于多數(shù)系統(tǒng)的優(yōu)化。算法的梯度逼近中按照精度要求，確定逐次逼近的步長(zhǎng)，在網(wǎng)孔規(guī)模較大時(shí)，可采用將網(wǎng)孔空間再次網(wǎng)格化的方法加速逼近過(guò)程。GPO算法對(duì)系統(tǒng)優(yōu)化的過(guò)程中需要進(jìn)行大量的運(yùn)算，可在有限的時(shí)間內(nèi)確定找到最優(yōu)值，隨著計(jì)算機(jī)科學(xué)的進(jìn)步，其對(duì)比隨機(jī)優(yōu)化算法，將愈加表現(xiàn)出其優(yōu)越的性能。

3)GSYN算法按照實(shí)際機(jī)器人行走的要求，規(guī)劃含起始步、中間步和停止步的完整運(yùn)動(dòng)過(guò)程。在機(jī)器人的行走過(guò)程中，算法根據(jù)反饋的ZMP數(shù)據(jù)，修改機(jī)器人運(yùn)動(dòng)的AZR，從而克服建模和環(huán)境誤差的干擾，可實(shí)現(xiàn)低能耗和高魯棒性的折中，較好地解決具有高度非線性特征的雙足機(jī)器人行走問(wèn)題。因此，該算法也作為一種新途徑，推廣到煤礦救援機(jī)器人的開(kāi)發(fā)中。