朱 敏, 肖 陽, 臧昭宇

(合肥工業(yè)大學(xué) 電氣與自動化工程學(xué)院,安徽 合肥 230009)

路徑規(guī)劃是機(jī)械臂運動規(guī)劃領(lǐng)域的重要研究課題之一,是指在已知的環(huán)境下,按照某種方法找出一條從起點到終點的無碰撞路徑[1-4]?？紤]到機(jī)械臂工作環(huán)境的日益復(fù)雜,路徑規(guī)劃的作用也越來越重要,許多智能優(yōu)化算法也已經(jīng)應(yīng)用于相關(guān)領(lǐng)域中,如RRT算法[5]、人工勢場法[6]、A*算法[7]、遺傳算法[8]等。其中,蟻群算法[9]具有正反饋的特點以及并行計算的優(yōu)勢,已被廣泛應(yīng)用于解決路徑規(guī)劃等問題。為了進(jìn)一步提高蟻群算法在機(jī)械臂路徑規(guī)劃中的效果,國內(nèi)學(xué)者對蟻群算法進(jìn)行了適當(dāng)優(yōu)化,文獻(xiàn)[10]在蟻群算法中引入粒子群算法對其部分較優(yōu)解進(jìn)行變異和交叉操作,得到了更優(yōu)的解;文獻(xiàn)[11]采取改進(jìn)的信息素更新策略和“慣性原則”完成最優(yōu)操作路徑的搜索;文獻(xiàn)[12]提出建立可視域搜索空間和采用自適應(yīng)調(diào)節(jié)揮發(fā)系數(shù)策略來加快蟻群算法的收斂速度;文獻(xiàn)[13]結(jié)合人工勢場法中強(qiáng)化目標(biāo)路徑的優(yōu)點,提升蟻群算法的收斂能力,并有效縮短了算法的迭代次數(shù)。

本文提出一種改進(jìn)的蟻群算法,將蟻群劃分為外層蟻群和內(nèi)層蟻群,分別設(shè)計不同的啟發(fā)函數(shù)來提高搜索效率,并引入安全因子提高機(jī)械臂運動的安全性,先利用外層蟻群在無信息素的狀態(tài)下規(guī)劃出一條參考路徑,增強(qiáng)該路徑上的初始信息素濃度,優(yōu)化信息素初值,同時改進(jìn)信息素更新規(guī)則并對信息素濃度加以限制,進(jìn)一步提升內(nèi)層蟻群的全局尋優(yōu)能力,搜索得到的最優(yōu)路徑經(jīng)由一系列運算和約束得到機(jī)械臂的位姿集合,再導(dǎo)入到虛擬機(jī)械臂中,由機(jī)械臂的運動仿真實驗可知,本文改進(jìn)蟻群算法能為機(jī)械臂在不同環(huán)境中規(guī)劃出一條符合運動要求的避障最優(yōu)路徑。

1 機(jī)械臂運動學(xué)建模

本文以品牌型號為IRB1600的六自由度串聯(lián)工業(yè)機(jī)械臂為研究對象,相應(yīng)的D-H參數(shù)見表1所列,機(jī)械臂的各個連桿坐標(biāo)系如圖1所示。

表1 IRB1600的D-H參數(shù)

圖1 IRB1600的連桿坐標(biāo)系

表1中:αi-1為連桿扭轉(zhuǎn)角,是相鄰軸線在同一平面內(nèi)的夾角;ai-1為連桿長度,是關(guān)節(jié)軸i-1與關(guān)節(jié)軸i之間公垂線的長度;di為連桿偏距,是沿2個相鄰連桿公共軸線方向的距離;θi為關(guān)節(jié)轉(zhuǎn)角變量,是兩相鄰連桿繞公共軸線旋轉(zhuǎn)的夾角。則坐標(biāo)系{i}相對于坐標(biāo)系{i-1}的變換矩陣為:

(1)

(2)

其中:n、o、a為3個列向量,表示機(jī)械臂的姿態(tài);p為機(jī)械臂末端的三維坐標(biāo)。

2 碰撞檢測

在實際工作環(huán)境中,障礙物多為不規(guī)則物體,建模難度高,本文采用長方體包絡(luò)法簡化障礙物,具體做法是將不規(guī)則的障礙物用體積最小的長方體包裹起來。

假設(shè)機(jī)械臂關(guān)節(jié)i和關(guān)節(jié)i+1處的坐標(biāo)分別為Pi(xi,yi,zi) 和Pi+1(xi+1,yi+1,zi+1),則連桿li所處直線的方向為M=(xi+1-xi,yi+1-yi,zi+1-zi)。以長方體平行于x軸的上平面N為例,P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)、P4(x4,y4,z4)為上平面N上的4個頂點,通過計算可得到平面N的法向量K=(k1,k2,k3)。若K·M=0,則表示直線與平面N平行;若K·M≠0,則表示直線與平面N相交,必定存在交點P0(x0,y0,z0)。交點若為碰撞點需滿足以下約束條件:首先交點在平面N的內(nèi)部;其次交點處于連桿線段上。由以上約束可得到如下公式:

(3)

3 基本蟻群算法

蟻群算法在第1次迭代時通過啟發(fā)信息尋找起始點到目標(biāo)點的可行解并分泌信息素,之后的搜索過程又受到信息素的吸引,形成正反饋,逐漸找到全局最優(yōu)目標(biāo)并收斂。

3.1 狀態(tài)轉(zhuǎn)移概率

狀態(tài)轉(zhuǎn)移是指蟻群的移動,需要先計算螞蟻的狀態(tài)轉(zhuǎn)移概率再按輪盤賭選擇下一步前往的位置,公式如下:

(4)

其中:集合{allowed}為第k只螞蟻下一步所能選擇的路徑節(jié)點集合;α為反映信息素在螞蟻選擇路徑的相對重要程度的參數(shù);β為反映啟發(fā)式信息在螞蟻選擇路徑的相對重要程度的參數(shù);τij(t)為連接節(jié)點(i,j)之間的信息素濃度;ηij(t)為連接節(jié)點(i,j)上的啟發(fā)函數(shù),取為1/dij。

3.2 信息素的更新規(guī)則

信息素的更新是指量化人工螞蟻在路徑上分泌的信息素及進(jìn)一步模擬自然界中信息素的揮發(fā)過程,公式如下:

(5)

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

基本蟻群算法有以下缺點:每個路徑節(jié)點的初始信息素相同且啟發(fā)值之間也不存在明顯差異,因此致使蟻群盲目搜索,難以找到全局最優(yōu)解。為了解決上述問題,本文進(jìn)行如下改進(jìn):

考慮到障礙物的復(fù)雜程度和工作空間的大小,將蟻群進(jìn)行劃分,即

(6)

q=m-p

(7)

其中:p為外層蟻群數(shù)量;q為內(nèi)層蟻群數(shù)量;Vobstacle為障礙物所占體積;Vworkspace為工作空間大小;m為螞蟻總數(shù)量。

4.1 設(shè)置安全因子

根據(jù)機(jī)械臂能否安全到達(dá),對三維空間內(nèi)各離散點計算安全值,即

(8)

其中:Safe(Pi)為當(dāng)前節(jié)點Pi搜索域內(nèi)的安全點;Pi+1為待選節(jié)點;S為搜索區(qū)域內(nèi)各離散點的安全因子。若離散點處于障礙物外且機(jī)械臂各連桿均未與障礙物發(fā)生碰撞,則將該點設(shè)為安全點,將其值設(shè)為1;否則將其值設(shè)為0。

4.2 外層蟻群啟發(fā)函數(shù)設(shè)計

對于外層蟻群來說,需要在無初始信息素的前提下規(guī)劃出合理路徑,因此本文在設(shè)計外層蟻群啟發(fā)函數(shù)時需考慮待選節(jié)點與目標(biāo)節(jié)點距離的影響,其啟發(fā)函數(shù)為:

(9)

其中:djg為當(dāng)前計算的待選節(jié)點到目標(biāo)節(jié)點的距離;min(djg)為所有待選節(jié)點到目標(biāo)節(jié)點距離的最小值。

4.3 內(nèi)層蟻群啟發(fā)函數(shù)設(shè)計

鑒于A*算法尋路速度較快,其啟發(fā)式成本由采用估價函數(shù)f(n)表示,通過f(n)來指導(dǎo)節(jié)點的擴(kuò)展與搜索,每一次都是搜索從起點到終點之間使得f(n)最小的路徑,估價函數(shù)方程為:

f(n)=g(n)+h(n)

(10)

其中:g(n)為從起點到當(dāng)前節(jié)點的實際路徑代價;h(n)為從當(dāng)前節(jié)點到終點的預(yù)估路徑代價。

因此,將A*算法的估價函數(shù)用作內(nèi)層蟻群的啟發(fā)式信息,以便于搜索全局最優(yōu)路徑,加快收斂速度;同時,將安全因子也添加到內(nèi)層蟻群算法的啟發(fā)函數(shù)中,改進(jìn)后的啟發(fā)函數(shù)為:

(11)

4.4 信息素不均勻分布

(12)

4.5 信息素更新規(guī)則的改進(jìn)

為提升算法的收斂速度、獲得最優(yōu)路徑,在更新蟻群信息素時引入狼群的獵物分配機(jī)制[14],狼群獵物分配機(jī)制的原理是在完成狩獵后,善于狩獵且身體強(qiáng)壯的狼會獲得大量食物,而身體瘦弱的狼只能分得少量食物,這樣整個狼群才能有更高的幾率在下一次狩獵時捕獲獵物,經(jīng)過多次迭代,該狼群的生存能力逐漸被提高。因此,借鑒狼群的獵物分配機(jī)制改進(jìn)信息素更新規(guī)則,最短路徑上的信息素濃度會增加,同時最長路徑上的信息素濃度降低,突出不同路徑信息素的差異化,從而提高了螞蟻中優(yōu)質(zhì)種群的生存能力,改進(jìn)后的信息素更新公式為:

τij(t+1)=(1-ρ)τij(t)+

(13)

其中:Lbest、Lworst分別為每次蟻群到達(dá)目標(biāo)節(jié)點的最優(yōu)路徑和最差路徑;δ、ω分別為每次搜索時找到最優(yōu)路徑和最差路徑的螞蟻數(shù)量。

4.6 信息素濃度限制

經(jīng)過上述改進(jìn),蟻群算法的收斂速度得到了提高,但會造成部分路徑上的信息素濃度過高,進(jìn)而降低算法對其他路徑的選擇,導(dǎo)致搜索出現(xiàn)停滯,為了防止這種情況產(chǎn)生,借鑒最大最小螞蟻系統(tǒng)將信息素的濃度范圍限定在τmin～τmax之間,具體公式為:

(14)

4.7 算法實現(xiàn)流程

(1) 初始化算法參數(shù),設(shè)置起點和終點以及安全因子。

(2) 按(6)式和(7)式蟻群分為外層蟻群和內(nèi)層蟻群,分別設(shè)計不同的啟發(fā)函數(shù)。

(3) 先由外層蟻群在無信息素狀態(tài)下進(jìn)行搜索,將搜尋到的最優(yōu)參考路徑轉(zhuǎn)化為初始信息素的強(qiáng)化值,由(12)式空間內(nèi)各點信息素初始分布。

(4) 再由內(nèi)層蟻群進(jìn)行路徑尋優(yōu),記錄每次循環(huán)的最優(yōu)路徑。

(5) 按(13)式對信息素進(jìn)行更新,并按(14)式限定信息素濃度范圍。

(6) 判斷循環(huán)次數(shù)是否達(dá)到最大值,若達(dá)到,則對搜索到的最優(yōu)路徑進(jìn)行碰撞檢測;否則跳轉(zhuǎn)到步驟(4)。

(7) 若沒有發(fā)生碰撞,則輸出路徑,結(jié)束;若發(fā)生碰撞,則跳轉(zhuǎn)到步驟(3)。

算法實現(xiàn)流程如圖2所示。

5 仿真分析

5.1 改進(jìn)的蟻群算法規(guī)劃路徑的平穩(wěn)性

本文使用MATLAB 2018a軟件,在CPU為corei5-4210U,運行內(nèi)存為8 GB的Win10計算機(jī)上進(jìn)行實驗。算法中各參數(shù)設(shè)置如下:α=1,β=5,ρ=0.9,Q=100,m=50,N=100。為驗證使用本文改進(jìn)的蟻群算法在不同復(fù)雜障礙環(huán)境下規(guī)劃出的路徑平穩(wěn)性更好,分別使用本文改進(jìn)蟻群算法、文獻(xiàn)[12]算法以及基本蟻群算法,在如下3種復(fù)雜環(huán)境下模擬機(jī)械臂末端執(zhí)行器進(jìn)行路徑規(guī)劃。

路徑規(guī)劃結(jié)果如圖3所示,在3種復(fù)雜度不同的環(huán)境下,使用本文改進(jìn)蟻群算法均可以為機(jī)械臂規(guī)劃出一條符合運動條件限制的避障路徑,根據(jù)不同障礙物環(huán)境下規(guī)劃的路徑來看,使用本文改進(jìn)的蟻群算法比文獻(xiàn)[12]算法和基本蟻群算法規(guī)劃出的路徑拐點更少,而且搜索到的路徑長度也相對優(yōu)于其他2種算法。因此,使用本文改進(jìn)的蟻群算法規(guī)劃路徑的平穩(wěn)性要優(yōu)于文獻(xiàn)[12]算法和基本蟻群算法。

5.2 改進(jìn)蟻群算法路徑規(guī)劃的高效性

為進(jìn)一步驗證本文對基本蟻群算法改進(jìn)的高效性,在環(huán)境3的條件下,分別用本文改進(jìn)的蟻群算法、文獻(xiàn)[12]算法和基本蟻群算法模擬機(jī)械臂末端的運動規(guī)劃路徑,3種算法均進(jìn)行20次仿真,統(tǒng)計結(jié)果見表2所列。3種算法處于最優(yōu)路徑長度的收斂曲線如圖4所示。

表2 3種算法仿真20次結(jié)果統(tǒng)計

圖4 3種算法最優(yōu)路徑收斂曲線

依據(jù)以上數(shù)據(jù)可知,使用本文改進(jìn)蟻群算法模擬機(jī)械臂末端進(jìn)行路徑規(guī)劃,比其他2種算法所得的平均路徑代價更優(yōu),收斂速度也更快,且在多次仿真中規(guī)劃出的路徑穩(wěn)定性更好,顯著地提高機(jī)械臂在路徑規(guī)劃中的效率。

為進(jìn)一步驗證本文改進(jìn)蟻群算法的可行性,使用RobotStudio軟件中的IRB1600機(jī)械臂進(jìn)行模擬實驗,仿真過程如圖5所示。

圖5 機(jī)械臂避障仿真過程

由圖5可知,機(jī)械臂本體未與障礙物發(fā)生接觸,體現(xiàn)了本文碰撞檢測算法的有效性。由以上仿真實驗可知,使用本文改進(jìn)的蟻群算法對機(jī)械臂進(jìn)行路徑規(guī)劃時,能夠為機(jī)械臂搜索到一條避免碰撞且能滿足運動要求的路徑。

6 結(jié) 論

針對蟻群算法在機(jī)械臂路徑規(guī)劃中存在的問題,如搜索路徑過長、收斂速度慢等,本文對蟻群算法作如下改進(jìn):將蟻群分為外層蟻群和內(nèi)層蟻群,分別設(shè)計不同的啟發(fā)函數(shù),并引入安全因子提高機(jī)械臂運動過程的安全性,利用外層蟻群初始化信息素,引導(dǎo)內(nèi)層蟻群進(jìn)行全局尋優(yōu),在更新信息素時引入狼群的獵物分配原則,限制部分路徑信息素濃度于合理范圍內(nèi)。改進(jìn)的算法通過在MATLAB和RobotStudio中的仿真驗證,結(jié)果表明改進(jìn)后的蟻群算法在收斂速度上,路徑代價相比基本蟻群算法和文獻(xiàn)[12]算法具有明顯的優(yōu)勢,而且在機(jī)械臂仿真中表現(xiàn)良好,可以快速地為機(jī)械臂規(guī)劃一條最優(yōu)路徑。