李江昊,閆亞楠,彭 丹

(1.燕山大學(xué) 信息科學(xué)與工程學(xué)院,河北 秦皇島 066004;2.燕山大學(xué) 理學(xué)院,河北 秦皇島 066004)

1 引 言

隨著機(jī)器人應(yīng)用的發(fā)展和機(jī)器人生產(chǎn)線的出現(xiàn),多機(jī)器人系統(tǒng)的研究引起了科研工作者們極大的興趣[1]。多移動機(jī)器人系統(tǒng)是指:多個單機(jī)器人形成期望的編隊隊形[2],來完成某項任務(wù),且可以找到最優(yōu)路徑。因此多移動機(jī)器人的路徑規(guī)劃與編隊研究成為當(dāng)前機(jī)器人技術(shù)的研究熱點(diǎn)之一[3]。目前眾多的科研工作者們已經(jīng)對此問題進(jìn)行了深入的探討和研究。韓青等[4]提出了一種基于平行導(dǎo)引律的多機(jī)器人編隊算法。通過虛擬領(lǐng)航者的參考位姿與目標(biāo)機(jī)器人的當(dāng)前位姿設(shè)計出平行導(dǎo)引律,并結(jié)合路徑規(guī)劃,設(shè)計出基于法向偏差的LQR(線性二次型調(diào)節(jié)器)軌跡跟蹤控制器,該算法在時間表現(xiàn)上更優(yōu),但穩(wěn)定性能欠缺。張大偉等[5]通過針對非完整移動機(jī)器人運(yùn)動學(xué)模型的特點(diǎn),并結(jié)合機(jī)器人之間的局部交互關(guān)系和李雅普諾夫算法,設(shè)計出一種具有魯棒性的編隊控制器,該控制器能夠有效解決多機(jī)器人系統(tǒng)中的對象模型參數(shù)不確定的問題,但是沒考慮編隊中的路徑優(yōu)化問題。易國等[6]提出了一種基于模糊控制的編隊算法。根據(jù)多移動機(jī)器人編隊的特點(diǎn),并結(jié)合機(jī)器人與障礙物之間的位姿關(guān)系,建立了一種機(jī)器人編隊避障行為的模糊規(guī)則,該算法能實現(xiàn)穩(wěn)定的避障編隊,但是無法在編隊中實現(xiàn)路徑尋優(yōu)。

本文提出了一種路徑尋優(yōu)與軌跡跟蹤相結(jié)合的編隊算法。首先在傳統(tǒng)蟻群算法的基礎(chǔ)上,提出一種改進(jìn)蟻群算法,即改進(jìn)傳統(tǒng)算法的參數(shù)和初始信息素矩陣來進(jìn)行路徑規(guī)劃,使其在收斂性和全局尋優(yōu)2個方面有了顯著提高。然后在規(guī)劃的路徑上進(jìn)行多機(jī)器人編隊。為了保證編隊的穩(wěn)定性,本文設(shè)計了基于李雅普諾夫算法的軌跡跟蹤控制器,仿真實驗證明了該控制器誤差小、穩(wěn)定性強(qiáng)。最后在最優(yōu)路徑和軌跡跟蹤相結(jié)合的基礎(chǔ)上,利用領(lǐng)導(dǎo)—跟隨法,實現(xiàn)了多機(jī)器人的編隊任務(wù)。

2 改進(jìn)蟻群算法的研究

2.1 改進(jìn)蟻群算法的構(gòu)建

傳統(tǒng)蟻群算法存在易陷入局部最優(yōu)、收斂速度慢、迭代次數(shù)多[7],為了提高該算法的收斂速度及全局尋優(yōu)能力,本文對其進(jìn)行以下改進(jìn):

1) 找到信息素和啟發(fā)式信息的權(quán)重啟發(fā)參數(shù)α、β的最佳范圍;當(dāng)α過小時收斂速度慢,當(dāng)α過大時,找不到最短路徑,α的最佳取值范圍為[2,5],同理得β的最佳取值范圍為[6,9]。最終選擇α=2、β=7作為最優(yōu)權(quán)重啟發(fā)參數(shù)組合。

2) 迭代后期信息素濃度高,易陷入局部最優(yōu),改進(jìn)信息素?fù)]發(fā)系數(shù)ρ。ρ取值對算法性能的影響如表1所示。

表1 ρ取值對算法性能的影響Tab.1 Algorithm performance impact on ρ

該算法中其他參數(shù)取值為:K=100表示迭代總次數(shù),Q=1表示信息素增強(qiáng)系數(shù),M=50表示每次迭代的螞蟻總數(shù),α=2表示信息素的啟發(fā)因子,β=7表示啟發(fā)式信息的啟發(fā)因子。

當(dāng)揮發(fā)系數(shù)ρ過大時,信息素積累速度慢,該算法全局搜索能力降低;當(dāng)ρ過小時,信息素累積過多,擾亂螞蟻選擇,收斂速度降低。因此為了增加其收斂速度和全局尋優(yōu)能力,本文將ρ設(shè)計成以迭代次數(shù)k為自變量的函數(shù)。由表1可知:ρ的最佳取值范圍為[0.2～0.5],分析ρ的變化規(guī)律,可推算出ρ的函數(shù)表達(dá)式ρ(k)為:

(1)

式中:k為當(dāng)前迭代次數(shù);ρ(k)為當(dāng)前迭代次數(shù)下的信息素取值。

為了驗證函數(shù)ρ(k)和ρ常值對于蟻群算法性能的影響,通過20次仿真得到表2所示結(jié)果。

表2 ρ函數(shù)與ρ常值對算法性能的影響Tab.2 Algorithm performance impact on function ρ and Constant ρ

通過表2可知:ρ=0.3和ρ(k)對該算法在平均路徑性能上差別不大,因為實驗次數(shù)有限,故最優(yōu)路徑存在一定的差距可以接受。但兩者在平均迭代次數(shù)上存在較大差距,顯然ρ(k)時的收斂速度明顯比ρ=0.3時的收斂速度快。

3) 初始信息素匱乏,收斂速度慢[8],根據(jù)啟發(fā)式信息,尋找初始信息素分布,提高收斂速度。

改進(jìn)思路是:在蟻巢和食物源間,理論上最短距離是兩點(diǎn)間直線距離,此直線稱為“中心直線”,即“中心直線”所在柵格上信息素濃度最大,“中心直線”所經(jīng)過的柵格稱“中心柵格”,在實際覓食中,會存在各種障礙物使螞蟻不能直線通過,必須在繞開障礙物的同時選擇一條最優(yōu)路徑,因此離“中心直線”距離越近的柵格上的信息素就越多的。

針對初始化信息素分布給如下計算方法:

1) 計算每個柵格到所有“中心柵格”的距離,兩柵格間距離越小,說明該柵格離“中心柵格”的位置越近,故該柵格上信息素濃度越大,反之濃度越小。

2) 已知每個柵格上的信息素濃度,計算經(jīng)過兩個柵格間路徑上的信息素濃度,假如螞蟻在柵格處,下一步走到柵格j處,則路徑(i,j)上的信息素濃度與兩個柵格信息素濃度之和成正比,由此求得的信息素矩陣稱為“柵格信息素矩陣”。

3) 障礙物所在柵格上信息素為零,且若柵格和柵格j不相鄰,則路徑(i,j)上的信息素濃度也為零,對此將2)中的“柵格信息素矩陣”,與鄰接矩陣相乘可得初始信息素矩陣。

表3 改進(jìn)初始信息素對算法性能的影響Tab.3 Algorithm performance impact on improved initial pheromone

由表3可知:改進(jìn)初始信息素后的算法相較于傳統(tǒng)算法在路徑尋優(yōu)和收斂速度方面都有提高。

2.2 改進(jìn)蟻群算法仿真及對比分析

采用柵格法進(jìn)行建模,能夠方便處理障礙物邊界問題[9]。本文所構(gòu)建的機(jī)器人柵格地圖空間為20 cm×20 cm,柵格邊長為1 cm,其中黑色代表障礙物,白色代表自由空間,由于本文研究的機(jī)器人是微小型機(jī)器人,即障礙物遠(yuǎn)遠(yuǎn)大于機(jī)器人,因此機(jī)器人在柵格間的移動可近似成一個質(zhì)點(diǎn)。

傳統(tǒng)算法收斂曲線和最優(yōu)路徑圖如圖1所示。改進(jìn)算法的收斂曲線和最優(yōu)路徑圖如圖2所示。

圖1 傳統(tǒng)蟻群仿真圖Fig.1 Traditional ant colony simulation

圖2 改進(jìn)蟻群仿真圖Fig.2 Improved ant colony simulation

由圖1和圖2可知:改進(jìn)算法收斂時的迭代次數(shù)為8次,傳統(tǒng)算法為15次,且兩者最短路徑都為28 cm。但在20次仿真中,達(dá)到最短路徑的次數(shù)傳統(tǒng)算法6次、改進(jìn)算法12次,改進(jìn)算法全局尋優(yōu)能力提高了30%。由2種算法平均迭代次數(shù)可知,改進(jìn)算法收斂速度提高了50%。由此可見,改進(jìn)算法相較于傳統(tǒng)算法在收斂速度和全局尋優(yōu)能力2個方面都有顯著提高。

3 軌跡跟蹤控制器的設(shè)計

3.1 李雅普諾夫穩(wěn)定性分析

評判一個系統(tǒng)的標(biāo)準(zhǔn)需首先證明該系統(tǒng)是穩(wěn)定的,而通過機(jī)器人運(yùn)動誤差構(gòu)造李雅普諾夫函數(shù)便可判斷動力系統(tǒng)的穩(wěn)定性[10]。本文利用李雅普諾夫直接法結(jié)合反推法尋找合適的軌跡跟蹤控制律[11]。

定義李雅普諾夫函數(shù)為:

(2)

式中:k>0,V≥0,當(dāng)且僅當(dāng)xe=ye=θe=0時,V=0。對式(2)進(jìn)行求導(dǎo)得:

(3)

在全局坐標(biāo)系下,機(jī)器人的運(yùn)動學(xué)方程為:

此時,利用Kaczmarz迭代算法循環(huán)遍歷矩陣U的所有行,并將當(dāng)前迭代Ui投影至與矩陣V中當(dāng)前選擇列所對應(yīng)的超平面上,即可求解子優(yōu)化問題式(8).與之類似,利用Kaczmarz迭代算法循環(huán)遍歷矩陣V的所有列,并將當(dāng)前迭代Vj投影至與矩陣U中當(dāng)前選擇行所對應(yīng)的超平面上,即可求解子優(yōu)化問題式(9).本文通過循環(huán)交替求解子凸優(yōu)化問題式(8)和式(9),即可由觀測矩陣Y的部分已知元素重構(gòu)感知矩陣X的所有未知元素.

(4)

機(jī)器人位姿誤差微分方程為:

(5)

將式(5)代入式(3)得:

(6)

(7)

式中:k1>0,k2>0將式(7)代入式(6)得

(8)

3.2 系統(tǒng)仿真及分析

圖3所示為基于PID算法的8字形軌跡跟蹤仿真圖。圖4所示為基于李雅普諾夫算法的8字形軌跡跟蹤仿真圖。由圖的仿真結(jié)果可知:PID控制器和李雅普控制器都能實現(xiàn)8字形軌跡跟蹤。在PID控制器中,x軸方向的誤差穩(wěn)定在[-1.16 m,1.16 m],y軸方向的誤差穩(wěn)定在[-0.3 m,0.3 m];而在李雅普控制器中,x軸和y軸方向的誤差最終都趨于0。可見,李雅普控制器在軌跡跟蹤中誤差更小,穩(wěn)定性更強(qiáng)。因此選用基于李雅普諾夫算法的控制器進(jìn)行機(jī)器人的圓形軌跡跟蹤。

圖3 基于PID控制器的8字形軌跡跟蹤仿真圖Fig.3 8-bit trajectory tracking simulation based on PID controller

圖4 基于李雅普控制器的8字形軌跡跟蹤仿真圖Fig.4 Simulation diagram of 8-shaped trajectory tracking based on Lyapu controller

當(dāng)軌跡跟蹤控制率如式(7),且其中參數(shù)如表4所示時,得到的機(jī)器人軌跡跟蹤圖,如圖5所示。

圖5 機(jī)器人圓形軌跡跟蹤圖Fig.5 Robot circular trajectory tracking diagram

表4 李雅普諾夫控制器仿真參數(shù)設(shè)置Tab.4 Lyapunov controller simulation parameter settings

圖6所示為李雅普控制器的圓形軌跡跟蹤圖,圖7所示為控制率變化圖。由仿真結(jié)果可知,基于李雅普諾夫算法的軌跡跟蹤控制器可以實現(xiàn)機(jī)器人的軌跡跟蹤,且在跟蹤過程誤差更小、穩(wěn)定性更好。

圖6 機(jī)器人圓形軌跡誤差仿真圖Fig.6 Robot circular trajectory error simulation

圖7 圓形軌跡控制率變化圖Fig.7 Circular trajectory control rate change diagram

4 多移動機(jī)器人的編隊控制

4.1 編隊控制器的設(shè)計

本文選領(lǐng)航-跟隨法進(jìn)行多機(jī)器人編隊,該方法易實現(xiàn),發(fā)展成熟。在領(lǐng)航-跟隨法中,若領(lǐng)航者發(fā)生故障,整個編隊系統(tǒng)將會面臨癱瘓,無法完成編隊任務(wù)。因此本文將已知參考軌跡賦給編隊隊形質(zhì)心,其它機(jī)器人只需要與該質(zhì)心保持相應(yīng)的距離和角度即可,且任一機(jī)器人發(fā)生故障,均不影響整個編隊系統(tǒng),此編隊控制方法如圖8所示。

圖8 編隊隊形示意圖Fig.8 Schematic diagram of formation formation

如圖8所示,3個機(jī)器人分別為R1、R2、R3組成三角隊形,其中R為質(zhì)心坐標(biāo),且每個機(jī)器人都滿足式(4)所示的運(yùn)動學(xué)方程。已知質(zhì)心參考軌跡,期望編隊隊形可通過設(shè)置每個機(jī)器人到質(zhì)心的參數(shù)(如距離參數(shù)Si和角度參數(shù)βi)來得到每個機(jī)器人的參考軌跡。已知質(zhì)心軌跡為R(xr(t),yr(t)),機(jī)器人Ri的參考軌跡可定義為:

(9)

在多機(jī)器人編隊中,若每個機(jī)器人都跟蹤自己的參考軌跡,就可實現(xiàn)多機(jī)器人的穩(wěn)定編隊。由此便將多機(jī)器人的編隊問題轉(zhuǎn)化成軌跡跟蹤問題,降低了編隊問題的復(fù)雜度[12],簡化了計算。

4.2 多機(jī)器人編隊仿真與分析

1) 本文主要分析3個機(jī)器人進(jìn)行等邊三角形編隊的情況,當(dāng)多機(jī)器人的質(zhì)心軌跡是沿著直線運(yùn)動時,其仿真結(jié)果如圖9所示。

圖9 多機(jī)器人直線編隊軌跡圖Fig.9 Multi-robot line formation trajectory

假設(shè)機(jī)器人的質(zhì)心軌跡為:

(10)

距離參數(shù)S1=S2=S3=2 m,若β1= π /4,則機(jī)器人R1、R2、R3的參考軌跡為:

(11)

(12)

(13)

2) 當(dāng)多機(jī)器人質(zhì)心軌跡為圓形軌跡,其仿真結(jié)果如圖10所示。

圖10 多機(jī)器人圓形編隊軌跡圖Fig.10 Multi-robot circular formation trajectory

假設(shè)移動機(jī)器人質(zhì)心的軌跡為:

(14)

距離參數(shù)S1=S2=S3=2 m,若β1=0,則機(jī)器人R1、R2、R3的參考軌跡分別為:

(15)

(16)

(17)

通過仿真分析發(fā)現(xiàn),在多機(jī)器人系統(tǒng)編隊中,當(dāng)質(zhì)心軌跡沿逆時針方向行駛時,機(jī)器人也沿逆時針方向行進(jìn)。由圖10可知機(jī)器人在編隊過程中一直保持著三角隊形的穩(wěn)定編隊。

5 結(jié) 論

本文提出了基于路徑尋優(yōu)和軌跡跟蹤的編隊方式。相比于文獻(xiàn)[4],本文提出的李雅普諾夫控制器增強(qiáng)了編隊的穩(wěn)定性;相比于文獻(xiàn)[5],本文利用改進(jìn)蟻群算法找到的最優(yōu)路徑提高了編隊的速度。更深一步的研究任務(wù)是多移動機(jī)器人的動態(tài)路徑規(guī)劃和進(jìn)行多機(jī)器人編隊的隊形變換。