靜態(tài)障礙物下的遍歷多任務(wù)目標(biāo)機(jī)器人路徑規(guī)劃

楊 帆 ，薛亞沖 ，李 靖

（1.河北工業(yè)大學(xué) 電子信息工程學(xué)院，天津 300400；2.河北工業(yè)大學(xué) 天津市電子材料與器件重點(diǎn)實(shí)驗(yàn)室，天津 300400）

針對(duì)移動(dòng)機(jī)器人導(dǎo)航技術(shù)的研究是移動(dòng)機(jī)器人研究領(lǐng)域的核心，而路徑規(guī)劃是機(jī)器人導(dǎo)航技術(shù)中的研究熱點(diǎn)[1].機(jī)器人路徑規(guī)劃的目的是為機(jī)器人規(guī)劃出一條從起始點(diǎn)至目標(biāo)點(diǎn)的最優(yōu)或近似最優(yōu)的行走路徑[2].目前，機(jī)器人路徑規(guī)劃的方法有多種，主要可分為全局路徑規(guī)劃和局部路徑規(guī)劃[3]，全局路徑規(guī)劃主要包括：柵格法、可視圖法和一些神經(jīng)網(wǎng)絡(luò)算法[4]等，局部路徑規(guī)劃主要包括：人工勢(shì)場(chǎng)法、蟻群算法、粒子群算法、遺傳算法和A*尋路算法等.而多任務(wù)目標(biāo)點(diǎn)的機(jī)器人路徑規(guī)劃相較于單點(diǎn)目標(biāo)的路徑規(guī)劃要更為復(fù)雜.

目前，在機(jī)器人路徑規(guī)劃的研究中，大多數(shù)是針對(duì)單任務(wù)目標(biāo)這一情況，對(duì)于多任務(wù)目標(biāo)的研究較少[5].蒲興成等[2]將反向?qū)W習(xí)策略思想應(yīng)用到粒子群算法中，這種改進(jìn)雖然一定程度上抑制了單個(gè)粒子的早熟現(xiàn)象，并且提高了算法收斂速度，但并沒有有效解決粒子群算法容易陷入局部最優(yōu)的弊端，而且算法仿真中沒有考慮目標(biāo)點(diǎn)較多的情況.曹潔等[4]提出了一種改進(jìn)蟻群算法對(duì)撿球機(jī)器人進(jìn)行多目標(biāo)路徑規(guī)劃，將蟻群算法中信息素強(qiáng)度Q設(shè)為動(dòng)態(tài)，這種改進(jìn)雖然解決了蟻群算法可能出現(xiàn)的停滯現(xiàn)象，且提高了一定的求解精度，但改進(jìn)后的算法還是容易陷入局部最優(yōu)，且算法在進(jìn)行路徑規(guī)劃時(shí)沒有考慮到障礙物的存在，算法應(yīng)用范圍受到極大限制.針對(duì)這些問題，本文提出了一種基于粒子群-遺傳-A*算法的靜態(tài)障礙物下的遍歷多任務(wù)目標(biāo)機(jī)器人路徑規(guī)劃.在進(jìn)行路徑規(guī)劃時(shí)，首先使用粒子群-遺傳算法規(guī)劃出最優(yōu)目標(biāo)點(diǎn)行走順序，再使用A*算法進(jìn)行避障行走.該算法將遺傳算法中的交叉和變異操作應(yīng)用到粒子群算法中，顯著提高了粒子群算法的穩(wěn)定性和尋優(yōu)能力.

1 算法理論基礎(chǔ)

1.1 柵格法

采用柵格法構(gòu)建地圖模型，柵格法在構(gòu)建地圖模型時(shí)，將障礙物區(qū)域即不可行走區(qū)域標(biāo)為1，將自由區(qū)域即可行走區(qū)域標(biāo)為0.所構(gòu)建的地圖模型詳盡程度取決于劃分的柵格大小[5].柵格越小，構(gòu)建的地圖模型越詳細(xì)，但會(huì)占用較大的存儲(chǔ)空間，并且會(huì)使得路徑規(guī)劃計(jì)算量成指數(shù)增加，規(guī)劃速度也會(huì)降低[6].柵格過大，會(huì)使得地圖模型不夠詳盡，不能準(zhǔn)確的描述出障礙區(qū)域和自由區(qū)域的信息，不利于有效路徑的規(guī)劃.

1.2 A*算法

A*算法是一種啟發(fā)式搜索算法，在進(jìn)行路徑規(guī)劃時(shí)，A*算法會(huì)對(duì)將要進(jìn)行搜索的點(diǎn)預(yù)估代價(jià)值，代價(jià)值預(yù)估函數(shù)為：

式中：G（n）表示從其實(shí)節(jié)點(diǎn)到任意節(jié)點(diǎn)n的代價(jià)；H（n）表示從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式評(píng)估代價(jià).

改變G（n）和H（n）的代價(jià)值能夠調(diào)節(jié)A*算法的搜索行為，當(dāng) G（n）過大，H（n）過小時(shí)，A* 算法擴(kuò)展節(jié)點(diǎn)會(huì)增多，搜索速度會(huì)變慢，但能夠規(guī)劃出一條最優(yōu)的行走路徑；當(dāng) G（n）過小，H（n）過大時(shí)，A* 算法搜索速度很快，但不能保證規(guī)劃出的路徑為最優(yōu).A*算法有2個(gè)存儲(chǔ)列表：OPEN（開啟列表）和CLOSE（關(guān)閉列表），OPEN列表中存放等待檢查方格，CLOSE列表中存放不需要檢查的方格.

A*算法在柵格法構(gòu)建的地圖模型中進(jìn)行路徑規(guī)劃時(shí)，機(jī)器人有8個(gè)可運(yùn)動(dòng)的方向[8]，分別為：前方、左方、右方、后方、左前、右前、左后、右后，如圖1所示.圖1中：A為起始方格；T為目標(biāo)節(jié)點(diǎn)；灰色區(qū)域?yàn)檎系K物區(qū)域（在后文的仿真試驗(yàn)中設(shè)為藍(lán)色）；實(shí)線箭頭為機(jī)器人最佳前進(jìn)方向.

圖1 A*算法規(guī)劃示意圖Fig.1 A*algorithm planning diagram

1.3 粒子群算法

粒子群算法（particle swarm optimization，PSO）[7]最早是由Eberhart和Kennedy于1995年提出，它的基本概念源于對(duì)鳥群覓食行為的研究.在PSO中，每個(gè)優(yōu)化問題的潛在解都可以想象成N維搜索空間上的一個(gè)點(diǎn)，稱之為“粒子”（particle）[8]，所有的粒子都有一個(gè)被目標(biāo)函數(shù)決定的適應(yīng)值（fitness value），每個(gè)粒子還有一個(gè)速度決定它們飛翔的方向和距離，然后粒子們就追隨當(dāng)前的最優(yōu)粒子在解空間中搜索[8].

粒子群算法的速度和位置更新公式為：

式中：w為保持原來速度的系數(shù)，又稱為慣性權(quán)重；n為迭代次數(shù)；D為粒子維度；C1是單個(gè)粒子歷史最優(yōu)值的權(quán)重系數(shù)；C2是所有粒子群體最優(yōu)值的權(quán)重系數(shù)；α、β為區(qū)間[0，1]之間的隨機(jī)數(shù)；γ為速度系數(shù)，設(shè)為1.

1.4 分級(jí)粒子群-遺傳算法

本文對(duì)普通粒子群-遺傳算法進(jìn)行了改進(jìn)，在其基礎(chǔ)上對(duì)粒子群進(jìn)行了等級(jí)劃分，分為：精英粒子、優(yōu)等粒子群、中等粒子群和劣等粒子群4個(gè)類別.其中精英粒子為單個(gè)粒子，是適應(yīng)度值最優(yōu)的粒子，本文中適應(yīng)度值為距離總和，因此將適應(yīng)度值最小的粒子設(shè)定為精英粒子；優(yōu)等粒子群由多個(gè)粒子組成，是除精英粒子外，適應(yīng)度值較小的前幾個(gè)粒子；劣等粒子群由多個(gè)粒子構(gòu)成，是適應(yīng)度值較大的最后幾個(gè)粒子；其余適應(yīng)度值處于中間位置的粒子全部設(shè)為中等粒子.精英粒子作為下次迭代中的父代粒子，不參與交叉和變異；優(yōu)等粒子群在下次迭代中不參與交叉操作，僅參與變異操作；中等粒子群在下次迭代中既參與交叉操作，又參與變異操作；劣等粒子群采取淘汰操作，即舍棄當(dāng)前的劣等粒子群，并重新初始化一批粒子加入到總粒子群中，新初始化加入的粒子數(shù)目與被淘汰的粒子數(shù)目相同.

本文實(shí)驗(yàn)中，粒子總數(shù)目為50，在對(duì)粒子群分級(jí)時(shí)，精英粒子個(gè)數(shù)設(shè)為1，優(yōu)等粒子群中粒子個(gè)數(shù)設(shè)為5，劣等粒子群中粒子數(shù)設(shè)為10，中等粒子群中粒子數(shù)設(shè)為34.

2 分級(jí)粒子群-遺傳算法結(jié)合A*算法進(jìn)行路徑規(guī)劃

在大多數(shù)情況下粒子群算法相比遺傳算法擁有更快的規(guī)劃速度，但由于粒子的速度和位置更新信息依賴于當(dāng)前搜索到的最優(yōu)解，這就導(dǎo)致粒子群算法在規(guī)劃時(shí)容易陷入局部最優(yōu)[10].而遺傳算法具有交叉算子和變異算子[11]，能夠產(chǎn)生具有新的解集的種群[12]，因而在搜索最優(yōu)解過程中不易陷入局部最優(yōu)，使得算法能夠更好的收斂于最優(yōu)解.因此，結(jié)合兩種算法的優(yōu)點(diǎn)，在粒子群算法中引入遺傳算法的交叉和變異操作，以此來擴(kuò)大粒子的多樣性，增強(qiáng)粒子的全局隨機(jī)搜索能力.

目前，遺傳算法中交叉方法有：?jiǎn)吸c(diǎn)交叉、局部鄰域交叉、多點(diǎn)交叉、均勻交叉等[13]，本文中采用隨機(jī)多點(diǎn)交叉的方法，即交叉點(diǎn)的位置隨機(jī)、數(shù)量隨機(jī)，在對(duì)粒子進(jìn)行變異操作時(shí)也采用隨機(jī)多點(diǎn)變異的方法.這種交叉和變異操作增加了粒子的多樣性，實(shí)驗(yàn)仿真證明，在處理復(fù)雜情況下的問題時(shí)，算法在全局搜索最優(yōu)解能力和穩(wěn)定性方面有顯著提升.

分級(jí)粒子群-遺傳-A*算法進(jìn)行機(jī)器人多任務(wù)目標(biāo)路徑規(guī)劃主要包括3部分：①構(gòu)建地圖模型；②尋找最優(yōu)行走路線；③進(jìn)行避障行走.其具體執(zhí)行步驟如下：

（1）使用柵格法構(gòu)建地圖模型，如圖2所示.柵格標(biāo)號(hào)方式如圖2（a）所示，構(gòu)建障礙物地圖時(shí)，將障礙物所對(duì)應(yīng)的標(biāo)號(hào)保存在障礙物信息函數(shù)map_obstacle中.考慮到機(jī)器人的安全和方便路徑規(guī)劃，對(duì)障礙物進(jìn)行擴(kuò)大處理，如圖2（b）所示，其中紅色區(qū)域?yàn)閷?shí)際障礙物大小，藍(lán)色區(qū)域?yàn)閿U(kuò)大后的障礙物區(qū)域，障礙物擴(kuò)大值為機(jī)器人的半徑R.

圖2 柵格法構(gòu)建地圖Fig.2 Build the map using grid method

（2）將目標(biāo)點(diǎn)信息導(dǎo)入地圖中（起始點(diǎn)也作為目標(biāo)點(diǎn)），計(jì)算任意2個(gè)目標(biāo)點(diǎn)之間的曼哈頓距離，并將所得距離保存在矩陣target_d中，target_d為n行n列方陣，n為目標(biāo)數(shù).

式中：X_targeti為目標(biāo)點(diǎn)i的橫坐標(biāo)；Y_targeti為目標(biāo)點(diǎn)i的縱坐標(biāo)；X_targetj為目標(biāo)點(diǎn)j的橫坐標(biāo)；Y_targetj為目標(biāo)點(diǎn)j的縱坐標(biāo).

（3）計(jì)算種群適應(yīng)度值并記錄當(dāng)前全體最優(yōu)和個(gè)體最優(yōu)，適應(yīng)度值函數(shù)fitness_d表示在不存在障礙物情況下機(jī)器人遍歷所有目標(biāo)點(diǎn)后的經(jīng)過的曼哈頓距離總和，單個(gè)粒子的適應(yīng)度值fitness_dm計(jì)算如公式（5）:

式中：m1，m2，…，mn為 1 ～ n 之間的正整數(shù)，且 m1≠m2≠…≠mn.

（4）對(duì)個(gè)體最優(yōu)進(jìn)行交叉和變異操作，記錄求得的新適應(yīng)度值，并將求得的值與歷史最優(yōu)值進(jìn)行比較，若當(dāng)前最優(yōu)小于歷史最優(yōu)，則替換歷史最優(yōu)，并刪除交叉和變異操作中產(chǎn)生的相同元素；若當(dāng)前最優(yōu)大于歷史最優(yōu)，則保持歷史最優(yōu)不變.

（5）返回步驟（3）重新執(zhí)行，若求出最優(yōu)解或者迭代次數(shù)達(dá)到最大值，終止求解最優(yōu)行走路線程序，并保存最優(yōu)行走順序.

（6）由于步驟（5）中求得的最優(yōu)行走順序是將起始點(diǎn)與目標(biāo)點(diǎn)混合排序的，因此需要將起始點(diǎn)提取到行走路線第一位，且保持行走順序不變.

（7）調(diào)用A*算法，并將map_obstacle中的障礙物信息導(dǎo)入到A*算法的CLOSE列表中，然后以Start點(diǎn)為機(jī)器人起始點(diǎn)，Target為各個(gè)目標(biāo)點(diǎn)，按照步驟（6）中行走順序進(jìn)行避障行走.進(jìn)行避障行走時(shí)，H（n）的值設(shè)為起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的距離，G（n）的值設(shè)為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離，即：

式中：Xstart表示起始節(jié)點(diǎn)橫坐標(biāo)；Ystart表示起始節(jié)點(diǎn)縱坐標(biāo)；Xnode表示當(dāng)前節(jié)點(diǎn)橫坐標(biāo)；Ynode表示當(dāng)前坐標(biāo)縱坐標(biāo)；Xtarget表示目標(biāo)節(jié)點(diǎn)橫坐標(biāo)；Ytarget表示目標(biāo)節(jié)點(diǎn)縱坐標(biāo).

（8）為機(jī)器人規(guī)劃出行走路程最短、安全無碰的行走路徑，程序終止運(yùn)行.

分級(jí)粒子群-遺傳-A*算法的路徑規(guī)劃流程圖如圖3所示.

圖3 分級(jí)粒子群-遺傳-A*算法路徑規(guī)劃流程Fig.3 Classification of PSO-GA-A*-algorithm path planning procedure

表1所示為粒子個(gè)數(shù)都為50，C1=C2=1時(shí)，各算法在不同目標(biāo)數(shù)下的求解情況.任務(wù)目標(biāo)點(diǎn)數(shù)不同時(shí)算法規(guī)劃對(duì)比如圖 4 所示.圖 4（a）和（b）、（c）和（d）、（e）和（f）分別為任務(wù)目標(biāo)數(shù)為5、15和30時(shí)的目標(biāo)點(diǎn)位置分布及規(guī)劃效果，圖 4 中（a）、（c）、（e）為粒子群-遺傳算法規(guī)劃，圖 4 中（b）、（d）、（f）為粒子群算法規(guī)劃.

由圖4和表1可以看出，當(dāng)目標(biāo)數(shù)量較少時(shí)，標(biāo)準(zhǔn)粒子群算法和粒子群-遺傳算法得出相同的最優(yōu)解，且標(biāo)準(zhǔn)粒子群算法在規(guī)劃時(shí)間上要優(yōu)于粒子群-遺傳算法；當(dāng)目標(biāo)數(shù)量較多，目標(biāo)點(diǎn)位置情況較復(fù)雜時(shí)，標(biāo)準(zhǔn)粒子群算法進(jìn)行規(guī)劃時(shí)，在迭代次數(shù)和時(shí)間上都遠(yuǎn)遠(yuǎn)高于粒子群—遺傳算法，且規(guī)劃出的行走順序不能保證為最優(yōu).目標(biāo)數(shù)較少時(shí)，粒子群-遺傳算法與分級(jí)粒子群-遺傳算法規(guī)劃效果差距不大，當(dāng)目標(biāo)數(shù)量為15時(shí)，在迭代次數(shù)上，分級(jí)粒子群-遺傳算法比粒子群-遺傳算法降低了25%，在規(guī)劃時(shí)間上，降低了7.5%；當(dāng)目標(biāo)數(shù)量為30時(shí)，在迭代次數(shù)上，分級(jí)粒子群-遺傳算法比粒子群-遺傳算法降低了約26.2%，在規(guī)劃時(shí)間上，降低了約11.1%.并且分級(jí)粒子群-遺傳算法得出的最終結(jié)果要優(yōu)于粒子群-遺傳算法的結(jié)果.

表1 3種算法規(guī)劃對(duì)比Tab.1 Comparison of three algorithms

分級(jí)粒子群-遺傳算法和粒子群-遺傳算法規(guī)劃時(shí)最小適應(yīng)度值變化如圖5所示.

圖5（a）和（b）分別為分級(jí)粒子群-遺傳算法和粒子群-遺傳算法在圖4（d）和（f）中目標(biāo)分布情況下規(guī)劃時(shí)的最小適應(yīng)度值變化曲線.紅色曲線為分級(jí)粒子群-遺傳算法規(guī)劃所得，藍(lán)色曲線為普通粒子群-遺傳算法規(guī)劃所得.可以看出，分級(jí)粒子群-遺傳算法不論是在迭代次數(shù)還是規(guī)劃結(jié)果上都優(yōu)于普通粒子群-遺傳算法.

簡(jiǎn)單和復(fù)雜靜態(tài)障礙物下的路徑規(guī)劃如圖6所示.

圖6（a）為簡(jiǎn)單靜態(tài)障礙物下的遍歷多目標(biāo)機(jī)器人路徑規(guī)劃圖，圖6（b）為復(fù)雜靜態(tài)障礙物下的遍歷多目標(biāo)機(jī)器人路徑規(guī)劃圖，圖中障礙物以藍(lán)色實(shí)心柵格表示，各目標(biāo)點(diǎn)與起始點(diǎn)之間的紅色連線為任務(wù)目標(biāo)點(diǎn)的執(zhí)行順序，藍(lán)色連線為規(guī)劃出的機(jī)器人行走路徑.任務(wù)目標(biāo)點(diǎn)個(gè)數(shù)為11，以標(biāo)有Target的綠色方塊表示；左下角標(biāo)有Start的綠色方為起始點(diǎn)，坐標(biāo)為（3.5，1.5）.圖 6（c）為在復(fù)雜靜態(tài)障礙物下規(guī)劃時(shí)適應(yīng)度值隨迭代次數(shù)的變化曲線，可以看出，當(dāng)?shù)螖?shù)約為27時(shí)，適應(yīng)度值達(dá)到最小，為69.45，且當(dāng)?shù)螖?shù)增加時(shí)，適應(yīng)度值保持不變，表明算法尋找到了最優(yōu)解.

圖4 任務(wù)目標(biāo)點(diǎn)數(shù)不同時(shí)算法規(guī)劃對(duì)比Fig.4 Comparison of algorithm planning in different task target points

圖5 分級(jí)粒子群-遺傳算法和粒子群-遺傳算法規(guī)劃時(shí)最小適應(yīng)度值變化Fig.5 Minimum fitness value that calculated by hierarchical particle swarm genetic algorithm and particle swarm genetic algorithm

圖6（b）起始點(diǎn)與目標(biāo)坐標(biāo)位置矩陣為ST，矩陣第一行為各目標(biāo)點(diǎn)的序號(hào)，第二行為目標(biāo)點(diǎn)橫坐標(biāo)，第三行為目標(biāo)點(diǎn)縱坐標(biāo).

圖6 簡(jiǎn)單和復(fù)雜靜態(tài)障礙物下的路徑規(guī)劃Fig.6 Path planning under simple and complicated static obstacles

經(jīng)過算法計(jì)算后的最佳行走順序?yàn)椋?—9—6—5—8—12—11—10—4—3—7—2—1.

由圖 6（a）和圖 6（b）可知，分級(jí)粒子群-遺傳-A*算法在障礙物為靜態(tài)的情況下進(jìn)行遍歷多目標(biāo)點(diǎn)的機(jī)器人路徑規(guī)劃是可行的，并且不論地圖障礙物簡(jiǎn)單或者復(fù)雜，任務(wù)目標(biāo)點(diǎn)數(shù)量少或者多，算法都能夠?yàn)闄C(jī)器人規(guī)劃出一條安全無碰、行走路程短的最優(yōu)路徑.

使用分級(jí)粒子群-遺傳算法尋找出最優(yōu)的任務(wù)目標(biāo)點(diǎn)行走順序，再使用A*算法為機(jī)器人規(guī)劃出一條安全無碰的避障行走路徑，最優(yōu)的目標(biāo)點(diǎn)行走順序使得機(jī)器人在行走完所有任務(wù)目標(biāo)點(diǎn)后產(chǎn)生最小的移動(dòng)耗費(fèi)，而A*尋路算法又為機(jī)器人規(guī)劃出目標(biāo)點(diǎn)間的最短、無碰行走路徑，將這2個(gè)最優(yōu)解結(jié)合起來，便組成了機(jī)器人多任務(wù)目標(biāo)點(diǎn)的行走最優(yōu)解，即規(guī)劃出最優(yōu)的行走路徑.

3 結(jié)論

本文提出了一種基于分級(jí)粒子群-遺傳-A*算法的機(jī)器人路徑規(guī)劃新方法，該方法將遺傳算法中的交叉和變異操作應(yīng)用于粒子群算法中，以此來擴(kuò)大粒子群的多樣性，該算法雖然在運(yùn)行時(shí)間上長(zhǎng)于粒子群算法，但在處理復(fù)雜問題時(shí)不易陷入局部最優(yōu)，且在穩(wěn)定性方面有了很大提升.仿真實(shí)驗(yàn)證明，該算法不論在障礙物簡(jiǎn)單或者復(fù)雜、目標(biāo)點(diǎn)數(shù)量少或者數(shù)量多的情況下都能夠?yàn)闄C(jī)器人規(guī)劃出最優(yōu)的行走路徑.并且在同等目標(biāo)點(diǎn)情況下，分級(jí)粒子群-遺傳算法相比于粒子群-遺傳算法，在迭代次數(shù)上降低約25%，規(guī)劃時(shí)間上降低約10%.因此，分級(jí)粒子群-遺傳-A*算法能夠成功應(yīng)用于移動(dòng)機(jī)器人在靜態(tài)障礙物存在的地圖上遍歷多個(gè)任務(wù)目標(biāo)點(diǎn)的路徑規(guī)劃，并且算法大幅度降低了計(jì)算成本，在多任務(wù)目標(biāo)點(diǎn)的路徑規(guī)劃上有著很好的應(yīng)用前景.