陸萬(wàn)榮，許江淳，王志偉，任 杰

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

0 引言

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

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

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

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

雙足機(jī)器人簡(jiǎn)化多連桿模型如圖1所示。

圖1 雙足機(jī)器人簡(jiǎn)化多連桿模型

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

Ap=[pxpypz]

(1)

(2)

(3)

(4)

1.2 正運(yùn)動(dòng)學(xué)建模

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

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

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

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

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

(5)

則:

(6)

(7)

1.3 逆運(yùn)動(dòng)學(xué)建模

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

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

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

建立等式：

(8)

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

(9)

整理得：

pysinθ4

(10)

(11)

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

(12)

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

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

(13)

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

2.1 三維線性倒立擺

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

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

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

(14)

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

(15)

(16)

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

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

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

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

ωi(t)≤ωj_max

(17)

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

(18)

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

③步行步長(zhǎng)和步寬由l_max和zc約束，滿足：

(19)

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

ω_maxT≤q

(20)

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

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

(21)

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

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

(22)

式中:z(t)為z軸方向擺動(dòng)腿運(yùn)動(dòng)軌跡。

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

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

(23)

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

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

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

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

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

⑤對(duì)Qt+1進(jìn)行競(jìng)標(biāo)賽選擇。

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

3 虛擬樣機(jī)仿真和分析

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

3.1 雙足機(jī)器人建模

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

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

動(dòng)態(tài)行走3D觀測(cè)圖如圖5所示。

圖5 動(dòng)態(tài)行走3D觀測(cè)圖

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

圖6 角速度變化圖

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

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

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

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

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

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

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

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