譚永營，晁智強，金毅，王飛

(1.陸軍裝甲兵學(xué)院 車輛工程系，北京 100072；2.66336部隊，河北 保定 074000)

0 引言

負載型四足步行平臺僅需地面一系列離散支撐點即可實現(xiàn)穩(wěn)定行走，特殊的行走方式使其對復(fù)雜地形的適應(yīng)能力強、對地面破壞作用小，在軍用物資或裝備運輸、野外資源勘探等領(lǐng)域發(fā)揮著重要作用[1-3]。隨著步行平臺技術(shù)的發(fā)展，復(fù)雜環(huán)境中步行平臺的行走控制成為研究熱點。靜步態(tài)是步行平臺復(fù)雜環(huán)境中運用的主要步態(tài)之一[4-6]。在負載型四足步行平臺靜步態(tài)行走過程中，由于其機身質(zhì)量較大、質(zhì)心位置較高，常見的逆向運動學(xué)或動力學(xué)控制方法實時性差，易導(dǎo)致頻繁的著地沖擊，使機體不能較好地跟蹤理想軌跡，滿足其適應(yīng)復(fù)雜地形的需求。因此，負載型四足步行平臺靜步態(tài)柔順行走控制，對于保證其在復(fù)雜環(huán)境中的行走穩(wěn)定性進而完成相應(yīng)的任務(wù)具有重要作用。

Kolter等[7-8]和Kalakrishnanm等[9]在LittleDog整機動力學(xué)建?；A(chǔ)上，規(guī)劃了LittleDog機體軌跡及足端擺動軌跡，運用阻抗控制方法對其靜步態(tài)行走進行控制。但LittleDog動力學(xué)建模較復(fù)雜，控制效果的好壞取決于某些參數(shù)設(shè)置的準(zhǔn)確性。Winker等借鑒了LittleDog靜步態(tài)行走控制方法，將此種方法應(yīng)用于液壓四足步行平臺(HyQ)靜步態(tài)行走，實現(xiàn)了HyQ靜步態(tài)行走控制，但建模準(zhǔn)確性對平臺靜步態(tài)行走性能的影響極大，復(fù)雜的動力學(xué)計算也增加了控制器負擔(dān)[10-11]。Wang等針對小型輪足式平臺，選取機體高度、俯仰角、橫滾角為主要控制自由度，運用虛擬懸掛模型及足端阻抗控制方式，實現(xiàn)了小型輪足式平臺靜步態(tài)行走[12]。LittleDog及HyQ中使用的靜步態(tài)行走控制方法計算量較大，且需要對地形特征較為準(zhǔn)確地建模，機身質(zhì)量較大的負載型步行平臺，建模過程中的誤差將導(dǎo)致足端產(chǎn)生較大沖擊，嚴重影響步行平臺穩(wěn)定性。對于選取主要控制自由度的方法，較大的機身質(zhì)量將會放大任一自由度的問題，使平臺無法保持穩(wěn)定性。

虛擬元件控制方法計算過程簡單、魯棒性較好，已應(yīng)用在仿人步行平臺、四足步行平臺對角步態(tài)和六足步行平臺三角步態(tài)行走控制中[13-15]。本文為實現(xiàn)負載型四足步行平臺靜步態(tài)行走柔順性，將虛擬元件引入平臺行走控制中。在步行平臺整體動力學(xué)建?；A(chǔ)上，將機體及擺動腿的運動控制需求轉(zhuǎn)變?yōu)橐幌盗刑摂M力作用，運用二次規(guī)劃方法實現(xiàn)平臺虛擬力分配，并得到支撐腿及擺動腿的關(guān)節(jié)力矩。步行平臺行走過程中能夠感受地形的變化，進而實時調(diào)整各驅(qū)動關(guān)節(jié)力矩，實現(xiàn)復(fù)雜地形中的柔順行走。

1 四足步行平臺模型及其運動學(xué)分析

負載型四足步行平臺模型如圖1中所示，包括軀干和4條腿。每條腿均有3個自由度，從上到下依次為胯側(cè)擺關(guān)節(jié)段、大腿和小腿，對應(yīng)的關(guān)節(jié)分別為胯側(cè)擺關(guān)節(jié)、胯縱擺關(guān)節(jié)和膝關(guān)節(jié)，且均為主動關(guān)節(jié)。為方便對步行平臺機體及各腿段位姿的分析，在其機體質(zhì)心處建立機體坐標(biāo)系{Obxbybzb}，在各腿胯側(cè)擺關(guān)節(jié)處建立各腿基坐標(biāo)系{Oi0xi0yi0zi0}，i(i=1，2,3,4)為腿的編號，并選擇大地上任一點建立絕對坐標(biāo)系{Oxyz}，如圖1所示。

圖1 步行平臺模型及各坐標(biāo)系Fig.1 Quadruped walking vehicle model and coordinates

各腿基坐標(biāo)系x軸豎直向下，z軸與xb軸方向相同(見圖1)。步行平臺相關(guān)參數(shù)如表1所示。

表1 步行平臺相關(guān)參數(shù)Tab.1 Relative parameters of quadruped walking vehicle

由于各腿結(jié)構(gòu)相同且安裝位置對稱，僅對右前腿進行運動學(xué)分析，其余各腿運動學(xué)分析參考右前腿進行。運用Denavit-Hartenberg(D-H)方法對右前腿進行運動學(xué)分析，在各驅(qū)動關(guān)節(jié)及足端建立如圖2所示坐標(biāo)系，其中{O11x11y11z11}為胯側(cè)擺關(guān)節(jié)坐標(biāo)系，{O12x12y12z12}為胯縱擺關(guān)節(jié)坐標(biāo)系，{O13x13y13z13}為膝關(guān)節(jié)坐標(biāo)系，{O14x14y14z14}為足端坐標(biāo)系。由旋轉(zhuǎn)變換矩陣連乘可得1腿足端在其基坐標(biāo)系中的坐標(biāo)為

(1)

圖2 D-H方法腿部坐標(biāo)系Fig.2 Leg coordinates used in D-H calculating method

將(1)式對時間求導(dǎo)，可得步行平臺足端點速度與關(guān)節(jié)角速度之間的關(guān)系[16]為

(2)

2 基于虛擬力優(yōu)化分配的四足步行平臺機體運動控制

虛擬元件控制方法是一種動力學(xué)近似的控制方法，其應(yīng)用過程中忽略了各部分的加速度，不考慮慣性力的作用[17-19]。四足步行平臺靜步態(tài)行走過程中行走速度不高、加速度較小，且在步態(tài)規(guī)劃時三足支撐階段設(shè)計為勻速運動，四足支撐階段存在較小的加速度，因此不考慮慣性力的方法是合理的。

2.1 四足步行平臺整體動力學(xué)分析

假設(shè)四足步行平臺的質(zhì)量集中于機體的形心。四足步行平臺靜步態(tài)行走過程中，其所受合外力主要包括外界環(huán)境直接作用于步行平臺機體的力、地面給足端的作用力、平臺自身及負載質(zhì)量的重力、空氣阻力等。為方便對四足步行平臺整體的動力學(xué)分析，根據(jù)平臺靜步態(tài)行走特點，可做出如下假設(shè)：

1)四足步行平臺靜步態(tài)行走過程中速度不高，可忽略空氣對步行平臺的阻力作用。

2)由于步行平臺足端與地面接觸面積較小，可將其看作點接觸，只考慮足端與地面之間力的作用，而忽略力矩的作用。

3)假設(shè)外界環(huán)境對機體無直接干擾力。

4)步行平臺靜步態(tài)行走時，每次擺腿過程只有一條腿擺動，且腿的質(zhì)量相對機體質(zhì)量較小。因此不考慮腿的質(zhì)量，且忽略擺腿對機體運動的影響。

四足步行平臺靜步態(tài)行走時的受力如圖3和圖4所示。圖3中：Fx、Fy、Fz分別為沿xb軸、yb軸、zb軸的合外力；Tx、Ty、Tz分別為繞xb軸、yb軸、zb軸的合外力矩；fix、fiy、fiz分別為支撐腿足端沿xb軸、yb軸、zb軸方向的力，i=1，2，3，4；g為重力加速度。

圖3 四足支撐受力Fig.3 Forces at four-leg supporting phase

圖4 三足支撐受力Fig.4 Forces at three-leg supporting phase

四足步行平臺靜步態(tài)行走過程中，需要控制支撐腿足端與地面之間的作用力，以保證其行走穩(wěn)定性。因此，由圖3和圖4及相應(yīng)的假設(shè)條件，可得步行平臺受力方程為

(3)

式中：xi、yi、zi分別為i腿足端在xb軸、yb軸、zb軸方向的位置坐標(biāo)；φy為機體俯仰角；n為支撐腿的數(shù)量。

2.2 四足步行平臺機體虛擬元件的添加

在應(yīng)用虛擬元件進行步行平臺行走控制時，通常是在步行平臺機體與參考坐標(biāo)系或機體與步行平臺某一部分之間添加虛擬元件，以實現(xiàn)平臺期望的運動[20]。由四足步行平臺絕對穩(wěn)定裕度定義可知，其行走過程穩(wěn)定性由機體質(zhì)心在支撐平面的投影與足底支撐多邊形的相對位置關(guān)系決定[4]。為保證步行平臺行走過程中的穩(wěn)定性，機體位置控制非常重要，而機體位置控制可通過控制作用于機體上的虛擬力實現(xiàn)。

為實現(xiàn)步行平臺各自由度的運動可控，將機體控制任務(wù)分解到平臺6個自由度上。在每個自由度的實際位置與期望位置間添加1維虛擬彈簧- 阻尼元件，以實現(xiàn)步行平臺整體的期望運動，如圖5所示。

圖5 步行平臺各自由度虛擬元件Fig.5 Vehicle’s virtual components for every degree of freedom

作用于質(zhì)心上的合力及合力矩是由各自由度彈簧阻尼元件特性決定的。根據(jù)各自由度添加的一維虛擬彈簧- 阻尼元件模型，可得平臺控制方程如下：

(4)

(5)

(6)

(7)

(8)

(9)

2.3 基于序列二次規(guī)劃的虛擬力優(yōu)化分配

在得到步行平臺機體虛擬力后，將虛擬力分配到支撐腿足端，以實現(xiàn)步行平臺行走控制。四足步行平臺靜步態(tài)行走占空系數(shù)大于等于0.75，在行走過程中存在三足支撐階段和四足支撐階段，兩個階段的虛擬力分配過程類似。參考文獻[20-21]中的虛擬元件控制方法應(yīng)用過程，針對多足步行平臺虛擬力分配中的靜不定問題，可通過添加約束矩陣或應(yīng)用優(yōu)化方法得到支撐腿足端的作用力。令四足步行平臺機體所受合外力等于平臺各自由度期望力，結(jié)合平臺受力方程可得

(10)

四足步行平臺靜步態(tài)行走過程中足端與地面之間須保證穩(wěn)定接觸，防止因支撐腿打滑導(dǎo)致的穩(wěn)定性問題。因此，足端力需滿足如下摩擦圓錐約束條件：

(11)

式中：μi為足端所處地面的靜摩擦系數(shù)。

在許多步行平臺靜步態(tài)行走研究中，為方便對支撐相的求解，往往假設(shè)足端與地面之間鉸接，而實際上地面在豎直方向上只能產(chǎn)生支撐力、無法產(chǎn)生向下的拉力。因此

fiz≥0.

(12)

結(jié)合(8)式～(10)式可以發(fā)現(xiàn)，方程中共有3n(n=3或4)個未知量，但只有6個約束方程，其解有無限個。文獻[22]利用序列二次規(guī)劃方法對四足步行平臺在陡坡間歇靜步態(tài)行走過程中的足端力進行求解，實現(xiàn)了平臺在高坡度地面的行走。下面參考文獻[22]中的方法，并結(jié)合負載型四足步行平臺協(xié)調(diào)靜步態(tài)行走特點，對其足端力進行求解。

對(8)式進行整理，可得

Af=b，

(13)

式中：A為支撐矩陣；f為足端力列陣；b為期望合外力及力矩列陣。

(13)式中各矩陣的元素如下：

為使足端力滿足上述約束條件，且足端力的絕對值能夠盡量小，構(gòu)建如下二次規(guī)劃方程：

(14)

式中：fd為足端力期望值；S為虛擬力約束矩陣；α為權(quán)重系數(shù)；W為足端力懲罰矩陣；C為摩擦約束矩陣；D為足端力方向變換矩陣；fmin為足端力允許的最小值；fmax為足端力允許的最大值。

fmin與fmax根據(jù)步行平臺行走過程中對足端力的期望值進行施加。α的值需恰當(dāng)選擇，防止兩個約束條件相互干涉，出現(xiàn)穩(wěn)定性問題，在此α取0.01.為簡化足端力計算過程，以便使用序列二次規(guī)劃方法進行求解，對足底摩擦圓錐進行線性化，采用棱錐代替摩擦圓錐。因此，(9)式變換為

(15)

(14)式中的約束矩陣C、D、fmin、fmax的計算公式分別如下：

式中：θir為第i條腿足端所處地面傾角；fizmin為第i條腿足端垂向力的最小值，在此取200 N；fizmax為第i條腿足端垂向力的最大值，在此取∞.根據(jù)以上各式求解相應(yīng)的二次規(guī)劃，便可得到滿足約束條件的足端力值。

2.4 四足步行平臺支撐腿驅(qū)動關(guān)節(jié)力矩的求取

由虛擬力優(yōu)化分配得到支撐腿足端的作用力后，由步行平臺足端力與關(guān)節(jié)力矩之間的雅克比矩陣，可得各驅(qū)動關(guān)節(jié)力矩[17]如下：

(16)

在支撐腿足端力求解過程中考慮了足端打滑的條件，但求解過程中必須得到各足端所處地形的法線方向。雖然地形信息可以由步行平臺搭載的地形感知系統(tǒng)得到，但也存在較大誤差。因此，在各支撐腿足端實際位置與期望位置之間添加虛擬的低剛度彈簧元件，在保持機體穩(wěn)定性的基礎(chǔ)上，對各支撐腿垂直方向的足端力進行調(diào)整，以補償?shù)匦胃兄^程中的誤差。各支撐腿足端誤差修正力可由(17)式求得：

(17)

在得到水平方向的誤差修正力后，利用試湊法對豎直方向的補償力進行計算，以平臺四足支撐為例：

Aefe=be，

(18)

式中：Ae為僅考慮垂直力的足端支撐矩陣；fe為支撐腿足端修正力列陣；be為期望的修正力列陣。(18)式中的矩陣元素如下：

(19)

(20)

3 四足步行平臺擺動腿運動控制

四足步行平臺靜步態(tài)行走過程中通過擺腿實現(xiàn)對障礙的跨越，仍以1腿為例對四足步行平臺的擺腿進行說明。在1腿基坐標(biāo)系的原點處建立如圖6所示的坐標(biāo)系{O10x1by1bz1b}，坐標(biāo)系各軸平行于機體坐標(biāo)系。將擺動腿的運動控制分解到沿坐標(biāo)軸3個方向，在每個方向足端實際位置與理想位置之間添加虛擬的變剛度彈性阻尼元件，實現(xiàn)對擺動腿的控制。

圖6 擺動腿虛擬元件控制Fig.6 Virtual model control of swing leg

為保證軌跡跟蹤的準(zhǔn)確性，在擺動段虛擬剛度較大而在落地段剛度減小，以實現(xiàn)足端的平穩(wěn)著地。相應(yīng)的控制方程如下：

(21)

由擺動腿足端沿各坐標(biāo)軸方向所受的力，結(jié)合擺動腿的雅克比矩陣，可得擺動腿各關(guān)節(jié)的力矩如下：

(22)

4 四足步行平臺靜步態(tài)行走狀態(tài)機設(shè)計

四足步行平臺靜步態(tài)行走過程中，為滿足邁腿過程中的穩(wěn)定性要求，機體通常在前后方向和左右方向調(diào)整位置[23-25]。四足步行平臺一個完整的步態(tài)周期如圖7所示。由圖7可見，步行平臺靜步態(tài)行走占空系數(shù)為5/6，每個步態(tài)周期存在4個三足支撐相(邁腿相)及2個四足支撐相(重心調(diào)整相)。理想情況下，邁3(4)腿及邁2(1)腿連續(xù)完成，中間不存在重心位置調(diào)整。圖7所示為理想情況下四足步行平臺靜步態(tài)行走一個完整的步態(tài)周期。按照逆時針順序?qū)⒉叫衅脚_的腿編號為1、2、3、4，如圖7(a)中所示。

圖7 步行平臺靜步態(tài)行走示意圖Fig.7 Locomotion strategy of static gait

四足步行平臺復(fù)雜地形行走過程中，不可避免地要調(diào)整姿態(tài)，以充分利用足端活動空間、適應(yīng)復(fù)雜地形。因此，在步行平臺每次擺動腿著地時刻，判斷地形變化是否超過設(shè)定的閾值。當(dāng)?shù)匦巫兓^設(shè)定閾值時，步行平臺進入姿態(tài)調(diào)整相。假設(shè)φy0為步行平臺初始姿態(tài)角，φys為感知到的地形坡度，φyc為設(shè)定的參考值。則當(dāng)|φy0-φys|≤φyc時φyd=φy0，此時不調(diào)整姿態(tài)；當(dāng)|φy0-φys|>φyc時φyd=φys，此時進行姿態(tài)調(diào)整。φys的計算公式如(23)式所示：

(23)

地形感知是在坐標(biāo)系{Obxbwybwzbw}中進行的，如圖8所示。

圖8 步行平臺地形感知Fig.8 Terrain sensing of walking vehicle

假設(shè)姿態(tài)調(diào)整時間為tc，為保證姿態(tài)角變化連續(xù)，采用5次曲線對其姿態(tài)變化過程進行規(guī)劃如下：

(24)

由于負載型四足步行平臺的機身質(zhì)量較大，其姿態(tài)調(diào)整在四足支撐階段完成，依靠各支撐腿與地面之間的相互作用力實現(xiàn)姿態(tài)的調(diào)整，而后進入正常的靜步態(tài)行走周期。各相位的切換基于機體的姿態(tài)調(diào)整時間、擺動腿的擺腿時間、觸地狀態(tài)，最終得到其狀態(tài)機如圖9所示。

圖9 狀態(tài)機原理圖Fig.9 Schematic diagram of state machine

5 四足步行平臺靜步態(tài)行走仿真實驗

在ADAMS/View軟件中建立四足步行平臺的虛擬樣機模型，在MATLAB/Simulink軟件中編寫相應(yīng)的控制程序，實現(xiàn)步行平臺靜步態(tài)行走。在仿真過程中，MATLAB/Simulink軟件計算得到的驅(qū)動關(guān)節(jié)力矩直接施加于步行平臺的各驅(qū)動關(guān)節(jié)。為驗證控制算法的有效性，分別對步行平臺平地及復(fù)雜地形行走進行聯(lián)合仿真。仿真過程中，步行平臺的控制參數(shù)設(shè)置如表2所示。

四足步行平臺平地行走過程中，邁腿時間Tsw及機體運動時間Tbm均為0.5 s.步行平臺行走過程中機體軌跡跟蹤誤差、姿態(tài)角變化及1腿豎直方向的足端力變化如圖10所示。

由圖10(a)可見，步行平臺前進方向及側(cè)向的軌跡跟蹤誤差在0附近波動，且變化范圍為-0.003～0.003 m，軌跡跟蹤誤差較小。著地沖擊對軌跡跟蹤幾乎無影響。由圖10(b)、圖10(c)、圖10(d)可見，步行平臺平地行走過程中機體橫滾角、俯仰角、偏航角均穩(wěn)定在目標(biāo)值0附近，且均在-0.001 5～0.001 5 rad范圍內(nèi)變化，步行平臺機體穩(wěn)定性較好。由圖10(e)可見，平臺擺動腿著地時刻仍然存在著地沖擊力，沖擊力的幅值約為900 N.與傳統(tǒng)方法相比，足端著地力已大為減小，步行平臺行走平穩(wěn)。平地行走仿真結(jié)果表明，虛擬元件的控制方法較好地實現(xiàn)了負載型四足步行平臺平地的靜步態(tài)行走。

表2 仿真參數(shù)設(shè)置Tab.2 Simulation parameters setting

圖10 步行平臺平地行走參數(shù)變化Fig.10 Walking parameter change of vehicle on flat ground

圖11 仿真中構(gòu)建的復(fù)雜地形及平臺行走過程截圖Fig.11 Rough terrain constructed in simulation and snapshots of vehicle while walking on rough terrain

為驗證控制算法的魯棒性，設(shè)計如圖11所示的復(fù)雜地形，臺階高度為10 cm，其余障礙的高度在2～8 cm之間。步行平臺行走過程中邁腿時間Tsw及機體運動時間Tbm均為1 s，且步行平臺對復(fù)雜地形的信息未知。步行平臺行走過程中機體姿態(tài)角及1腿豎直方向足端力的變化如圖12所示。

圖12 步行平臺復(fù)雜地形行走相關(guān)參數(shù)變化Fig.12 Walking parameter change of vehicle on rough terrain

由圖12(a)、圖12(c)可見，步行平臺行走過程中機體橫滾角、偏航角仍穩(wěn)定在0附近，但由于障礙的影響變化范圍有所增大，特別是在48 s左右，后腿與臺階之間的干涉導(dǎo)致機體橫滾角、偏航角出現(xiàn)一定偏差，但各參數(shù)的變化均在可接受的范圍內(nèi)。在圖12(b)中，靜步態(tài)行走過程中步行平臺根據(jù)地形特點計算得到俯仰角目標(biāo)值，機體俯仰角實時跟隨其目標(biāo)值，以適應(yīng)地形的變化。由圖12中(d)可見，雖然行走過程中存在障礙，但足端沖擊力仍然較小，步行平臺平穩(wěn)通過復(fù)雜地形。由步行平臺復(fù)雜地形行走過程虛擬樣機仿真可得，虛擬元件的控制方法實現(xiàn)了步行平臺對復(fù)雜地形的適應(yīng)性。

6 結(jié)論

本文提出了一種基于虛擬元件的負載型四足步行平臺靜步態(tài)行走控制方法。通過在平臺機體添加虛擬元件及虛擬力的優(yōu)化分配實現(xiàn)機體運動控制，在擺動腿足端添加虛擬元件實現(xiàn)擺動腿的控制；設(shè)計了平臺靜步態(tài)行走狀態(tài)機，并對其靜步態(tài)行走進行了仿真實驗。得出主要結(jié)論如下：

1)步行平臺機體虛擬元件控制結(jié)合虛擬力的優(yōu)化分配實現(xiàn)了平臺機體對期望軌跡的準(zhǔn)確跟蹤，保證了步行平臺靜步態(tài)行走的穩(wěn)定性。

2)擺動腿變參數(shù)虛擬元件控制減小了擺動腿著地沖擊力，保證了四足步行平臺復(fù)雜地形靜步態(tài)行走過程的平穩(wěn)性。

3)虛擬元件控制方法結(jié)合步行平臺狀態(tài)機的設(shè)計實現(xiàn)了平臺對復(fù)雜地形的適應(yīng)性，步行平臺能夠根據(jù)地形變化及時調(diào)整姿態(tài)，證明了控制方法的有效性。