胡兵　向鳳紅　毛劍琳

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

關(guān)鍵詞：路徑規(guī)劃；自動導(dǎo)引小車；RRT；雙向搜索；自適應(yīng)目標(biāo)引力

DOIDOI：10.11907/rjdk.172598

中圖分類號：TP311

文獻標(biāo)識碼：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.

英文關(guān)鍵詞Key Words：path planning； AGV； RRT； bidirectional search； adaptive target gravity

0引言

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

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

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

1問題描述

AGV的運行環(huán)境較為復(fù)雜，包括靜態(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)境，包括車間柱子、設(shè)備、噪音及電磁干擾等。它影響到AGV行走路徑的選擇及AGV的穩(wěn)定性。運行環(huán)境（狀態(tài)空間）可以分為障礙空間與自由空間。為了避開障礙物規(guī)劃出一條合理路徑，AGV在障礙空間應(yīng)放慢運行速度，并改變方向避開障礙物；在自由空間應(yīng)加快運行速度向目標(biāo)點生長。因工作環(huán)境不同，AGV的形狀大小會不一樣，為了研究需要可設(shè)AGV為一圓點狀，空間中的障礙物形狀不一，包括車間柱子、設(shè)備等，可隨機設(shè)置。AGV所處的虛擬工作環(huán)境可定義為如圖1所示，此時在AGV的路徑規(guī)劃中需解決的兩個核心問題如下：

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

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

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

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

2.1經(jīng)典RRT算法

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

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

2.2引入目標(biāo)引力的雙向RRT算法

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

Step1：分別以初始點和目標(biāo)點為起點同時生成隨機樹Ti和Tj。當(dāng)兩棵樹擴展生長后于某一點相遇時，路徑產(chǎn)生。算法在初始點Xinit和Xgoal同時開始構(gòu)造隨機樹，從任意一個隨機樹中選取與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（）在當(dāng)前搜索樹上選取離Xrand距離最近的節(jié)點Xnear來擴展；函數(shù)select_input（）在Xrand與Xnear結(jié)合下得到輸入U，根據(jù)給定的標(biāo)準(zhǔn)，在輸入U中選擇一個輸入，使得Xnear盡可能接近Xrand，生成一個節(jié)點Xnear；函數(shù)new_state（）得到新節(jié)點Xnew。每次擴展得到新節(jié)點后均要判斷其是否在障礙區(qū)域內(nèi)，若是，則返回到for循環(huán)重新選取新的隨機點，若否，則直接將其加入當(dāng)前樹中，當(dāng)加入的新節(jié)點與目標(biāo)點間的距離足夠小或重合時路徑搜索結(jié)束，生成規(guī)劃路徑。

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

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

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

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

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

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

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

2.3引入自適應(yīng)目標(biāo)引力策略

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

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

Collision（Xnew）=Xrandρ>kρ

Xgoalρ

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

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

3實驗結(jié)果與分析

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

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

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

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

4結(jié)語

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

參考文獻參考文獻：

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

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

[3]郭二東，劉楠嶓，吳立輝，等.一種基于遺傳算法的AGV路徑規(guī)劃方法[J].科技創(chuàng)新與生產(chǎn)力，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].大連理工大學(xué)學(xué)報，2014（6）：637643.

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

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

[10]張浩杰，龔建偉，姜巖，等.基于變維度狀態(tài)空間的增量啟發(fā)式路徑規(guī)劃方法研究[J].自動化學(xué)報，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].計算機應(yīng)用，2013，33（1）：199201.

責(zé)任編輯（責(zé)任編輯：何麗）