司玉文, 黃紹服,3*

(1.安徽理工大學機械工程學院, 淮南 232000; 2.安徽理工大學環(huán)境友好材料與職業(yè)健康研究院(蕪湖), 蕪湖 241003;3.南京航空航天大學江蘇省精密與微細制造技術重點實驗室, 南京 210016)

隨著多模態(tài)融合感知、非結構化場景人工智能分析與柔性本體等核心技術[1]的突破,移動機器人感知、理解和控制能力將進一步提升,從而更自如地在非標準化環(huán)境[2]中執(zhí)行復雜任務。移動機器人最優(yōu)運動規(guī)劃[3-4]為通過感知外界環(huán)境信息,借助智能算法規(guī)劃一條無障礙、距離短的路徑,同時將路徑映射到機械臂的關節(jié)空間,使機械臂能量損耗最低。路徑規(guī)劃是運動規(guī)劃的基礎,解決研究對象能否安全、快速到達目標點的問題。常用的路徑規(guī)劃算法[5]有搜索算法[6]、采樣算法[7]與仿生算法[8]。蟻群優(yōu)化算法[9](ant colony optimization,ACO)優(yōu)化效率低,但不容易陷入局部最優(yōu),易于尋找全局最優(yōu)解;人工勢場法[10](artificial potential field,APF)計算簡單,規(guī)劃的路徑安全且平穩(wěn),但復雜多變的勢場環(huán)境易形成局部最小點,導致目標不可達與局部最優(yōu)問題。

為了在復雜環(huán)境應用ACO算法規(guī)劃路徑,文獻[11]對ACO算法數(shù)學模型進行改進,引入多目標性能指標,使得規(guī)劃的路徑具有較高的實時性和穩(wěn)定性。文獻[12]設置輔助蟻群的初始信息素,提高路徑搜索效率;加入偽隨機狀態(tài)轉移策略,避免算法過早收斂。文獻[13]采用模糊邏輯改進ACO算法,使得算法在避障最小化標準中找到平衡的路線。APF算法易于實現(xiàn),但適應場景單一。文獻[14]通過A*算法完成全局路徑規(guī)劃,以多個子目標點除以全局最優(yōu)路徑為APF算法的虛擬目標點,同時通過切換目標點完成局部路徑規(guī)劃。文獻[15]將改進A*算法與APF算法相結合,在APF中引入偏航增益系數(shù)解決局部陷阱的問題。文獻[16]將APF算法與快速隨機擴展樹算法相結合,在快速隨機擴展樹算法中引入目標重力概念,大大縮減了路徑長度。勢場蟻群算法是近年興起的融合算法,特點是借助APF算法優(yōu)化ACO算法的搜索效率,同時ACO算法可以減少APF算法計算時陷入局部最優(yōu)問題。文獻[17]將跳點搜索算法與ACO算法的兩種搜索策略相互結合,勢場函數(shù)中加入勢場合力遞減系數(shù),在蟻群啟發(fā)函數(shù)中融合改進勢場函數(shù)啟發(fā)信息,提高了全局路徑規(guī)劃的效率。文獻[18]在蟻群啟發(fā)函數(shù)中加入勢場力遞減系數(shù),綜合考慮勢場路徑長度、平緩性與平滑性因子,構造新的多因子啟發(fā)信息函數(shù),提升算法在復雜環(huán)境下的適應力。

現(xiàn)有文獻在特定的低維環(huán)境對勢場蟻群算法進行改進,但是忽略了工業(yè)機器人多應用于高維環(huán)境,為了提高算法的效率,增加算法的實用性,現(xiàn)對人工勢場法的斥力函數(shù)進行改進,解決目標不可達與易陷入局部最優(yōu)問題。在蟻群算法中引入勢場函數(shù),構造IAPF-ACO算法,在三維環(huán)境下進行避障規(guī)劃,驗證算法的可行性。將IAPF-ACO算法規(guī)劃路徑應用到工業(yè)機器人運動仿真,并在關節(jié)空間內對路徑點平滑處理,驗證算法適用性。

1 人工勢場法與蟻群算法

1.1 人工勢場法

人工勢場法是一種構造抽象虛擬力場尋找優(yōu)化路徑的算法,常應用于局部路徑規(guī)劃。斥力勢場與引力勢場是算法的基本單元,該算法基本原理是工作范圍內構造引力場,障礙物周圍構造斥力場,斥力與引力的疊加將引導研究對象從起點到安全到達目標點。圖1是APF算法的工作示意圖。

圖1 APF算法原理示意圖

算法的運行機制是通過構造斥力場函數(shù)與引力場函數(shù)進行路徑尋優(yōu)。引力勢場的勢能與研究對象和目標點間的距離有關,距離越遠,研究對象所受的勢能越高;距離越近,研究對象所受的勢能越低。對虛擬力場解析進行算法的數(shù)學建模,引力場函數(shù)如式(1)所示。

(1)

式(1)中:η為正比例增益系數(shù);ρ(q,qg)為一個矢量,表示研究對象的位置q和目標點位置qg之間的歐幾里德距離|q-qg|,矢量方向是從研究對象的位置指向目標點位置。相應的引力Fatt(X)為引力場的負梯度。

Fatt(X)=-?Uatt(X)=ηρ(q,qg)

(2)

斥力勢場的勢能與研究對象和障礙物間的距離有關,當研究對象的位置處于障礙物影響范圍時,兩者距離越遠,所受的勢能值越小;當研究對象未處于障礙物影響范圍時,其受到的勢能值為零。式(3)是斥力勢場函數(shù)。

(3)

式(3)中:k為正比例系數(shù);ρ(q,qo)是一個矢量,方向為從障礙物指向研究對象,大小是研究對象與障礙物間的距離|q-qo|;ρo為常數(shù),表示障礙物影響的閾值距離。相應的斥力Freq(X)為斥力場的負梯度。

(4)

1.2 蟻群算法

蟻群算法是一種尋找優(yōu)化路徑的概率型算法,其靈感來源于蟻群覓食的過程。信息素是蟻群獨一無二的信息傳遞方式,也是ACO算法解決優(yōu)化問題的核心。基本工作原理是蟻群覓食的過程中,螞蟻在較短路徑上釋放的信息素越多,隨著時間推進,較短路徑的信息素濃度不斷增高,最終蟻群會集中到信息素濃度最高的路徑。圖2是蟻群工作原理示意圖,路徑上的黑色凝聚物是螞蟻。

圖2 蟻群工作原理示意圖

從圖2(a)可知,螞蟻在覓食的過程中會遍歷所有路徑,路徑的螞蟻通行頻率與信息素呈正相關。從圖2(b)可知,隨著螞蟻一次次往返,大部分螞蟻已經在最短路徑上進行運動。通過對螞蟻的覓食過程進行數(shù)學建模,得到ACO算法的數(shù)學模型,算法的尋優(yōu)過程是通過轉移概率規(guī)則與信息素更新規(guī)則實現(xiàn)。

1.2.1 轉移概率規(guī)則

螞蟻M從路徑點i到j的狀態(tài)轉移概率為

(5)

式(5)中:τij為兩路徑點之間的信息素濃度;C為螞蟻M未訪問路徑點集合;η為啟發(fā)函數(shù);α為信息素因子;β為啟發(fā)函數(shù)因子。

1.2.2 局部信息素更新規(guī)則

螞蟻M從路徑點i到j的狀態(tài)轉移概率為

(6)

式(6)中:ρ1為局部信息素衰減指數(shù);L為兩路徑點之間距離;Q1為局部信息素強度。

1.2.3 全局信息素更新規(guī)則

螞蟻M從路徑點i到j的狀態(tài)轉移概率為

(7)

式(7)中:ρ2為全局信息素衰減指數(shù);K為當前迭代次數(shù);Q2為局部信息素強度。

2 融合改進勢場法的蟻群算法

2.1 改進斥力勢場函數(shù)

傳統(tǒng)APF算法在機器人路徑尋優(yōu)過程中,根據(jù)勢場函數(shù)知當障礙物與目標點距離少于斥力影響范圍時,目標點的引力降為零,而障礙物斥力不為零,此時機器人雖到達目標點,但在斥力勢場作用下不停止運動,從而導致目標不可達問題。同時當機器人處與某個路徑點時,如果若干個障礙物的合斥力與目標點的引力大小相等、方向相反,即合力為0,這將導致機器人不再“受力”繼續(xù)進行路徑搜尋,陷入局部最優(yōu)。

圖3 改進勢場模型原理圖

改進后斥力場函數(shù)如式(8)所示。

(8)

(9)

式(9)中:Freq1的方向是從障礙物指向機器人;Freq2的方向是從機器人指向目標點。

2.2 構造勢場啟發(fā)函數(shù)

ACO算法中轉移概率規(guī)則解決蟻群下一步搜尋方向的問題,搜尋方向對路徑長度有直接聯(lián)系。啟發(fā)函數(shù)η是規(guī)則的核心組成部分,被定義為當前節(jié)點至下一節(jié)點距離的倒數(shù),表達式為

(10)

式(10)中:dij為當前結點位置i與下一可達結點j之間的歐幾里德距離。

根據(jù)式(10)可知,ACO算法的啟發(fā)函數(shù)僅與研究對象當前位置到下一位置的距離有關,當大量障礙物在目標點周圍時,由于ACO算法的啟發(fā)函數(shù)考慮條件較少,可能導致算法陷入局部最優(yōu)。同時由于ACO算法的正反饋性,如果初代螞蟻未能搜索到最優(yōu)路徑,將會引導蟻群留下大量信息素使得其他螞蟻選擇初始路徑,最終可能導致搜索的路徑較長,甚至規(guī)劃一條不安全路徑。而在人工勢場法中,由于存在假設的引力場與斥力場,目標點與障礙物在整個規(guī)劃過程中對研究對象存有合力場的合力作用,因此算法的局部搜索能力顯著。故從算法的全局性出發(fā),提出改進勢場蟻群優(yōu)化算法,將改進勢場模型引入ACO算法,在啟發(fā)信息函數(shù)中考慮目標點與障礙物在全局規(guī)劃過程中對研究對象的影響。具體實現(xiàn)方法是目標點周圍構建引力勢場,障礙物周圍建立斥力勢場,在啟發(fā)函數(shù)中增加合力場影響因子,同時加入下一節(jié)點與目標點的距離信息因子,構造勢場啟發(fā)信息函數(shù)如式(11)所示。

(11)

式(11)中:β為合勢場力Fsum與螞蟻覓食路徑的夾角,螞蟻在初始搜索階段主要依賴合勢場力與目標點的作用。加入目標點對螞蟻的動態(tài)影響因子,改善了ACO算法初期收斂速度慢的不足,同時提高算法的全局搜索能力。當節(jié)點勢場合力為0時,蟻群啟發(fā)信息計算主要依靠目標點、下一節(jié)點對當前節(jié)點的作用,有效避免算法陷入局部最優(yōu)。

3 仿真驗證

3.1 路徑規(guī)劃

通過對IAPF-ACO算法的分析,針對三維路徑規(guī)劃給出了具體實現(xiàn)流程,流程如表1所示。

表1 IAPF-ACO算法流程

為驗證IAPF-ACO算法的可行性,在MATLAB中搭建工作環(huán)境進行算法驗證。第一種環(huán)境地圖存在4個體積不等且離散排列的障礙物,初始點和目標點分別是(1,10,11)、(21,10,6)。采用柵格地圖法進行環(huán)境建模,整個空間被劃分為21×21×21塊柵格區(qū)域,后按照表1流程進行避障路徑規(guī)劃。規(guī)劃結果如圖4、圖5所示。

圖4 離散環(huán)境避障規(guī)劃

圖5 離散環(huán)境規(guī)劃結果

圖4是3種算法進行迭代計算規(guī)劃的路徑結果,其中GACO算法是在ACO算法的基礎上,對啟發(fā)函數(shù)進行改進,引入目標點對路徑規(guī)劃的影響因素,即當前位置到目標點的歐式距離。3種算法都可以在離散的障礙物環(huán)境尋找一條無碰撞且安全的路徑,其中IAPF-ACO算法綜合考慮障礙物對研究對象的影響,規(guī)劃的路線是從障礙物之間穿過的一條安全路徑,而另兩種路徑是越過障礙物,增加了路徑規(guī)劃成本。路徑長度是判斷算法尋優(yōu)結果的重要組成部分,圖5是3條路徑迭代尋優(yōu)的過程示意圖。

圖5是3種算法進行200次迭代計算結果,IAPF-ACO算法因考慮導向性在初始規(guī)劃階段快速進入收斂區(qū)間,而另兩種算法在迭代50次左右才逐漸收斂到平穩(wěn)區(qū)間,證明本文算法在初始階段搜索效率較優(yōu)。迭代尋優(yōu)完成時,IAPF-ACO算法規(guī)劃長度最優(yōu)結果是29.49,GACO算法最優(yōu)結果是30.41,ACO最優(yōu)結果是31.44,IAPF-ACO算法規(guī)劃路徑長度較另兩種分別縮短了3.02%、6.2%。為了驗證算法在大體積障礙物環(huán)境下的可行性,在 21×21×21塊的三維空間中構建3個長方體的障礙物,設置初始點(1,10,11)和目標點(21,10,6),規(guī)劃結果如圖6、圖7所示。

圖6 聚集環(huán)境避障規(guī)劃

圖7 聚集環(huán)境規(guī)劃結果

從圖6可以看到3種算法可以規(guī)劃一條安全的避障路徑,IAPF-ACO規(guī)劃的路線呈階梯形狀,增加了實際應用中的平穩(wěn)性,ACO算法規(guī)劃路徑波動較大,增大了碰撞風險。圖7是3條路徑迭代尋優(yōu)的過程示意圖。

圖7是3種算法進行200次迭代計算結果, IAPF-ACO算法在迭代115次時收斂到最優(yōu)結果,其余算法約160次收斂到最優(yōu)結果,證明算法在收斂效果較優(yōu)。其中IAPF-ACO規(guī)劃最優(yōu)結果是22.07,較GACO規(guī)劃路徑縮短了6.24%,較ACO規(guī)劃路徑縮短了14.56%。大體積物品是生活中常見的障礙物,工業(yè)機器人碰撞到可能會損壞機器本身,為了驗證算法在聚集環(huán)境不同迭代次數(shù)下尋優(yōu)結果,分別在50、100、200次迭代下進行算法尋優(yōu),結果如表2所示。

表2 3種算法迭代實驗結果

每種算法在各個設置條件下運行多次,取5個不重復結果。迭代50次時IAPF-ACO算法規(guī)劃均值是23.642,較ACO規(guī)劃均值縮短14.59%,較GACO規(guī)劃均值縮短11.42%。從表2中可知,在多種運行條件下IAPF-ACO算法結果均值低于其余兩種算法,證明了IAPF-ACO算法的可行性。

3.2 機械臂避障仿真

關節(jié)空間規(guī)劃、笛卡爾空間規(guī)劃是機械臂兩種空間規(guī)劃方式。關節(jié)空間規(guī)劃是已知機械臂末端軌跡點通過逆運動學求解各個關節(jié)變量值進行插值擬合,其規(guī)劃時不需要逐個軌跡點計算逆運動學解,計算量較笛卡爾空間規(guī)劃大大減少,不會造成數(shù)據(jù)冗余,同時可以避免產生奇異值。優(yōu)化關節(jié)突變造成的能量損耗,可以有效延長機械臂的使用壽命。常用的平滑策略有B樣條插值、多項式插值等方法,綜合考慮平滑時間與效果等特點,采用3次B樣條插值進行關節(jié)軌跡插值。

為了驗證本文方法的有效性,在MATLAB中搭建機械臂測試工況,構建長方體障礙物模型。機械臂模型為應用于弧焊、裝配和物料搬運等領域的工業(yè)機器人ABB1410。在關節(jié)空間對IAPF-ACO算法規(guī)劃的路徑點進行平滑,圖8是關節(jié)5的平滑結果。

圖8 關節(jié)5平滑結果

由圖8知,未經平滑時關節(jié)5的多個關節(jié)點之間會發(fā)生突變,經過平滑后各關節(jié)點之間可以保證快速、連續(xù)的經過,并且可以遍歷所有關節(jié)點,可以驗證平滑函數(shù)的有效性。采用3次B樣條插值函數(shù)對6個關節(jié)曲線進行平滑處理,處理完成的曲線加載到機械臂仿真程序中,運行結果如圖9所示。

圖9 機械臂避障仿真

由圖9可知,在搭建的測試工況中,機械臂可以安全地從障礙物之間到達目標點,運動軌跡是一條安全、無碰撞的路徑,可知IAPF-ACO算法規(guī)劃路徑是有效路徑。從局部放大圖可知運行軌跡是平滑的、連續(xù)的,證明3次B樣條函數(shù)應用效果較好。

4 結論

針對APF算法尋優(yōu)目標不可達,易陷入局部最優(yōu)的問題,對斥力場模型函數(shù)進行改進,引入目標調節(jié)因子,使得尋優(yōu)至目標點時合力為零。針對ACO算法初始收斂慢,尋優(yōu)效果不良的問題,對啟發(fā)信息函數(shù)進行改進,引入改進APF算法模型,使得算法快速收斂至最優(yōu)路徑。對3種算法進行迭代實驗,收集不同迭代次數(shù)結果,實驗結果證明IAPF-ACO算法均優(yōu)于ACO、GACO算法。在關節(jié)空間內將規(guī)劃的路徑進行平滑處理,避障仿真結果證明規(guī)劃的路徑是一條安全、平滑的路徑。