（西安工業(yè)大學(xué) 電子信息工程學(xué)院，西安710000）

路徑規(guī)劃是指在有障礙物的外界環(huán)境中，規(guī)劃出一條從指定起點到目標(biāo)點的運動路徑，使機(jī)器人在行走過程中能順利、無碰撞地躲避所有障礙物[1]。本文主要針對路徑規(guī)劃過程中會出現(xiàn)將局部范圍探索出的當(dāng)前最優(yōu)路徑誤認(rèn)為是全局最優(yōu)路徑以及在進(jìn)行路徑規(guī)劃過程中探索的路徑過于彎折間接致使路徑長度增加這兩點進(jìn)行研究[2]。

對于路徑規(guī)劃的方法，國內(nèi)外學(xué)者已有不少研究。人工勢場法由Khatib 提出，該算法實時性好、計算量小但易產(chǎn)生局部死鎖及路徑震蕩[1]；粒子群算法由Kennedy 和Eberhart 提出，該算法搜索速度快、調(diào)整參數(shù)少但缺乏速度的動態(tài)調(diào)節(jié)導(dǎo)致不易收斂[2]；人工蜂群法由Karaboga 提出，該算法魯棒性強(qiáng)、易于實現(xiàn)但收斂速度很慢，而且搜索范圍不夠全面[3]；蟻群算法是Marco Dorigo 提出的，該算法模擬螞蟻覓食的行為，在覓食的過程中螞蟻會留下信息素，越短的路徑信息素越多，每個螞蟻在搜索路徑的過程中會獲取其他螞蟻留下的信息素，按照遺留的信息素螞蟻會找到一條最短的覓食路徑[4]。蟻群算法魯棒性強(qiáng)、精度與并行性高且易于實現(xiàn)，同時蟻群算法比較容易與其他算法相融合來彌補各自的缺陷，相較其他算法本身較有優(yōu)勢，但是易陷入局部最優(yōu)，搜索時間長。

本文首先定義概率選擇函數(shù)，對傳統(tǒng)蟻群算法的狀態(tài)轉(zhuǎn)移概率進(jìn)行改進(jìn)，通過降低拐彎次數(shù)來減少路徑長度，且使算法規(guī)劃出來的路徑更加平滑；同時引入Logistic 模型對傳統(tǒng)的蟻群算法進(jìn)行優(yōu)化，通過該模型混沌變量的隨機(jī)、遍歷性的特點，對小部分路徑的信息素進(jìn)行調(diào)整，保證算法兼顧全局尋優(yōu)。該方法結(jié)構(gòu)簡單易于計算，可以減少用蟻群算法進(jìn)行路徑規(guī)劃時的迭代次數(shù)，提高收斂速度及全局搜索能力。

1 環(huán)境建模與傳統(tǒng)蟻群算法

1.1 柵格法

環(huán)境建模有3種方法：柵格法、幾何法、拓?fù)浞╗3]。在已知的二維環(huán)境中，本文采用柵格法進(jìn)行建模。該方法是將工作環(huán)境劃分為若干大小相同的柵格，如圖1所示，建立直角坐標(biāo)系，每個柵格可以用坐標(biāo)（x，y）表示，黑色柵格表示障礙柵格屬于不可行區(qū)域，白色柵格表示自由柵格屬于可行區(qū)域[4]。在柵格中標(biāo)明機(jī)器人的起點和目標(biāo)點，每個柵格都是一個單位的正方形，M×M為柵格規(guī)模大小。

圖1 柵格模型Fig.1 Grid model

1.2 傳統(tǒng)蟻群算法

螞蟻在覓食的時候會在自己爬行過的路徑上遺留信息素，找到食物返回的時候會順著自己留的信息素原路返回，這樣這條路徑上的信息素就會增加，吸引其他螞蟻也順著這條路徑覓食，路徑越短爬行的螞蟻會越來越多，路徑上信息素也會越來越多[5]。

傳統(tǒng)蟻群算法中，假設(shè)螞蟻數(shù)量為K，螞蟻k 在從當(dāng)前位置i 處到下一節(jié)點j 的狀態(tài)轉(zhuǎn)移概率為

式中：α為信息素啟發(fā)因子；β為啟發(fā)函 數(shù)因 子；allowed表示可選擇的路徑數(shù)組，τij（t）表示點i 與j之間的信息素濃度；ηij（t）表示螞蟻k 從點i 到j(luò) 的啟發(fā)信息，且螞蟻完成一次覓食時各路徑信息素更新為

式中：ρ為信息素?fù)]發(fā)因子，其值為0＜ρ＜1；Q為信息素強(qiáng)度；Lk為螞蟻k 在本次循環(huán)中所走過的路徑總長度。

2 Logistic 模型

Logistic 映射是一個典型的混沌系統(tǒng)，利用其混沌運動的遍歷性和隨機(jī)性的特性可以進(jìn)行優(yōu)化搜索，其基本思想是先產(chǎn)生一組混沌變量，每一個混沌量對應(yīng)一條長度大于所有路徑長度均值的路徑，對這一組路徑的信息素進(jìn)行混動擾動來進(jìn)行調(diào)整，產(chǎn)生新的信息素，通過迭代新的信息素來計算最優(yōu)路徑[6]。

Logistic 模型的迭代公式為

式中：μ為控制參量；hi為混沌變量，當(dāng)μ 取4時，0≤h0≤1，Logistic 完全處于混沌狀態(tài)。

3 改進(jìn)蟻群算法

3.1 概率選擇函數(shù)

傳統(tǒng)蟻群算法中狀態(tài)轉(zhuǎn)移概率只將路徑上螞蟻遺留的信息素作為參考因素，選擇條件比較單一，易導(dǎo)致螞蟻走出來的路徑較長。自由柵格的空地屬性可包括信息素、自由柵格占比及與目標(biāo)點的方向夾角。機(jī)器人進(jìn)行路徑探索從當(dāng)前柵格選擇下一柵格的時候，選擇周圍自由柵格較多而障礙柵格較少時會使機(jī)器人盡量避免拐彎的次數(shù)，走出來的路線不會太曲折，自由柵格較多時下一時刻的選擇也越多，可以減少總體路徑長度而且能夠提高路徑的平滑度。當(dāng)在一個二維環(huán)境里面需要規(guī)劃多條不同目標(biāo)的最優(yōu)路徑時可以適量減少路線交叉[7]。

假設(shè)當(dāng)前機(jī)器人所在柵格為i，如圖2所示機(jī)器人選擇下一個柵格j 的時候可以從柵格i 周圍的8個柵格中選擇一個，而柵格j周圍每個自由柵格占的概率是1/7，障礙柵格占的概率為0。路徑規(guī)劃的過程中需要讓機(jī)器人盡可能地選擇自由柵格多的方向，所以在螞蟻k 從柵格i 選擇柵格j時，定義一個自由柵格概率選擇函數(shù)：

圖2 自由柵格概率選擇Fig.2 Free grid probability selection

為了避免機(jī)器人在選擇路徑時局限于自由柵格多的方向，導(dǎo)致路線繞圈使路徑變得更長，如圖3所示，假設(shè)A為螞蟻k 當(dāng)前所在點，周圍有A1～A8個選擇，若恰好A1、A2與A3周圍自由柵格概率一樣時需要考慮當(dāng)前點與目標(biāo)點B 的方向是否相近，通過計算每條待選路徑的方向與當(dāng)前點和目標(biāo)點方向的夾角來選擇下一柵格，夾角越小越靠近目標(biāo)點方向[8]。所以本文定義了一個角度概率選擇函數(shù)（在直角坐標(biāo)系下各柵格點間的距離均可由坐標(biāo)求得）：

式中：θi是選擇柵格Ai方向與當(dāng)前點和目標(biāo)點方向的夾角；θ總是所有夾角的總和；Pθi代表柵格Ai靠近目標(biāo)點方向的程度。

圖3 角度概率選擇Fig.3 Angle probability selection

因此，概率選擇函數(shù)為

則傳統(tǒng)蟻群算法里面的狀態(tài)轉(zhuǎn)移概率可以改進(jìn)為式中：γ為概率選擇函數(shù)影響因子。

3.2 全局信息素篩選

在傳統(tǒng)的蟻群算法中，當(dāng)所有螞蟻走完以后會把所有的信息素更新一次，但是有些路徑太長，它們的信息素會阻礙螞蟻的判斷，造成無效搜索，因此需要有選擇地更新信息素。

從所有螞蟻進(jìn)行一次覓食的過程中就開始記錄該路徑長度，在完成一次覓食后按路徑長度從小到大排序，并求出所有M條路徑的平均值。對小于平均路徑長度的路徑上的信息素可以直接更新，對大于平均路勁長度的路徑上的信息素需要進(jìn)行篩選后再進(jìn)行更新。

假設(shè)NL是大于平均路徑長度Lmean的路徑個數(shù)，在NL個路徑上任意一條路徑上的分布的信息素是τl，l=1，2，…，NL定義混沌變量：

代入Logistic 模型的迭代公式中為

式中：τmax與τmin是在NL個路徑上的分布的信息素的最大值和最小值；ic是混沌迭代次數(shù)。通過混沌迭代得到混沌序列H=（h1，h2，…，hic），則混沌擾動后得到的新的信息素為

因此，全局信息素更新方式為

3.3 改進(jìn)蟻群算法路徑規(guī)劃流程

將本文提到的改進(jìn)傳統(tǒng)蟻群算法的方法應(yīng)用到機(jī)器人路徑規(guī)劃中，算法流程如圖4所示。

步驟1建立環(huán)境模型，設(shè)置螞蟻個數(shù)k，迭代次數(shù)N，信息素啟發(fā)因子α，目標(biāo)點個數(shù)S，啟發(fā)函數(shù)因子β，信息素?fù)]發(fā)因子ρ，信息素強(qiáng)度Q；

步驟2螞蟻k 通過式（11）選擇路徑，并記錄每只螞蟻覓食的路徑長度；

步驟3螞蟻k 搜索完所有目標(biāo)個數(shù)，否則返回步驟2；

步驟4對生成的M條路徑進(jìn)行排序并計算平均值；

步驟5將路徑長度大于平均值的路徑上的信息素用式（14）進(jìn)行混動擾動產(chǎn)生新信息素，利用式（15）進(jìn)行信息素更新；

步驟6 滿足迭代次數(shù)后輸出最優(yōu)路徑，否則返回步驟2。

圖4 改進(jìn)后蟻群算法框圖Fig.4 Improved ant colony algorithm block diagram

4 實驗仿真

根據(jù)以上提出的算法，首先利用Matlab 進(jìn)行算法驗證，然后在Ubuntu 的ROS系統(tǒng)中建立實際地圖通過Gazebo 和Rviz 平臺進(jìn)行算法移植實際應(yīng)用。

如圖5和圖6所示是在Matlab 上對改進(jìn)前和改進(jìn)后的算法進(jìn)行了對比，結(jié)果如表1所示，可以看出改進(jìn)后的算法路徑更平滑，路徑長度更短。

圖5 改進(jìn)前的路徑規(guī)劃Fig.5 Path planning before improvement

圖6 改進(jìn)后的路徑規(guī)劃Fig.6 Improved path planning

表1 仿真數(shù)據(jù)Tab.1 Simulation data

在Ubuntu 的ROS 環(huán)境下建立實際地圖并移植算法，利用Gazebo 和Rviz 進(jìn)行實驗仿真。Gazebo是3D 物理仿真平臺，主要是創(chuàng)建一個虛擬的仿真環(huán)境，能夠產(chǎn)生數(shù)據(jù)；Rviz是3D可視化工具，與Gazebo一起實驗，把現(xiàn)有的數(shù)據(jù)可視化顯示。

本文將改進(jìn)前后的算法以插件的形式移植到ROS系統(tǒng)中進(jìn)行調(diào)用，在自己搭建的Gazebo 環(huán)境中進(jìn)行實際應(yīng)用并在Rviz 平臺進(jìn)行顯示。結(jié)果如圖7和圖8所示，可以得出改進(jìn)后的算法規(guī)劃出的路徑轉(zhuǎn)彎次數(shù)少，路徑更加平滑，減少了路徑長度，降低了搜索時間。

5 結(jié)語

圖7 Gazebo 創(chuàng)建的實際地圖Fig.7 Actual map created by Gazebo

圖8 Rviz 顯示規(guī)劃的路徑Fig.8 Rviz shows the planned path

本文利用概率選擇函數(shù)和Logistic 模型對傳統(tǒng)蟻群算法的搜索速度過慢及路徑質(zhì)量不高的問題進(jìn)行改進(jìn)，改進(jìn)后的算法在路徑長度、轉(zhuǎn)彎次數(shù)、搜索時間上均有所提高，并通過matlab 和ROS系統(tǒng)進(jìn)行了算法的有效性和實用性的驗證。