基于動(dòng)捕系統(tǒng)的機(jī)器人步態(tài)規(guī)劃與實(shí)驗(yàn)研究

孫 軍,呂連杰,孫詩華,李東華

(1.沈陽建筑大學(xué)機(jī)械工程學(xué)院,遼寧 沈陽 110168;2.中國工業(yè)互聯(lián)網(wǎng)研究院遼寧分院,遼寧 沈陽 110020)

目前人形機(jī)器人的步態(tài)規(guī)劃方法使機(jī)器人行走耗能較大而且行走表現(xiàn)僵硬。隋振等[1]進(jìn)行機(jī)器人步態(tài)規(guī)劃時(shí)，假設(shè)質(zhì)心距離地面始終保持相同高度,基于機(jī)器人三維線性倒立擺模型規(guī)劃出質(zhì)心參考軌跡。王亞波[2]利用機(jī)器人質(zhì)心高度不變的假設(shè)條件,依據(jù)離線規(guī)劃的質(zhì)心、踝關(guān)節(jié)位置軌跡,通過逆運(yùn)動(dòng)學(xué)求得機(jī)器人下肢各關(guān)節(jié)角度。張?jiān)耓3]采用線性倒立擺模型對(duì)機(jī)器人進(jìn)行步態(tài)規(guī)劃,其假設(shè)機(jī)器人行走過程中腰部總是保持水平狀態(tài),即假設(shè)倒立擺模型中質(zhì)心運(yùn)動(dòng)約束平面的高度是固定的,因此髖關(guān)節(jié)高度也固定。E.Dong等[4]采用倒立擺模型進(jìn)行機(jī)器人步態(tài)規(guī)劃，假設(shè)機(jī)器人髖關(guān)節(jié)高度固定不變。但仿生生物力學(xué)研究表明,人類周期步行中髖關(guān)節(jié)相對(duì)地面的高度呈周期性變化,兩髖關(guān)節(jié)的高度不是始終保持一致[5]。上述研究雖然降低了自由度間的復(fù)雜協(xié)調(diào)關(guān)系及步態(tài)規(guī)劃過程中的計(jì)算量,但規(guī)劃出的機(jī)器人行走步態(tài)與人類的步行方式不符。

鑒于上述分析,筆者使用Perception Neuron PRO動(dòng)捕系統(tǒng)采集人體下肢關(guān)節(jié)運(yùn)動(dòng)數(shù)據(jù),預(yù)處理后生成人體步行樣本,利用幾何縮放生成機(jī)器人步行樣本,從中提取機(jī)器人運(yùn)動(dòng)學(xué)參數(shù);采用五次樣條插值規(guī)劃下肢關(guān)節(jié)參數(shù)化運(yùn)動(dòng)軌跡;使用多目標(biāo)粒子群優(yōu)化算法得出最優(yōu)運(yùn)動(dòng)學(xué)參數(shù),從而得到機(jī)器人理想的關(guān)節(jié)位置軌跡；利用逆運(yùn)動(dòng)學(xué)求出下肢各關(guān)節(jié)角度,完成機(jī)器人的步態(tài)規(guī)劃；研究表明:模仿人類步行的步態(tài)規(guī)劃方法使機(jī)器人實(shí)現(xiàn)穩(wěn)定行走,行走過程能量消耗較低,行走步態(tài)的擬人性效果好。

1 動(dòng)捕系統(tǒng)

Perception Neuron PRO由慣性傳感器等硬件設(shè)備和配套軟件組成,每一個(gè)慣性傳感器都集成了3軸陀螺儀,3軸加速度計(jì)和3軸電子羅盤[6]。電子羅盤的動(dòng)態(tài)范圍:360 °,陀螺儀范圍:±2 000 rad/s,加速計(jì)范圍:±16 m/s2。筆者利用該動(dòng)捕系統(tǒng)進(jìn)行人體運(yùn)動(dòng)數(shù)據(jù)采集實(shí)驗(yàn)。

2 人體步行樣本獲取及預(yù)處理

實(shí)驗(yàn)使用動(dòng)捕系統(tǒng)采集人正常速度行走的運(yùn)動(dòng)數(shù)據(jù),實(shí)驗(yàn)前在場地畫線,讓實(shí)驗(yàn)者練習(xí)多次直線行走后進(jìn)行運(yùn)動(dòng)數(shù)據(jù)采集。由于實(shí)驗(yàn)設(shè)備會(huì)受磁場干擾,導(dǎo)致實(shí)驗(yàn)數(shù)據(jù)不光滑并出現(xiàn)噪聲點(diǎn)[7],采用遞推平均濾波和savgol_filter函數(shù)進(jìn)行平滑處理。

圖1為髖關(guān)節(jié)預(yù)處理前后的運(yùn)動(dòng)數(shù)據(jù)。由圖可知,平滑處理后得到了較為光滑的運(yùn)動(dòng)數(shù)據(jù),并且人體髖關(guān)節(jié)豎直方向的運(yùn)動(dòng)軌跡存在周期性波動(dòng)。

圖1 左髖關(guān)節(jié)預(yù)處理前后的運(yùn)動(dòng)數(shù)據(jù)Fig.1 Motion data of the left hip joint before and after preprocessing

3 人形機(jī)器人步行樣本的生成

3.1 人形機(jī)器人—NAO

Softbank有一款以NAO命名的人形機(jī)器人,全身共13個(gè)關(guān)節(jié),每只足有4個(gè)壓力傳感器來確定每只腳壓力中心位置,通過電阻值大小來反應(yīng)承受壓力值大小[8]。

3.2 樣本的幾何縮放匹配

由于人與機(jī)器人在肢體長度等方面存在差異[9],因此將預(yù)處理得到的人體髖、踝關(guān)節(jié)的位置進(jìn)行幾何縮放。人體下肢骨骼與機(jī)器人下肢骨骼具有相同的結(jié)構(gòu),因此可以在兩個(gè)骨骼之間加入一個(gè)中間骨骼實(shí)現(xiàn)樣本的幾何縮放[10]。

在確定前進(jìn)方向和豎直方向的縮放比例時(shí),選擇人體與機(jī)器人的下肢長度作為決定因素;在確定橫向方向的縮放比例時(shí),選擇人體和機(jī)器人的兩髖關(guān)節(jié)及兩踝關(guān)節(jié)的距離作為決定因素。

三種骨骼的對(duì)應(yīng)關(guān)系如圖2所示,L1、L2、L3代表人體骨骼肢體長度,H1、H2、H3代表中間骨骼肢體長度,D1、D2、D3代表機(jī)器人骨骼肢體長度。將三種類型骨骼的上身都簡化成連桿,則有如下關(guān)系式:

(1)

H1+H2+H3=D1+D2+D3.

(2)

圖2 三種骨骼的對(duì)應(yīng)關(guān)系Fig.2 Relationship of three bone linkages

3.2.1 縮放系數(shù)

通過上節(jié)的分析后計(jì)算從人體骨骼到中間骨骼的縮放比例。設(shè)Shk為縮放比例,lrd為機(jī)器人步行參數(shù),lmd為人體步行參數(shù),則有:

(3)

其中,h取值為1、2,依次代表髖關(guān)節(jié)、踝關(guān)節(jié);k取值為1、2、3,依次代表橫向方向、前進(jìn)方向、豎直方向;d取值為1、2、3、4,依次代表下肢總長度、兩髖關(guān)節(jié)之間的距離、兩踝關(guān)節(jié)之間的距離、大腿與小腿長度之和。

3.2.2 髖關(guān)節(jié)位置

將人體骨骼通過上一步的比例縮放后即可得到中間骨骼。由于兩骨骼各關(guān)節(jié)長度都保持相同的比例,為得到中間骨骼髖關(guān)節(jié)的位置,僅需要將第2節(jié)預(yù)處理后的人體髖關(guān)節(jié)每一幀的動(dòng)作數(shù)據(jù)進(jìn)行縮放,縮放結(jié)果同樣也是機(jī)器人髖關(guān)節(jié)的位置。方程為

(4)

(5)

(6)

3.2.3 踝關(guān)節(jié)位置

將上一步計(jì)算得到的髖關(guān)節(jié)的位置結(jié)合縮放比例即可求得機(jī)器人踝關(guān)節(jié)位置。求解方程:

(7)

(8)

3.3 人形機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)的提取

從3.2節(jié)幾何縮放后得到的機(jī)器人踝、髖關(guān)節(jié)運(yùn)動(dòng)數(shù)據(jù)中提取出步長(l)、步幅(s)、步寬(w)、步高(Sh)、步態(tài)周期(T)、髖部波紋(Hw)、髖關(guān)節(jié)側(cè)向擺動(dòng)幅度(K)。其中，步幅等于2倍的步長[11]，將人體步態(tài)周期擴(kuò)大n倍后得到T,n∈[4,6]。

4 人形機(jī)器人參數(shù)化軌跡規(guī)劃

依照人體髖、踝關(guān)節(jié)的運(yùn)動(dòng)軌跡曲線并結(jié)合機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)進(jìn)行機(jī)器人髖、踝關(guān)節(jié)參數(shù)化的軌跡規(guī)劃。軌跡規(guī)劃方式有正弦函數(shù)變上限積分等，通過對(duì)比各方法的優(yōu)劣性,分析各方法軌跡規(guī)劃結(jié)果與3.2節(jié)幾何縮放后的關(guān)節(jié)運(yùn)動(dòng)數(shù)據(jù)之間的相似度,結(jié)果顯示五次樣條插值算法規(guī)劃的關(guān)節(jié)運(yùn)動(dòng)軌跡的平均相似度為95%以上,因此選用該方法。

4.1 踝關(guān)節(jié)軌跡規(guī)劃

采用五次樣條插值算法計(jì)算擺動(dòng)腳踝關(guān)節(jié)的運(yùn)動(dòng)軌跡。踝關(guān)節(jié)前進(jìn)方向的運(yùn)動(dòng)軌跡多項(xiàng)式和約束條件分別為

Ya(t)=a6t5+a5t4+a4t3+a3t2+a2t+a1.

(9)

(10)

其中,t∈[t0,t0+T];n∈(0,1)。

任取一個(gè)步態(tài)周期中的機(jī)器人運(yùn)動(dòng)參數(shù)，得到踝關(guān)節(jié)前進(jìn)方向的軌跡多項(xiàng)式為

Ya(t)=-0.019 4t5+0.409 4t4-

3.059 4t3+8.539 8t2.

(11)

踝關(guān)節(jié)在豎直方向的運(yùn)動(dòng)軌跡分為上升期和下降期,約束條件分別為

(12)

(13)

其中,n∈(0,1)。

則可得到運(yùn)動(dòng)軌跡曲線如圖3所示,軌跡多項(xiàng)式見式(14)。

(14)

圖3 踝關(guān)節(jié)在Z方向的軌跡曲線Fig.3 Ankle joint trajectory curve in Z direction

由圖可知，機(jī)器人踝關(guān)節(jié)前進(jìn)速度基本保持不變，擺動(dòng)腿上升過程中,踝關(guān)節(jié)在豎直方向的運(yùn)動(dòng)速度為先增大后減少。

4.2 髖關(guān)節(jié)軌跡規(guī)劃

將右髖關(guān)節(jié)在橫向方向的運(yùn)動(dòng)分為三部分,t0時(shí)刻髖關(guān)節(jié)相對(duì)于起步前兩髖關(guān)節(jié)中心點(diǎn)的距離為定值f,則可求解得到一個(gè)步行周期內(nèi)的髖關(guān)節(jié)軌跡多項(xiàng)式:

(15)

髖關(guān)節(jié)X軸上的軌跡曲線如圖4所示。

圖4 髖關(guān)節(jié)在X方向的軌跡曲線Fig.4 Hip joint trajectory curve in X direction

右髖關(guān)節(jié)在前進(jìn)和豎直方向軌跡曲線求取方法與踝關(guān)節(jié)類似。

5 人形機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)優(yōu)化

為了將機(jī)器人步行樣本中的一組最優(yōu)步態(tài)樣本應(yīng)用到機(jī)器人的步態(tài)規(guī)劃當(dāng)中,通過分析得出：機(jī)器人步行樣本中的每個(gè)步態(tài)周期的運(yùn)動(dòng)學(xué)參數(shù)都不一致,影響機(jī)器人步態(tài)的主要參數(shù)有步長、步高等。因此,本節(jié)將使用優(yōu)化算法獲取最優(yōu)步態(tài)樣本,通過3.3節(jié)提取的運(yùn)動(dòng)學(xué)參數(shù)的邊界范圍如下:

(16)

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

(1)穩(wěn)定性函數(shù)

將支撐腳掌的區(qū)域作為穩(wěn)定區(qū)域,將質(zhì)心在地面的投影點(diǎn)到支撐腳的中心的距離作為目標(biāo)函數(shù),質(zhì)心由髖關(guān)節(jié)位置表示，穩(wěn)定性函數(shù)如下:

(17)

其中,(xc,yc)代表質(zhì)心的位置;(xf,yf)代表支撐腳的中心位置。

(2)能量消耗函數(shù)

質(zhì)心升高導(dǎo)致的能量消耗E1的表達(dá)式如下:

E1=Mg(Hw+Hchsin(η+α)).

(18)

其中,M為機(jī)器人總質(zhì)量;Hch為質(zhì)心到髖關(guān)節(jié)的距離;假設(shè)用線段lch連接髖關(guān)節(jié)與質(zhì)心,則α代表兩髖關(guān)節(jié)的連線與線段lch的夾角。擺動(dòng)腿抬升消耗的能量E2表達(dá)式:

E2=mgsh.

(19)

其中,m為擺動(dòng)腿的質(zhì)量。

所以總體能耗E的目標(biāo)函數(shù)表達(dá)式為

E=E1+E2.

(20)

(3)步行速度函數(shù)

步行速度是步長和步行周期的比值，將步行速度的倒數(shù)作為目標(biāo)函數(shù),表達(dá)式為

(21)

5.2 基于多目標(biāo)粒子群的參數(shù)優(yōu)化與仿真

多目標(biāo)粒子群優(yōu)化算法采用速度-位置模型,具有算法簡單易實(shí)現(xiàn)等優(yōu)點(diǎn)[12]。針對(duì)優(yōu)化目標(biāo),引入了Skew Tent混沌映射初始化種群[13],以提高種群初始解分布的均勻性。

采用Matlab進(jìn)行編程,設(shè)定N=100,D=6,T=100,種群搜索空間如式(16)所示。分析算法運(yùn)行得到的目標(biāo)函數(shù)值,發(fā)現(xiàn)若機(jī)器人在一個(gè)步態(tài)周期內(nèi)具有較高的步行穩(wěn)定性,則會(huì)以降低步行速度為代價(jià)。針對(duì)這種現(xiàn)象并結(jié)合本研究對(duì)機(jī)器人步行速度要求不高的情況,選取了一組最優(yōu)的設(shè)計(jì)變量值。然后從機(jī)器人的步行樣本中選出一組與最優(yōu)設(shè)計(jì)變量相似度最高的參數(shù)作為機(jī)器人行走的運(yùn)動(dòng)學(xué)參數(shù)。選取運(yùn)動(dòng)學(xué)參數(shù)如下:l=10.4 cm,Sh=3.8 cm,T=2.1s,HW=3.5 cm,k=6.6 cm,η=21.5°。

6 實(shí)驗(yàn)驗(yàn)證

6.1 逆運(yùn)動(dòng)學(xué)

將機(jī)器人運(yùn)動(dòng)學(xué)解耦為前向和側(cè)向平面上。借鑒文獻(xiàn)[14]的運(yùn)動(dòng)學(xué)模型建立思想,進(jìn)行前向運(yùn)動(dòng)學(xué)建模時(shí),忽略側(cè)向運(yùn)動(dòng)的影響,在進(jìn)行側(cè)向運(yùn)動(dòng)學(xué)建模時(shí),考慮前向運(yùn)動(dòng)腿部彎曲對(duì)其產(chǎn)生的影響[14]。建立機(jī)器人運(yùn)動(dòng)學(xué)模型。參考文獻(xiàn)[15]的逆運(yùn)動(dòng)學(xué)解析法。利用解析法對(duì)理想的機(jī)器人下肢關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行逆運(yùn)動(dòng)學(xué)求解。

由于機(jī)器人的硬件限制,下肢各關(guān)節(jié)角度都有活動(dòng)范圍,因此逆運(yùn)動(dòng)學(xué)求解的關(guān)節(jié)角度應(yīng)滿足:

θimin<θi<θimax.

(22)

其中,i=1,2,…,n。

若機(jī)器人的關(guān)節(jié)角度超出極限范圍會(huì)對(duì)關(guān)節(jié)電機(jī)造成影響,所以針對(duì)異常的關(guān)節(jié)角度應(yīng)采用如下處理方式:

(23)

通過編寫python語言代碼求解得到機(jī)器人下肢各關(guān)節(jié)運(yùn)動(dòng)角度θi,圖5為機(jī)器人右腿各關(guān)節(jié)的角度變化曲線。由圖可知，右髖關(guān)節(jié)翻滾角和右踝關(guān)節(jié)翻滾角存在大小相等、方向相反的關(guān)系,這是由于在機(jī)器人側(cè)向平面內(nèi),只有髖、踝關(guān)節(jié)的兩個(gè)翻滾自由度,故行走中髖、踝關(guān)節(jié)翻滾角度軌跡變化曲線遵循上述關(guān)系。

圖5 右腿各關(guān)節(jié)角度變化曲線Fig.5 Change curve of right leg joint angle

右髖關(guān)節(jié)俯仰角起初隨著時(shí)間的變化而減小，是因?yàn)闄C(jī)器人右擺腿初始運(yùn)動(dòng)過程中，右大腿繞髖關(guān)節(jié)俯仰自由度做擺動(dòng)，髖關(guān)節(jié)俯仰角度發(fā)生伸展，從而使擺動(dòng)腿向前擺動(dòng)。上述時(shí)間變化過程中，右膝關(guān)節(jié)俯仰角逐漸增大，是由于隨著機(jī)器人擺動(dòng)腿向前運(yùn)動(dòng)，膝關(guān)節(jié)俯仰角度發(fā)生屈曲，從而使擺動(dòng)腿的離地高度逐漸增加。隨著時(shí)間的變化，髖關(guān)節(jié)俯仰角度開始出現(xiàn)增大的趨勢，是因?yàn)橛覕[動(dòng)腿下落過程中，髖關(guān)節(jié)俯仰角度發(fā)生屈曲，使機(jī)器人右大腿高度降低。而這段時(shí)間變化過程中，膝關(guān)節(jié)俯仰角度開始逐漸減小，是為了使機(jī)器人擺動(dòng)腳能夠平穩(wěn)著地，使機(jī)器人從右腿擺動(dòng)狀態(tài)過渡到右腿支撐狀態(tài)，所以膝關(guān)節(jié)俯仰角度發(fā)生了伸展變化。機(jī)器人右踝關(guān)節(jié)俯仰角隨著時(shí)間的變化先減小后增大，是因?yàn)闄C(jī)器人擺動(dòng)腿的腳掌與地面始終保持平行的狀態(tài)，隨著擺動(dòng)腿的運(yùn)動(dòng)，踝關(guān)節(jié)俯仰角度先后發(fā)生伸展和屈曲，從而使機(jī)器人能夠穩(wěn)定的平腳著地。

6.2 基于虛擬NAO機(jī)器人的仿真實(shí)驗(yàn)

使用NAO提供的編程工具Choregraphe作為實(shí)驗(yàn)平臺(tái)。在新建的指令盒內(nèi)使用python語言編寫驅(qū)動(dòng)機(jī)器人行走的指令代碼,將軟件連接虛擬機(jī)器人并運(yùn)行文件,讓機(jī)器人按照規(guī)劃的步態(tài)行走,實(shí)際運(yùn)行狀態(tài)如圖6所示。從仿真效果來看,機(jī)器人可以實(shí)現(xiàn)穩(wěn)定行走,說明步態(tài)規(guī)劃合理可行。

圖6 虛擬機(jī)器人行走仿真Fig.6 Walking simulation of virtual robot

6.3 基于NAO的物理樣機(jī)實(shí)驗(yàn)

首先使用無線網(wǎng)絡(luò)將物理樣機(jī)與計(jì)算機(jī)相連接,然后在Choregraphe指令盒內(nèi)調(diào)用AL motion 模塊中的Angle Interpolation并將關(guān)節(jié)角度序列編寫到指令代碼中驅(qū)動(dòng)機(jī)器人行走。根據(jù)機(jī)器人行走過程中腳底壓力傳感器的位置和壓力值計(jì)算出實(shí)際ZMP軌跡[16],如圖7所示。圖中ZMP軌跡全部位于支撐多邊形內(nèi)部,在支撐區(qū)域中心附近發(fā)生小范圍波動(dòng),但整體穩(wěn)定裕度較大。

圖7 ZMP軌跡曲線Fig.7 ZMP trajectory curve

圖8、圖9分別為髖關(guān)節(jié)和踝關(guān)節(jié)的規(guī)劃軌跡和仿真軌跡,選用EDR算法(編輯距離算法)判斷兩種軌跡曲線的相似性,結(jié)果顯示相似度較高,由于規(guī)劃的髖、踝關(guān)節(jié)軌跡與人體步行樣本的平均相似度較高,間接表明仿真軌跡與人體步行樣本具有較高相似性,反應(yīng)出機(jī)器人行走步態(tài)的擬人特性。

圖8 髖關(guān)節(jié)縱向位移對(duì)比Fig.8 of Thelongitudinal displacement comparison of hipjoint

圖9 踝關(guān)節(jié)縱向位移對(duì)比Fig.9 The longitudinal displacement comparison of ankle joint

從實(shí)驗(yàn)結(jié)果來看,機(jī)器人可以按照規(guī)劃的步態(tài)穩(wěn)定行走,從而驗(yàn)證了步態(tài)規(guī)劃和運(yùn)動(dòng)學(xué)參數(shù)優(yōu)化的有效性,如圖10所示。

圖10 NAO物理樣機(jī)實(shí)驗(yàn)Fig.10 Physical prototype experiment of NAO

7 結(jié) 論

(1)對(duì)人體步行樣本進(jìn)行幾何縮放,解決了機(jī)器人與人在肢體長度、幾何配置等方面存在的差異,使人體運(yùn)動(dòng)軌跡映射到機(jī)器人的運(yùn)動(dòng)空間上。

(2)五次樣條插值算法規(guī)劃的參數(shù)化軌跡平均相似度為95%以上；使用多目標(biāo)粒子群優(yōu)化算法得到滿足三個(gè)設(shè)定目標(biāo)的最優(yōu)運(yùn)動(dòng)學(xué)參數(shù),使機(jī)器人獲得能耗較低的行走步態(tài),若機(jī)器人想擁有高穩(wěn)態(tài)的步態(tài),則步行速度會(huì)降低。

(3)在側(cè)向平面內(nèi),隨著機(jī)器人步態(tài)運(yùn)動(dòng),髖、踝關(guān)節(jié)翻滾角度出現(xiàn)大小相等、方向相反的變化趨勢；分析規(guī)劃數(shù)據(jù)與仿真數(shù)據(jù)的相似性得出模仿人類步行的步態(tài)規(guī)劃方法使機(jī)器人行走步態(tài)具有擬人特性。