基于改進AG算法的機器人動態路徑規劃方法

王楠 張軍 解鵬

摘要：為了充分發揮Agoraphilic（AG）算法的優越性，使其可以在動態環境中有效地進行路徑規劃，對傳統AG算法進行了研究和改進，在計算自由空間力時增加了機器人和動態障礙物之間的相對速度分量，該分量可分解為2個方向的分力，一個分力使機器人向背離障礙物的方向運動，另一個分力使機器人向垂直于障礙物的方向運動，充當機器人繞行的動力。利用Matlab進行了仿真實驗，將改進的AG算法和幾種其他動態路徑規劃方法進行了對比。改進后的AG算法使機器人能夠迅速躲避動態障礙物，有效地進行動態避障。研究方法不僅可以解決動態環境中機器人躲避動態障礙物并到達目標點的問題，而且與其他動態路徑規劃算法相比，具有路徑長度更短、耗時更少、路徑更平滑等優點。

關鍵詞：計算機仿真； 移動機器人； 路徑規劃； 動態環境； 改進AG算法； 動態避障

中圖分類號：TP399文獻標志碼：Adoi： 10.7535/hbgykj.2018yx03005

隨著科技的迅猛發展，移動機器人越來越普遍地出現在人們的生活中，在深海作業、消防作業、航空航天、國防部門、娛樂場所甚至醫院、銀行等地方都能看到它們的身影。移動機器人眾多研究領域中最基本同時也是最重要的一個領域就是路徑規劃問題。路徑規劃要完成的任務是找到一條從起點到終點的最優路徑，同時需要避開環境中的障礙物，這是機器人智能化的體現[1-2]。由于機器人的工作環境通常是動態不確定的，所以機器人動態路徑規劃也成為當前研究的熱點。

目前根據對各領域常用路徑規劃算法的研究，將動態環境下機器人路徑規劃方法分為傳統路徑規劃方法和智能仿生路徑規劃方法，傳統算法主要有人工勢場法[3-4]、模糊邏輯法、可視圖法等，人工勢場法的優點是結構簡單、計算量小、實時性好， 在實時避障和平滑的軌跡控制方面應用廣泛，但是機器人易出現合力為零、陷入局部極小點的問題；模糊邏輯法的優點是魯棒性較強，并且在環境中存在不確定性時便于與其他方法融合，但是需要人工經驗的支撐，而且在計算過程中模糊表和推理的規則可能出現迅速膨脹，影響算法性能；可視圖法的優點是方便簡易，缺點是若環境中起點和目標點改變則可視圖便不再適用，因此靈活度低。智能仿生算法也叫生物啟發式算法，是根據生物在復雜的動態環境中尋找食物或遷徙中得到的啟發，主要有蟻群算法[5-6]、遺傳算法、粒子群算法等，蟻群算法的優點是采用分布式并行運算機制，具有很強的魯棒性和適應性，且很容易與其他算法相結合，但是易出現停滯現象，或者陷入局部最優解；遺傳算法的優點是在全局搜索上顯示了明顯優勢，計算量小，缺點是在局部搜索上局限性較大，劣勢較明顯；粒子群算法的優點是能快速收斂，算法簡單、魯棒性強，但是易于陷入局部最優，并且若算法中的參數設置不合適可能喪失粒子的多樣性。綜上所述，傳統方法和智能算法仍存在改進的空間。

由IBRAHIM等[7]提出的Agoraphilic（AG）算法是一種基于虛擬立場技術（VFF）的反應性的移動機器人路徑規劃算法，文獻＼[8＼]中論述了它的優點是只使用一個單一的由周邊自由空間產生的吸引力控制機器人運動，所以避免了局部最優解的問題；隨后MCFETRIDGE等[9]論證了通過該算法進行導航可以產生更平滑的路徑，實現了更高的平移速度且避免路徑振蕩；HONG等[10]也提到AG算法的目標是為機器人尋找一個開放的可前行空間而不是單純避障，從而進一步確保了機器人運行的安全。但是動態環境中動態障礙物的運動速度和方向具有不確定性，機器人在避障的過程中可能會與障礙物發生碰撞，上述使用該算法的文獻并沒有考慮這個問題，這使得該算法的應用領域一直局限于靜態環境中，直至2015年BILBEISI等[11]進一步論證了AG算法是一種反應性移動機器人路徑規劃機制，計算復雜性低，為充分發揮其優越性本文對基本AG算法進行改進使其更適合于實時動態導航，通過增加相對速度分量最終使其能夠完成在動態環境中的路徑規劃，仿真實驗證明了改進算法具有可行性，并通過對比實驗證明了該方法較傳統動態人工勢場法和智能動態蟻群算法均具有路徑長度更短、更平滑、可視性更強等優勢。

1基本AG算法

Agoraphilic（AG）算法是一種基于虛擬力場技術（VFF）[12]的反應性移動機器人路徑規劃算法。該算法是勢場法的一種形式，它有別于傳統的經典人工勢場法，使用自由空間力（FSF）作為吸引力控制機器人向環境中的自由空間運動。

AG算法的步驟概括如下：首先初始化參數，機器人在路徑規劃中不斷利用傳感器檢測周圍環境信息[13]，算法中使用直方圖來存儲本地環境地圖，該直方圖中包含著某些確定的值，表示某個對象例如障礙物等在特定位置出現的可能性，然后計算直方圖中每個扇區的期望和力的值，每個扇區的力正比于距離值的平方，最終根據機器人所受的合力及方向角的值確定機器人下一步要走的位置，更新機器人的位置。

AG算法中最重要的階段為計算機器人所受的合力，現設λk為自由空間直方圖（FSH）中第k個扇區的期望，dk為扇區k中的距離值，ρ0為障礙物影響距離，即障礙物在該范圍內會對機器人產生影響，ρ（XR，Xi）為障礙物到機器人的距離，dobs為機器人到障礙物的距離，Δk為扇區k方向的單位矢量，k為當前的扇區，K為FSH中扇區的總數，使用下述公式計算每個單獨扇區的力：

FK=（λkdk）2×Δk，k=1，2，…，K， （1）

dk=dobs（ρ（XR，Xi）≤ρ0），dmax（ρ（XR，Xi）>ρ0）。 （2）

將各個扇區的力匯總，作用在機器人上的合力為

FFs=∑Kk=1FK。 （3）

用FFsx表示合力在x軸方向分量，FFsy表示合力在y方向分量，機器人當前的前進角度為

θFs=tan-1FFsyFFsx。 （4）

由于上述基本AG算法只考慮了障礙物是靜態的情況，當環境中出現動態障礙物時，動態障礙物的運動速度和運動方向具有不確定性[14-15]，當動態障礙物的運動軌跡與機器人運動軌跡出現交點時可能會發生碰撞的情況，上述算法并不適用，所以在AG算法的基礎上進行改進，以使其能夠用于動態環境中的機器人路徑規劃。

2改進的AG算法

2.1基本原理

在基本AG算法的自由空間力中加入機器人和障礙物的相對速度分量，該相對速度分量的增加可以起到使機器人遠離障礙物運動的作用，從而使機器人能夠迅速躲避動態障礙物，有效地進行動態避障。圖1為增加速度分量后機器人受到的總的合力。

用VR表示機器人的速度，Vi表示動態障礙物的速度，qi為障礙物所在位置，q為機器人當前位置，VRi表示機器人與障礙物的相對速度在兩者連線上的分量：

VRi=（VR-Vi）TΔRi。 （5）

動態障礙物與機器人之間會產生與速度及位置有關的勢場：

U（qi，Vi）=ηVVRi， （6）

F=-ΔU。 （7）

根據式（6）和式（7）可以推出：

ΔVVRi=ΔRi， （8）

ΔqVRi=VRiΔRi-（VR-Vi）‖qi-q‖。 （9）

用VRi⊥ΔRi⊥表示與VRi垂直方向的速度：

VRi⊥ΔRi⊥=VR-Vi-VRi， （10）

FV1=ηV1ΔiR， （11）

FV2=ηV2×VRi⊥ΔRi⊥ρ（XR，Xi）。（12）

式中：ηV為速度增益系數，表明速度分量影響大小，則增加的相對速度分量可看成由FV1和FV2組成，其中FV1的方向為障礙物和機器人連線上由障礙物指向機器人的方向，該分力使機器人向背離障礙物的方向運動，FV2的方向為與機器人和障礙物連線垂直的方向，該分力使機器人向垂直于障礙物的方向運動，充當機器人繞行的動力。

最終增加的相對速度分量為

FV=FV1+FV2。 （13）

計算出作用在機器人上的合力如式（14）所示：

F′Fs=FFs+FV，VRi>0，FFs，VRi≤0。 （14）

當VRi>0時表示障礙物朝向機器人運動，此時應該采取避障措施，當VRi≤0時表示障礙物遠離機器人運動，此時不用采取避障措施。機器人當前的前進角度為

θ′Fs=tan-1F′FsyF′Fsx。 （15）

2.2算法描述

綜上所述，加入相對速度分量后具體的改進AG算法流程圖如圖2所示。

改進AG算法的實現步驟如下。

1）參數初始化，包括設置機器人的起點和目標點的位置坐標、靜態障礙物的位置坐標、直方圖初始距離值等。

2）機器人根據傳感器獲取周圍環境信息和動態障礙物的速度，同時計算到障礙物的距離，生成圖3所示自由空間直方圖（FSH）。在直方圖的每個扇區中包含著機器人在特定方向的距離信息，提供了一個距離指示，該機器人在發生碰撞之前可在給定的方向移動。

生成自由空間直方圖之前的步驟是用最大的距離值在直方圖內初始化每個扇區，該距離值可以從下列2個值中選取，第1個為環境中有源區域的尺寸，以使機器人最初假定它被定位在開放區域，該有源區域的最大距離是機器人活動窗口的半徑raw；第2個距離值是當目標在機器人視覺范圍內（FOV）出現時機器人當前位置到目標的距離dgoal，該值的設置是為了使機器人在接近障礙物的情況下仍然可以到達目標。每個扇區的最后初始化值是通過式（16）給定的，

dmax=min（raw，dgoal），目標點在FOV內；raw，其他。 （16）

3） 計算直方圖中每個扇區的期望。如果沒有期望的影響，機器人將試圖找到最大的自由空間并且直接朝其前進而不管目標在哪，因此期望的增加是為了使機器人能夠朝向目標運動。一個扇區的期望不僅是可用的自由空間區域的大小，同時也包括如何使扇形角更接近目標[16]。所以扇區中最大的期望將會在具有可用的最大自由空間并且朝向目標的扇區中產生，基本AG算法期望采用固定值，本文中使用式（17）來計算期望：

λ=ηλ×1ρ（XR，Xg）， （17）

式中：ηλ為期望增益系數，表明期望的影響大小；ρ（XR，Xg）為機器人到目標的距離，通過式（17）計算的期望會使得機器人朝向目標點運動，且機器人離目標的距離越近該期望值越大，機器人朝向目標運動的動力也越大，從而使機器人最終到達目標點。

4）計算每個扇區的力，初始的力正比于每個扇區中的距離值，加入相對速度分量后通過式（14）和式（15）計算機器人所受合力及當前方向角。

5）根據所受的合力及方向角的值確定機器人下一步要走的位置，更新機器人的坐標。

6）判斷機器人是否到達目標點作為終止算法的條件。

3對比實驗及結果分析

使用Matlab進行仿真實驗，采用Windows操作系統，CPU為1.70 GHz、內存為4 GB。

3.1基本AG算法與改進AG算法對比實驗

首先將基本AG算法和改進AG算法在具有靜態和動態障礙物的環境中進行對比實驗，地圖的尺寸為10×10，靜態障礙物的分布相同，機器人起始點為（0，0），目標點為（10，10），機器人活動窗口半徑r=5，障礙物影響范圍大小ρ0=2.5，期望增益系數ηλ=500，機器人的速率均為2，動態障礙物T1的速率為1.18，動態障礙物T2的速率為1。

使用基本AG算法進行仿真實驗時機器人路徑如圖4所示，其中小圓圈代表靜態障礙物，障礙物T1會和機器人發生碰撞，交點位置為（2.875，1.625）。

采取改進的AG算法進行路徑規劃如圖5所示，可以看出改進后的算法能夠有效避開動態障礙物T1。

3.2改進AG算法與動態蟻群、動態人工勢場法對比實驗

采用動態蟻群算法進行路徑規劃時主要參數為蟻群數量m，啟發因子α和期望啟發因子β，這些參數的設置對結果起到重要的影響。本文中參數的優化配置過程如下：首先分析蟻群數量對結果的影響，設置α=1，β= 5，分別選擇螞蟻數目m為10，30，50，80，100，150進行仿真實驗。結果表明：當螞蟻數量為80時，最優路徑長度與收斂迭代次數均為最小值；啟發因子α對算法的影響為α越高，螞蟻越傾向于選擇其他螞蟻所經過的路徑，期望啟發因子β越高螞蟻更傾向選擇更近的路徑點。仿真時，當信息啟發因子α過小時收斂速度慢，α過大時算法會出現過早收斂，螞蟻很難找到最優路徑，因此取最優組合為α=1，β=7。

綜上所述，設置動態蟻群算法的參數：蟻群數目m=80，啟發因子α=1，期望啟發因子β=7，迭代次數N=100進行仿真實驗。

采用動態人工勢場法進行路徑規劃時，取障礙物影響距離ρ0=2.5，機器人起始點為（0，0），目標點為（10，10）。

進行對比實驗時環境中存在固定參數和隨機設置的變量，固定的參數為地圖尺寸、機器人初始位置、目標位置、機器人初始速率，表1中列出了這些參數的值。環境中的變量為靜態障礙物的個數及位置，動態障礙物的個數、初始位置、速率、運動方向，在實驗過程中這些變量隨機取值。

參數名稱地圖尺寸機器人

初始位置目標點機器人初始速率/

（柵格邊長·s-1）參數值10×10（0，0）（10，10）2

在該環境下分別用改進AG算法、動態蟻群算法、動態人工勢場法進行20組隨機實驗，得到不同的機器人路徑長度和運行時間，得到的統計結果如表2所示。

表3對兩組隨機實驗中變量的取值進行了說明。圖6、圖7從左至右分別為兩組不同的隨機實驗環境里采用改進AG算法、動態蟻群算法、動態人工勢場法得到的路徑規劃。

表4列出了兩組隨機實驗的運行結果。

隨機實驗算法名稱路徑長度/柵格邊長運行時間/s改進AG算法15.557.78實驗1動態蟻群算法16.588.29動態人工勢場法16.918.46改進AG算法15.397.70實驗2動態蟻群算法16.498.25動態人工勢場法17.198.60

通過上述對比實驗和統計表格可知：相比于傳統AG算法，改進AG算法可以進行動態避障且最終到達目標點；而且相比于其他動態路徑規劃算法，20組對比實驗中改進AG算法得到的實驗結果優于動態蟻群算法的概率為90%，優于動態人工勢場法的概率為95%，即采取改進AG算法進行動態路徑規劃得到的路徑長度更短、耗時更少，同時從實驗路徑圖中可以看出，改進AG算法得到的路徑更加平滑，可視性更高。

4結語

介紹了解決機器人路徑規劃的一種基于勢場的AG算法，該方法通過自由空間力來控制機器人使其運動。為了解決動態環境中機器人路徑規劃的問題，本文對基本的AG算法進行了改進，在計算機器人所受的最終合力時增加了相對速度分量，該分量的增加能夠使機器人遠離障礙物運動，實現動態避障。仿真結果表明了該方法的可行性，可以使機器人找到一條從起點到終點并且避開靜態和動態障礙物的路徑，并且通過對比實驗表明了該方法具有優勢，即較其他動態路徑規劃方法路徑長度更短、耗時更少。

本文的研究仍然存在不足之處。首先對于AG算法的改進和創新仍有許多可提升和努力的空間；其次本文研究的是單個機器人的運動，在更為復雜的環境中如果需要多機器人共同完成困難的任務，路徑規劃的難度將大大增加，所以多機器人系統的路徑規劃問題仍需進一步研究；最后隨著機器人應用領域的不斷擴大，其應用領域逐漸從低維空間擴展到高維空間，因此高維空間機器人路徑規劃仍是目前研究的熱點和難點，例如將機器人放入三維空間中。未來機器人路徑規劃研究可朝這些方向繼續努力。

參考文獻/References：

[1]DELMERICO J， MUEGGLER E， NITSCH J， et al. Active autonomous aerial exploration for ground robot path planning[J]. IEEE Robotics and Automation Letters， 2017， 2（2）： 664-671.

[2]張啟飛，郭太良.基于多階段決策的機器人全局路徑規劃算法[J].計算機工程， 2016，42（10）：297-302.

ZHANG Qifei， GUO Tailiang. Global path planning algorithm of robot based on multistage decision[J]. Computer Engineering， 2016，42（10）：297-302.

[3]溫素芳，郭光耀.基于改進人工勢場法的移動機器人路徑規劃[J].計算機工程與設計，2015，36（10）：2819-2822.

WEN Sufang， GUO Guangyao. Path planning of mobile robot based on improved artificial potential field approach[J].Computer Engineering and Design， 2015，36（10）：2819-2822.

[4]丁家如，杜昌平，趙耀，等. 基于改進人工勢場法的無人機路徑規劃算法[J].計算機應用，2016， 36（1）：287-290.

DING Jiaru， DU Changping， ZHAO Yao， et al. Path planning algorithm for unmanned aerial vehicles based on improved artificial potential field[J]. Computer Applications， 2016，36（1）：287-290.

[5]屈鴻，黃利偉，柯星.動態環境下基于改進蟻群算法的機器人路徑規劃研究[J].電子科技大學學報，2015，3（2）：261-265.

QU Hong， HUANG Liwei， KE Xing. Research of improved ant colony based robot path planning under dynamic environment[J]. Journal of University of Electronic Science and Technology of China， 2015， 3（2）：261-265.

[6]張成，凌有鑄，陳孟元.改進蟻群算法求解移動機器人路徑規劃[J].電子測量與儀器學報，2016，30（11）： 1759-1764.

ZHANG Cheng， LING Youzhu， CHEN Mengyuan. Path planning of mobile robot based on an improved ant colony algorithm[J]. Journal of Electronic Measurement and Instrumentation， 2016， 30（11）：1759-1764.

[7]IBRAHIM M Y， MCFETRIDGE L. The agoraphilic algorithm： A new optimistic approach for mobile robot navigation[C]//2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Como：IEEE，2001：1334-1339.

[8]MCFETRIDGE L， IBRAHIM M. Behavior fusion via free-space force shaping[J]. IEEE International Conference on Industrial Technology， 2003， 2：818-823.

[9]MCFETRIDGE L， IBRAHIM M. A new methodology of mobile robot navigation： The agoraphilic algorithm[J]. Robotics and Computer-Integrated Manufacturing， 2009， 25（3）：545-551.

[10]HONG J， PARK K. A new mobile robot navigation using a turning point searching algorithm with the consideration of obstacle avoidance[J].The International Journal of Advanced Manufacturing Technology， 2011， 52：763-775.

[11]BILBEISI G， AL-MADI N， AWAD F. PSO-AG： A multi-robot path planning and obstacle avoidance algorithm[C]// 2015 IEEE Jordan Conference on Applied Electrical Engineering and Computing Technologies. Amman， IEEE， 2015： 1-6.

[12]RASHID M B， SUNDARESAN S， JAYASEKERA T， et al. VFF-Monte Carlo framework for phonon transport in nanostructures[C]//International Workshop on Computational Electronics. West Lafayette： IEEE， 2015：1-2.

[13]HWU T， WANG A，OROS N， et al. Adaptive robot path planning using a spiking neuron algorithm with axonal delays[J]. IEEE Transactions on Cognitive & Developmental Systems， 2016（99）：1-1.

[14]黃立新，耿以才.基于動態人工勢場法移動機器人路徑規劃研究[J].計算機測量與控制，2017， 25（2）： 164-166.

HUANG Lixin， GENG Yicai. Robot path planning based on dynamic potential field method [J].Computer Measurement & Control， 2017， 25（2）： 164-166.

[15]JIANG M， CHEN Y， ZHENG W， et al. Mobile robot path planning based on dynamic movement primitives [C]//Control Conference （CCC）. Dalian： IEEE， 2016：980-985.

[16]黃海衛，孔令成，譚治英.室內服務機器人實時目標識別與定位系統設計[J].計算機工程與設計，2016，37（8）：2229-2232.

HUANG Haiwei， KONG Lingcheng， TAN Zhiying. Real-time object recognition and localization system for indoor service robot [J]. Computer Engineering and Design， 2016， 37（8）：2229-2232.第35卷第3期河北工業科技Vol.35，No.3