楊 帆 ，薛亞沖 ，李 靖

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

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

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

1 算法理論基礎

1.1 柵格法

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

1.2 A*算法

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

式中：G（n）表示從其實節(jié)點到任意節(jié)點n的代價；H（n）表示從節(jié)點n到目標點的啟發(fā)式評估代價.

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

A*算法在柵格法構建的地圖模型中進行路徑規(guī)劃時，機器人有8個可運動的方向[8]，分別為：前方、左方、右方、后方、左前、右前、左后、右后，如圖1所示.圖1中：A為起始方格；T為目標節(jié)點；灰色區(qū)域為障礙物區(qū)域（在后文的仿真試驗中設為藍色）；實線箭頭為機器人最佳前進方向.

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

1.3 粒子群算法

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

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

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

1.4 分級粒子群-遺傳算法

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

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

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

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

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

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

（1）使用柵格法構建地圖模型，如圖2所示.柵格標號方式如圖2（a）所示，構建障礙物地圖時，將障礙物所對應的標號保存在障礙物信息函數(shù)map_obstacle中.考慮到機器人的安全和方便路徑規(guī)劃，對障礙物進行擴大處理，如圖2（b）所示，其中紅色區(qū)域為實際障礙物大小，藍色區(qū)域為擴大后的障礙物區(qū)域，障礙物擴大值為機器人的半徑R.

圖2 柵格法構建地圖Fig.2 Build the map using grid method

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

式中：X_targeti為目標點i的橫坐標；Y_targeti為目標點i的縱坐標；X_targetj為目標點j的橫坐標；Y_targetj為目標點j的縱坐標.

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

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

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

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

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

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

式中：Xstart表示起始節(jié)點橫坐標；Ystart表示起始節(jié)點縱坐標；Xnode表示當前節(jié)點橫坐標；Ynode表示當前坐標縱坐標；Xtarget表示目標節(jié)點橫坐標；Ytarget表示目標節(jié)點縱坐標.

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

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

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

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

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

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

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

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

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

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

圖4 任務目標點數(shù)不同時算法規(guī)劃對比Fig.4 Comparison of algorithm planning in different task target points

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

圖6（b）起始點與目標坐標位置矩陣為ST，矩陣第一行為各目標點的序號，第二行為目標點橫坐標，第三行為目標點縱坐標.

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

經過算法計算后的最佳行走順序為：1—9—6—5—8—12—11—10—4—3—7—2—1.

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

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

3 結論

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