基于改進RRT算法的AGV路徑規(guī)劃研究

胡兵　向鳳紅　毛劍琳

摘要：路徑規(guī)劃是自動導引小車（AGV）控制中的核心問題之一。針對經典RRT算法在靜態(tài)全局狀態(tài)空間中隨機采樣搜索節(jié)點時隨機性大與效率低的問題，提出了一種改進的RRT（快速搜索隨機樹）路徑規(guī)劃算法。該算法結合雙向搜索功能與自適應目標引力思想，利用雙向搜索速度快與自適應目標引力朝目標點方向生長的特性，使AGV在規(guī)劃路徑時路徑搜索效率更高，路徑更平滑。實驗仿真結果證明，改進的RRT算法可以在有效提高路徑搜索效率的同時生成最優(yōu)路徑。

關鍵詞：路徑規(guī)劃；自動導引小車；RRT；雙向搜索；自適應目標引力

DOIDOI：10.11907/rjdk.172598

中圖分類號：TP311

文獻標識碼：A文章編號文章編號：16727800（2018）003002804

英文摘要Abstract：The path planning of automatic guided vehicle is one of the core issues in the control. For the classical RRT algorithm in global static random sampling in the state space search node is random and low efficiency problem， we propose an improved RRT （rapidly randomexploring trees） path planning algorithm. This algorithm combines the two the search function and adaptive target gravity theory， using the twoway search speed and characteristics of adaptive target gravity toward the target growth path， AGV path planning in higher searching efficiency and smoother paths. The experimental simulation results. The results show that the improved RRT algorithm can effectively improve the path search efficiency and generate the optimal path at the same time.

英文關鍵詞Key Words：path planning； AGV； RRT； bidirectional search； adaptive target gravity

0引言

自動導引小車（Automatic Guided Vehicles，AGV）作為物流業(yè)、制造業(yè)、機場、軍事以及煙草業(yè)等行業(yè)中的自動化柔性制造設備，得到了越來越多的關注[1]。路徑規(guī)劃是指機器人在當前環(huán)境中按照一定標準搜索出一條從起始狀態(tài)點到目標狀態(tài)點，并且能夠繞開障礙物的最優(yōu)或次優(yōu)路徑，是自動導引小車控制中的核心問題之一[2]。AGV在運行環(huán)境中會遇到包括充電樁、工作站及存貯站等障礙物，而其主要任務就是避障及路徑規(guī)劃，最后生成一條暢通的路徑。

AGV在運行過程中除需要獲取周圍信息避開障礙物外，還需要使規(guī)劃出的路徑長度和時間更短，效率更高。許多國內外學者正在研究AGV的路徑規(guī)劃問題，研究出許多可行方法，包括人工勢場法、遺傳算法、神經網絡法及RRT算法等[3]。其中，RRT算法由Steven M LaValle[4]于1998年首先提出，為了進一步優(yōu)化路徑，后來學者在其基礎上進行改進后提出偏向RRT[5]、雙向RRT[67]等算法。雙向RRT算法在一定程度上解決了經典RRT算法在搜索路徑時速度慢的問題，而隨機性大的問題未能解決。

針對經典RRT算法在靜態(tài)全局狀態(tài)空間中隨機采樣搜索節(jié)點時隨機性大與效率低的問題，本文提出一種將雙向搜索功能與自適應目標引力相結合的RRT算法，即在雙向RRT算法的基礎上加入人工勢場法中的目標引力思想策略[8]，并使目標引力具有自適應周圍環(huán)境的功能。借助自適應目標引力讓隨機搜索樹不僅能夠朝著目標點方向擴展生長，還能順利地避開障礙物，在此過程中無需建模和對全局空間隨機采樣。改進后的RRT算法能夠在AGV路徑規(guī)劃時提高路徑搜索效率和避障能力。

1問題描述

AGV的運行環(huán)境較為復雜，包括靜態(tài)環(huán)境與動態(tài)環(huán)境[9]，本文針對靜態(tài)環(huán)境對AGV的規(guī)劃路徑進行優(yōu)化。靜態(tài)環(huán)境是指AGV運行路線周圍屬于恒態(tài)環(huán)境，包括車間柱子、設備、噪音及電磁干擾等。它影響到AGV行走路徑的選擇及AGV的穩(wěn)定性。運行環(huán)境（狀態(tài)空間）可以分為障礙空間與自由空間。為了避開障礙物規(guī)劃出一條合理路徑，AGV在障礙空間應放慢運行速度，并改變方向避開障礙物；在自由空間應加快運行速度向目標點生長。因工作環(huán)境不同，AGV的形狀大小會不一樣，為了研究需要可設AGV為一圓點狀，空間中的障礙物形狀不一，包括車間柱子、設備等，可隨機設置。AGV所處的虛擬工作環(huán)境可定義為如圖1所示，此時在AGV的路徑規(guī)劃中需解決的兩個核心問題如下：

（1）效率問題。AGV在全局空間中進行路徑規(guī)劃，需要獲取全局環(huán)境信息，增加了算法的時間復雜度。

（2）避障問題。障礙物是AGV在運行中必不可少的環(huán)境因素，AGV不能合理地避開障礙物，將不能順利到達目標點，最終無法規(guī)劃出一條通暢的路徑。

2AGV路徑規(guī)劃優(yōu)化算法

AGV的路徑規(guī)劃問題與許多因素有關，包括時間、空間等，經典RRT算法不能很好地滿足問題的需求。因此，針對該問題，本文提出一種基于自適應目標引力的雙向RRT路徑規(guī)劃算法。

2.1經典RRT算法

經典RRT算法是從狀態(tài)空間[10]中的一個初始點出發(fā)，通過隨機采樣擴展增加新節(jié)點的方式生成一個隨機擴展樹，當隨機樹中的新節(jié)點包含了目標點或進入了目標區(qū)域時，初始點到目標點間將至少形成一條以隨機樹新節(jié)點構成的路徑。

假設AGV的工作空間屬于二維，系統(tǒng)的狀態(tài)空間為C，從初始點Xinit開始來搜索擴展隨機樹T，對隨機樹T進行逐步擴展構建，其構建過程如圖2所示。

2.2引入目標引力的雙向RRT算法

經典RRT算法在AGV路徑規(guī)劃中，從初始點到目標點隨機搜索生成的路徑避障能力很強，但搜索效率低。針對AGV路徑規(guī)劃問題（1），本文在經典RRT算法基礎上提出引入目標引力的雙向RRT算法。

Step1：分別以初始點和目標點為起點同時生成隨機樹Ti和Tj。當兩棵樹擴展生長后于某一點相遇時，路徑產生。算法在初始點Xinit和Xgoal同時開始構造隨機樹，從任意一個隨機樹中選取與Xrand距離最近的節(jié)點Xnear，在Xrand與Xnear的連線上找到一個距離Xrand最近的點Xnew，將其加入隨機樹中。同時再尋找另一個隨機樹中距離Xnew最近的點，在擴展過程中試圖用相同的算法將兩樹連接起來，若兩樹中的兩節(jié)點距離足夠小或重合，則可確定Ti與Tj連通，基本算法如下所示：

RRT_BCV（Xinit，Xgoal）

1： Ti（Xinit），Tj（Xgoal）

2： for k=1 to K do

3： Xrand=random_state（）；

4： if not（BCV_CONNECT（Ti，Xrand）=trapped） then

5： if（BCV_CONNECT（Tj，Xnew）=reached） then

6： Return PATH（Ti，Tj）；

7： swap（Ti，Tj）；

8： Return（Ti，Tj）；

BCV_CONNECT（T，X）

1： Repart

2： Xnear=nearest_neighbor（Xnear，T）；

3： u=select_input（Xrand，Xnear）；

4： Xnew=new_state（Xnear，u，t）；

5： if collision（Xnew）

6： continue；

7： T.add.vertex（Xnew）；

8： T.add.edge（Xnear，Xnew）；

在隨機樹Ti與Tj的生長過程中，函數(shù)random_state（）在狀態(tài)空間C中隨機選取一點Xrand；函數(shù)nearest_neighbor（）在當前搜索樹上選取離Xrand距離最近的節(jié)點Xnear來擴展；函數(shù)select_input（）在Xrand與Xnear結合下得到輸入U，根據(jù)給定的標準，在輸入U中選擇一個輸入，使得Xnear盡可能接近Xrand，生成一個節(jié)點Xnear；函數(shù)new_state（）得到新節(jié)點Xnew。每次擴展得到新節(jié)點后均要判斷其是否在障礙區(qū)域內，若是，則返回到for循環(huán)重新選取新的隨機點，若否，則直接將其加入當前樹中，當加入的新節(jié)點與目標點間的距離足夠小或重合時路徑搜索結束，生成規(guī)劃路徑。

雙向RRT算法具有路徑搜索速度較快和隨機性大的特點[11]。若隨機性大的問題繼續(xù)存在，隨機樹生成的路徑會缺少光滑性，路徑搜索效率不高，而且多次規(guī)劃后得到的路徑不盡相同，也不一定為最優(yōu)路徑。

Step2：對于雙向RRT算法隨機性大的問題，許多學者提出了不同的方法，本文將人工勢場法中的目標引力思想加入到雙向RRT算法，其功能是確定目標后使隨機樹朝著目標點或目標區(qū)域擴展生長。該算法只需在局部進行隨機采樣，不僅減小了雙向RRT算法的隨機性，而且提高了算法的路徑搜索效率，改善了路徑的光滑性，確保規(guī)劃出最優(yōu)路徑。

引入目標引力思想的雙向RRT算法在規(guī)劃路徑時，其關鍵方法是在路徑從初始點隨機擴展后的每一個節(jié)點處都引入一個目標引力函數(shù)，新節(jié)點計算方法如式（1）所示。

Xnew=Xnear+ρXrand-Xnear‖Xrand-Xnear‖+Xgoal-Xnear‖Xgoal-Xnear‖（1）

其中，ρ為朝著隨機點方向生長的步長，k為引力場系數(shù)，k與ρ之積kρ為朝著目標點方向生長的步長。

引入目標引力思想后的雙向RRT算法有效地減小了路徑規(guī)劃時的隨機性。與雙向RRT算法相比，該算法不僅具有原來隨機方向的隨機點Xrand，還增加了目標區(qū)域方向的目標點Xgoal，目標點Xgoal是新節(jié)點Xnew生成的關鍵影響因素，新節(jié)點Xnew的位置會隨著步長的變化而變化。當ρ>kρ時，新節(jié)點將朝著隨機點Xrand方向生長，此時Xrand跟雙向RRT算法的隨機點的選取很接近，具有很大的隨機性，強避障能力得以彰顯；當ρ

AGV在實際應用中所處的工作環(huán)境較為復雜。在現(xiàn)實環(huán)境的工作空間從初始點到目標點路徑規(guī)劃時，障礙物是不可避免的因素，無障礙物只是理想環(huán)境。引入目標引力思想的雙向RRT算法適用于無障礙的理想環(huán)境與少障礙物的環(huán)境，當遇到多障礙物的復雜工作環(huán)境時，因沒有雙向RRT算法一樣大的隨機性，可能不能順利地繞開障礙物，從而不能規(guī)劃出有效路徑。

2.3引入自適應目標引力策略

引入目標引力的雙向RRT路徑規(guī)劃算法的AGV在有障礙物的環(huán)境中可能不能規(guī)劃出合理高效的最優(yōu)路徑，無法實現(xiàn)它的實際價值，也顯現(xiàn)不出雙向RRT算法避障能力強的優(yōu)良特性。針對問題（2），本文在問題（1）改進后的基礎上提出自適應目標引力的雙向RRT算法。

隨機樹在擴展新節(jié)點Xnew時起著關鍵作用，即在Xrand與Xnear的連線上以一個步長為距離來確定生長新節(jié)點Xnew。在障礙物環(huán)境下，自適應目標引力可以有效地利用雙向RRT算法的隨機性與目標引力使路徑朝向目標點發(fā)展。利用函數(shù)collision（Xnew）判斷隨機樹生長時步長在狀態(tài)空間C中隨障礙物的有無來自適應變化，方法如式（2）所示。

Collision（Xnew）=Xrandρ>kρ

Xgoalρ

其中，Xrand代表隨機樹朝隨機點方向擴展，Xgoal代表隨機樹朝目標點方向擴展。當AGV在運動范圍內遇到障礙物時，取ρ>kρ，此時目標引力比較小，隨機樹將具有雙向RRT算法隨機性大的特性，繞開障礙物朝著隨機點Xrand方向擴展，不會碰到障礙物，具有很強的避障能力；而在運動范圍內沒有遇到障礙物時，取ρ

自適應目標引力的雙向RRT算法不僅保證了兩棵隨機樹從初始點和目標點分別朝著目標點和初始點方向生長，而且保證了雙向RRT算法的強避障能力和快速搜索效率，路徑光滑性也得以改進，從而使路徑達到最優(yōu)。自適應目標引力的雙向RRT算法具有的優(yōu)良特性使得它更適合解決高維空間和復雜環(huán)境下的運動規(guī)劃問題[4]。自適應目標引力的雙向RRT算法在遇到障礙物時的隨機樹擴展示意圖如圖3所示。

3實驗結果與分析

設AGV為圓點狀，將自適應目標引力的雙向RRT算法的仿真數(shù)據(jù)與目標引力雙向RRT算法的仿真數(shù)據(jù)進行比較，驗證前者算法的優(yōu)越性與正確性。實驗環(huán)境為windows 2010，Intel（R） Core（TM）2、3GHZ、4G內存，編譯工具為MATLAB 2014b。圖4中左下角的AGV為圓點狀，即為根節(jié)點；狀態(tài)空間1 000×1 000，X軸與Y軸坐標范圍均為[0，1 000]，AGV的起點位置為原點[0，0]，終點位置為目標點[1 000，1 000]；兩點間的直線距離為1 414，障礙物為黑色斑塊，隨機設置，形狀大小不一；從初始點（原點）出發(fā)的隨機樹生長線用黑色細線表示，從目標點（終點）出發(fā)的隨機樹生長線用灰色細線表示；規(guī)劃出的路徑用灰色粗線表示。針對所提供的實驗環(huán)境對每組實驗進行25次實驗。

實驗1：首先通過計算機對目標引力的雙向RRT算法進行仿真實驗，圖4是對應的實驗仿真圖，從初始點出發(fā)的隨機樹與從目標點出發(fā)的隨機樹在狀態(tài)空間C中同時擴展搜索，生成新節(jié)點，在某點相遇后生成一條路徑。由于目標引力作用，新節(jié)點的位置比較集中，圖5是路徑規(guī)劃仿真結果。

實驗2：通過計算機對自適應目標引力的雙向RRT算法進行仿真實驗，圖6是對應的實驗仿真圖。由于引入自適應目標引力策略，生成較少的新節(jié)點后就能將路徑連通，圖7是路徑規(guī)劃圖。

表1是每組實驗中25次實驗數(shù)據(jù)的平均值，通過實驗數(shù)據(jù)可以得出，AGV在靜態(tài)的全局狀態(tài)空間運行時，與實驗1相比，實驗2在實驗1的基礎上引入自適應目標引力策略后可以很好地避開障礙物，不會像雙向RRT算法一樣隨機擴展新節(jié)點，減少了路徑生成時間，降低了時間復雜度，提高了搜索效率，使自適應目標引力的雙向RRT算法在已知靜態(tài)環(huán)境下規(guī)劃出的路徑更接近最優(yōu)解。實驗表明：與目標引力的雙向RRT算法相比，自適應目標引力的雙向RRT算法具有明顯的優(yōu)越性和正確性。

4結語

針對AGV路徑規(guī)劃的效率問題與避障問題，本文以經典RRT算法為基礎提出一種雙向搜索功能與自適應目標引力相結合的RRT算法。雙向搜索雖能提高路徑搜索效率，但隨機性大而不能達到最優(yōu)路徑[12]。引入目標引力的雙向RRT算法既保證了雙向RRT算法良好的路徑搜索效率，又解決了其隨機性大的問題，無需對全局空間進行搜索，避障能力卻有所減弱，可能導致目標不可達。自適應目標引力的雙向RRT算法在擴展新節(jié)點時通過周圍環(huán)境有無障礙物來控制隨機樹的生長方向，不僅能夠引導新節(jié)點朝著目標點方向擴展生長，還能有效避開障礙物，算法復雜度低，路徑搜索效率高，同時規(guī)劃出的最優(yōu)路徑比之前更光滑。實驗仿真數(shù)據(jù)證明：自適應目標引力的雙向RRT算法在AGV路徑規(guī)劃的效率問題與避障問題上具有兼得特性，該改進方法對進一步深入研究AGV路徑規(guī)劃問題具有實際參考價值。

參考文獻參考文獻：

[1]趙德云，楊厚華，王哲.基于模糊神經網絡控制的AGV避障路徑規(guī)劃仿真[J].機電工程，2010，27（9）：2731.

[2]肖本賢，齊東流，劉海霞，等.動態(tài)環(huán)境中基于模糊神經網絡的AGV路徑規(guī)劃[J].系統(tǒng)仿真學報，2006，18（9）：24012404.

[3]郭二東，劉楠嶓，吳立輝，等.一種基于遺傳算法的AGV路徑規(guī)劃方法[J].科技創(chuàng)新與生產力，2016，6（8）：8788.

[4]LAVALLE S M. Department of computer science[C]. TR9811，Ames，USA，lows State University： Technical Repeat，1998.

[5]LAVALLE S M，KUFFNER J. Proceedings of algorithmic and computational robotics： new directions[C]. Rapidly Randomexploring Trees：Progress and Prospects，2001：293308.

[6]QURESHI AH，AYAZ，Y. Intelligent bidirectional rapidlyexploring random trees for optimal motion planning in complex cluttered environments[J].Robotics and Autonomous Systems，2015，68（1）：111.

[7]王凡，馮楠，胡小鵬.一種基于RRTConCon改進的路徑規(guī)劃算法[J].大連理工大學學報，2014（6）：637643.

[8]郝利波，侯媛彬.基于一種改進的RRT算法的足球機器人路徑規(guī)劃[J].西安科技大學學報，2011，31（1）：8185.

[9]馬志遠.智能控制下的AGV路徑規(guī)劃研究[J].河南科技，2013（19）：98.

[10]張浩杰，龔建偉，姜巖，等.基于變維度狀態(tài)空間的增量啟發(fā)式路徑規(guī)劃方法研究[J].自動化學報，2012，39（10）：16021610.

[11]HI LIN.2DSpan resampling of BiRRT in dynamic path planning[J].International Journal of Automation and Smart Te，2015，4（4）：3948.

[12]莫棟成，劉國棟.改進的快速探索隨機樹雙足機器人路徑規(guī)劃算法[J].計算機應用，2013，33（1）：199201.

責任編輯（責任編輯：何麗）