基于改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃

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

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

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

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

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

1.1 柵格法

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

圖1 柵格模型Fig.1 Grid model

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

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

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

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

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

2 Logistic 模型

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

Logistic 模型的迭代公式為

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

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

3.1 概率選擇函數(shù)

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

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

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

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

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

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

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

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

3.2 全局信息素篩選

在傳統(tǒng)的蟻群算法中，當(dāng)所有螞蟻?zhàn)咄暌院髸?huì)把所有的信息素更新一次，但是有些路徑太長(zhǎng)，它們的信息素會(huì)阻礙螞蟻的判斷，造成無(wú)效搜索，因此需要有選擇地更新信息素。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)仿真

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

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

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

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

5 結(jié)語(yǔ)

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

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

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