基于自適應(yīng)模糊算法的四足機(jī)器人運(yùn)動(dòng)VMC方法

眭耀宇，楊忠，游雨龍，許昌亮，周國興

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)證該控制方法的合理性。

1 四足機(jī)器人模型建立

本文所采用的四足機(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è)方向上的輸出力均是可控的。

2 VMC 虛擬力模型

2.1 VMC 控制原理

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é)的虛擬力。

2.2 擺動(dòng)相控制

擺動(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ù)形式為

2.3 支撐相控制

在四足機(jī)器人運(yùn)動(dòng)的過程中，與地面接觸的腿被稱為支撐腿，支撐腿的關(guān)節(jié)力矩的計(jì)算公式為

3 基于自適應(yīng)模糊的 VMC 控制器

3.1 控制器結(jié)構(gòu)

在四足機(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。

3.2 模糊化

建立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ù)

3.3 自適應(yīng)模糊規(guī)則

自適應(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ī)則

3.4 清晰化

采用Mamdani法進(jìn)行了模糊推理，采用重心法進(jìn)行解模糊的計(jì)算，計(jì)算公式如式（6）所示：

式中：z0為自適應(yīng)模糊虛擬模型控制清晰化后輸出的值，zi為模糊控制量論域中的值，utz(i)為zi的隸屬度值。

模糊控制的 ?Kp、 ?Kd輸出控制量曲面如圖6~7所示。

圖6 Kp 曲面

圖7 Kd 曲面

4 仿真驗(yàn)證

為了驗(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)化前。

5 結(jié)論

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)控制上。