一種防震蕩的機器人路徑規劃算法

伏明蘭，萬宇杰

(淮北師范大學計算機科學與技術學院，安徽 淮北 235000)

0 引 言

移動機器人的路徑規劃問題是指如何在有障礙物的工作環境中，找到一條從給定起點到終點的最優路徑，使機器人能夠安全繞過所有障礙物而不發生碰撞，且路徑長度最短。路徑規劃算法一般分為全局規劃和局部規劃[1]。在全局規劃中，機器人根據全局環境信息一次性規劃出從起點到終點的安全路徑。當環境中只有靜態障礙時，該問題又稱為基于靜態信息的全局路徑規劃問題。針對該類問題，有很多解決方法，而由于該問題屬于NP 難題，所以基于啟發式策略的路徑規劃算法得到了廣泛研究，包括遺傳算法[2]、粒子群算法[3，4]、蟻群優化算法[5，6]，以及上述各種算法的融合[7，8]。然而這些算法用在機器人路徑規劃中容易出現震蕩問題[9]，為了克服該問題，在“基于靜態信息的全局規劃算法”的基礎上，改進螞蟻選擇下一步柵格的決策策略，使之能盡快繞開障礙物，朝著目的地前進。

1 問題描述

M.B.Metea 首次提出柵格法并用于求解分級地形的自動化路徑規劃問題[10]。需要解決的問題描述如下[5]：

對于給定的基于柵格模型和靜態全局信息的二維環境，機器人R的起點為gbegin，目的地為gend。機器人R在二維環境中的運動區域為有限的凸多邊形，記為AS，其邊界默認為障礙物，內部分布著的靜態障礙物記為Sb1,Sb2,…,Sbn。以AS左上角為原點建立直角坐標系Σ0。并以Ra為步長將X，Y分別進行劃分，由此形成一個個柵格，如圖1所示[5]。

圖1 柵格坐標模型

AS區域內靜態障礙物所占柵格的集合記為OS={o1,o2,…,om}∈A(m≥n)。AS內剩余柵格所組成的集合稱為可移動區域，記為A。g∈AS的坐標記為g(x,y)。柵格序號集記為C={}1,2,…,M，序號與柵格坐標的對應關系如圖1 所示。機器人R在t時刻的位置記為PR(t)=(xR(t),yR(t))。為了描述問題方便，本文對其他一些符號做了約定。

定義1[5]：anti={1,2,…,k,…,m}表示第i 個螞蟻家族(i= 1,2)，eij表示gi和gj之間的連線。τij(t)表示在t時刻邊eij上的信息素。螞蟻k在ti時刻的位置記為P(xi(ti),yi(ti))，簡記為Pi或P(ti)。相鄰柵格gi和gh之間的距離為1或，因此定義螞蟻在gi處的視野域為：

定義2[5]：在時刻ti，螞蟻k在gi處的可行域為：Wki(gi(xi,yj))={g|g∈BRi(gi(xi,yi))，g?OS,i∈C}；禁忌表包含所有已走過的柵格：tabuk={P(t0),P(t1),…,P(ti)}。

定義3[5]：螞蟻選擇gi的啟發函數如式（1）所示（其中D為常數）：

其中，K∈{1,2}表示螞蟻家族的編號。

定義4：在ti時刻，螞蟻k到目標柵格gend(xend,yend)的向量v可用式（2）表示：

其中(xk,yk)代表螞蟻k當前所在柵格的位置坐標。v和向量（1,0）的夾角θ的計算如式（3）所示：

2 改進的基于靜態信息的全局規劃算法

2.1 基于蟻群算法的全局路徑規劃算法描述

在此首先給出經典的“基于靜態信息的全局規劃算法”，稱為“算法1”的描述[5]。因為螞蟻家族ant1和ant2的區別只在于出發點和目標點不一樣，其他基本思想都一樣，所以算法描述過程只考慮ant1中的螞蟻。算法的步驟描述如下：

步驟1：初始化禁忌表tabuk(k= 1,2,…,m) 為gbegin。τij(0) ←τ0（τ0為常數）。初始化迭代器nc、m和最大代次數MaxNc的值，其中m≤4。

步驟2：?k∈ant1以當前柵格gi為中心，i∈C，按下述步驟選擇下一步要走的柵格gj，gj?tabuk：

(1)若gj∈BRbegin(gbegin)，對于i,j= 1,2,…,m，則，且。其中上標ki和kj表示不同螞蟻選擇不同的柵格。

(2)若gi?BRbegin(gbegin)，?k根據式（4）和（5）選擇gj∈Wki(gi)，gj?tabuk。

其中S為按照概率隨機選擇的柵格的編號；q為隨機數，0 ＜q≤1；q0和β為常數。

步驟3：按式（6）更新邊eij的信息素。

當τij(n+ 1) ＜τmin時，令τij(n+ 1) =τmin。

步驟4：如果有螞蟻相遇，則執行步驟5，否則令i=j，并返回步驟2，直到有螞蟻相遇或所有柵格都選擇完畢或有螞蟻直接到達了目標柵格。

步驟5：連接相遇螞蟻所經過的路徑，計算路徑長度，并記錄最優路徑。

步驟6：按式（7）對全局信息素進行更新：

步驟7：更新nc的值，若nc＜MaxNc，則重新初始化禁忌表，重復步驟2至步驟7，直到nc=MaxNc。

2.2 算法1中的震蕩現象

當障礙物比較大，而目標柵格在障礙物的后面，此時依照算法1進行路徑規劃，就容易出現震蕩現象，比如圖2所示情形。

圖2 靜態障礙柵格設計

圖2 所示是一個20×20 的有限運動區域AS，黑色柵格為靜態障礙柵格。gbegin=g25為出發柵格，gend=g211為目標柵格。在此障礙柵格的設置情況下，利用算法1求從gbegin到gend的最優路徑。

仿真環境為PC Intel 2.4GHz，1G。算法1各參數設置如下：m= 3，τ0= 2.0，D= 20，q0= 0.8，β=2.0，ρ= 0.05，Q1= 20，α= 0.05，Q2= 20，尋食最大代數為100，仿真結果如圖3所示。

圖3 算法1仿真結果

由仿真結果可以看出，當螞蟻繞過較大障礙物時發生了震蕩。其原因是螞蟻在選擇下一步柵格時，除了考慮信息素的大小，還需考慮當前柵格離目標點的距離。

假設當前柵格gi的鄰域柵格編號如圖4 所示。從圖5 可以看出，當螞蟻移動到C 點(gi=g153)時，其所有的可行柵格中，1號柵格相對于4號柵格離目標g211更近，根據算法1，選擇1號柵格的概率更大。但由圖5 可知，要盡快繞過障礙物，4 號柵格優于1 號柵格。為了解決該問題，在此規定一個檢查順序，比如在圖5 中當螞蟻k來到C點時給定其檢查順序orderk={6,5 or 7, 4 or 8, 3 or 1,2}（iorj代表隨機選擇先檢查i號或j號鄰域柵格）。該順序只在螞蟻第一次遇到該障礙物的時候計算，其計算過程稱為“過程1”，分為如下3步：

圖4 gi的鄰域柵格編號

圖5 螞蟻的移動位置示例

（1）求當前柵格gi到目標柵格gend的向量v；（2）根據公式（3）求向量v和向量（1,0）的夾角θ；（3）根據θ的取值，從表1中獲得鄰域柵格查找順序orderk。

表1 根據θ得領域柵格查看的順序

本文的算法稱為“算法2”，主要對算法1的步驟2 做了進一步改進，其他的步驟類似，故算法2 只對步驟2進行描述：

（1）設obs←0，值為0 代表螞蟻k在上一步中選擇的優先柵格是可行點。

（2）當q≤q0時，根據下面的“過程2”選擇下一步柵格gj（螞蟻k的當前柵格記為gi，要選擇的下一個柵格記為gj）：

過程2：選擇下一柵格的策略

1.計算下一步優先選擇的柵格：

2.IF(gfirst為可行節點)

螞蟻進入該柵格，令gj=gfirst。

3.ELSE IF(obs= 0)

當遇到新的障礙物時，調用過程1重新計算orderk。按照orderk定義的順序，依次檢查鄰域柵格是否為可行點。gj的值即為第1次檢測到的可行柵格的編號。并記obs=1。

4.ELSE IF(obs= 1)

按照原有orderk定義的順序，依次檢查鄰域柵格是否為可行點。gj的值即為第1次檢測到的可行柵格的編號。并記obs= 0。

（3）當q＞q0時，螞蟻k按照式（5）定義的概率隨機選擇下一步要走的柵格序號gj。

3 改進后算法2的仿真實驗

仿真1：參數設置同2.2 節。算法1、算法2 運行20次，迭代次數都設為100。算法1和算法2得到的最優路徑的平均長度分別為35和26。

算法2 的仿真結果如圖6 所示。 其中gbegin=g25，gend=g211，圖6（a）、圖6（b）和圖6（c）所示路徑的長度分別為27、24和28。由仿真結果可以看出，當螞蟻遇到較大障礙物時，算法2能較快地避開障礙物，得到一條更短的路徑。

圖6 算法2仿真結果

仿真2：目標值設為26，算法1 和算法2 分別運行6次。兩種算法達到目標值時的迭代次數和所用時間如表2所示，時間單位為毫秒。

表2 迭代次數和運行時間比較

由表2 可見，在大多數情況下，算法2 所需的迭代次數和運行時間要少于算法1。

4 總 結

本文在基于靜態信息的路徑規劃算法的基礎上，改變螞蟻選擇下一步柵格的策略，從而有效克服了路徑規劃中的震蕩問題。本文算法還有需要進一步改進的地方，比如當螞蟻繞開障礙物時可以將當前柵格和繞開上一障礙物的柵格之間的曲線拉直，該問題可由Bresenham 算法解決[11]，這些都在進一步的研究和實現中。