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

程 滿 楊光永 徐天奇 黃卓群 劉 葉

（云南民族大學(xué)電氣信息工程學(xué)院 昆明 650500）

1 引言

自動導(dǎo)引車（AGV）在自動化、靈活性方面具有獨特的優(yōu)勢，使AGV 成為智慧倉儲，智能物流自動化上的最優(yōu)選擇。路徑規(guī)劃是AGV 的核心，所以設(shè)計合理高效的路徑規(guī)劃算法十分重要［1］。AGV的路徑規(guī)劃目標在于通過采用某個算法，在包含有各種障礙物的空間中，避開障礙物于自由空間中安全行駛，最終生成一條從起點到終點的無碰撞的安全路徑。路徑規(guī)劃的核心是算法，算法的高效、安全、路徑最優(yōu)等指標經(jīng)常作為算法優(yōu)劣的衡量項。目前對于一些常見算法的分類主要有全局路徑規(guī)劃和局部路徑規(guī)劃；圖搜索算法和幾何結(jié)構(gòu)搜索算法；傳統(tǒng)算法和人工智能算法。因為算法本身某些方面的局限性，所以在面對不同問題或者不同環(huán)境的時候不同的算法及其改進算法被用來解決某些類型的問題，采取算法某些方面的優(yōu)點，去粗取精的混合算法也越來越受到研究者的喜愛［2～3］。

RRT 算法于1998 年由Lavall 所提出的［4］，基于環(huán)境空間的隨機采樣點規(guī)劃的一種算法，節(jié)點的擴展不需要預(yù)處理，建模簡單，速度快并且是一種概率完備算法，同時在高維空間中表現(xiàn)優(yōu)秀，受到很多研究人員的關(guān)注，各種改進算法也相繼提出［5～11］。Lavalle 等隨后又提出了雙向隨機樹（bi-RRT）［12］，在起始點和目標節(jié)點同時生長兩棵樹，兩個方向進行擴展，加快算法的速度；RRT-connect 算法又是在bi-RRT 算法的基礎(chǔ)上引入了貪婪的思想；Frazzoli 等提出了RRT*算法［13］，在生成新節(jié)點的時候，通過比較代價替換父節(jié)點，隨著迭代次數(shù)的增加生成的路徑向最優(yōu)路徑逼近；Nasir 等提出RRT*-smart 算法，在損失算法的隨機性的代價下獲得收斂速度的提升；劉成菊等提出變步長的RRT算法，改變隨機樹擴展節(jié)點時候的步長，加快收斂速度［14～16］。

2 RRT算法的改進

針對RRT 算法的缺陷，本文的改進重點在于通過在擴展節(jié)點的時候采用貪婪的思想，對已經(jīng)規(guī)劃的好路徑進行兩次重新選擇父節(jié)點和布線，使生成的路徑接近于最優(yōu)路徑；加入啟發(fā)函數(shù)，將目標節(jié)點也加入算法的考量范圍，使RRT 算法的擴展具有方向性，不再盲目擴展；將節(jié)點的擴展集中在一定區(qū)域，剔除冗余節(jié)點，避免多余無用節(jié)點的反復(fù)擴展，加快算法的運行速度。

2.1 選擇低成本樹進行重新布線

當隨機樹在自由狀態(tài)空間已經(jīng)生成的時候，RRT 算法的規(guī)劃器都是選擇Xrand最近的Xnearest，并將Xnearest與Xrand連接起來，按照步長生成節(jié)點Xnew，不能保證算法的成本約束，當使用低成本樹優(yōu)化之后，規(guī)劃器將低成本的節(jié)點連接起來，從起始點到當前節(jié)點保持距離成本的最小值，保障生成路徑質(zhì)量。

H 節(jié)點是最新生成的Xnew節(jié)點，Xnew的父節(jié)點Xnearest如圖1 所示為E 節(jié)點，起始節(jié)點Xstart為A節(jié)點。如圖1 所示，在第一次優(yōu)化之前，節(jié)點路徑為A-B-C-E-H，路徑代價為11?，F(xiàn)在進行第一次優(yōu)化，以節(jié)點H為圓心，以一定長度為半徑，作一個圓，將H 與I、F、G、K 連接，長度都為2，到達節(jié)點H有多條路徑，例如：A-B-I-H、A-B-C-E-F-H、A-B-C-E-G-H、A-J-K-H。將這些路徑包括之前未優(yōu)化之前的那條原始路徑的代價進行比較，選擇最短的路徑代價的那條路徑并且將之前的父節(jié)點變換成最短路徑的父節(jié)點，第一次優(yōu)化后的路徑如圖2所示。

圖1 第一次優(yōu)化示意圖

圖2 第一次優(yōu)化重新布線

圖3 第二次優(yōu)化示意圖

在圖2 新路徑代價為最短的A-B-I-H，此時代價為7。將之前的H 實連接線去掉，并將虛線所示的I 和H 的連接線變成實現(xiàn)，運用貪婪思想計算新節(jié)點設(shè)定一定值半徑范圍內(nèi)的所有節(jié)點的代價值計算，取其中最小代價為所走路徑，這樣生成的路徑比較接近于最優(yōu)路徑。第二次優(yōu)化，重復(fù)第一次優(yōu)化的步驟。

連接H-E、H-F、H-G、H-K 比較到達E、F、G、K這四個節(jié)點，通過現(xiàn)有樹的代價和通過H節(jié)點到達的代價，選擇代價小的方式到達，重新布線。第二次優(yōu)化如圖4所示。

圖4 第二次優(yōu)化重新布線

2.2 啟發(fā)式變步長

傳統(tǒng)的RRT 算法并沒有將目標節(jié)點考量進去，樹的擴展是隨機的，每次擴展的步長也是一個固定的步長，現(xiàn)在改進的RRT 算法將目標節(jié)點加入考量范圍，樹的擴展具有了方向性，擴展的方向不再僅是由隨機方向的隨機點Xrand單獨控制，而是隨機方向的Xrand和目標終點的Xend共同控制。新的步長擴展公式為

當無障礙物在行駛的路徑附近時，β>α，引導(dǎo)樹的擴展方向更多的朝著目標節(jié)點，可以加快目標節(jié)點的搜索速度；如果發(fā)現(xiàn)障礙物，改變α和β的值，此時α>β，樹的搜索更多地朝向隨機搜索方向，α和β兩個值大小的變化，有助于整個路徑規(guī)劃系統(tǒng)更好地逃避出局部最小值。

2.3 正態(tài)分布

當隨機變量服從數(shù)學(xué)期望為μ和方差為σ2的正態(tài)分布時，記作N(μ,σ2)。概率密度函數(shù)表示為

二維標準正態(tài)分布為

若用隨機變量v來表示，v=[xy]T即：

由標準正態(tài)分布推廣到一般v=A(X-u)。

將正態(tài)分布加入算法中，主要是為了減少相對狀態(tài)空間，將樹的擴展限制在某些區(qū)域，使規(guī)劃的效率提高。本文所使用的多元正態(tài)分布公式：

其中Σ 是協(xié)方差矩陣，Σ=UΛUT，u1,u2是協(xié)方差矩陣的特征向量，λ1、λ2是相應(yīng)的特征向量的特征值。

圖5 起點位置為（20，40），終點位置為（90，40），可以有效地將隨機采樣點集中在一定區(qū)域。該算法以起點開始，隨迭代增加，樹開始擴展，通過規(guī)劃生成Xnew節(jié)點，此節(jié)點的擴展不僅是在隨機點的方向上，而且偏向目標區(qū)域，正是因為具有這種偏向性，可以更快地找到第一個解，當?shù)谝粋€解決方案生成之后，用這條路徑作為參考，通過正態(tài)分布生成樣本點，最后找到一條高質(zhì)量的路徑。如上圖所示，圖中的正態(tài)分布的形狀取決于兩個因素，分別為Cbest和Cmin。整個正態(tài)分布的區(qū)域可以表示為長度為Cbest，寬度為，其中Cbest是所有可行解決方案中成本代價最小的，Cmin是起始點與目標節(jié)點之間的歐氏距離，λ1=Cbest/2 ，，若是無障礙空間，那么λ2的值會降到0，也就是可行路徑的最優(yōu)路徑此時就是最短路徑，直接生成一條直線連接起始點和終點。

圖5 正態(tài)采樣點分布圖

圖6 無障礙RRT算法仿真

圖7 無障礙RRT改進算法仿真

表1 圖6和圖7參數(shù)比較

表2 圖8和圖9參數(shù)比較

表3 圖10和圖11參數(shù)比較

圖8 狹窄通道RRT算法仿真

圖9 狹窄通道RRT改進算法仿真

圖10 普通環(huán)境RRT算法仿真

圖11 普通環(huán)境RRT改進算法仿真

3 改進算法仿真對比實驗

所有的對比仿真實驗都是在個人PC 上完成的，基于Matlab-R2018a，環(huán)境地圖大小設(shè)置為1000×1000 。個人PC 硬件配置：處理器Inter（R）Core（TM）i7-10875H、內(nèi)存為16G、系統(tǒng)版本為Windows10。

3.1 無障礙物環(huán)境對比實驗

為了看出更直觀的效果，首先在無障礙物環(huán)境中進行實驗，起點設(shè)置為（100，500），目標節(jié)點設(shè)置為（900，500）。RRT 算法的步長為15，實驗環(huán)境的地圖單位為m，時間單位為s，無障礙環(huán)境下的仿真實驗的迭代次數(shù)設(shè)置為2000次。

由于算法的隨機性，將實驗50 次的數(shù)據(jù)取其平均值，改進算法相較于傳統(tǒng)算法來講，運行效率大大提高，算法運行時間縮減了63.95%，距離相較于理想距離更加接近，采樣節(jié)點減少了93.48%，行走的線路較于傳統(tǒng)算法而言，行駛路徑較平滑，易于跟蹤行駛。

3.2 狹窄通道環(huán)境對比實驗

當我們探索的路徑需用通過一個很狹窄的通道時，過于狹窄的通道會導(dǎo)致我們路徑被碰到的概率極其之低，找到路徑時間的長短很隨機，全靠運氣。有時運氣好，RRT很快就在狹窄通道找到了路徑，運氣差的時候，就可能一直無法通過。目前有好多學(xué)者都在對RRT 算法進行改進，目的在于解決通過狹窄通道這個難題。改進算法將目標節(jié)點加入到了啟發(fā)的一部分，加強了算法通過狹窄通道的能力。以下仿真實驗設(shè)置一個寬度只有10m 的狹窄通道。

改進后的算法是啟發(fā)式變步長的生長，當擴展開始，無障礙物時候，主要考慮目標節(jié)點的啟發(fā)式生長，當在障礙物附近的時候，主要考慮隨機生長狀態(tài)，可以很好地避障的同時穿過狹窄通道。由于算法的隨機性，取50 次試驗的平均數(shù)據(jù)，算法的運行時間減少24.95%，行走的路徑接近于最短路徑，采樣節(jié)點減少了41.38%。

3.3 普通環(huán)境對比實驗

改進后的算法相較于傳統(tǒng)算法而言，效率大大提高，運行時間減少了39.37%，行駛距離減少了24.41%，采樣節(jié)點減少了58.15%，行駛路徑較于傳統(tǒng)算法而言，路徑較為平緩，轉(zhuǎn)彎的次數(shù)較少，易于跟蹤行駛。

4 結(jié)語

改進后的算法在AGV 的路徑規(guī)劃中采用了正態(tài)分布的思想，將節(jié)點的隨機分布探索范圍集中在正態(tài)分布的區(qū)域，避免了無效冗余節(jié)點的探索，大大提高算法的執(zhí)行效率。由傳統(tǒng)的固定步長變成了變步長啟發(fā)式生長，動態(tài)調(diào)整兩個分量的大小，以達到不同環(huán)境下的側(cè)重點不同，距離目標過遠，重點在于目標節(jié)點的啟發(fā)生長，當附近存在障礙物時，那么此時的重點在于隨機生長，可以有效地避障，避免局部最小值。在Matlab上完成了仿真對比實驗，驗證了算法改進后，生成質(zhì)量更高的路徑，探索節(jié)點減少，效率提高，生成較平滑的路徑有利于跟蹤行駛，同時在面對狹窄通道時也可以快速順利通過。