胡平志，李澤滔

(貴州大學(xué)電氣工程學(xué)院，貴州 貴陽(yáng) 550025)

1 引言

近年來(lái)，隨著微型計(jì)算機(jī)控制技術(shù)的完善，各式各樣的機(jī)器人不斷出現(xiàn)，其中適應(yīng)復(fù)雜地形的足式機(jī)器人成為研究熱門(mén)。不同于輪式、履帶式和旋翼式機(jī)器人，足式機(jī)器人關(guān)節(jié)數(shù)量眾多，運(yùn)動(dòng)方式復(fù)雜，其步態(tài)規(guī)劃方式為機(jī)器人運(yùn)動(dòng)控制的基礎(chǔ)。針對(duì)機(jī)器人步態(tài)規(guī)劃算法，文獻(xiàn)[1-3]提出了模仿動(dòng)物運(yùn)動(dòng)的步態(tài)規(guī)劃算法，這類算法具有較好的穩(wěn)定性，但過(guò)分依賴于對(duì)動(dòng)物的觀察，適用于特定仿生形態(tài)的機(jī)器人。這3篇文獻(xiàn)的算法分別對(duì)應(yīng)了馬(4足)、螞蟻(6足)以及螃蟹(8足)，與本文所研究的機(jī)器人形態(tài)和足數(shù)均不一樣，這些算法難以適應(yīng)其他形態(tài)機(jī)器人以及本文所提及的四足機(jī)器人。

文獻(xiàn)[4-6]提出了基于智能算法的機(jī)器人步態(tài)規(guī)劃，這類算法能夠較為廣泛地適用于不同形態(tài)機(jī)器人。其中，文獻(xiàn)[4]提出了基于智能決策算法的路徑規(guī)劃；文獻(xiàn)[5]提出了結(jié)合遺傳算法的基于零力矩點(diǎn)軌跡規(guī)劃的四足機(jī)器人步態(tài)規(guī)劃；文獻(xiàn)[6]提出了基于馬爾可夫決策的六足機(jī)器人算法，并且還提出了離散化軌跡的分析方法。

基于上述參考文獻(xiàn)，本文提出基于改進(jìn)蟻群算法的步態(tài)規(guī)劃算法，將機(jī)器人的4足轉(zhuǎn)化為四維空間坐標(biāo)，并結(jié)合離散化軌跡的方法，將各種步態(tài)轉(zhuǎn)化為四維空間內(nèi)的離散點(diǎn)，從而將抽象的步態(tài)規(guī)劃問(wèn)題，轉(zhuǎn)化為四維空間里的路徑規(guī)劃問(wèn)題。值得注意的是，與傳統(tǒng)步態(tài)規(guī)劃問(wèn)題尋求最短路徑不同，本文算法的目的是規(guī)劃一條最長(zhǎng)路徑，因此需要對(duì)傳統(tǒng)路徑規(guī)劃算法進(jìn)行改進(jìn)。路徑規(guī)劃算法數(shù)量眾多[7 -16]，常用路徑規(guī)劃算法包括遺傳算法、人工勢(shì)場(chǎng)算法、蟻群算法、迪杰斯特拉算法和模擬退火算法等?？紤]到由四足運(yùn)動(dòng)步態(tài)點(diǎn)轉(zhuǎn)化得到的四維空間中離散點(diǎn)眾多，且由于路徑相位不同，最優(yōu)路徑不止一條，為了能廣泛搜索路徑，避免遺漏，本文選擇群智算法對(duì)四足機(jī)器人擺動(dòng)相步態(tài)進(jìn)行搜索，由于蟻群算法在相關(guān)領(lǐng)域發(fā)展較為成熟，故本文采用蟻群算法求取最優(yōu)路徑，即最優(yōu)步態(tài)規(guī)劃。

為了確保機(jī)器人能夠穩(wěn)定運(yùn)行，本文針對(duì)所研究機(jī)器人討論了零力矩點(diǎn)控制方案。最后，結(jié)合樣機(jī)實(shí)驗(yàn)，證明了由本文算法搜索得到的步態(tài)，能夠滿足本文所針對(duì)的機(jī)器人。

2 機(jī)器人建模

2.1 單機(jī)械腿建模

為實(shí)現(xiàn)對(duì)每個(gè)機(jī)械足的精確控制，需要對(duì)每個(gè)機(jī)械足進(jìn)行抽象建模，如圖1a所示的一條機(jī)械腿可以被抽象成如圖1b所示的模型，進(jìn)而建立如圖1c所示的空間直角坐標(biāo)系。

Figure 1 Robot single-leg modeling圖1 機(jī)器人單腿建模

通過(guò)DH(由Denavit和Hartenberg所提出的一種建立機(jī)器人運(yùn)動(dòng)學(xué)模型的算法)矩陣法[17,18]，對(duì)單條腿建立運(yùn)動(dòng)學(xué)方程，計(jì)算化簡(jiǎn)之后得到的方程組如式(1)所示：

(1)

其中,px,py,pz表示機(jī)器人末端點(diǎn)P在直角坐標(biāo)系中的空間坐標(biāo)，各參數(shù)實(shí)際取值如表1所示。

Table 1 Variables of 3-DOF mechanical leg表1 三自由度機(jī)械腿變量

表1中,θw為第w關(guān)節(jié)輸出角度,dw為第w關(guān)節(jié)旋轉(zhuǎn)軸與第w+1關(guān)節(jié)旋轉(zhuǎn)軸之間沿軸方向距離，aw為第w關(guān)節(jié)旋轉(zhuǎn)軸與第w+1關(guān)節(jié)旋轉(zhuǎn)軸之間的法線距離，αw為第w關(guān)節(jié)旋轉(zhuǎn)軸與第w+1關(guān)節(jié)旋轉(zhuǎn)軸之間的夾角。由此方程組可以求解運(yùn)動(dòng)學(xué)逆模型，一條機(jī)械腿的運(yùn)動(dòng)，實(shí)際上就是控制θ1，θ2，θ3從而控制末端點(diǎn)P擬合空間內(nèi)曲線DE的過(guò)程，配合步態(tài)控制策略，4條機(jī)械腿按照一定的順序運(yùn)動(dòng)，即可以使得機(jī)器人做出相應(yīng)的運(yùn)動(dòng)。

2.2 四足機(jī)器人逆模型求解

為了有效控制機(jī)械腿運(yùn)動(dòng)，需要求解逆模型，但是該模型解析解求解困難，以下不加證明地給出一種迭代求解運(yùn)動(dòng)學(xué)逆模型數(shù)值解的方法。為簡(jiǎn)化計(jì)算，模型方程組被轉(zhuǎn)化到圓柱坐標(biāo)中，由于被轉(zhuǎn)化為圓柱坐標(biāo)，坐標(biāo)由(x,y,z)轉(zhuǎn)化為(ρ,θ1,z)，其中ρ=|(x2+y2)0.5|為極軸長(zhǎng)度，θ1=arctan(y/x)為極角，同時(shí)θ1也是第1關(guān)節(jié)的輸出角，如圖2所示。

Figure 2 Rectangular coordinate to polar coordinate圖2 直角坐標(biāo)變換到圓柱坐標(biāo)

圖2中

(2)

方程組(1)變化為如式(3)所示:

(3)

由ρ=a1+a2cos(θ2)+a3cos(θ3-θ2)可以直接分解得:

(4)

簡(jiǎn)記為θ3=g(θ2,ρ)。

利用θ2控制z的變化，并修正由θ3變化帶來(lái)的偏差Δz，由z=a2sin(θ2)-a3sin(θ3-θ2)變換可得到關(guān)于sin(θ2)的一元二次方程a·sin2(θ2)+b·sin(θ2)+c=0，從而可以求解出θ2。

sin(θ2)的一元二次方程相關(guān)參數(shù)如式(5)所示：

(5)

Figure 3 Topology of quadruped robot圖3 四足機(jī)器人拓?fù)鋱D

由一元二次方程求根公式可得sin(θ2)，再利用反三角函數(shù)求解出θ2，如式(6)所示：

θ2=arcsin((2z(a2+a3cos(θ3))±

((-2z(a2+a3cos(θ3)))2-4((a2+a3cos(θ3))2+

(6)

將θ2表達(dá)式簡(jiǎn)記為θ2=f(θ3,z)，聯(lián)立式(3)～式(6)可得到如式(7)所示的迭代式：

(7)

2.3 四足機(jī)器人拓?fù)鋱D

為確定機(jī)器人運(yùn)動(dòng)策略，需要對(duì)機(jī)器人做出如圖3所示的簡(jiǎn)化，將機(jī)器人抽象為拓?fù)鋱D。

如圖3所示，其中圖3a為機(jī)器人實(shí)物模型三維俯視圖，在圖3b中，機(jī)器人的12個(gè)舵機(jī)被抽象為12個(gè)方格，由于4條腿關(guān)于機(jī)器人幾何軸中心對(duì)稱，所以將其按照順時(shí)針?lè)譃锳，B，C，D4個(gè)組，每組3個(gè)關(guān)節(jié)，序號(hào)由內(nèi)向外依次從3減小至1。

由文獻(xiàn)[6]的啟發(fā)，將4條機(jī)械腿的運(yùn)動(dòng)軌跡進(jìn)行分段，本文主要討論如圖4所示直線行走步態(tài)，由于篇幅和主題限制本文暫不討論軌跡生成問(wèn)題。

Figure 4 Discretization trajectory圖4 離散化軌跡

如圖4所示，一條機(jī)械腿運(yùn)動(dòng)過(guò)程被劃分為7個(gè)階段，其中階段1～階段5，機(jī)械足末端點(diǎn)P與地面緊貼為四足支撐相，按照從1～5的順序擬合一條曲線，到達(dá)第5階段后，抬起機(jī)械腿，使得P點(diǎn)脫離地面，到達(dá)第6階段，向前運(yùn)動(dòng)到達(dá)第7階段后，放下機(jī)械腿，使得P點(diǎn)重新緊貼地面，回到第1階段。其中，第6和第7這2個(gè)階段為三足支撐，單足擺動(dòng)相，這2個(gè)階段不直接對(duì)機(jī)器人運(yùn)動(dòng)做出貢獻(xiàn)，本文算法僅討論1～5階段四足支撐相。其中,A3,B3,C3,D3表示機(jī)器人4條腿的第3關(guān)節(jié)。

由于機(jī)器人有4條腿，所以存在一個(gè)運(yùn)動(dòng)的先后次序，可以看出因?yàn)槭侵芷谶\(yùn)動(dòng)，所以ABCD的運(yùn)動(dòng)次序與BCDA、CDAB、DABC等運(yùn)動(dòng)次序?qū)嵸|(zhì)上是一致的，只是相位不同，按照循環(huán)排列方式，其策略總數(shù)如式(8)所示，一共6種方式:

(8)

3 四維空間路徑規(guī)劃

3.1 四維空間建立

因?yàn)?條腿的運(yùn)動(dòng)本質(zhì)是相互獨(dú)立、線性無(wú)關(guān)的，本文算法利用這一特點(diǎn)，將4條腿運(yùn)動(dòng)軌跡，抽象建立為一個(gè)四維直角坐標(biāo)系O-ABCD，如圖5所示。

Figure 5 Four-dimensional space coordinate system圖5 四維空間坐標(biāo)系

圖5中OA代表A腿的支撐相1～5階段，OB，OC，OD同理，每個(gè)坐標(biāo)軸只取1～5 這5個(gè)離散的整數(shù)點(diǎn)，對(duì)應(yīng)4條機(jī)械支撐相滑動(dòng)相的5個(gè)步態(tài)點(diǎn)。

3.2 適應(yīng)度函數(shù)

規(guī)劃?rùn)C(jī)器人在一個(gè)運(yùn)動(dòng)周期中，4條腿的步態(tài)可以被視為在O-ABCD坐標(biāo)系下尋找一個(gè)四維坐標(biāo)序列，使得在該周期里機(jī)器人走過(guò)的路程最長(zhǎng)，即尋找在該空間中的最長(zhǎng)路徑。在這一定義下，適應(yīng)度函數(shù)如式(9)所示:

(9)

其中,Lt代表第t輪迭代機(jī)器人走過(guò)的路程總長(zhǎng)度，Lt的值為8個(gè)步態(tài)點(diǎn)之間7個(gè)間隔距離Lstep之和，每個(gè)間隔距離Lstep又等于機(jī)器人從一個(gè)狀態(tài)點(diǎn)轉(zhuǎn)移到下一個(gè)狀態(tài)點(diǎn)之后，4條機(jī)械腿位移dleg的總和。

3.3 機(jī)器人行走規(guī)則

控制機(jī)器人行走有如下規(guī)則：

(1)為保持機(jī)器人運(yùn)動(dòng)平穩(wěn)，任意時(shí)刻，機(jī)器人至少有3條腿接觸地面，作為支撐。

(2)為保證任意一個(gè)機(jī)械結(jié)構(gòu)不過(guò)度磨損，機(jī)器人運(yùn)動(dòng)時(shí)4條腿應(yīng)同時(shí)動(dòng)作，且動(dòng)作幅度一致。

(3)為保證機(jī)器人快速向某一方向運(yùn)動(dòng)，在機(jī)械腿緊貼地面的階段(即階段1～階段5)，機(jī)械足任意時(shí)刻的運(yùn)動(dòng)必須與機(jī)器人整體運(yùn)動(dòng)方向平行且相反。

對(duì)于規(guī)則(2)，可以引申為任意一條腿只能由第n階段轉(zhuǎn)移至第n+m階段，為方便規(guī)劃，統(tǒng)一定義m=1，即任意一條腿只能由第n階段轉(zhuǎn)移至第n+1階段，特別地，第5階段只能轉(zhuǎn)移至第1階段。并且應(yīng)該由4條腿一起驅(qū)動(dòng)使得機(jī)器人移動(dòng)，這也就意味著，當(dāng)某一條機(jī)械腿運(yùn)動(dòng)至第5階段后，由于該機(jī)械腿懸空，其余3條腿應(yīng)該保持原地不動(dòng)，等待該條腿通過(guò)第6、第7階段回到第1階段。

對(duì)于規(guī)則(3)，可以引申為階段轉(zhuǎn)移只能增加不能減少，特別地，第5階段只能轉(zhuǎn)移至第1階段。若出現(xiàn)階段減少，則引入適當(dāng)懲罰機(jī)制。

由規(guī)則(2)和規(guī)則(3)可以得到如表2所示規(guī)則表。

Table 2 Reward and punishment rule表2 獎(jiǎng)勵(lì)與懲罰規(guī)則

由此可以明顯看到，獎(jiǎng)勵(lì)方式與機(jī)械腿運(yùn)動(dòng)對(duì)機(jī)器人整體運(yùn)動(dòng)的貢獻(xiàn)有關(guān)，即只要能有利于驅(qū)動(dòng)機(jī)器人向某一特定方向運(yùn)動(dòng)，則該轉(zhuǎn)移方式將得到獎(jiǎng)勵(lì)，特別地由階段5轉(zhuǎn)移到階段1，由于只有1條腿運(yùn)動(dòng)，需要特殊的獎(jiǎng)勵(lì)方式。

如表2所示，任意一條腿由前一階段向后一階段轉(zhuǎn)移，將出現(xiàn)總共25種情形，不同情形對(duì)應(yīng)的獎(jiǎng)勵(lì)g不同。

在本文算法中，獎(jiǎng)勵(lì)模式1對(duì)應(yīng)g=+1，獎(jiǎng)勵(lì)模式2對(duì)應(yīng)g=+4，保持不動(dòng)對(duì)應(yīng)g=0，懲罰對(duì)應(yīng)g=-10,由此得到轉(zhuǎn)移規(guī)則矩陣G,如式(10)所示:

(10)

3.4 改進(jìn)蟻群算法

第step步的獎(jiǎng)勵(lì)rewardstep為4條腿獎(jiǎng)勵(lì)收益總和，單條腿收益gi,j由轉(zhuǎn)移規(guī)則矩陣確定,如式(11)所示，gi,j代表該條腿由第i階段轉(zhuǎn)移到第j階段的收益，對(duì)應(yīng)矩陣G第i行第j列元素。

(11)

當(dāng)rewardstep=4時(shí),說(shuō)明4條腿都在進(jìn)行“鄰階段正向轉(zhuǎn)移”,或者只有一條腿在從“第5階段返回第1階段”，其余腿均“保持不動(dòng)”，所以定義第t輪迭代中距離啟發(fā)函數(shù)ηstep(t)如式(12)所示:

(12)

其中，Kd是距離倍率，可以調(diào)節(jié)啟發(fā)函數(shù)的影響力，Ed為期望值，為使得ηstep(t)得到最大值時(shí)，rewardstep的值與期望值Ed接近，應(yīng)使得|rewardstep-Ed|趨近于0。

結(jié)合規(guī)則(1)和規(guī)則表2，在獎(jiǎng)勵(lì)規(guī)則下只有得到分?jǐn)?shù)rewardstep為4才是最優(yōu)選擇，所以選擇Ed=4+ε，其中|ε|應(yīng)遠(yuǎn)小于1但是大于0。

結(jié)合式(11)與適應(yīng)度函數(shù)式(9)可知，距離適應(yīng)度函數(shù)與獎(jiǎng)勵(lì)適應(yīng)度函數(shù)是線性相關(guān)的，為便于計(jì)算取二者之間系數(shù)為1，如式(13)所示:

(13)

值得注意的是，整條路徑并不是單純的rewardstep=4的點(diǎn)的集合，而是這些點(diǎn)按照一定順序排列得到的結(jié)果。

τstep(t+1)=(1-ρ)τstep(t)+Δτstep,

ρ∈(0,1),

(14)

本文算法規(guī)劃路線為最長(zhǎng)路徑，相較于傳統(tǒng)蟻群算法尋找最短路徑有所不同。本文提出的改進(jìn)蟻群算法流程圖如6所示。

Figure 6 Flow chart of improved ant colony algorithm圖6 改進(jìn)蟻群算法流程圖

4 仿真結(jié)果

本次仿真采用Matlab 2014a在Windows 7系統(tǒng)中運(yùn)行，仿真迭代初始條件如表3所示，50個(gè)個(gè)體進(jìn)行20次迭代，在四維空間中尋找最長(zhǎng)路徑。

值得注意的是，由于該四維空間內(nèi)離散點(diǎn)數(shù)量眾多，但是蟻群總數(shù)較少，為避免本文算法陷入局部最優(yōu)，同時(shí)擴(kuò)大搜索范圍，所以每次迭代都會(huì)重新隨機(jī)選擇螞蟻初始位置，以增加樣本數(shù)量。

如圖7所示，圖中縱軸是單次迭代路程的平均值，在20輪迭代后，路徑長(zhǎng)度逐漸收斂于(1000,1200)，之所以無(wú)法收斂于某個(gè)固定值，是由于每次迭代初始位置是隨機(jī)的，不論迭代多少次，都會(huì)隨機(jī)出現(xiàn)非最優(yōu)解，從而導(dǎo)致最終的結(jié)果總是存在隨機(jī)波動(dòng)。盡管如此可以看出該平均值依然是收斂的，經(jīng)過(guò)多次迭代仿真，結(jié)果依然收斂，其中5次迭代仿真結(jié)果如圖8所示。

Table 3 Initial condition of iteration表3 迭代初始條件

Figure 7 Iterative convergence of the algorithm圖7 算法迭代收斂

Figure 8 Convergence after multiple iteratios圖8 多次迭代收斂

經(jīng)過(guò)多次仿真實(shí)驗(yàn)得到如式(15)所示6組4足支撐相控制規(guī)則矩陣，符合之前的分析。由第1行可以看出，每一組之間只是列的順序不同，即4條腿的運(yùn)動(dòng)先后次序不同，符合式(8)的預(yù)期。

(15)

5 零力矩點(diǎn)控制

因?yàn)橥葟碾A段5返回到階段1需要經(jīng)過(guò)階段6和階段7位置擺動(dòng)相的變換，所以仍然需要擴(kuò)充式(15)所示矩陣。

同時(shí)如圖9所示，黑色圓形A、B、C、D代表與地面直接接觸做支撐的腿，虛線圓形A代表?yè)u擺相時(shí)抬起的腿，以A腿抬起為例，虛線圓形M代表機(jī)器人的重心，虛線框代表支撐面。在支撐相時(shí)四足做支撐，此時(shí)重心M落在支撐面四邊形ABCD上，所以機(jī)器人可以穩(wěn)定站立。但是在搖擺相時(shí)，抬起A腿后，支撐面由四邊形ABCD變?yōu)槿切蜝CD，重心M落在支撐面BCD之外，這使得搖擺相時(shí)機(jī)器人將出現(xiàn)站立不穩(wěn)進(jìn)而跌倒的現(xiàn)象。為解決這一問(wèn)題，則需要另外3條腿做出相應(yīng)調(diào)整，使得機(jī)器人在一條腿處于搖擺相時(shí)，重心重新落在由其余3條腿構(gòu)成的支撐面內(nèi)，如圖10所示。

Figure 9 Gravity center movement in swing phase圖9 擺動(dòng)相重心偏移

Figure 10 Correction for gravity center movement in pre-action圖10 預(yù)動(dòng)作修正重心偏移

在A腿抬起之前，4條腿向CA連線方向偏移(如圖10a所示)，使得重心M向AC方向偏移，落在三角形BCD內(nèi)(如圖10b所示)，最后A腿再抬起來(lái)(如圖10c所示)。這樣才能符合零力矩點(diǎn)控制，使得機(jī)器人穩(wěn)定運(yùn)行。由此，在機(jī)器人單腿抬起擺動(dòng)之前，需要再添加一個(gè)如圖10a所示的預(yù)動(dòng)作步態(tài)，以式(15)中第1個(gè)矩陣為例進(jìn)行詳細(xì)論證，式(15)中第1個(gè)矩陣加入預(yù)動(dòng)作后的擴(kuò)展規(guī)則表如表4所示。其中，步態(tài)1～步態(tài)3、步態(tài)6～步態(tài)8、步態(tài)11～步態(tài)13、步態(tài)16～步態(tài)18為支撐相；特別地，步態(tài)1與步態(tài)2、步態(tài)6與步態(tài)7、步態(tài)11與步態(tài)12、步態(tài)16與步態(tài)17時(shí)4條腿同時(shí)驅(qū)動(dòng)機(jī)器人沿直線向前運(yùn)動(dòng)；步態(tài)3、步態(tài)8、步態(tài)13、步態(tài)18為預(yù)動(dòng)作，此時(shí)4腿支撐且發(fā)生偏移，為接下來(lái)擺動(dòng)相保持平穩(wěn)做準(zhǔn)備；步態(tài)4、步態(tài)5、步態(tài)9、步態(tài)10、步態(tài)14、步態(tài)15、步態(tài)16、步態(tài)19、步態(tài)20為擺動(dòng)相，此時(shí)除了抬起的那一條腿擺動(dòng)到階段6和階段7外，其余腿均保持在支撐相預(yù)動(dòng)作階段，以維持機(jī)器人3足支撐的穩(wěn)定性。

Table 4 Extended rule 表4 擴(kuò)展規(guī)則

表4中“*”表示需要調(diào)整位置，當(dāng)?shù)趇條腿抬起時(shí)，第j條腿末端點(diǎn)P坐標(biāo)由(xj,yj)調(diào)整為(Xj,Yj)的規(guī)則如式(16)所示:

(16)

該規(guī)則表示末端點(diǎn)P坐標(biāo)沿直線(Yj-yj)=k(Xj-xj)由(xj,yj)移動(dòng)距離l到(Xj,Yj)。由于是一個(gè)二元二次方程組，則必然存在4個(gè)解，如式(17)所示：

(17)

其中,k為方向系數(shù)，這里取k=1，kxi,j與kyi,j為符號(hào)系數(shù)，取值為±1，i為抬起腿的序號(hào)，j為放下腿的序號(hào)，這些參數(shù)決定了其余3條腿的偏移方向，同時(shí)取l=20 mm，l決定了偏移的距離，其中kxi,j與kyi,j的取值規(guī)則如圖11所示。

Figure 11 ZMP controlling topology圖11 ZMP控制拓?fù)鋱D

如圖11a所示，點(diǎn)P的偏移方向根據(jù)kxi,j與kyi,j的取值，一共有4種方式，如表5所示。

Table 5 Movement directions 表5 偏移方向

但是，如圖11b所示，以A腿離開(kāi)地面，BCD作支撐為例，當(dāng)A腿離開(kāi)地面后，重心會(huì)向A偏移，為平衡這一偏移，其余3足應(yīng)沿著箭頭A方向運(yùn)動(dòng)，但是由于機(jī)器人的重心對(duì)稱特性，每一條足運(yùn)動(dòng)坐標(biāo)變化方向均不一樣，以B腿為例：B腿末端點(diǎn)x坐標(biāo)應(yīng)減小，y坐標(biāo)應(yīng)增大，即對(duì)應(yīng)kxi,j=-1，kyi,j=1，對(duì)應(yīng)方向4；同理，C腿偏移方向?yàn)?，D腿偏移方向?yàn)?，A在預(yù)動(dòng)作時(shí)偏移方向?yàn)?。由此可以得到偏移控制規(guī)則如表6所示。

Table 6 Movement control rules table表6 偏移控制規(guī)則表

表6中，數(shù)字1～4為表5中偏移方向代號(hào)，“預(yù)”代表本次偏移只在預(yù)動(dòng)作時(shí)有效。將表6與表4結(jié)合即可以控制機(jī)器人運(yùn)行。以表4中步態(tài)4為例，此時(shí)D腿抬起，所以A腿在表4中步態(tài)2的基礎(chǔ)上向4方向偏移，B腿在表4中步態(tài)3的基礎(chǔ)上向3方向偏移，C腿在步驟點(diǎn)4的基礎(chǔ)上向2方向偏移。

6 樣機(jī)測(cè)試

本次實(shí)驗(yàn)樣機(jī)機(jī)體結(jié)構(gòu)采用3D打印建造，材質(zhì)為PLA(聚乳酸)，采用TS-315總線數(shù)字舵機(jī)作為伺服系統(tǒng)，可同時(shí)采用stm32嵌入式做脫機(jī)控制，或者采用PC機(jī)數(shù)據(jù)線做在線控制。本次測(cè)試采用PC機(jī)數(shù)據(jù)線做在線控制，機(jī)器人樣機(jī)如圖12所示。

為標(biāo)定機(jī)械足順序，將其中一只采用黃色材料打印，由于圖片轉(zhuǎn)為灰度圖，圖中顏色應(yīng)為淺灰色。左上方淺色的腿為B，沿順時(shí)針?lè)较蛞来螢锽CDA，由前文可知該運(yùn)動(dòng)方式等效于ABCD運(yùn)動(dòng)方式。該機(jī)器人按照表4所示規(guī)則，運(yùn)行20步行走100 mm的錄像截圖如圖13所示。

Figure 12 Robot prototype 圖12 機(jī)器人樣機(jī)

Figure 13 Video capture of robot prototype walking圖13 機(jī)器人樣機(jī)行走錄像截圖

經(jīng)過(guò)10次實(shí)驗(yàn)，得到數(shù)據(jù)取平均值，與設(shè)置值的對(duì)比如表7所示。

Table 7 Comparison of experimental data表7 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

由于實(shí)際測(cè)量工具最小分度值為毫米(mm)和度(°)，存在較大誤差，所以平均值全部保留整數(shù)。由于未作防滑處理，機(jī)器人在地面行走時(shí)難免出現(xiàn)打滑，之后應(yīng)在機(jī)器腿末端做防滑處理。

如圖13所示，前2列為本文算法搜索出來(lái)的驅(qū)動(dòng)機(jī)器人前進(jìn)的8個(gè)步態(tài)，直接驅(qū)動(dòng)機(jī)器人前進(jìn)；第3列為偏移重心的預(yù)動(dòng)作，為搖擺相時(shí)機(jī)器人的平衡做準(zhǔn)備；前3列為4足支撐相，第4列和第5列為搖擺相，使得處于末位5的腿重新擺回初始位1。從1到20分別對(duì)應(yīng)20個(gè)步態(tài)，機(jī)器人從右往左沿直線運(yùn)動(dòng)，整個(gè)過(guò)程中，機(jī)器人重心平穩(wěn)，運(yùn)行穩(wěn)定，符合本文設(shè)定的機(jī)器人行走標(biāo)準(zhǔn)。

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

本文針對(duì)四足機(jī)器人的特性，將四足步態(tài)規(guī)劃問(wèn)題轉(zhuǎn)化為四維空間中需求最長(zhǎng)路徑問(wèn)題，并改進(jìn)蟻群算法，將搜尋最短路徑轉(zhuǎn)化為搜尋單位時(shí)間內(nèi)的最長(zhǎng)路徑。并針對(duì)四足機(jī)器人在水平地面上直線行走步態(tài)規(guī)劃進(jìn)行了研究。在多次仿真模擬中，從2.28×1010種步態(tài)組合中搜索出了24種符合約束條件的步態(tài)，表明了本文算法的有效性和快速性。得到基礎(chǔ)的8個(gè)步態(tài)后，根據(jù)零力矩點(diǎn)控制對(duì)矩陣擴(kuò)充，有效解決了重心不穩(wěn)的問(wèn)題，進(jìn)一步優(yōu)化了機(jī)器人運(yùn)行的穩(wěn)定性。最后通過(guò)程序控制樣機(jī)測(cè)試，機(jī)器人樣機(jī)在地面上穩(wěn)定行走，未出現(xiàn)搖晃和明顯偏移，從而驗(yàn)證了本文算法的實(shí)用性。