眭耀宇,楊忠,游雨龍,許昌亮,周國興
1.南京航空航天大學(xué) 自動(dòng)化學(xué)院, 江蘇 南京 211106
2.南京航空航天大學(xué) 無人機(jī)研究院, 江蘇 南京 211106
腿足式機(jī)器人能夠在非結(jié)構(gòu)式環(huán)境中具有較好的靈活性與適應(yīng)性[1]。在足式機(jī)器人的領(lǐng)域中,雙足機(jī)器人的穩(wěn)定性欠佳,六足及以上又缺乏四足機(jī)器人的靈活性,因此,四足機(jī)器人已然成為新興的研究熱點(diǎn)[2]。四足機(jī)器人是MIMO的非線性動(dòng)力學(xué)系統(tǒng),迄今為止,四足機(jī)器人常用的控制方式有中樞神經(jīng)控制、虛擬模型控制和零力矩點(diǎn)控制法3種。
基于零力矩點(diǎn)(zero moment point,ZMP)[3?4]的控制方式能夠?qū)崿F(xiàn)穩(wěn)定的步行,在抗干擾方面也有優(yōu)勢,但是由于其需要實(shí)時(shí)考察其質(zhì)心位置,并將關(guān)節(jié)軌跡通過逆運(yùn)動(dòng)學(xué)求解出,所需計(jì)算量較為巨大,甚至有些步態(tài)無法滿足ZMP的要求;生物控制方法[5?6],相比于廣泛存在的基于模型和基于行為的控制方法,不需要進(jìn)行復(fù)雜的數(shù)學(xué)計(jì)算和分析,方法相對簡單,但是要想用于實(shí)際的工程中,需要結(jié)合生物運(yùn)動(dòng)資料或者優(yōu)化算法來確定控制的參數(shù)。而且在復(fù)雜地形中,機(jī)器人會(huì)受到來自環(huán)境的較大擾動(dòng),野外的地形往往不平整[7],因此很大程度上中樞模式發(fā)生器(central pattern generator,CPG)算法將不再適用。
虛擬模型控制(virtual model control,VMC)[8?9]方法綜合考慮機(jī)器人軀體位置、速度及姿態(tài)的控制,通過參數(shù)可調(diào),對位置與姿態(tài)進(jìn)行綜合跟蹤,無需對逆運(yùn)動(dòng)學(xué)進(jìn)行求解,控制效率高。麻省理工大學(xué)研制的Cheetah3[10]與Mini Cheetah采用虛擬模型控制與模型預(yù)測控制相結(jié)合的方式估計(jì)出足端觸地的虛擬力[11],并完成了跳躍[12]、后空翻等動(dòng)作。張國騰等[13]在四足機(jī)器人上融合了虛擬模型控制,并得到了較好的控制效果。但是現(xiàn)有的虛擬模型控制并沒有實(shí)現(xiàn)參數(shù)的跟隨變化,因此引入模糊自適應(yīng)參數(shù),根據(jù)反饋誤差對其控制參數(shù)進(jìn)行自適應(yīng)。
基于此,本文對四足機(jī)器人的運(yùn)動(dòng)控制開展研究,將自適應(yīng)模糊控制算法和VMC算法同時(shí)運(yùn)用到足端軌跡跟蹤控制上,以實(shí)現(xiàn)四足機(jī)器人的控制參數(shù)在線優(yōu)化,增強(qiáng)控制系統(tǒng)的穩(wěn)定性和魯棒性,同時(shí),進(jìn)行仿真實(shí)驗(yàn),驗(yàn)證該控制方法的合理性。
本文所采用的四足機(jī)器人模型為全肘式,如圖1所示。機(jī)器人有4條腿,圖上方定義為機(jī)器人的前方,每條腿由3個(gè)關(guān)節(jié)組成,從上至下依次為:髖關(guān)節(jié)、大腿關(guān)節(jié)和小腿關(guān)節(jié)。
圖1 四足機(jī)器人模型
采用D-H法建立四足機(jī)器人的運(yùn)動(dòng)學(xué)模型。由于四足機(jī)器人的四足對稱,4條腿的坐標(biāo)系建系都類似,因此只需給出四足機(jī)器人單腿簡化的三維模型,保留了髖關(guān)節(jié)、大腿和小腿關(guān)節(jié)的自由度。如圖2所示, θ1、 θ2、 θ3分別為髖關(guān)節(jié)、大腿和小腿關(guān)節(jié)的旋轉(zhuǎn)角度。 θ1沿水平向下為正, θ2沿垂直向右為正, θ3沿小腿延長線向內(nèi)為正,單腿坐標(biāo)系的原點(diǎn)建立在髖關(guān)節(jié)的旋轉(zhuǎn)中心上,x、y、z軸分別沿圖2的箭頭方向?yàn)檎?/p>
圖2 單腿的運(yùn)動(dòng)學(xué)建模
圖2中,L1、L2、L3分別為髖關(guān)節(jié)、大腿關(guān)節(jié),小腿關(guān)節(jié)的長度,其數(shù)值在下文中給出。定義左前腿、右前腿、左后腿、右后腿為LF、RF、LR、RR。
根據(jù)圖2單腿模型,可以求得四足機(jī)器人的正運(yùn)動(dòng)學(xué)方程為
式中:x、y、Z為足端關(guān)于所對應(yīng)的單腿髖關(guān)節(jié)坐標(biāo)系的坐標(biāo),L1、L2、L3分別為髖關(guān)節(jié)、大腿關(guān)節(jié)、小腿關(guān)節(jié)的長 度,si= s in(→?),ci= c os(→?),sivˉ= s in(i+j),cij= c os(i+j),λ為偏置參數(shù),右前右后腿為1,左前左后腿為?1。
根據(jù)式(1)所述的四足機(jī)器人運(yùn)動(dòng)學(xué)方程,對x、y、z分 別求 θ1、 θ2、 θ3偏導(dǎo),可得相應(yīng)的雅克比矩陣:
在腿部空間內(nèi),J為滿秩,因此機(jī)器人在足端的3個(gè)方向上的輸出力均是可控的。
VMC所使用的控制方式,是通過假想的元件來連接內(nèi)外部作用點(diǎn),產(chǎn)生虛擬的作用力來驅(qū)使機(jī)器人的足端追蹤期望軌跡。通過使用正向運(yùn)動(dòng)學(xué)求導(dǎo)得到機(jī)器人足端的雅克比矩陣,將足端的虛擬作用力映射到其關(guān)節(jié)力矩。其中,足端所給予的虛擬構(gòu)件一般由彈簧與阻尼器組成,這樣既保證其具有一定的伸展量,也保證能夠更加穩(wěn)定。
虛擬力的計(jì)算由式(2)給出:
式中:f∈R3為所需的虛擬力,Kp、Kd∈R3×3分別為彈性系數(shù)矩陣與阻尼系數(shù)矩陣,P、V∈R3分別為足端的實(shí)際位置和速度,Pd、Vd∈R3分別為足端的期望位置和速度。當(dāng)Kp=0時(shí),可以控制機(jī)器人追蹤期望的速度,而當(dāng)同時(shí)啟用Kp、Kd時(shí),可以控制機(jī)器人追蹤期望的軌跡。
關(guān)節(jié)力矩與虛擬力之間的關(guān)系可以用式(3)來呈現(xiàn):
式中: τ ∈R3為各關(guān)節(jié)的力矩,F(xiàn)∈R3為式(3)得出的各關(guān)節(jié)的虛擬力。
擺動(dòng)的腿使用貝塞爾曲線[14]對足端的位置進(jìn)行跟蹤。貝塞爾曲線由線段與節(jié)點(diǎn)組成,節(jié)點(diǎn)是可拖動(dòng)的支點(diǎn)。貝塞爾曲線通過起始點(diǎn)和終止點(diǎn),并與起始點(diǎn)和終止點(diǎn)的折線相切,在對四足機(jī)器人路徑規(guī)劃過程中至少需要三階貝塞爾曲線才能生成曲率連續(xù)的路徑。貝塞爾曲線被廣泛用于四足機(jī)器人的足端軌跡規(guī)劃,其優(yōu)點(diǎn)在于控制簡單、描述能力強(qiáng)、易于生成平滑的曲線等。本文 采 取 的 軌 跡 為 三 次 貝 塞 爾 曲 線 ,P0、P1、P2、P3共4個(gè)點(diǎn)在平面或在三維空間中定義了三次方貝茲曲線,由于所使用的軌跡是對稱的,所以在運(yùn)用時(shí)有部分參數(shù)重疊。曲線的參數(shù)形式為
在四足機(jī)器人運(yùn)動(dòng)的過程中,與地面接觸的腿被稱為支撐腿,支撐腿的關(guān)節(jié)力矩的計(jì)算公式為
在四足機(jī)器人運(yùn)動(dòng)過程中時(shí),極易受到路面情況以及本身模型參數(shù)攝動(dòng)影響。在跟蹤的過程中,若VMC參數(shù)Kp、Kd固定,則控制器魯棒性與抗擾性有限,容易造成力傳遞不平衡,從而使得控制效果(例如位置跟蹤精度、響應(yīng)速度等)下降。
針對上述問題,本文在虛擬模型控制的基礎(chǔ)上,增加模糊控制的理論,設(shè)計(jì)一種自適應(yīng)模糊虛擬模型控制器。該控制器能夠根據(jù)實(shí)際位置與期望位置的誤差及誤差的變化率在線調(diào)節(jié)虛擬模型控制中的彈簧系數(shù)與阻尼系數(shù),從而改善腿部跟蹤期望軌跡的能力。自適應(yīng)模糊控制器包括:模糊化、模糊推理和清晰化,對應(yīng)的內(nèi)容為輸入隸屬度函數(shù)、制定控制規(guī)則和邏輯判斷。圖3為自適應(yīng)模糊VMC控制系統(tǒng)結(jié)構(gòu)圖。
圖3 自適應(yīng)模糊 VMC 控制系統(tǒng)結(jié)構(gòu)
此模糊控制器主要是需要完成變量的模糊化、模糊控制規(guī)則的制定以及清晰化3方面。
設(shè)Kp0、Kd0是虛擬模型控制中初始的彈簧系數(shù)與阻尼系數(shù), ?Kp、 ?Kd是模糊控制器輸出的2個(gè)參數(shù),Kp、Kd是自適應(yīng)模糊VMC控制的最后所得的參數(shù)。自適應(yīng)模糊VMC控制器由式(5)給出:
具體的邏輯為:取得實(shí)際的軌跡與期望的軌跡的差值e及其微分c,將其模糊化,再通過知識(shí)庫與邏輯判斷,得到模糊整定的 ?Kp、 ?Kd,最后通過式(5)計(jì)算得出Kp、Kd。
建立Mamdani型二維模糊雙輸入雙輸出控制器。輸入為控制變量偏差e以及其微分c,自適應(yīng)虛擬模型控制器的輸出量為控制器的調(diào)整值?Kp、 ?Kd。自適應(yīng)虛擬模型控制的輸入與輸出的論域等級(jí)均為
輸入量的模糊子集描述為
式中:NB、NM、NS、Z0、PM、PS、PB分別代表負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大。如圖4、圖5所示,選擇隸屬度函數(shù)為均勻分布的三角形函數(shù)。
圖4 模糊輸入輸出
圖5 隸屬度函數(shù)
自適應(yīng)虛擬模型控制中的彈簧系數(shù)Kp與阻尼系數(shù)Kd,當(dāng)彈簧系數(shù)過大時(shí),系統(tǒng)的響應(yīng)速度快,容易產(chǎn)生超調(diào)使系統(tǒng)不穩(wěn)定;當(dāng)彈簧系數(shù)小時(shí),系統(tǒng)響應(yīng)速度變慢,無法很好的跟蹤軌跡,達(dá)到期望的效果。
綜合各方面因素,建立2個(gè)參數(shù)的模糊規(guī)則如表1~2所示。
表1 Kp 規(guī)則
表2 Kd 規(guī)則
采用Mamdani法進(jìn)行了模糊推理,采用重心法進(jìn)行解模糊的計(jì)算,計(jì)算公式如式(6)所示:
式中:z0為自適應(yīng)模糊虛擬模型控制清晰化后輸出的值,zi為模糊控制量論域中的值,utz(i)為zi的隸屬度值。
模糊控制的 ?Kp、 ?Kd輸出控制量曲面如圖6~7所示。
圖6 Kp 曲面
圖7 Kd 曲面
為了驗(yàn)證自適應(yīng)模糊VMC算法的準(zhǔn)確性與穩(wěn)定性,開展仿真實(shí)驗(yàn),測試該算法在四足機(jī)器人tort步態(tài)下的性能。仿真所使用參數(shù)如表3所示。
表3 仿真實(shí)驗(yàn)參數(shù)
建立對應(yīng)的四足機(jī)器人模型,實(shí)驗(yàn)采用四足機(jī)器人典型的tort步態(tài)測試行走過程中控制器對于期望軌跡的跟蹤能力。仿真過程如圖8所示。
圖8 仿真過程
對tort步態(tài)進(jìn)行仿真實(shí)驗(yàn),圖9~圖11為仿真實(shí)驗(yàn)過程中x、y、z實(shí)際與期望的軌跡曲線,在圖9~圖16中出現(xiàn)的t均代表仿真的單位時(shí)間步長??梢钥闯鲈趚、y軸方向上曲線對于期望值有良好的跟蹤能力,z軸雖然在跟蹤過程中有短暫延時(shí),但最終也能夠穩(wěn)定的跟蹤上期望軌跡。
圖9 x 軸跟蹤曲線
圖10 y 軸跟蹤曲線
圖11 z軸跟蹤曲線
圖12所示,四足機(jī)器人運(yùn)動(dòng)過程中的質(zhì)心實(shí)際軌跡與期望軌跡誤差均保持在0.01m以內(nèi),運(yùn)動(dòng)平穩(wěn),控制效果良好。圖13展示了運(yùn)動(dòng)過程中機(jī)體滾轉(zhuǎn)角、俯仰角與偏航角的變化曲線圖。在運(yùn)動(dòng)剛開始時(shí),由于機(jī)器人有起身動(dòng)作,姿態(tài)角有較大的波動(dòng),進(jìn)入平穩(wěn)行走狀態(tài)后,姿態(tài)角保持穩(wěn)定,波動(dòng)較小。
圖12 質(zhì)心運(yùn)動(dòng)曲線
圖13 姿態(tài)角變化曲線
圖14和圖15分別展示了在運(yùn)動(dòng)過程中Kp與Kd參數(shù)的變化曲線圖。
圖14 K p變化曲線
圖15 K d變化曲線
為進(jìn)一步證明增加模糊算法對系統(tǒng)跟蹤能力的提升,引入足端軌跡跟蹤性能指標(biāo)C。
式中:x、y、z為實(shí)際軌跡的三軸坐標(biāo),xd、yd、zd為期望軌跡的三軸坐標(biāo)。
在每一個(gè)軌跡點(diǎn)進(jìn)行C值的計(jì)算,最后得到關(guān)于C指標(biāo)的曲線,與不采用模糊算法進(jìn)行對比,如圖16所示。
圖16 C 曲線對比
C1為使用模糊算法優(yōu)化過后的曲線,C2為未使用模糊算法優(yōu)化的曲線。由圖16分析可知,在任意時(shí)刻優(yōu)化后C值均低于優(yōu)化前。
1)本文建立了四足機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型,通過D-H法對機(jī)器人的四條單腿12個(gè)關(guān)節(jié)進(jìn)行運(yùn)動(dòng)學(xué)建模,使用貝塞爾曲線作為擺動(dòng)相軌跡,解決機(jī)器人的基本運(yùn)動(dòng)問題。
2)本文在四足機(jī)器人的動(dòng)力學(xué)控制中采用虛擬模型控制,使機(jī)器人關(guān)節(jié)之間更加柔順。針對傳統(tǒng)的虛擬模型參數(shù)固定,不能根據(jù)運(yùn)動(dòng)過程調(diào)節(jié)的問題,在此基礎(chǔ)上,引入模糊控制的算法,優(yōu)化虛擬模型控制中的控制參數(shù),以達(dá)到更好的跟蹤效果。
3)通過仿真驗(yàn)證,從跟蹤三軸期望軌跡,質(zhì)心運(yùn)動(dòng)軌跡,足端跟蹤性能指標(biāo)等方面,證明了模糊算法的有效性,使四足機(jī)器人具有更好的跟蹤期望軌跡的能力。
4)本文僅研究了單腿控制,在未來工作中,將把重點(diǎn)放在四腿聯(lián)動(dòng)控制上。