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

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

1 引言

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

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

2 RRT算法的改進(jìn)

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

2.1 選擇低成本樹(shù)進(jìn)行重新布線

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

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

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

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

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

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

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

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

2.2 啟發(fā)式變步長(zhǎng)

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

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

2.3 正態(tài)分布

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

二維標(biāo)準(zhǔn)正態(tài)分布為

若用隨機(jī)變量v來(lái)表示，v=[xy]T即：

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

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

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

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

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

圖6 無(wú)障礙RRT算法仿真

圖7 無(wú)障礙RRT改進(jìn)算法仿真

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

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

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

圖8 狹窄通道RRT算法仿真

圖9 狹窄通道RRT改進(jìn)算法仿真

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

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

3 改進(jìn)算法仿真對(duì)比實(shí)驗(yàn)

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

3.1 無(wú)障礙物環(huán)境對(duì)比實(shí)驗(yàn)

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

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

3.2 狹窄通道環(huán)境對(duì)比實(shí)驗(yàn)

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

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

3.3 普通環(huán)境對(duì)比實(shí)驗(yàn)

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

4 結(jié)語(yǔ)

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