基于IAPF-ACO的工業機器人運動規劃

司玉文, 黃紹服,3*

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

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

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

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

1 人工勢場法與蟻群算法

1.1 人工勢場法

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

圖1 APF算法原理示意圖

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

(1)

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

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

(2)

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

(3)

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

(4)

1.2 蟻群算法

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

圖2 蟻群工作原理示意圖

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

1.2.1 轉移概率規則

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

(5)

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

1.2.2 局部信息素更新規則

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

(6)

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

1.2.3 全局信息素更新規則

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

(7)

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

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

2.1 改進斥力勢場函數

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

圖3 改進勢場模型原理圖

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

(8)

(9)

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

2.2 構造勢場啟發函數

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

(10)

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

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

(11)

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

3 仿真驗證

3.1 路徑規劃

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

表1 IAPF-ACO算法流程

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

圖4 離散環境避障規劃

圖5 離散環境規劃結果

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

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

圖6 聚集環境避障規劃

圖7 聚集環境規劃結果

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

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

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

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

3.2 機械臂避障仿真

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

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

圖8 關節5平滑結果

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

圖9 機械臂避障仿真

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

4 結論

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