基于改進(jìn)ACS算法的移動(dòng)機(jī)器人路徑規(guī)劃研究

馬小陸 梅宏 龔瑞 王兵 吳紫恒

摘 ? 要：針對(duì)蟻群系統(tǒng)（Ant Colony System，ACS）算法存在收斂速度慢、路徑不平滑、易陷入局部最優(yōu)等缺點(diǎn)，提出了一種基于萬有引力搜索策略的ACS算法. 為了解決算法初期由于地圖信息匱乏，導(dǎo)致蟻群尋路盲目性較大的問題，提出了簡化ACS算法對(duì)初始信息素濃度進(jìn)行更新. 引入萬有引力算法搜索策略，提升了算法收斂速度，且有效解決了局部最優(yōu)問題. 對(duì)每次迭代獲取到的最優(yōu)路徑進(jìn)行優(yōu)化，減少了路徑的轉(zhuǎn)折點(diǎn)數(shù)量、提升了路徑平滑性. 仿真試驗(yàn)表明，改進(jìn)算法能夠有效提升算法的收斂速度、路徑平滑性. 將改進(jìn)算法應(yīng)用到實(shí)際的移動(dòng)機(jī)器人導(dǎo)航試驗(yàn)中，試驗(yàn)結(jié)果表明，改進(jìn)算法能夠有效解決移動(dòng)機(jī)器人的路徑規(guī)劃問題，且有效提升移動(dòng)機(jī)器人的導(dǎo)航效率.

關(guān)鍵詞：移動(dòng)機(jī)器人;路徑搜索;最優(yōu)路徑;蟻群系統(tǒng)算法;萬有引力算法

中圖分類號(hào)：P242.6 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)志碼：A

Research on Path Planning of Mobile Robot

Based on Improved ACS Algorithm

MA Xiaolu1，2?，MEI Hong1，2，GONG Rui1，2，WANG Bing1，2，WU Ziheng1，2

（1. School of Electrical and Information Engineering，Anhui University of Technology，Maanshan 243032，China;

2. Anhui Province Key Laboratory of Special and Heavy Load Robot，Maanshan 243032，China）

Abstract：Aiming at the shortcomings of slow convergence speed， unsmooth path and easy to fall into local optimum in Ant Colony System（ACS） algorithm， an ACS algorithm based on gravitational search strategy is proposed. Firstly， in order to solve the problem that the lack of map information in the initial stage of the algorithm leads to the great blindness problem of ant colony algorithm， a simplified ant colony algorithm is proposed to update the initial pheromone concentration; secondly， the search strategy of gravity algorithm is introduced to improve the speed of the later algorithm and effectively solve the local optimal problem; finally， the optimal path obtained by each iteration is optimized， which reduces the number of turning points and improves the smoothness of the path. Simulation results show that the improved algorithm can effectively improve the convergence speed and path smoothness of the algorithm. Additionally， the improved algorithm is applied to the actual mobile robot navigation experiment. The experimental results show that the improved algorithm can effectively solve the path planning problem of mobile robot， and effectively improve the efficiency of robot navigation.

Key words：mobile robots; path search; optimal path;Ant Colony System（ACS） algorithm;Gravitational Search Algorithm（GSA）

路徑規(guī)劃問題是移動(dòng)機(jī)器人研究的重點(diǎn)對(duì)象之一，是指移動(dòng)機(jī)器人依據(jù)現(xiàn)有信息規(guī)劃出一條從起始位置安全到達(dá)目標(biāo)位置，且滿足各項(xiàng)性能指標(biāo)的完整路徑[1]. 近年來，國內(nèi)外學(xué)者對(duì)路徑規(guī)劃問題進(jìn)行了大量的研究，并取得了一定的成果. 傳統(tǒng)路徑規(guī)劃算法有Dijkstra算法[2]、A*算法[3]、人工勢場法[4]等，隨著移動(dòng)機(jī)器人工作空間復(fù)雜度的提升，逐漸涌現(xiàn)出一系列的智能仿生算法，如遺傳算法[5]、粒子群算法[6]、人工蜂群算法[7]等.

螞蟻系統(tǒng)（Ant System，AS）是由Dorigo等[8]提出的一種模擬自然界中螞蟻覓食行為的仿生算法. 雖然AS算法已經(jīng)能夠有效解決移動(dòng)機(jī)器人路徑規(guī)劃問題，但是依舊存在收斂速度慢、易陷入局部最優(yōu)等問題，因此，Dorigo等[9]于1997年提出了蟻群系統(tǒng)（Ant Colony System，ACS）. ACS算法具有并行性、強(qiáng)魯棒、易實(shí)現(xiàn)等優(yōu)點(diǎn)，可以有效解決移動(dòng)機(jī)器人路徑規(guī)劃問題，但是ACS算法仍存在尋路速度慢、易陷入局部最優(yōu)、路徑不平滑等問題. 針對(duì)上述問題，國內(nèi)外學(xué)者提出了多種優(yōu)化方法. 文獻(xiàn)[10]引入了蟻周模型更新信息素，提高了蟻群搜索效率，降低局部最優(yōu)概率;文獻(xiàn)[11]提出了一種A*算法和ACS算法的融合算法，加快了算法的收斂速度，提高了最優(yōu)路徑的質(zhì)量;文獻(xiàn)[12]引入了信息素閾值，提高了算法的全局搜索能力，增加了最優(yōu)路徑的多樣性;文獻(xiàn)[13]提出了一種自適應(yīng)啟發(fā)函數(shù)，提高了蟻群的尋路效率，加快了算法的收斂速度.

針對(duì)ACS算法收斂速度慢、易陷入局部最優(yōu)和路徑轉(zhuǎn)折點(diǎn)數(shù)量過多等問題，本文在ACS算法的基礎(chǔ)上提出了一種引力蟻群系統(tǒng)（Gravitational Search Ant Colony System，GSACS）算法. 首先，引入了簡化的ACS算法，對(duì)初始信息素濃度進(jìn)行更新，降低了算法初期蟻群尋路的盲目性;同時(shí)，引入了萬有引力搜索策略，提升了算法路徑尋優(yōu)的收斂速度，有效克服了局部最優(yōu)問題;最后，提出了路徑優(yōu)化方法，減少了路徑轉(zhuǎn)折點(diǎn)數(shù)量，提升了移動(dòng)機(jī)器人的導(dǎo)航效率. 仿真和實(shí)驗(yàn)結(jié)果證明，GSACS算法所規(guī)劃出的路徑是有效且可行的，并且相比于ACS算法，尋路效率更高.

1 ? 蟻群系統(tǒng)算法

自然界中，螞蟻在覓食時(shí)會(huì)不斷向周圍環(huán)境中釋放一種名為信息素的物質(zhì)，當(dāng)某條路徑上的信息素濃度逐漸增多，則越來越多的螞蟻都會(huì)選擇該路徑行走[14]. 在ACS算法中，若螞蟻k當(dāng)前位置為節(jié)點(diǎn)i，將根據(jù)偽隨機(jī)比率規(guī)則計(jì)算出后續(xù)行走的節(jié)點(diǎn)j，其公式如式（1）所示.

j = arg

i∈Akmax[τα

ij（t）η β

ij（t）]，q≤q0

pk

ij（t），q>q0 ? （1）

式中：q0（q0∈[0，1]）為常數(shù);q（q∈[0，1]）為隨機(jī)數(shù). 若q≤q0，則按照上述公式篩選后續(xù)行走節(jié)點(diǎn);若q>q0，則按照pk

ij（t）選擇后續(xù)行走節(jié)點(diǎn). pk

ij（t）如式（2）所示.

pk

ij（t） = ，j∈Ak

0， ? ? ?Otherwise ? ? ? （2）

式中：α為信息啟發(fā)因子;β為距離啟發(fā)因子;Ak為螞蟻k可以行走鄰節(jié)點(diǎn)的集合;η β

ij（t）為節(jié)點(diǎn)i到節(jié)點(diǎn)j的啟發(fā)信息，其公式如式（3）所示.

ηij = ? ? ?（3）

當(dāng)螞蟻由節(jié)點(diǎn)i移動(dòng)到節(jié)點(diǎn)j時(shí)，更新路徑〈i，j〉上的局部信息素濃度，其公式如式（4）所示.

τij（t + 1） = （1 - ζ）τij（t） + ζτ0 ? ? ? ?（4）

式中：ζ為局部信息素濃度揮發(fā)系數(shù);τ0為路徑〈i，j〉上的初始信息素濃度值.

當(dāng)所有螞蟻完成一次迭代尋路時(shí)，則會(huì)更新當(dāng)前最優(yōu)路徑上的全局信息素濃度[15]，其公式如式（5）（6）所示.

τij（t + n） = （1 - ρ）τij（t） + ρΔτij ? ? ? ?（5）

Δτ k

ij =

，Tij∈T

0， ? ?Otherwise ? ? ? （6）

式中：ρ為全局信息素濃度揮發(fā)系數(shù);Δτ k

ij為路徑〈i，j〉上的信息素濃度增量;Lk表示第k只螞蟻搜索到的路徑長度.

2 ? 萬有引力算法

2.1 ? 基本原理

萬有引力算法（Gravitational Search Algorithm，GSA）[16]是Rashedi等于2009年提出的一種新的啟發(fā)式智能優(yōu)化算法. 根據(jù)牛頓的萬有引力定律可知，空間中兩個(gè)物體間的相互作用力F與慣性質(zhì)量成正比，與距離的平方成反比，引力F如式（7）所示.

F = G ? ? ? ? （7）

式中：G為引力常數(shù);m1和m2分別為兩個(gè)物體的質(zhì)量;r為兩物體之間的距離.

GSA算法通過種群的粒子位置移動(dòng)來尋找最優(yōu)解，隨著算法的不斷迭代，粒子依靠相互作用力在空間中不停的搜索運(yùn)動(dòng)，直至搜索到最優(yōu)解. 由于GSA存在收斂速度過快、全局搜索能力較差、局部最優(yōu)問題較為嚴(yán)重等問題，導(dǎo)致該算法并沒有被廣泛應(yīng)用到移動(dòng)機(jī)器人路徑規(guī)劃任務(wù)中.

2.2 ? 數(shù)學(xué)模型

假設(shè)一個(gè)d維空間內(nèi)有N個(gè)粒子，定義第i個(gè)粒子的位置如式（8）所示.

Xi = （x1

i，x2

i，…，xk

i，…，xn

i），i=1，2，…，N ? ?（8）

在t時(shí)刻，粒子i在第d維空間上受到粒子j的引力大小為F k

ij（t），其公式如式（9）所示.

F k

ij（t）=G（t）（x k

j（t）-x k

i（t）） ? ?（9）

式中：Mi（t）和Mj（t）分別表示t時(shí)刻粒子i和粒子j的質(zhì)量;ε為一個(gè)很小的常數(shù);Rij（t）為t時(shí)刻粒子i與j之間的歐幾里得距離，其公式如式（10）所示;G（t）為t時(shí)刻下的引力系數(shù)，其公式如式（11）所示.

Rij（t） =‖Xi（t），Xj（t）‖2 ? ? ? ?（10）

G（t） = G0 e ? ? ? ? ? （11）

式中：G0為初始時(shí)引力常數(shù);α為一個(gè)常量;T為最大迭代次數(shù).

為了增強(qiáng)隨機(jī)可能性，定義在t時(shí)刻，粒子Xi在k維空間中所受引力合力F k

i（t）等于周邊所有粒子對(duì)其引力之和，F(xiàn) k

i（t）如式（12）所示.

F k

i（t） = [][j=1， j≠i] rj F ?j

i（t） ? ? ? ?（12）

式中：rj（rj∈[0，1]）為隨機(jī)數(shù). 在t時(shí)刻，粒子i在k維空間上的加速度a k

i（t）如式（13）所示.

a k

i（t） = ? ? ? ? （13）

式中：Mi（t）為粒子i的慣性質(zhì)量，其公式如式（14）（15）所示.

mi（t） = ? ? ? ? （14）

Mi（t） = ? ? ? ? （15）

式中：Si （t）為粒子i在t時(shí)刻的適應(yīng)度值;B（t）為粒子i在t時(shí)刻最優(yōu)適應(yīng)度值;W（t）為粒子i在t時(shí)刻最差適應(yīng)度函數(shù). B（t）和W（t）分為最小化和最大化問題進(jìn)行定義.

1）最小化問題：

B（t） =Si（t） ? ? ? ?（16）

W（t） =Sj（t） ? ? ? ?（17）

2）最大化問題：

B（t） = ?Si（t） ? ? ? ? （18）

W（t） = ?Sj（t） ? ? ? ? （19）

在每迭代一次中，粒子i的速度和位置分別按式（20）（21）進(jìn)行更新：

vk

i（t + 1） = ri vk

i（t） + ak

i（t） ? ? ? （20）

xk

i（t + 1） = xk

i（t） + vk

i（t + 1） ? ? ? （21）

3 ? 改進(jìn)的ACS算法

ACS算法是目前應(yīng)用較為廣泛的一種路徑規(guī)劃算法，但依舊存在收斂速度慢、易陷入局部最優(yōu)、路徑不平滑等問題. 針對(duì)以上問題，本文提出了引力蟻群系統(tǒng)算法，首先引入一種簡化ACS算法，對(duì)初始信息素濃度進(jìn)行更新;其次引入GSA搜索策略，提升算法的收斂速度;最后提出了一種路徑優(yōu)化方法，提升了路徑的平滑性.

3.1 ? 改進(jìn)的初始信息素分配規(guī)則

在傳統(tǒng)ACS算法中，柵格地圖各節(jié)點(diǎn)上的初始信息素濃度均為τ0，從而導(dǎo)致蟻群在算法初期的收斂性較差、尋路時(shí)間較長. 針對(duì)上述問題，本文對(duì)初始信息素分配規(guī)則進(jìn)行了改進(jìn)，引入了簡化ACS算法，對(duì)初始信息素濃度重新分配，在算法尋路初期加入環(huán)境的先驗(yàn)知識(shí)，提高了算法前期的收斂速度.

簡化ACS算法的種群數(shù)量為1，即只派出一只螞蟻進(jìn)行路徑搜索. 由零點(diǎn)定理可知，最優(yōu)路徑一般存在于起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)為頂點(diǎn)的矩形區(qū)域內(nèi).定義該螞蟻在尋路時(shí)，只在鄰節(jié)點(diǎn)中篩選最大信息素和距離的節(jié)點(diǎn)，同時(shí)簡化ACS算法，不進(jìn)行局部信息素濃度更新，其轉(zhuǎn)移公式如式（22）所示.

pk

ij（t） = arg max[τα

ij（t）η β

ij（t）]，j∈Ak

0， ? ?Otherwise ? ? ? （22）

由于初始信息素濃度為均值，由式（22）可知，螞蟻在行走時(shí)，傾向于選擇朝向目標(biāo)節(jié)點(diǎn)的方向行走，由于缺少地圖的先驗(yàn)知識(shí)，螞蟻易陷入“死鎖”狀態(tài)，即無后續(xù)可行走節(jié)點(diǎn). 針對(duì)上述問題，本文采用回溯法重新計(jì)算其父節(jié)點(diǎn)的后續(xù)可移動(dòng)節(jié)點(diǎn)，并將當(dāng)前節(jié)點(diǎn)放入禁忌表中. 重復(fù)上述過程，直至螞蟻搜索到目標(biāo)節(jié)點(diǎn)，此時(shí)簡化的ACS算法完成了一條完整路徑的規(guī)則.

簡化ACS算法完成路徑搜索后，將會(huì)更新此路徑上的全局信息素濃度，其余節(jié)點(diǎn)上的濃度不變. 信息素更新公式如式（23）所示.

τ（Lacs） = ωτ0，ω > 1 ? ? ?（23）

式中：ω為初始信息素濃度增加系數(shù).

如圖1所示，黑色折線為簡化ACS算法搜索到的一條完整路徑，此時(shí)更新灰色節(jié)點(diǎn)上的初始信息素濃度，其他節(jié)點(diǎn)上的初始信息素濃度不變. 由蟻群的狀態(tài)轉(zhuǎn)移概率可知，蟻群更傾向于選擇信息素濃度更高的節(jié)點(diǎn)行走，因此初始信息素濃度的不均勻分配將有效降低蟻群尋路的盲目性，提升算法的收斂速度.

3.2 ? 改進(jìn)的啟發(fā)信息函數(shù)

傳統(tǒng)ACS算法路徑尋優(yōu)時(shí)，螞蟻k從當(dāng)前節(jié)點(diǎn)i搜索后續(xù)的行走節(jié)點(diǎn)j時(shí)，只受到信息素濃度和距離這兩個(gè)因素的影響. 迭代次數(shù)越多，最優(yōu)路徑上的信息素濃度越高，最終所有螞蟻都將聚集到這條路徑上，但是由于ACS算法的全局搜索能力較強(qiáng)，蟻群尋路盲目性較大，從而導(dǎo)致算法的收斂速度受到很大影響. 針對(duì)上述問題，本文引入了GSA搜索策略，提升了算法尋路時(shí)的收斂速度.

3.2.1 ? 引力蟻群規(guī)則

在GSACS算法中，將螞蟻視為粒子，螞蟻k對(duì)其他螞蟻產(chǎn)生引力，同時(shí)螞蟻k也受到其他螞蟻引力的影響，目標(biāo)點(diǎn)也對(duì)所有螞蟻產(chǎn)生引力. 目標(biāo)節(jié)點(diǎn)的慣性質(zhì)量Mgoal（t） = 1.

慣性質(zhì)量由蟻群的適應(yīng)度函數(shù)值決定，本文定義螞蟻k的適應(yīng)度函數(shù)值fk（t）為當(dāng)前位置距離目標(biāo)節(jié)點(diǎn)的歐幾里得距離. 其公式如式（24）所示.

fk（t） = ? ? ? ?（24）

式中：（xk，yk）和（xg，yg）分別為螞蟻k當(dāng)前位置節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的坐標(biāo).

螞蟻k受到的引力Fk（t）為所有螞蟻和目標(biāo)節(jié)點(diǎn)對(duì)螞蟻k的引力合力，其公式如式（25）所示.

Fk（t） = [][j=1， j≠k] rj Fkj（t） + rgoal Fkgoal（t） ? ? ? ?（25）

式中：Fkj（t）為螞蟻k受到螞蟻j的引力;Fkgoal（t） 為螞蟻k受到目標(biāo)節(jié)點(diǎn)的引力;rgoal（rgoal∈[0，1]）為隨機(jī)數(shù).

螞蟻k的加速度ak（t）如式（26）所示.

ak（t） = ? ? ? ? ?（26）

式中：Mk（t）為螞蟻k的慣性質(zhì)量.

如圖2所示，F(xiàn)kj為螞蟻k受螞蟻j的引力，F(xiàn)kgoal為螞蟻k受目標(biāo)點(diǎn)的引力，矢量合力Fk為螞蟻k向目標(biāo)節(jié)點(diǎn)G行走的加速力，由于合力Fk的大小是由蟻群和目標(biāo)節(jié)點(diǎn)共同決定的，從而可以有效避免蟻群陷入局部最優(yōu).

3.2.2 ? 改進(jìn)的啟發(fā)信息函數(shù)

在GSACS算法中，螞蟻k搜索后續(xù)節(jié)點(diǎn)的啟發(fā)信息由引力啟發(fā)信息和距離啟發(fā)信息組成. 引力啟發(fā)信息為螞蟻k在柵格地圖中所受到引力的合力，其公式如式（27）所示.

ηGS（t） = γ ? ? ? ? ? （27）

式中：γ為常數(shù);ak為螞蟻k受到引力的合力而獲得的加速度;θ為螞蟻可移動(dòng)節(jié)點(diǎn)和加速度ak方向的夾角.

由式（27）可知，引力啟發(fā)信息在加速度和方向夾角的共同作用下，使螞蟻k更傾向于選擇加速度更大的節(jié)點(diǎn)行走. 引力啟發(fā)信息使蟻群逐漸聚集到一條路徑上，雖然收斂速度更快，但是全局搜索能力較弱，且易出現(xiàn)局部最優(yōu)解. GSACS算法在啟發(fā)信息中引入加速度系數(shù)ξ，其公式如式（28）所示.

ξ = ? ? ? ? ? （28）

式中：N為當(dāng)前迭代次數(shù);Nmax為最大迭代次數(shù).

算法尋路初期，在加速度系數(shù)ξ的影響下，螞蟻k的加速度為0，引力啟發(fā)信息為1，此時(shí)蟻群尋路完全由距離啟發(fā)信息發(fā)揮作用;隨著迭代次數(shù)增多，引力啟發(fā)信息隨之增大，加速度逐漸對(duì)蟻群尋路產(chǎn)生影響.

由距離啟發(fā)信息和引力啟發(fā)信息可知，改進(jìn)算法的啟發(fā)信息ηij（t）如式（29）所示.

ηij（t） = ηd ηGS = γ ? ? ? （29）

螞蟻k在柵格環(huán)境行走時(shí)，分別受到蟻群和目標(biāo)節(jié)點(diǎn)對(duì)其的引力作用，本文定義移動(dòng)機(jī)器人在工作空間內(nèi)可向周圍8個(gè)方向移動(dòng). 如圖3所示，假設(shè)機(jī)器人當(dāng)前兩條可行路徑分別為①和②，矢量合力Fk與兩條可行路徑的夾角θ1 < θ2，且由式（1）（2）和式（29）可知，合力Fk與可行路徑夾角越小，啟發(fā)信息ηij（t）值越大，則該路徑的轉(zhuǎn)移概率越大，從而螞蟻k選擇路徑①行走的概率也越大.

3.3 ? 路徑平滑性處理

GSACS算法是基于柵格地圖進(jìn)行路徑規(guī)劃，由于算法只選擇長度最短的路徑作為最終路徑，并不對(duì)路徑轉(zhuǎn)折點(diǎn)數(shù)量進(jìn)行判斷. 蟻群在搜索后續(xù)節(jié)點(diǎn)時(shí)，僅由信息素濃度和啟發(fā)信息決定，蟻群更優(yōu)于選擇信息素更大、距離目標(biāo)節(jié)點(diǎn)更近的節(jié)點(diǎn)作為后續(xù)節(jié)點(diǎn)，從而導(dǎo)致算法規(guī)劃出的路徑轉(zhuǎn)折點(diǎn)數(shù)量過多.

本文在當(dāng)前路徑的基礎(chǔ)上，進(jìn)行路徑平滑性處理，提升了ACS算法規(guī)劃路徑的平滑性，使得移動(dòng)機(jī)器人行走更加平滑，減少運(yùn)行時(shí)間、提升工作效率. 如圖4所示，實(shí)線和虛線分別為兩條長度相等的路徑，其中實(shí)線為算法規(guī)劃出的路徑，虛線為平滑處理后的路徑，由圖可以看出，虛線只存在一個(gè)轉(zhuǎn)折點(diǎn)，路徑更加平滑，從而更適合移動(dòng)機(jī)器人的行走.

路徑平滑性處理方法如下：

設(shè)優(yōu)化ACS算法當(dāng)前獲取到的最短路徑為P = {S，x1，…，xi，…，xp，G}，其中S為起始節(jié)點(diǎn);G為目標(biāo)節(jié)點(diǎn);xi為完整路徑的中間節(jié)點(diǎn). 首先定義起始節(jié)點(diǎn)S為當(dāng)前優(yōu)化節(jié)點(diǎn)，并依據(jù)起始節(jié)點(diǎn)S和目標(biāo)節(jié)點(diǎn)G獲取過渡節(jié)點(diǎn)T，若路徑〈S，T，G〉不與障礙物發(fā)生碰撞，則稱該路徑為合理路徑;反之則稱該路徑為不合理路徑，同時(shí)開始計(jì)算起始節(jié)點(diǎn)S和后續(xù)節(jié)點(diǎn)xp的過渡節(jié)點(diǎn)T1，并判斷路徑〈S，T，xp，G〉是否為合理路徑，若該路徑為合理路徑，則結(jié)束起始節(jié)點(diǎn)S的路徑平滑，選取節(jié)點(diǎn)x1和目標(biāo)節(jié)點(diǎn)G開始計(jì)算過渡節(jié)點(diǎn)，不斷重復(fù)上述過程，直至選取的節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)重合，此時(shí)已經(jīng)完成了路徑平滑性處理. 過渡節(jié)點(diǎn)的公式如式（30）（31）所示.

xk = xi + dx - dy，if dx>dy

xi， ? ?Otherwise ? ? ? （30）

yk = yi + dy - dx，if dy>dx

yi， ? ?Otherwise ? ? ? （31）

式中：dx和dy分別為當(dāng)前計(jì)算兩個(gè)節(jié)點(diǎn)的橫、縱坐標(biāo)之差，公式如式（32）（33）所示.

dx = x2 - x1 ? ? ?（32）

dy = y2 - y1 ? ? ?（33）

式中：（x1，y1）和（x2，y2）分別為當(dāng)前計(jì)算的兩個(gè)節(jié)點(diǎn)的坐標(biāo).

路徑平滑處理過程如圖5所示，其中S為起始節(jié)點(diǎn)，G為目標(biāo)節(jié)點(diǎn). 蟻群迭代N次獲取當(dāng)前最優(yōu)路徑后，首先依據(jù)節(jié)點(diǎn)S和節(jié)點(diǎn)G獲取過渡節(jié)點(diǎn)T，判斷路徑〈S，T，G〉是否為合理路徑，如圖5（a）所示. 由于路徑〈S，T，G〉為不合理路徑，此時(shí)將依據(jù)節(jié)點(diǎn)S和節(jié)點(diǎn)x獲取過渡節(jié)點(diǎn)T并判斷路徑〈S，T，x〉是否合理，如果路徑合理，則利用路徑〈S，T，x，G〉替代原路徑〈S，x，G〉，如圖5（b）所示. 若發(fā)現(xiàn)了一條優(yōu)化路徑，則結(jié)束節(jié)點(diǎn)S的路徑優(yōu)化，并選取后續(xù)節(jié)點(diǎn)x1作為當(dāng)前優(yōu)化節(jié)點(diǎn)，如圖5（c）所示. 依據(jù)節(jié)點(diǎn)x1和節(jié)點(diǎn)G獲取過渡節(jié)點(diǎn)T，并判斷路徑〈x1，T，G〉是否合理，若路徑合理，則利用路徑〈S，x1，T，G〉替代原路徑，反之則繼續(xù)進(jìn)行后續(xù)節(jié)點(diǎn)的路徑優(yōu)化，如圖5（d）所示. 直至當(dāng)前判斷節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)G重合時(shí)，則結(jié)束路徑優(yōu)化，并利用優(yōu)化路徑替換原路徑.

由上述路徑優(yōu)化過程可知，平滑處理后路徑的轉(zhuǎn)折點(diǎn)數(shù)量更少、路徑更加平滑，從而可以減少移動(dòng)機(jī)器人導(dǎo)航時(shí)的轉(zhuǎn)彎次數(shù)，使得移動(dòng)機(jī)器人在工作環(huán)境中行使更加靈活，降低移動(dòng)機(jī)器人運(yùn)行時(shí)間及能耗的損失，提高工作效率.

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

4.1 ? 仿真驗(yàn)證

為了驗(yàn)證GSACS算法的有效性，利用不同規(guī)格的柵格地圖進(jìn)行仿真試驗(yàn). 仿真用的計(jì)算機(jī)性能參數(shù)為：主頻為2.80 GHz的英特爾酷睿i7-7700處理器，內(nèi)存大小為12 G，運(yùn)行的系統(tǒng)為Windows10，利用Visual Studio 2017軟件分別對(duì)ACS算法和GSACS算法進(jìn)行仿真試驗(yàn). 試驗(yàn)中2種算法參數(shù)設(shè)置如表1所示.

30×30的柵格仿真環(huán)境如圖6所示，其中起始節(jié)點(diǎn)坐標(biāo)為（1，1），目標(biāo)節(jié)點(diǎn)坐標(biāo)為（30，30），黑色柵格為障礙物，黑色折線為兩種算法規(guī)劃出的最優(yōu)路徑. 由圖6可以得出，GSACS算法較ACS算法而言，路徑轉(zhuǎn)折點(diǎn)數(shù)量更少，從而規(guī)劃出的路徑平滑性更高，且有效克服了局部最優(yōu)問題.

圖7為ACS算法和GSACS算法在30×30柵格中的最短路徑收斂圖. 由圖7可以得出，ACS算法和GSACS算法的收斂迭代次數(shù)分別為47次和34次，GSACS算法的收斂速度要明顯優(yōu)于ACS算法，獲取的最優(yōu)路徑長度也要優(yōu)于ACS算法.

為了驗(yàn)證GSACS算法在不同柵格地圖環(huán)境下的有效性和實(shí)用性，分別采用另外幾組環(huán)境模型進(jìn)行路徑尋優(yōu)試驗(yàn)，試驗(yàn)結(jié)果如表2所示. 由表2可以得出，GSACS算法在不同柵格地圖環(huán)境下路徑尋優(yōu)的收斂速度、路徑長度、收斂時(shí)間、轉(zhuǎn)折點(diǎn)數(shù)量等，均要優(yōu)于ACS算法，且隨著環(huán)境復(fù)雜度的提升，GSACS算法的優(yōu)越性越明顯.

4.2 ? 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證算法的有效性和可行性，將ACS算法和GSACS算法分別應(yīng)用到實(shí)際的基于ROS（Robot Operating System）的移動(dòng)機(jī)器人導(dǎo)航實(shí)驗(yàn)中. 本文所使用的移動(dòng)機(jī)器人底盤是四輪行走結(jié)構(gòu)的輪式機(jī)器人，其中前輪為被動(dòng)輪，后輪為驅(qū)動(dòng)輪，移動(dòng)機(jī)器人實(shí)物如圖8所示.

移動(dòng)機(jī)器人底座控制平臺(tái)主要有激光雷達(dá)、STM32控制板、光電編碼器、驅(qū)動(dòng)器、直流無刷電機(jī)、蓄電池、前后輪和多種接口等組成. STM32作為平臺(tái)控制的核心，將編碼器數(shù)據(jù)發(fā)送給上位機(jī)，上位機(jī)會(huì)根據(jù)編碼器信息和激光雷達(dá)數(shù)據(jù)計(jì)算角速度和線速度發(fā)送給STM32，STM32將速度分解成機(jī)器人左右輪的速度，之后通過中斷引腳給電機(jī)驅(qū)動(dòng)器輸送高低電平，并由驅(qū)動(dòng)器根據(jù)接收到的PWM脈沖信號(hào)控制電機(jī)轉(zhuǎn)動(dòng)，從而實(shí)現(xiàn)移動(dòng)機(jī)器人的自主導(dǎo)航.

由于ACS算法中的信息啟發(fā)因子α和距離啟發(fā)因子β在實(shí)際應(yīng)用中非常重要，目前參數(shù)的設(shè)置基本依靠實(shí)驗(yàn)統(tǒng)計(jì)數(shù)據(jù)和經(jīng)驗(yàn)值來確定. 本文為了確定α和β的取值，選取不同環(huán)境進(jìn)行移動(dòng)機(jī)器人導(dǎo)航實(shí)驗(yàn)，在參數(shù)變化范圍內(nèi)設(shè)置不同的參數(shù)組合，每次實(shí)驗(yàn)只改變一個(gè)參數(shù)的值，獲取ACS算法尋路時(shí)間和柵格地圖中路徑長度的實(shí)驗(yàn)數(shù)據(jù)，從而確定最優(yōu)參數(shù)的組合. 測試參數(shù)為：α∈{1，2，…，7，8}、 β∈{1，2，…，7，8}，參數(shù)默認(rèn)設(shè)為α=1、 β=7，柵格地圖分辨率R=0.05. 實(shí)驗(yàn)結(jié)果如圖9和圖10所示.

由圖9可以看出，當(dāng)β不變，信息啟發(fā)因子α不斷增大時(shí)，算法很難搜索到最優(yōu)路徑，且尋路時(shí)間不斷增加;由圖10可以看出，當(dāng)α不變，距離啟發(fā)因子β不斷增大時(shí)，尋路時(shí)間和最優(yōu)路徑長度逐漸減小.

由上述分析可知，α和β的作用是相互耦合的，且α和β的組合設(shè)置對(duì)算法的性能有很大影響，初始信息素τ0對(duì)算法的結(jié)果影響不大，因此本文選取最優(yōu)組合為α = 1、 β = 7、τ0 = 0.000 3 .

導(dǎo)航實(shí)驗(yàn)中，在相同環(huán)境下，選擇相同的起點(diǎn)和目標(biāo)點(diǎn)分別進(jìn)行2組對(duì)比實(shí)驗(yàn)，2種算法生成的最終路徑分別如圖11和圖12中標(biāo)注路徑的折線所示.

由圖11和圖12可以得出，ACS算法規(guī)劃出的路徑存在轉(zhuǎn)折點(diǎn)數(shù)量過多、局部最優(yōu)等問題，上述問題可能會(huì)導(dǎo)致移動(dòng)機(jī)器人行走時(shí)頻繁轉(zhuǎn)向，降低導(dǎo)航效率，甚至減少移動(dòng)機(jī)器人的使用壽命. GSACS算法規(guī)劃出的路徑更加平滑，有效克服了局部最優(yōu)問題，從而GSACS算法規(guī)劃出的路徑質(zhì)量更優(yōu). 2種算法路徑尋優(yōu)的試驗(yàn)數(shù)據(jù)如表3所示.

由表3可知，目標(biāo)點(diǎn)1下ACS算法和GSACS算法尋路時(shí)間分別為484.8 ms和340.6 ms，GSACS算法的尋路效率提升了約29.7%.目標(biāo)點(diǎn)2下ACS算法和GSACS算法的尋路時(shí)間分別為703.4 ms和532.1 ms，GSACS算法尋路效率提升了約24.3%. 由上述分析可知，GSACS算法相比于ACS算法，尋路效率更高、路徑質(zhì)量更優(yōu).

5 ? 結(jié) ? 論

本文針對(duì)ACS算法在路徑規(guī)劃時(shí)存在收斂速度慢、易陷入局部最優(yōu)、路徑轉(zhuǎn)折點(diǎn)數(shù)量多等缺點(diǎn)，提出了一種基于GSA搜索策略的改進(jìn)ACS算法. 該算法首先引入了一種簡化的ACS算法，對(duì)初始信息素濃度進(jìn)行更新，減少了蟻群尋路時(shí)的盲目性、提高了算法前期的收斂速度;其次引入了GSA搜索策略，對(duì)螞蟻尋找后續(xù)行走節(jié)點(diǎn)的策略進(jìn)行了改進(jìn)，提高了算法的收斂速度;最后提出了路徑優(yōu)化方法，減少了規(guī)劃路徑的轉(zhuǎn)折點(diǎn)數(shù)量，提升了路徑的光滑度. 仿真試驗(yàn)結(jié)果表明，改進(jìn)算法相比于ACS算法，收斂速度更快、路徑質(zhì)量更優(yōu)化、尋路效率更高. 導(dǎo)航實(shí)驗(yàn)結(jié)果得出，改進(jìn)后的GSACS算法能夠有效解決路徑規(guī)劃任務(wù)，且機(jī)器人導(dǎo)航效率要優(yōu)于ACS算法.

參考文獻(xiàn)

[1] ? ?霍鳳財(cái)，遲金，黃梓健，等. 移動(dòng)機(jī)器人路徑規(guī)劃算法綜述[J]. 吉林大學(xué)學(xué)報(bào)（信息科學(xué)版），2018，36（6）：639—647.

HUO F C，CHI J，HUANG Z J，et al. Review of path planning for mobile robots[J]. Journal of Jilin University（Information Science Edition），2018，36（6）：639—647.（In Chinese）

[2] ? ?DIJKSTRA E W. A note on two problems in connexion with graphs[J]. Numerische Mathematik，1959，1（1）：269—271.

[3] ? ?HART P E，NILSSON N J，RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics，1968，4（2）：100—107.

[4] ? ?KHATIB O. Real-time obstacle avoidance system for manipulators and mobile robots[J]. International Journal of Robotics Research，1986，5（1）：90—98.

[5] ? ?李敏，黃敏，程智鋒，等. 遺傳算法在路徑規(guī)劃上的應(yīng)用[J]. 計(jì)算機(jī)系統(tǒng)應(yīng)用，2020，29（8）：255—260.

LI M，HUANG M，CHENG Z F，et al. Application of genetic algorithm in path planning[J]. Computer Systems & Applications，2020，29（8）：255—260. （In Chinese）

[6] ? ?宋道軍，王杰，虎嘯，等. 改進(jìn)粒子群算法的無功補(bǔ)償方案優(yōu)化以及對(duì)配電網(wǎng)電能質(zhì)量的改善[J]. 電測與儀表，2020，57（18）：18—23.

SONG D J，WANG J，HU X，et al. Optimization of reactive power compensation scheme based on improved particle swarm optimization algorithm and improvement of power quality of distribution network[J]. Electrical Measurement & Instrumentation，2020，57（18）：18—23.（In Chinese）

[7] ? ?KARABOGA D，BASTURK B. A powerful and efficient algorithm for numerical function optimization：artificial bee colony （ABC）algorithm[J]. Journal of Global Optimization，2007，39（3）：459—471.

[8] ? ?DORIGO M，MANIEZZO V，COLORNI A. Ant system：optimization by a colony of cooperating agents[J]. IEEE Transactions on Systems，Man，and Cybernetics，Part B，1996，26（1）：29—41.

[9] ? ?DORIGO M，GAMBARDELLA L M. Ant colony system：a cooperative learning approach to the traveling salesman problem[J]. IEEE Transactions on Evolutionary Computation，1997，1（1）：53—66.

[10] ?鮑文杰，朱信忠，趙建民，等. 加權(quán)值多態(tài)蟻群算法[J]. 軟件工程，2016，19（4）：1—4.

BAO W J，ZHU X Z，ZHAO J M，et al. Weighted value polymorphic ant colony algorithm[J]. Software Engineering，2016，19（4）：1—4. （In Chinese）

[11] ?朱艷，游曉明，劉升，等. 基于改進(jìn)蟻群算法的機(jī)器人路徑規(guī)劃問題研究[J].計(jì)算機(jī)工程與應(yīng)用，2018，54（19）：129—134.

ZHU Y，YOU X M，LIU S，et al. Research for robot path planning problem based on improved Ant Colony System（ACS）algorithm[J]. Computer Engineering and Applications，2018，54（19）：129—134.（In Chinese）

[12] ?張成，凌有鑄，陳孟元. 改進(jìn)蟻群算法求解移動(dòng)機(jī)器人路徑規(guī)劃[J]. 電子測量與儀器學(xué)報(bào)，2016，30（11）：1758—1764.

ZHANG C，LING Y Z，CHEN M Y. Path planning of mobile robot based on an improved ant colony algorithm[J]. Journal of Electronic Measurement and Instrumentation，2016，30（11）：1758—1764.（In Chinese）

[13] ?胡澮冕，于修成. 基于雙向搜索策略的改進(jìn)蟻群路徑規(guī)劃算法[J]. 農(nóng)業(yè)裝備與車輛工程，2019，57（7）：9—12.

HU H M，YU X C. Improved ant colony path planning algorithm based on bidirectional search strategy[J]. Agricultural Equipment & Vehicle Engineering，2019，57（7）：9—12. （In Chinese）

[14] ?周茂杰，張翠. 基于改進(jìn)蟻群算法的旅游線路優(yōu)化[J]. 現(xiàn)代計(jì)算機(jī)，2018（15）：24—27.

ZHOU M J，ZHANG C. Tourism route optimization based on improved ant colony algorithm[J]. Modern Computer，2018（15）：24—27. （In Chinese）

[15] ?張馳，李?yuàn)檴?，史顏俊，? 蟻群-勢場算法在水下重力輔助導(dǎo)航航跡規(guī)劃中的應(yīng)用[J]. 測繪學(xué)報(bào)，2020，49（7）：865—873.

ZHANG C，LI S S，SHI Y J，et al. Application of ant colony-potential field algorithm in underwater gravity matching navigation track planning[J]. Acta Geodaetica et Cartographica Sinica，2020，49（7）：865—873. （In Chinese）

[16] RASHEDI E，NEZAMABADI-POUR H，SARYAZDI S. GSA：a gravitational search algorithm[J]. Information Sciences，2009，179（13）：2232—2248.

收稿日期：2020-09-22

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目（61472282），National Natural Science Foundation of China（61472282）;安徽高校自然科學(xué)研究重點(diǎn)項(xiàng)目（KJ2019A0065），Key Projects of Natural Science Research in Anhui Universities（KJ2019A0065）;安徽省科技重大專項(xiàng)項(xiàng)目（202003a05020028），Major Special Projects of Science and Technology in Anhui Province（202003a05020028）;特種重載機(jī)器人安徽省重點(diǎn)實(shí)驗(yàn)室開放課題（ZJQR004-2020），Open Project of Anhui Province Key Laboratory of Special and Heavy Load Robot（ZJQR004-2020）

作者簡介：馬小陸（1979—），男，安徽蕪湖人，安徽工業(yè)大學(xué)副教授，博士

通信聯(lián)系人，E-mail：77578249@qq.com