基于勢(shì)場(chǎng)-蟻群算法的平滑機(jī)器人路徑規(guī)劃

彭 湘，向鳳紅，毛劍琳

(昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院，云南，昆明 650500)

1 引言

隨著機(jī)器人智能化的不斷發(fā)展，移動(dòng)機(jī)器人逐漸從最初僅應(yīng)用于工業(yè)產(chǎn)業(yè)到如今廣泛應(yīng)用在教育、娛樂(lè)、醫(yī)療等環(huán)境中[1]。因此人們對(duì)機(jī)器人的性能要求也越來(lái)越高，機(jī)器人在環(huán)境中的行走精確度要求也逐漸提高。移動(dòng)機(jī)器人路徑規(guī)劃是指機(jī)器人在已知或未知環(huán)境中，規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑[2-3]，此路徑可以存在一個(gè)或多個(gè)約束條件，例如時(shí)間最優(yōu)，路徑最優(yōu)或次優(yōu)等。路徑規(guī)劃主要分為全局路徑規(guī)劃和局部路徑規(guī)劃[3]，全局路徑規(guī)劃主要針對(duì)靜態(tài)環(huán)境，環(huán)境中障礙物是靜止的且位置確定；局部路徑規(guī)劃主要靠機(jī)器人攜帶的傳感器獲取周圍環(huán)境信息，主要應(yīng)用于障礙物不斷變化的動(dòng)態(tài)環(huán)境中。

針對(duì)移動(dòng)機(jī)器人的路徑規(guī)劃，國(guó)內(nèi)外學(xué)者提出了一系列方法[5]，常用的包括一些傳統(tǒng)算法，如Dijkstra算法、模擬退火算法，以及近年來(lái)新興的智能仿生算法[6]，如遺傳算法、蜂群算法、魚群算法等。不同的算法均根據(jù)不同的性能指標(biāo)有著不同的優(yōu)缺點(diǎn)。蟻群算法最早是由Marco Dorigo等人于1991年提出[7]，是一種通過(guò)模擬螞蟻的覓食行為，在給定空間進(jìn)行路徑規(guī)劃的智能仿生算法，因其具有良好的魯棒性、正反饋性以及易與其它算法結(jié)合的優(yōu)點(diǎn)，廣泛應(yīng)用于機(jī)器人的路徑規(guī)劃中。文獻(xiàn)[8]針對(duì)復(fù)雜環(huán)境下的路徑規(guī)劃，改進(jìn)了傳統(tǒng)蟻群算法，但規(guī)劃的最優(yōu)路徑存在尖峰，算法的全局能力也有待提高；文獻(xiàn)[9]利用聚類算法對(duì)環(huán)境的識(shí)別能力，提出了一種自適應(yīng)搜索半徑蟻群路徑規(guī)劃算法，解決了蟻群全局收斂速度較慢問(wèn)題，但算法并未考慮蟻群搜索時(shí)陷入局部最優(yōu)的情況；文獻(xiàn)[10]對(duì)柵格環(huán)境下機(jī)器人的規(guī)劃路徑進(jìn)行平滑處理，減少了路徑的轉(zhuǎn)折次數(shù)和轉(zhuǎn)折角度，但文中平滑方法并未達(dá)到完全消除路徑拐角的作用，優(yōu)化后的路徑仍然存在拐角。

鑒于以上特點(diǎn)，本文提出一種勢(shì)場(chǎng)-蟻群優(yōu)化(Improve Ant Colony Optimization with Artificial Potential Field，IACO-APF)平滑機(jī)器人路徑規(guī)劃方法，主要思想是：考慮機(jī)器人所受的勢(shì)場(chǎng)合力和下一節(jié)點(diǎn)到目標(biāo)點(diǎn)的距離，綜合構(gòu)造啟發(fā)信息，不僅提高了算法的全局性和收斂速度，多種因素共同作用還能避免蟻群陷入“死鎖”；將信息素限制在一定范圍內(nèi)，防止機(jī)器人在搜索過(guò)程中因信息素過(guò)高陷入局部極小值；最后引入三次B樣條曲線對(duì)最優(yōu)路徑做平滑處理，消除了路徑拐角，機(jī)器人的行走也更加流暢。

2 建模

2.1 問(wèn)題假設(shè)

為使移動(dòng)機(jī)器人更精準(zhǔn)、有效地規(guī)劃出一條符合實(shí)際工作環(huán)境的路徑，這里對(duì)機(jī)器人的工作環(huán)境作如下假設(shè)：①機(jī)器人僅靜止和勻速運(yùn)動(dòng)兩種狀態(tài)，且可在兩種狀態(tài)間自由切換；②機(jī)器人自身攜帶傳感器以獲取周圍環(huán)境信息，機(jī)器人在工作時(shí)間內(nèi)均能檢測(cè)到附近障礙物位置；③忽略環(huán)境中除障礙物以外的力對(duì)機(jī)器人的影響；④機(jī)器人工作空間為二維有限空間。

2.2 環(huán)境建模

首先建立移動(dòng)機(jī)器人的路徑規(guī)劃模型。假設(shè)機(jī)器人工作空間是一個(gè)二維平面，已知障礙物靜止且位置確定，為了更好的模擬實(shí)際工作環(huán)境，本文采用柵格法[11]來(lái)劃分移動(dòng)機(jī)器人的工作環(huán)境。柵格法是機(jī)器人建模中的常用方法，但柵格法只能近似的模擬機(jī)器人的工作環(huán)境，因此這里采用膨脹法對(duì)障礙物進(jìn)行處理，即對(duì)柵格內(nèi)不足一個(gè)柵格的障礙物按一個(gè)柵格處理。

圖1 膨脹法處理障礙物

其中白色柵格表示機(jī)器人可自由通過(guò)的區(qū)域，黑色柵格表示障礙物，機(jī)器人無(wú)法通過(guò)此區(qū)域。柵格大小為R。以柵格圖左下角為原點(diǎn)建立笛卡爾坐標(biāo)系，從左至右為x軸，從下至上為y軸，對(duì)建立的柵格圖從左至右，從上至下編碼，依次為1、2、3…..。

圖2 柵格圖編碼

3 融合算法

單一算法本身往往存在各種不同的缺陷，而算法之間根據(jù)算法特性互相融合可以彌補(bǔ)算法的缺陷，得到更優(yōu)的效果。蟻群在覓食時(shí)的搜索機(jī)制與移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃的相似性使得蟻群算法在機(jī)器人路徑規(guī)劃中有較好的應(yīng)用，但傳統(tǒng)蟻群算法在前期搜索時(shí)由于信息素濃度較低，啟發(fā)信息僅受下一可能前往的節(jié)點(diǎn)作用等因素，導(dǎo)致搜索存在“盲目性”，收斂速度較慢，并且容易陷入“死鎖”，從而停滯搜索。在規(guī)劃過(guò)程中，隨著迭代次數(shù)增加，信息素濃度逐漸升高，蟻群又極易出現(xiàn)“早熟”現(xiàn)象。因此本文提出勢(shì)場(chǎng)-蟻群算法，利用人工勢(shì)場(chǎng)力的作用，使得蟻群在搜索前期，主要靠加入勢(shì)場(chǎng)合力的啟發(fā)信息作用，能夠加快蟻群前期的搜索速度，并在信息素更新時(shí)限定信息素范圍，避免蟻群因過(guò)快收斂而陷入局部最優(yōu)，提高路徑規(guī)劃的效率。

3.1 人工勢(shì)場(chǎng)法

根據(jù)人工勢(shì)場(chǎng)法的思想，移動(dòng)機(jī)器人在工作環(huán)境中受虛擬勢(shì)場(chǎng)的影響，機(jī)器人受到目標(biāo)點(diǎn)引力和障礙物斥力的作用，從而規(guī)劃出一條無(wú)碰撞的路徑。在任一位置處，機(jī)器人受到的引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)的和為

Usum=Uatt+Urep

(1)

其中，引力勢(shì)場(chǎng)表示為

(2)

其中：ξ是尺度因子，d2(P，G)表示物體當(dāng)前位置Pi到目標(biāo)點(diǎn)G的距離。對(duì)引力勢(shì)場(chǎng)求導(dǎo)即可得到引力：

Fatt=-ΔUatt(P)=ξd(P，G)

(3)

斥力勢(shì)場(chǎng)表示為

(4)

其中：d表示移動(dòng)機(jī)器人到障礙物的距離，d0表示障礙物斥力場(chǎng)的作用范圍。同理，斥力表示為

Frep(P)=-?Urep(P)

(5)

機(jī)器人受到的勢(shì)場(chǎng)合力表示為

Fsum=Fatt+Frep

(6)

圖3 人工勢(shì)場(chǎng)受力圖

3.2 改進(jìn)勢(shì)場(chǎng)-蟻群算法

3.2.1 構(gòu)造啟發(fā)信息函數(shù)

傳統(tǒng)蟻群算法啟發(fā)信息函數(shù)表示為：

(7)

由于傳統(tǒng)蟻群算法在構(gòu)造啟發(fā)信息函數(shù)僅考慮螞蟻當(dāng)前節(jié)點(diǎn)i到下一節(jié)點(diǎn)j的距離dij，導(dǎo)致螞蟻的搜索全局性大大下降，故本文從算法的全局性出發(fā)，在啟發(fā)信息函數(shù)中考慮人工勢(shì)場(chǎng)合力、目標(biāo)點(diǎn)在全局產(chǎn)生的引力勢(shì)場(chǎng)對(duì)機(jī)器人產(chǎn)生的吸引力，以及下一節(jié)點(diǎn)j與目標(biāo)點(diǎn)G間的距離djG，綜合構(gòu)造新的啟發(fā)信息函數(shù)為

(8)

3.2.2 信息素更新

蟻群在完成一次迭代，所有的螞蟻都到達(dá)目標(biāo)點(diǎn)以后，需要對(duì)全局信息素進(jìn)行更新。為了避免螞蟻在搜索過(guò)程中因?yàn)槟陈窂叫畔⑺剡^(guò)高而陷入搜索停滯狀態(tài)，本文受最大最小螞蟻系統(tǒng)(MMAS)[12-13]啟發(fā)，在信息素更新時(shí)，將信息素限定在一定范圍，用公式表示為

(9)

其中：τmin為規(guī)定最小信息素濃度，τmax為規(guī)定最大信息素濃度。當(dāng)信息素小于最小信息時(shí)速濃度或大于最大信息素濃度時(shí)，信息素就會(huì)被按照式(9)重置。

4 平滑路徑規(guī)劃實(shí)現(xiàn)

由于柵格法的局限性，移動(dòng)機(jī)器人只能從一個(gè)柵格的中心點(diǎn)移向另一個(gè)柵格的中心點(diǎn)，因此規(guī)劃出的最優(yōu)路徑往往是一條帶有尖峰的不平滑路徑，若存在路徑拐角過(guò)大或拐角過(guò)多帶來(lái)的外力作用可能會(huì)造成機(jī)器人不必要的能量損失，不符合實(shí)際環(huán)境對(duì)機(jī)器人的性能要求。為了更好的模擬實(shí)際環(huán)境，得到一條平滑且優(yōu)于最優(yōu)路徑的結(jié)果，本文引入三次B樣條曲線對(duì)最優(yōu)路徑拐點(diǎn)尖峰作平滑處理。

4.1 三次B樣條曲線

B樣條曲線具有可微性、凸包含性以及局部可修改性等特點(diǎn)，能夠滿足曲線節(jié)點(diǎn)間的狀態(tài)約束[14]。其中最廣泛使用的是三次B樣條曲線，三次B樣條曲線在節(jié)點(diǎn)具有二階連續(xù)性，滿足機(jī)器人在運(yùn)動(dòng)時(shí)速度與加速度連續(xù)，且曲線落在控制點(diǎn)所形成的三角形內(nèi)[15]，因此可將三次B樣條曲線用于路徑規(guī)劃。三次B樣條曲線特性如圖4所示。

圖4 三次B樣條曲線特性

由n+1個(gè)控制點(diǎn)Pi構(gòu)成的k次B樣條曲線方程的數(shù)學(xué)表達(dá)式為：

(10)

其中：Ni，k(u)表示第i個(gè)k階的B樣條基函數(shù)，表示為

(11)

當(dāng)k為3時(shí)，此時(shí)即為三次B樣條曲線，基函數(shù)表示為

(12)

因此，三次B樣條曲線方程為：

0≤u≤1

(13)

4.2 路徑平滑實(shí)現(xiàn)

三次B樣條曲線平滑路徑流程圖如圖5所示。

圖5 三次B樣條曲線平滑路徑

5 仿真

5.1 算法步驟

Step1：柵格法建模，設(shè)置移動(dòng)機(jī)器人的起始點(diǎn)S、目標(biāo)點(diǎn)G；初始化勢(shì)場(chǎng)-蟻群算法各參數(shù)；設(shè)置三次B樣條曲線參數(shù)；

Step3：判斷螞蟻是否到達(dá)目標(biāo)點(diǎn)G：若是，則一次迭代結(jié)束，否則，繼續(xù)搜索；

Step4：完成一次迭代后，更新全局信息素；

Step5：判斷當(dāng)前迭代次數(shù)Nc是否達(dá)到最大迭代次數(shù)Ncmax，若是，則輸出最優(yōu)路徑，否則，轉(zhuǎn)到Step2，直到Nc=Ncmax；

Step6：對(duì)輸出的最優(yōu)路徑作平滑處理，輸出最終優(yōu)化結(jié)果。

5.2 仿真及分析

本文采用MATLAB 2014a為仿真工具，移動(dòng)機(jī)器人工作環(huán)境設(shè)置為mxn的柵格地圖，起始點(diǎn)柵格序號(hào)為1，終止點(diǎn)柵格序號(hào)為mxn。設(shè)定仿真參數(shù)為：Ncmax=200次，螞蟻個(gè)數(shù)m=50只，信息素因子α=1，啟發(fā)因子β=7，信息素?fù)]發(fā)系數(shù)ρ=0.3，信息素增強(qiáng)系數(shù)Q=1。為了驗(yàn)證算法的可行性，首先在10x10的柵格地圖環(huán)境下進(jìn)行仿真，仿真結(jié)果如圖6所示。

圖6 勢(shì)場(chǎng)-蟻群算法(10x10)

可以看到，在仿真結(jié)果圖6中，算法在10x10柵格環(huán)境下運(yùn)行成功且找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的有效路徑，路徑長(zhǎng)度為14.7279，驗(yàn)證了本文提出的算法是有效可行的。

為了進(jìn)一步驗(yàn)證算法的性能，實(shí)驗(yàn)繼續(xù)在20x20的柵格環(huán)境下對(duì)比傳統(tǒng)蟻群算法和本文提出的勢(shì)場(chǎng)-蟻群算法。仿真結(jié)果如圖7、圖8所示。

圖7 傳統(tǒng)蟻群算法(20x20)

圖8 勢(shì)場(chǎng)-蟻群算法(20x20)

對(duì)比圖7、8仿真結(jié)果，傳統(tǒng)蟻群算法收斂圖曲線在找到最優(yōu)路徑后仍舊出現(xiàn)了幾次波動(dòng)，此后才趨于平穩(wěn)，最終迭代28次后進(jìn)入收斂狀態(tài)，而改進(jìn)勢(shì)場(chǎng)-蟻群算法僅用6次迭代就已收斂，迭代次數(shù)上減少了78.6%。在路徑長(zhǎng)度方面，傳統(tǒng)蟻群算法路徑較改進(jìn)勢(shì)場(chǎng)-蟻群算法長(zhǎng)1.1715，并且傳統(tǒng)蟻群算法規(guī)劃的路徑拐點(diǎn)較多，假設(shè)將此應(yīng)用在實(shí)際環(huán)境中，拐點(diǎn)過(guò)多會(huì)導(dǎo)致機(jī)器人在行走過(guò)程中耗能較多，而本文算法規(guī)劃的路徑拐點(diǎn)數(shù)僅為4，較傳統(tǒng)蟻群減少了69.2%。仿真結(jié)果表明，改進(jìn)勢(shì)場(chǎng)-蟻群算法在搜索效率和收斂速度上較傳統(tǒng)蟻群算法都有較大改進(jìn)，同時(shí)改進(jìn)勢(shì)場(chǎng)-蟻群算法規(guī)劃出的路徑更短，路徑拐點(diǎn)也大大減少。實(shí)驗(yàn)在20x20柵格環(huán)境下的運(yùn)行情況對(duì)比見(jiàn)表1。

表1 算法結(jié)果對(duì)比

針對(duì)算法規(guī)劃的最優(yōu)路徑，采用三次B樣條曲線優(yōu)化的最終結(jié)果如圖8所示，可以看出，針對(duì)拐點(diǎn)處的尖峰路徑，通過(guò)設(shè)置三次B樣條曲線的控制點(diǎn)，可以使平滑后的路徑成功落在控制點(diǎn)與拐點(diǎn)包圍的三角形內(nèi)，并且路徑長(zhǎng)度比原最優(yōu)路徑減少了9.0603，對(duì)比文獻(xiàn)[16]使用的平滑方法，采用三次B樣條曲線處理路徑能夠完全消除路徑上任意拐點(diǎn)，達(dá)到100%平滑路徑的目的。

圖9 平滑優(yōu)化結(jié)果

6 結(jié)束語(yǔ)

針對(duì)機(jī)器人在靜態(tài)環(huán)境下的全局路徑規(guī)劃，本文在傳統(tǒng)蟻群算法的基礎(chǔ)上提出勢(shì)場(chǎng)-蟻群融合算法。實(shí)驗(yàn)結(jié)果表明，相較于傳統(tǒng)蟻群算法而言，勢(shì)場(chǎng)-蟻群算法在收斂速度、全局性等問(wèn)題上都有較大的改進(jìn)，同時(shí)算法還解決了傳統(tǒng)蟻群算法存在的易陷入“死鎖”和局部最優(yōu)解的問(wèn)題。在規(guī)劃出最優(yōu)路徑后，采取三次B樣條曲線對(duì)路徑進(jìn)行優(yōu)化處理，完全平滑了路徑尖峰，還縮短了路徑長(zhǎng)度。整個(gè)算法在優(yōu)化后運(yùn)行效率和運(yùn)算結(jié)果都得到了大大的提升。在今后的后續(xù)工作中，將針對(duì)參數(shù)變化和動(dòng)態(tài)環(huán)境對(duì)規(guī)劃過(guò)程帶來(lái)的影響等問(wèn)題做進(jìn)一步研究。