動態(tài)雙足機器人建模及步態(tài)優(yōu)化設(shè)計

陸萬榮，許江淳，王志偉，任 杰

(昆明理工大學(xué)信息工程與自動化學(xué)院，云南 昆明 650500)

0 引言

目前，雙足機器人是人工智能領(lǐng)域的一大研究熱點。與傳統(tǒng)的輪式、履帶和爬行機器人相比，它對行走環(huán)境適應(yīng)性好、能耗低、靈活性高，在醫(yī)療、教育、危險工業(yè)、航空航天和服務(wù)業(yè)等領(lǐng)域具有良好的應(yīng)用前景和使用價值[1]。因此，國內(nèi)外相關(guān)機構(gòu)對雙足機器人相關(guān)特性展開了深入研究，如Goswami教授提出了Compass-like機器人模型，進行了數(shù)值仿真和動態(tài)分析[2]；密歇根大學(xué)的Park等人實現(xiàn)了Rabbit機器人的動態(tài)行走等[3]。從1968年早稻田大學(xué)的WAP系列機器人到2006年WABIAN-2R機器人，再從美國的COG機器人到中國的BHR機器人，經(jīng)過幾十年的發(fā)展，盡管取得了不錯的研究成果，但這一領(lǐng)域仍然有許多難以解決的問題，如穩(wěn)定性、能耗和柔韌度等。

本文通過對機器人的穩(wěn)定性和能耗尋優(yōu)，建立多目標(biāo)進化函數(shù)，實現(xiàn)機器人的穩(wěn)定行走。以質(zhì)心、步行周期、步行單元和步長等參數(shù)來描述雙足機器人的行走過程。在尋優(yōu)過程中，利用改進的擴展遺傳算法(strength pareto evaluation algorithm，SPEA)，從穩(wěn)定性和能耗兩個方面進行計算，得到仿真需要的相關(guān)參數(shù)。在仿真過程中，與文獻[4]中的原始倒立擺規(guī)劃方案和文獻[5]中提到的遺傳算法優(yōu)化方案進行比較，驗證了本文所述方案的有效性。

1 D-H坐標(biāo)法雙足機器人建模

1.1 位置和姿態(tài)的坐標(biāo)描述

雙足機器人簡化多連桿模型如圖1所示。

圖1 雙足機器人簡化多連桿模型

D-H建模方法是在機器人每根連桿上都建立一個坐標(biāo)系，以4×4的齊次變換矩陣表示相鄰連桿之間的坐標(biāo)關(guān)系[6]。建立好坐標(biāo)系后，則空間中的任意一點都可以用一個3×1的位置矢量來表示。位置矢量Ap在直角坐標(biāo)系{A}中可以用3×1的列向量表示為：

Ap=[pxpypz]

(1)

(2)

(3)

(4)

1.2 正運動學(xué)建模

正運動學(xué)是雙足機器人仿真的基礎(chǔ)，可以計算雙足機器人的重心，描述雙足機器人的空間姿態(tài)[7]。圖1中，將雙足機器人簡化為只考慮踝關(guān)節(jié)、膝關(guān)節(jié)和髖關(guān)節(jié)的12自由度機器人，相鄰自由度之間桿件視為均質(zhì)的連桿，忽略連桿之間的摩擦力和間隙。每個連桿處的自由度均為獨立的從屬坐標(biāo)系。

D-H模型參數(shù)見表1。

表1 D-H模型參數(shù)表

表1中：θi為垂直于關(guān)節(jié)i軸線的兩個公垂線的夾角，即連桿夾角；di為沿關(guān)節(jié)i軸線的兩個公垂線的距離，即連桿距離；ai-1兩關(guān)節(jié)軸線沿公垂線的距離，即連桿長度；αi-1為垂直于αi所在平面內(nèi)兩關(guān)節(jié)軸線的夾角，即連桿扭角。

當(dāng)連桿參數(shù)和坐標(biāo)系確定后，依次左乘低一階的變換矩陣，就能得到坐標(biāo)系{i}對于起始坐標(biāo)系的變換矩陣：

(5)

則:

(6)

(7)

1.3 逆運動學(xué)建模

逆運動學(xué)則與正運動學(xué)相反，已知相對于基礎(chǔ)坐標(biāo)系的位姿，根據(jù)連桿參數(shù)求各關(guān)節(jié)角度值[8]。在1.2節(jié)中已經(jīng)把雙足機器人簡化為12自由度的連桿模型。根據(jù)機器人逆運動學(xué)理論，當(dāng)自由度小于等于12時，逆運動學(xué)問題必有解且為點數(shù)值解。

本文計算逆解使用效率高、實時控制性好的封閉解法。求解思路是將位姿矩陣的逆矩陣按次序乘以正運動學(xué)方程，找出右端矩陣中的常數(shù)項與左端建立相等關(guān)系式，出現(xiàn)多解情況時，要根據(jù)實際情況合理選擇。在求解之前，假設(shè)雙足機器人身軀始終不傾斜，足底與地面水平接觸。由踝關(guān)節(jié)、膝關(guān)節(jié)和髖關(guān)節(jié)行走時自由度之間的關(guān)系和假設(shè)條件，支撐腿和擺動腿的數(shù)學(xué)模型如圖2所示。圖2中：θ1=-θ3；θ2=0；θ4+θ5+θ6=0。

圖2 支撐腿和擺動腿數(shù)學(xué)模型

建立等式：

(8)

將對應(yīng)矩陣代入，就能得到如下關(guān)系式：

(9)

整理得：

pysinθ4

(10)

(11)

求得θ4后，根據(jù)下式求解θ5：

(12)

解得θ5=atan2(sinθ5,cosθ5),因此θ6=-(θ4+θ5)。

支撐腿的計算過程與上述方法一致，限于篇幅這里不再贅述，只給出結(jié)果：

(13)

2 步態(tài)優(yōu)化設(shè)計

2.1 三維線性倒立擺

雙足機器人是一個高階、強耦合、非線性的復(fù)雜系統(tǒng)。對于這樣一個復(fù)雜系統(tǒng)，可以先通過假設(shè)作一些簡化處理：①將機器人所有部件質(zhì)量看作一個支點；②倒立擺運動只限定在垂直地面的平面上；③以落腳點為支點，可繞其轉(zhuǎn)動，連桿長度可調(diào)節(jié)[9-11]。

三維線性倒立擺(3D linear inverted pendulum,3D LIP)是目前雙足步行規(guī)劃中使用廣泛的模型。三維線性倒立擺如圖3所示。

機器人腳底在x、y、z所受力的分量為fx、fy、fz,所受力的力矩分量為τx、τy、τz。根據(jù)文獻[11]提出的原理，能量消耗在力矩較小時變小，τx、τy均為0，那么可以將該模型簡化為x軸、y軸方向兩個部分。其運動方程如下：

(14)

解微分方程得到質(zhì)心關(guān)于時間的函數(shù)：

(15)

(16)

只要知道了質(zhì)心在x、y方向的初始位置和速度，就可以完成整個周期的質(zhì)心(COM)規(guī)劃。

圖3 三維線性倒立擺示意圖

2.2 多目標(biāo)進化算法及改進策略

①髖關(guān)節(jié)的最大角速度ω_max與任意時刻關(guān)節(jié)j的最大角速度滿足：

ωi(t)≤ωj_max

(17)

②質(zhì)心到踝關(guān)節(jié)的長度與最大等效腿長滿足：

(18)

式中：pos_z(t)、p_l(t)、p_r(t)分別為質(zhì)心和左右踝關(guān)節(jié)的位置；ld2為最大腿長l_max的平方與髖關(guān)節(jié)到質(zhì)心的距離d的平方之和。

③步行步長和步寬由l_max和zc約束，滿足：

(19)

④設(shè)單步時間T內(nèi)，髖關(guān)節(jié)轉(zhuǎn)過的角度為q，有：

ω_maxT≤q

(20)

本文將問題抽象為多目標(biāo)函數(shù)優(yōu)化問題，分別得到關(guān)于穩(wěn)定性和能耗的目標(biāo)函數(shù)。

①質(zhì)心到支撐腿中心的距離是影響機器人行走穩(wěn)定性的重要因素，越靠近支撐中心越穩(wěn)定。因此，穩(wěn)定性目標(biāo)函數(shù)表示為：

(21)

式中：x2、y2分別為x、y方向任意時刻t支撐中心與質(zhì)心位置差值的平方。

②將擺動腿擺動的高度作為能耗大小的重要指標(biāo)，能耗目標(biāo)函數(shù)表示為：

(22)

式中:z(t)為z軸方向擺動腿運動軌跡。

對以上目標(biāo)函數(shù)的最優(yōu)化問題，可以轉(zhuǎn)換為構(gòu)造懲罰函數(shù)對約束條件hi(x)的最小化問題。設(shè)懲罰函數(shù)為：

p(x,M)=M∑imin [0,hi(x)]2

(23)

M為懲罰因子，將約束條件帶入目標(biāo)函數(shù)，利用改進SPEA算法求解Pareto最優(yōu)解集。設(shè)初始種群P的規(guī)模為N，存檔集Q和局部存檔集規(guī)模均為M。改進SPEA算法步驟如下。

①初始化種群P和存檔集Q、L。

②計算P、Q和L中所有個體的適應(yīng)度。

③將Pt、Qt、LT中的非支配個體保存到Qt+1中，若其規(guī)模不為M，則調(diào)整其規(guī)模滿足分布要求。

④如果滿足設(shè)置的停止條件，則將Qt+1中的非支配個體返回；否則繼續(xù)。

⑤對Qt+1進行競標(biāo)賽選擇。

⑥對Qt+1進行交叉、變異操作，并將結(jié)果保存至Pt+1，且更新Pt+1的部分個體，令t=t+1，繼續(xù)步驟②。

3 虛擬樣機仿真和分析

采用ADAMS和Matlab聯(lián)合仿真進行仿真測試,ADAMS中可以建立逼真的三維模型，并具有動力學(xué)和運動學(xué)特性。通過相關(guān)接口將參數(shù)特性傳遞給Matlab/Simulink。后者又可以對步態(tài)進行規(guī)劃，將規(guī)劃好的步態(tài)信息通過控制模塊再傳遞給ADAMS，從而建立起機器人的動態(tài)行走并且進行實時的性能分析評估。

3.1 雙足機器人建模

雙足機器人的各部分參數(shù)取值來自175 cm成年人體型特征，按一定的比例縮小選取。繪制小腿、大腿、腳部和上體質(zhì)量塊是需要通過邊緣剪切、過孔/打孔的方式繪制對應(yīng)關(guān)節(jié)的轉(zhuǎn)動樣式。而關(guān)節(jié)繪制時，為了滿足步態(tài)規(guī)劃的準確性，需要注意轉(zhuǎn)動軸的個數(shù)；數(shù)量不同，繪制的復(fù)雜度就不一樣。踝關(guān)節(jié)前向、側(cè)向兩個轉(zhuǎn)動軸需要交于一點；髖關(guān)節(jié)前向、側(cè)向和豎直三個轉(zhuǎn)動軸也需要交于一點；而膝關(guān)節(jié)較為簡單，只有前向俯仰一個旋轉(zhuǎn)軸。建立好模型后，各部件之間仍然是相互獨立的，需要在部件與連桿之間建立聯(lián)系，在ADAMS中對不同部件添加約束，完成完整的建模過程。各部分和整體組合模型如圖4所示。

圖4 各部分和整體模型示意圖

動態(tài)行走3D觀測圖如圖5所示。

圖5 動態(tài)行走3D觀測圖

3.2 仿真結(jié)果和分析

圖6 角速度變化圖

從圖6可以看出，雙足機器人側(cè)向搖擺具有整體性和一致性。踝關(guān)節(jié)和髖關(guān)節(jié)角度變化情況相同，呈周期性變化。

CM、CM1、CM2分別表示本文優(yōu)化方案、遺傳算法優(yōu)化方案和原始倒立擺方案左腿質(zhì)心在前向、側(cè)向、和豎直方向三個方向速度的變化曲線。通過對比，遺傳算法方案前向速度變化范圍較大，原始倒立擺方案存在明顯突刺，而本文使用的方案變化情況較為折中。

左腿質(zhì)心不同方向速度變化如圖7所示。

圖7 左腿質(zhì)心不同方向速度變化圖

左腿質(zhì)心前向加速度變化如圖8所示。從圖8可以看出，在擺動腿和支撐對互換時，加速度變化都比較明顯，但是CM1和CM2產(chǎn)生的突刺幅度較大，對質(zhì)心加速度的影響更大。

圖8 左腿質(zhì)心前向加速度變化圖

4 結(jié)束語

本文對雙足機器人的步態(tài)規(guī)劃進行了仿真研究。先在雙足機器人多連桿模型上進行D-H法正逆運動學(xué)建模。接著進行了步態(tài)優(yōu)化設(shè)計，建立了以穩(wěn)定性和能耗為目標(biāo)的尋優(yōu)函數(shù)，利用改進的SPEA算法解得最佳參數(shù)。最后利用得到的參數(shù)使用ADAMS/Matlab進行聯(lián)合仿真分析，與原始倒立擺規(guī)劃和遺傳優(yōu)化方案作對比。由雙足機器人左腿質(zhì)心在前向、側(cè)向和豎直方向的速度和加速度對比結(jié)果可知，本文提出的優(yōu)化方案變化幅度最為平緩，穩(wěn)定性效果最優(yōu)，可以為機器人步態(tài)規(guī)劃方面的相關(guān)研究提供一定的參考。