杜威,楊忠,許昌亮,張輝斌
(南京航空航天大學(xué) 自動(dòng)化學(xué)院,江蘇 南京 210016)
四足機(jī)器人(quadrupedrobot,QR)屬于足式機(jī)器人,相對于履帶式和輪式機(jī)器人,具有機(jī)動(dòng)性好、環(huán)境破壞程度小、主動(dòng)隔振等優(yōu)點(diǎn),應(yīng)用前景良好。該型機(jī)器人涉及力學(xué)、機(jī)械學(xué)、生物學(xué)、計(jì)算機(jī)、人工智能等多學(xué)科,相關(guān)技術(shù)一直是國內(nèi)外機(jī)器人領(lǐng)域研究的熱點(diǎn)之一。以美國卡耐基梅隆大學(xué)的波士頓動(dòng)力實(shí)驗(yàn)室為代表,2005年發(fā)布了BigDog大狗機(jī)器人,又先后推出了如AlphaDog、LittleDog、Cheetah等產(chǎn)品,引起行業(yè)內(nèi)的廣泛關(guān)注。
能否在不同地形條件下實(shí)現(xiàn)平穩(wěn)運(yùn)動(dòng),是一個(gè)機(jī)器人是否具有實(shí)際使用價(jià)值的評判標(biāo)準(zhǔn)。斜面是自然界中常見的地形,一些崎嶇的地形也可以等效為斜面。因此,研究四足機(jī)器人在斜面上的運(yùn)動(dòng)控制具有重要意義。經(jīng)典控制和現(xiàn)代控制在設(shè)計(jì)過程中一般是基于被控對象的精確數(shù)學(xué)模型。對于四足機(jī)器人來說,因其具有非線性、高耦合、模型復(fù)雜等特點(diǎn),采用傳統(tǒng)的控制理論和方法往往存在局限性,難以取得滿意的控制效果[1]。模糊控制是建立在模糊集合論基礎(chǔ)上的一種基于語言規(guī)則與模糊推理的控制理論,在控制非線性、高耦合及參數(shù)和結(jié)構(gòu)不確定的復(fù)雜過程中具有較好的控制效果。
在對四足機(jī)器人運(yùn)動(dòng)學(xué)建模的基礎(chǔ)上,從機(jī)身姿態(tài)、足端軌跡以及步態(tài)周期3個(gè)方面分析并設(shè)計(jì)了四足機(jī)器人斜面運(yùn)動(dòng)方式,并建立了一種應(yīng)用于四足機(jī)器人斜面運(yùn)動(dòng)的模糊控制系統(tǒng),最后通過仿真實(shí)驗(yàn)證明所設(shè)計(jì)的控制系統(tǒng)具有較好的控制效果。
四足機(jī)器人簡化模型由機(jī)身、大腿和小腿等構(gòu)件組成,構(gòu)件間由旋轉(zhuǎn)關(guān)節(jié)連接,連接部分分別為負(fù)責(zé)橫向擺動(dòng)的臀關(guān)節(jié)、負(fù)責(zé)上下抬腿的髖關(guān)節(jié)以及膝關(guān)節(jié)。臀關(guān)節(jié)負(fù)責(zé)內(nèi)外擺,實(shí)現(xiàn)機(jī)身左右擺動(dòng);髖關(guān)節(jié)和膝關(guān)節(jié)分別負(fù)責(zé)大腿和小腿的抬起和落下,實(shí)現(xiàn)機(jī)器人前后運(yùn)動(dòng)。四足機(jī)器人采用內(nèi)膝肘式設(shè)計(jì),以前進(jìn)方向?yàn)榛鶞?zhǔn),將左前腿、左后腿、右前腿以及右后腿分別命名為LF、LH、RF、RH。關(guān)節(jié)連桿結(jié)構(gòu)見圖1。
圖1 四足機(jī)器人的關(guān)節(jié)連桿結(jié)構(gòu)及坐標(biāo)系定義
采用D-H參數(shù)法[2]確定機(jī)器人各構(gòu)件間的位姿關(guān)系。四足機(jī)器人的坐標(biāo)系定義示意如圖1所示。以RH為例,定義世界坐標(biāo)系{W},機(jī)身坐標(biāo)系{B},以及足端坐標(biāo)系{E}。同時(shí)定義臀關(guān)節(jié)坐標(biāo)系{1},髖關(guān)節(jié)坐標(biāo)系{2},膝關(guān)節(jié)坐標(biāo)系{3},以上3個(gè)坐標(biāo)系坐標(biāo)原點(diǎn)分別位于3個(gè)關(guān)節(jié)位置,坐標(biāo)系方向由D-H法規(guī)范確定。
基于上述四足機(jī)器人的關(guān)節(jié)連桿結(jié)構(gòu)以及坐標(biāo)系定義,可從足端坐標(biāo)系沿各關(guān)節(jié)連桿依次向機(jī)身坐標(biāo)系進(jìn)行齊次變換,得到足端坐標(biāo)系到機(jī)身坐標(biāo)系的轉(zhuǎn)換矩陣:
Tbe=Ab1A12A23A3e
(1)
式中:Ab1,A12,A23,A3c為坐標(biāo)系{B}、{1}、{2}、{3}、{E}之間的齊次變換矩陣。由D-H參數(shù)可確定上述各矩陣,進(jìn)一步可以得到:
(2)
式中:si=sinθi,ci=cosθi,sij=sin(θi+θj),cij=cos(θi+θj);l1、l2、l3與θi為圖1中所示的各連桿及對應(yīng)角度;(a,b,c)為坐標(biāo)系{1}的坐標(biāo)原點(diǎn)相對于機(jī)身坐標(biāo)系{B}的位置。下標(biāo)i、j分別取1、2、3。
由式(2)可以得出:
(3)
(4)
矩陣Rbe反映了足端坐標(biāo)系到機(jī)身坐標(biāo)系的旋轉(zhuǎn)關(guān)系,三維矢量Pbe反映了足端在機(jī)身坐標(biāo)系中與質(zhì)心的位置關(guān)系,兩者共同體現(xiàn)了機(jī)器人足端的位姿。
斜面運(yùn)動(dòng)規(guī)劃的關(guān)鍵是保證四足機(jī)器人在行走過程中的穩(wěn)定性。四足機(jī)器人本身具有較好的結(jié)構(gòu)穩(wěn)定性,但在斜面運(yùn)動(dòng)過程中,由于受到重力的作用,機(jī)體姿態(tài)以及運(yùn)動(dòng)步態(tài)的變化都會(huì)對其運(yùn)動(dòng)的穩(wěn)定性造成影響[3]。本節(jié)從機(jī)體姿態(tài)、足端軌跡以及步態(tài)周期3個(gè)方面對四足機(jī)器人的斜面運(yùn)動(dòng)規(guī)劃進(jìn)行研究。
四足機(jī)器人在斜面運(yùn)動(dòng)時(shí)的機(jī)體姿態(tài)一般有水平姿態(tài)以及平行于斜面姿態(tài)兩種[4],如圖2所示。相對于水平姿態(tài)而言,平行于斜面姿態(tài)具有前后腿的運(yùn)動(dòng)范圍相當(dāng)、足端軌跡規(guī)劃簡單、穩(wěn)定裕度高等優(yōu)勢[4],故四足機(jī)器人在斜面運(yùn)動(dòng)過程中采用平行于斜面姿態(tài)。
圖2 四足機(jī)器人兩種機(jī)體運(yùn)動(dòng)姿態(tài)
常用的足端軌跡曲線包括拋物線、心形線、擺線等。本文選取一種零沖擊足端軌跡的規(guī)劃方法,其具有沖擊小,曲線圓滑,落地角度垂直不易打滑等優(yōu)點(diǎn)[5],在坐標(biāo)系{W}下,水平與豎直方向的足端軌跡數(shù)學(xué)表達(dá)式分別如式(5)與式(6)。式中,S為步幅,H為抬腿高度,Tm為擺動(dòng)相周期。
(5)
(6)
步態(tài)選擇方面,選取穩(wěn)定性高的三角步態(tài)作為四足機(jī)器人在斜面上的運(yùn)動(dòng)步態(tài),并采用LF-RH-RF-LH的腿部運(yùn)動(dòng)順序以獲得最優(yōu)的穩(wěn)定裕度[6]。本文的運(yùn)動(dòng)步態(tài)時(shí)序圖如圖3所示。其中,黑色矩形表示腿部處于支撐相,白色矩形表示腿部處于擺動(dòng)相。
圖3 三角步態(tài)運(yùn)動(dòng)時(shí)序
用模糊控制方法解決控制問題時(shí),重點(diǎn)在于對控制中所可能出現(xiàn)的各種情形加以分析,依據(jù)控制者的經(jīng)驗(yàn)和先驗(yàn)知識,尋求解決的一般方法,并用模糊控制規(guī)則的形式加以體現(xiàn),模糊控制的精度將依賴于模糊控制規(guī)則制定得是否合理和詳盡[7-8]。
圖4是開環(huán)斜面運(yùn)動(dòng)過程中的機(jī)身滾轉(zhuǎn)角變化仿真圖。1.3s與2.1s分別是第一個(gè)運(yùn)動(dòng)周期內(nèi)RH與LH處于擺動(dòng)相的時(shí)刻,可以看到這兩個(gè)時(shí)刻的機(jī)身滾轉(zhuǎn)角絕對值較大。觀察機(jī)器人的爬坡過程,可以發(fā)現(xiàn)RH與LH處于擺動(dòng)項(xiàng)時(shí),機(jī)器人機(jī)身會(huì)發(fā)生明顯傾斜,即RH處于擺動(dòng)相,機(jī)身明顯向右傾;LH處于擺動(dòng)相,機(jī)身明顯向左傾?,F(xiàn)做如下處理:若機(jī)身右傾,令LH的臀關(guān)節(jié)角度減少、髖關(guān)節(jié)角度增加;若機(jī)身左傾,令RH的臀關(guān)節(jié)角度減少、髖關(guān)節(jié)角度增加。以上操作均可減少機(jī)身的滾轉(zhuǎn)角度絕對值,使機(jī)身趨近水平位置。
圖4 開環(huán)運(yùn)動(dòng)過程的機(jī)身滾轉(zhuǎn)角變化
基于以上分析,設(shè)計(jì)應(yīng)用于四足機(jī)器人斜面運(yùn)動(dòng)的模糊控制系統(tǒng)。確定模糊控制系統(tǒng)為雙輸入四輸出,四足機(jī)器人機(jī)身的滾轉(zhuǎn)角(az)以及滾轉(zhuǎn)角的變化率(wz)作為輸入變量:
az∈X1=[-0.15,0.15] (rad)
wz∈X2=[-1,1] (rad)
LH的臀關(guān)節(jié)轉(zhuǎn)動(dòng)角度和髖關(guān)節(jié)轉(zhuǎn)動(dòng)角度(anglelh1、anglelh2)以及RH的臀關(guān)節(jié)轉(zhuǎn)動(dòng)角度和髖關(guān)節(jié)轉(zhuǎn)動(dòng)角度(anglerh1、anglerh2)作為輸出變量:
模糊控制系統(tǒng)的結(jié)構(gòu)圖如圖5所示。
圖5 模糊控制系統(tǒng)結(jié)構(gòu)
選定滾轉(zhuǎn)角az的模糊論域AZ={-2,-1,0,1,2};模糊集為{NB,NS,Z,PS,PB},分別對應(yīng)“負(fù)大”、“負(fù)小”、“零”、“正小”、“正大”;隸屬度函數(shù)種類選擇為“trimf”,如示意圖6所示。wz、anglelh1、anglerh1、anglelh2以及anglerh2的隸屬度函數(shù)圖像均與圖6類似,后文僅給出隸屬度函數(shù)對應(yīng)的參數(shù)。
圖6 az的隸屬度函數(shù)
選定滾轉(zhuǎn)角wz的模糊論域WZ={-2,-1,0,1,2};模糊集為{NB,NS,Z,PS,PB},分別對應(yīng)“負(fù)大”、“負(fù)小”、“零”、“正小”、“正大”;隸屬度函數(shù)種類選擇為“trimf”,隸屬度函數(shù)參數(shù)分別為:[-3, -2, -1] ,[-2, -1, 0] ,[-1, 0 ,1] ,[0, 1, 2] ,[1, 2, 3] 。
anglelh1、anglerh1、anglelh2以及anglerh2四者的論域與隸屬度函數(shù)設(shè)置相同。對應(yīng)論域分別為ANGLELH1、ANGLERH1、ANGLELH2、ANGLERH2;論域內(nèi)容均設(shè)置為{0,1,2,3};模糊集為{Z,S,M,B},分別對應(yīng)“零”、“小”、“中”、“大”;隸屬度函數(shù)種類選擇為“trimf”,隸屬度函數(shù)參數(shù)為[-1, 0, 1] ,[0, 1, 2] ,[1, 2, 3] ,[2, 3, 4] 。
根據(jù)2.1節(jié)的分析,對于所述雙輸入四輸出的模糊控制器采用以下的條件語句來描述控制規(guī)則:
If(Aisa)and(Bisb)then(Cisc)and(Disd)and(Eise)and(Fisf)
對于本文中應(yīng)用于四足機(jī)器人斜面運(yùn)動(dòng)的模糊控制器,設(shè)計(jì)規(guī)則如下:
規(guī)則1:若四足機(jī)器人的滾轉(zhuǎn)角(az)為正大(PB)且滾轉(zhuǎn)角的變化率(wz)為正大(PB),則LH的臀關(guān)節(jié)轉(zhuǎn)角(anglelh1)為零(Z)且LH的髖關(guān)節(jié)轉(zhuǎn)角(anglelh2)為零(Z)且RH的臀關(guān)節(jié)轉(zhuǎn)角(anglerh1)為大(B)且RH的髖關(guān)節(jié)轉(zhuǎn)角(anglerh2)為大(B)。
依照同樣的方法寫出其他規(guī)則,可以得到模糊控制規(guī)則表,如表1所示。表中LH1、LH2、RH1、RH2分別表示anglelh1、anglerh1、anglelh2、anglerh2。
表1 模糊控制規(guī)則
仿真實(shí)驗(yàn)采用Matlab與Adams的聯(lián)合仿真方式[9]。在Adams中建立四足機(jī)器人的三維模型,在Matlab中建立四足機(jī)器人的控制系統(tǒng),通過使用Adams/control模塊建立兩者間的數(shù)據(jù)通信通道。
需要說明的是,為減少M(fèi)atlab與Adams之間交換的數(shù)據(jù)量,在聯(lián)合仿真過程中僅針對LH與RH的臀關(guān)節(jié)與髖關(guān)節(jié)進(jìn)行數(shù)據(jù)交互。在開始仿真前,根據(jù)第一章規(guī)劃的斜面運(yùn)動(dòng)方式,預(yù)先設(shè)計(jì)開環(huán)狀態(tài)下所有關(guān)節(jié)的運(yùn)動(dòng)情況,并通過SPLINE函數(shù)導(dǎo)入到Adams模型中的對應(yīng)關(guān)節(jié)。仿真開始后,控制器生成的控制信號將會(huì)疊加在4個(gè)被控關(guān)節(jié)角之上,形成新的關(guān)節(jié)角度以驅(qū)動(dòng)模型運(yùn)動(dòng),完成聯(lián)合仿真[10]。
四足機(jī)器人機(jī)身及其他關(guān)節(jié)連桿的材料均選擇為鋁,密度為2.74e-6kg/mm3。為簡化模型,機(jī)身設(shè)計(jì)為長方體,長為300mm,寬為75mm,高為20mm;大小腿均設(shè)計(jì)為圓柱體,高為120mm,底面半徑為12.5mm;斜面設(shè)計(jì)坡度為5°,靜摩擦系數(shù)為0.3,動(dòng)摩擦系數(shù)為0.5。運(yùn)動(dòng)規(guī)劃方面,采用穩(wěn)定性較高的三角步態(tài),設(shè)置步幅為40mm,抬腿高度為50mm,運(yùn)動(dòng)周期為1.6s。
圖7為四足機(jī)器人在一個(gè)運(yùn)動(dòng)周期內(nèi)的行走過程示意圖。圖8為四足機(jī)器人分別在開環(huán)與模糊控制下運(yùn)動(dòng)過程的機(jī)身滾轉(zhuǎn)角變化對比圖。其中,虛線為開環(huán)運(yùn)動(dòng)狀態(tài)下機(jī)身滾轉(zhuǎn)角的變化情況,實(shí)線為經(jīng)模糊控制后機(jī)身滾轉(zhuǎn)角的變化情況。圖9為坐標(biāo)系下機(jī)身質(zhì)心在斜面運(yùn)動(dòng)過程中的運(yùn)動(dòng)軌跡,模型運(yùn)動(dòng)方向?yàn)閄軸負(fù)方向。
圖7 四足機(jī)器人的運(yùn)動(dòng)過程
圖9 坐標(biāo)系下機(jī)身質(zhì)心運(yùn)動(dòng)軌跡
對四足機(jī)器人的斜面運(yùn)動(dòng)方式進(jìn)行規(guī)劃,提出了一種應(yīng)用于四足機(jī)器人斜面運(yùn)動(dòng)的模糊控制方案。使用Matlab與Adams的聯(lián)合仿真進(jìn)行仿真實(shí)驗(yàn),證明了所設(shè)計(jì)的斜面運(yùn)動(dòng)規(guī)劃滿足四足機(jī)器人的正常運(yùn)動(dòng),所設(shè)計(jì)的模糊控制系統(tǒng)可以有效增加機(jī)器人的運(yùn)動(dòng)穩(wěn)定性,達(dá)到較好的控制效果。