基于改進蟻群算法的四足機器人步態規劃*

胡平志，李澤滔

(貴州大學電氣工程學院，貴州 貴陽 550025)

1 引言

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

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

基于上述參考文獻，本文提出基于改進蟻群算法的步態規劃算法，將機器人的4足轉化為四維空間坐標，并結合離散化軌跡的方法，將各種步態轉化為四維空間內的離散點，從而將抽象的步態規劃問題，轉化為四維空間里的路徑規劃問題。值得注意的是，與傳統步態規劃問題尋求最短路徑不同，本文算法的目的是規劃一條最長路徑，因此需要對傳統路徑規劃算法進行改進。路徑規劃算法數量眾多[7 -16]，常用路徑規劃算法包括遺傳算法、人工勢場算法、蟻群算法、迪杰斯特拉算法和模擬退火算法等。考慮到由四足運動步態點轉化得到的四維空間中離散點眾多，且由于路徑相位不同，最優路徑不止一條，為了能廣泛搜索路徑，避免遺漏，本文選擇群智算法對四足機器人擺動相步態進行搜索，由于蟻群算法在相關領域發展較為成熟，故本文采用蟻群算法求取最優路徑，即最優步態規劃。

為了確保機器人能夠穩定運行，本文針對所研究機器人討論了零力矩點控制方案。最后，結合樣機實驗，證明了由本文算法搜索得到的步態，能夠滿足本文所針對的機器人。

2 機器人建模

2.1 單機械腿建模

為實現對每個機械足的精確控制，需要對每個機械足進行抽象建模，如圖1a所示的一條機械腿可以被抽象成如圖1b所示的模型，進而建立如圖1c所示的空間直角坐標系。

Figure 1 Robot single-leg modeling圖1 機器人單腿建模

通過DH(由Denavit和Hartenberg所提出的一種建立機器人運動學模型的算法)矩陣法[17,18]，對單條腿建立運動學方程，計算化簡之后得到的方程組如式(1)所示：

(1)

其中,px,py,pz表示機器人末端點P在直角坐標系中的空間坐標，各參數實際取值如表1所示。

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

表1中,θw為第w關節輸出角度,dw為第w關節旋轉軸與第w+1關節旋轉軸之間沿軸方向距離，aw為第w關節旋轉軸與第w+1關節旋轉軸之間的法線距離，αw為第w關節旋轉軸與第w+1關節旋轉軸之間的夾角。由此方程組可以求解運動學逆模型，一條機械腿的運動，實際上就是控制θ1，θ2，θ3從而控制末端點P擬合空間內曲線DE的過程，配合步態控制策略，4條機械腿按照一定的順序運動，即可以使得機器人做出相應的運動。

2.2 四足機器人逆模型求解

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

Figure 2 Rectangular coordinate to polar coordinate圖2 直角坐標變換到圓柱坐標

圖2中

(2)

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

(3)

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

(4)

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

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

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

(5)

Figure 3 Topology of quadruped robot圖3 四足機器人拓撲圖

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

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

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

(6)

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

(7)

2.3 四足機器人拓撲圖

為確定機器人運動策略，需要對機器人做出如圖3所示的簡化，將機器人抽象為拓撲圖。

如圖3所示，其中圖3a為機器人實物模型三維俯視圖，在圖3b中，機器人的12個舵機被抽象為12個方格，由于4條腿關于機器人幾何軸中心對稱，所以將其按照順時針分為A，B，C，D4個組，每組3個關節，序號由內向外依次從3減小至1。

由文獻[6]的啟發，將4條機械腿的運動軌跡進行分段，本文主要討論如圖4所示直線行走步態，由于篇幅和主題限制本文暫不討論軌跡生成問題。

Figure 4 Discretization trajectory圖4 離散化軌跡

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

由于機器人有4條腿，所以存在一個運動的先后次序，可以看出因為是周期運動，所以ABCD的運動次序與BCDA、CDAB、DABC等運動次序實質上是一致的，只是相位不同，按照循環排列方式，其策略總數如式(8)所示，一共6種方式:

(8)

3 四維空間路徑規劃

3.1 四維空間建立

因為4條腿的運動本質是相互獨立、線性無關的，本文算法利用這一特點，將4條腿運動軌跡，抽象建立為一個四維直角坐標系O-ABCD，如圖5所示。

Figure 5 Four-dimensional space coordinate system圖5 四維空間坐標系

圖5中OA代表A腿的支撐相1～5階段，OB，OC，OD同理，每個坐標軸只取1～5 這5個離散的整數點，對應4條機械支撐相滑動相的5個步態點。

3.2 適應度函數

規劃機器人在一個運動周期中，4條腿的步態可以被視為在O-ABCD坐標系下尋找一個四維坐標序列，使得在該周期里機器人走過的路程最長，即尋找在該空間中的最長路徑。在這一定義下，適應度函數如式(9)所示:

(9)

其中,Lt代表第t輪迭代機器人走過的路程總長度，Lt的值為8個步態點之間7個間隔距離Lstep之和，每個間隔距離Lstep又等于機器人從一個狀態點轉移到下一個狀態點之后，4條機械腿位移dleg的總和。

3.3 機器人行走規則

控制機器人行走有如下規則：

(1)為保持機器人運動平穩，任意時刻，機器人至少有3條腿接觸地面，作為支撐。

(2)為保證任意一個機械結構不過度磨損，機器人運動時4條腿應同時動作，且動作幅度一致。

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

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

對于規則(3)，可以引申為階段轉移只能增加不能減少，特別地，第5階段只能轉移至第1階段。若出現階段減少，則引入適當懲罰機制。

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

Table 2 Reward and punishment rule表2 獎勵與懲罰規則

由此可以明顯看到，獎勵方式與機械腿運動對機器人整體運動的貢獻有關，即只要能有利于驅動機器人向某一特定方向運動，則該轉移方式將得到獎勵，特別地由階段5轉移到階段1，由于只有1條腿運動，需要特殊的獎勵方式。

如表2所示，任意一條腿由前一階段向后一階段轉移，將出現總共25種情形，不同情形對應的獎勵g不同。

在本文算法中，獎勵模式1對應g=+1，獎勵模式2對應g=+4，保持不動對應g=0，懲罰對應g=-10,由此得到轉移規則矩陣G,如式(10)所示:

(10)

3.4 改進蟻群算法

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

(11)

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

(12)

其中，Kd是距離倍率，可以調節啟發函數的影響力，Ed為期望值，為使得ηstep(t)得到最大值時，rewardstep的值與期望值Ed接近，應使得|rewardstep-Ed|趨近于0。

結合規則(1)和規則表2，在獎勵規則下只有得到分數rewardstep為4才是最優選擇，所以選擇Ed=4+ε，其中|ε|應遠小于1但是大于0。

結合式(11)與適應度函數式(9)可知，距離適應度函數與獎勵適應度函數是線性相關的，為便于計算取二者之間系數為1，如式(13)所示:

(13)

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

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

ρ∈(0,1),

(14)

本文算法規劃路線為最長路徑，相較于傳統蟻群算法尋找最短路徑有所不同。本文提出的改進蟻群算法流程圖如6所示。

Figure 6 Flow chart of improved ant colony algorithm圖6 改進蟻群算法流程圖

4 仿真結果

本次仿真采用Matlab 2014a在Windows 7系統中運行，仿真迭代初始條件如表3所示，50個個體進行20次迭代，在四維空間中尋找最長路徑。

值得注意的是，由于該四維空間內離散點數量眾多，但是蟻群總數較少，為避免本文算法陷入局部最優，同時擴大搜索范圍，所以每次迭代都會重新隨機選擇螞蟻初始位置，以增加樣本數量。

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

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

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

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

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

(15)

5 零力矩點控制

因為腿從階段5返回到階段1需要經過階段6和階段7位置擺動相的變換，所以仍然需要擴充式(15)所示矩陣。

同時如圖9所示，黑色圓形A、B、C、D代表與地面直接接觸做支撐的腿，虛線圓形A代表搖擺相時抬起的腿，以A腿抬起為例，虛線圓形M代表機器人的重心，虛線框代表支撐面。在支撐相時四足做支撐，此時重心M落在支撐面四邊形ABCD上，所以機器人可以穩定站立。但是在搖擺相時，抬起A腿后，支撐面由四邊形ABCD變為三角形BCD，重心M落在支撐面BCD之外，這使得搖擺相時機器人將出現站立不穩進而跌倒的現象。為解決這一問題，則需要另外3條腿做出相應調整，使得機器人在一條腿處于搖擺相時，重心重新落在由其余3條腿構成的支撐面內，如圖10所示。

Figure 9 Gravity center movement in swing phase圖9 擺動相重心偏移

Figure 10 Correction for gravity center movement in pre-action圖10 預動作修正重心偏移

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

Table 4 Extended rule 表4 擴展規則

表4中“*”表示需要調整位置，當第i條腿抬起時，第j條腿末端點P坐標由(xj,yj)調整為(Xj,Yj)的規則如式(16)所示:

(16)

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

(17)

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

Figure 11 ZMP controlling topology圖11 ZMP控制拓撲圖

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

Table 5 Movement directions 表5 偏移方向

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

Table 6 Movement control rules table表6 偏移控制規則表

表6中，數字1～4為表5中偏移方向代號，“預”代表本次偏移只在預動作時有效。將表6與表4結合即可以控制機器人運行。以表4中步態4為例，此時D腿抬起，所以A腿在表4中步態2的基礎上向4方向偏移，B腿在表4中步態3的基礎上向3方向偏移，C腿在步驟點4的基礎上向2方向偏移。

6 樣機測試

本次實驗樣機機體結構采用3D打印建造，材質為PLA(聚乳酸)，采用TS-315總線數字舵機作為伺服系統，可同時采用stm32嵌入式做脫機控制，或者采用PC機數據線做在線控制。本次測試采用PC機數據線做在線控制，機器人樣機如圖12所示。

為標定機械足順序，將其中一只采用黃色材料打印，由于圖片轉為灰度圖，圖中顏色應為淺灰色。左上方淺色的腿為B，沿順時針方向依次為BCDA，由前文可知該運動方式等效于ABCD運動方式。該機器人按照表4所示規則，運行20步行走100 mm的錄像截圖如圖13所示。

Figure 12 Robot prototype 圖12 機器人樣機

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

經過10次實驗，得到數據取平均值，與設置值的對比如表7所示。

Table 7 Comparison of experimental data表7 實驗數據對比

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

如圖13所示，前2列為本文算法搜索出來的驅動機器人前進的8個步態，直接驅動機器人前進；第3列為偏移重心的預動作，為搖擺相時機器人的平衡做準備；前3列為4足支撐相，第4列和第5列為搖擺相，使得處于末位5的腿重新擺回初始位1。從1到20分別對應20個步態，機器人從右往左沿直線運動，整個過程中，機器人重心平穩，運行穩定，符合本文設定的機器人行走標準。

7 結束語

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