王海芳， 崔陽(yáng)陽(yáng)， 李鳴飛， 李廣宇

(東北大學(xué)秦皇島分校 控制工程學(xué)院， 河北 秦皇島 066004)

路徑規(guī)劃是指在包含障礙物的給定區(qū)域內(nèi)搜索到一條從初始起點(diǎn)到目標(biāo)終點(diǎn)的安全無(wú)碰撞、可行的路徑[1].基于采樣的路徑規(guī)劃算法是目前廣泛應(yīng)用于機(jī)器人路徑規(guī)劃的算法之一[2]，在基于采樣的算法中，應(yīng)用最廣泛的是漸進(jìn)最優(yōu)快速擴(kuò)展隨機(jī)樹(shù)(rapidly-exploring random trees star, RRT*)算法，該采樣算法無(wú)需在工作空間中明確表示障礙物信息，通過(guò)碰撞檢測(cè)方法連接一組沒(méi)有碰撞的節(jié)點(diǎn)來(lái)獲得可行路徑.但要獲得最優(yōu)路徑，RRT*算法需要不斷地增加采樣節(jié)點(diǎn)數(shù)量，花費(fèi)大量計(jì)算時(shí)間獲得最優(yōu)路徑，從計(jì)算角度看，這給計(jì)算機(jī)在高維空間下的內(nèi)存增加了很大的負(fù)擔(dān).針對(duì)RRT*算法收斂到最優(yōu)值效率低的局限性，Nasir等[3]提出了一種RRT*-smart算法，該算法通過(guò)啟發(fā)式函數(shù)的方式實(shí)現(xiàn)隨機(jī)點(diǎn)的智能采樣，提高了最優(yōu)路徑的收斂速度.在此基礎(chǔ)上，楊國(guó)田等[4]提出一種改進(jìn)的RRT*FN算法，該算法有較快的搜索速度，對(duì)內(nèi)存有限的嵌入式飛行機(jī)器人有較好的適用性和可行性.文獻(xiàn)[5]提出將RRT*FN算法擴(kuò)展到動(dòng)態(tài)場(chǎng)景，使用兩個(gè)貪婪啟發(fā)式路徑運(yùn)動(dòng)優(yōu)化方案，在較短時(shí)間內(nèi)找到求解路徑，減少收斂時(shí)間，提高路徑整體收斂效率.Luo等[6]提出了一種Informed-RRT*的變體，利用目標(biāo)偏置引導(dǎo)策略搜索目標(biāo)點(diǎn)，利用橢球形子集細(xì)化路徑，從而找到最優(yōu)路徑.

傳統(tǒng)RRT*FN算法通過(guò)限制樹(shù)中節(jié)點(diǎn)數(shù)量解決這一問(wèn)題，當(dāng)樹(shù)中節(jié)點(diǎn)數(shù)量超過(guò)給定閾值時(shí)，隨機(jī)刪除待刪除的無(wú)子節(jié)點(diǎn)的候選節(jié)點(diǎn).在多復(fù)雜障礙物和狹窄的二維靜態(tài)環(huán)境下，傳統(tǒng)的RRT*FN算法收斂到最優(yōu)路徑會(huì)出現(xiàn)效率低的問(wèn)題，本文結(jié)合目標(biāo)偏向、啟發(fā)采樣策略、刪除遠(yuǎn)離目標(biāo)點(diǎn)和路徑中無(wú)子節(jié)點(diǎn)的方法對(duì)基本RRT*FN算法加以改進(jìn)，并對(duì)生成路徑進(jìn)行優(yōu)化處理，從而提高算法效率和路徑質(zhì)量.

1 算法基本原理

1.1 問(wèn)題描述

令X?Rn表示規(guī)劃問(wèn)題的狀態(tài)空間，Xobs?X表示障礙區(qū)域，Xfree?XXobs表示可行使區(qū)域，給定初始節(jié)點(diǎn)xstart∈Xfree，目標(biāo)節(jié)點(diǎn)xgoal∈Xfree.機(jī)器人可行路徑由連續(xù)函數(shù)表示s:[0,1]→Rn，Σ表示所有的可行路徑集合，給定代價(jià)函數(shù)s:Σ→R≥0，則尋找最優(yōu)路徑規(guī)劃問(wèn)題可以表示為

s*=arg min{c(s)|s(0)=xstart,s(1)=xgoal,

?x∈[0,1],s(x)∈Xfree}.

(1)

式中：s*為代價(jià)函數(shù)值最小的路徑集合；R≥0表示非負(fù)實(shí)數(shù)集合.

1.2 基本RRT*FN算法[7]

RRT*FN算法是在RRT*算法的基礎(chǔ)上發(fā)展而來(lái)的.基本的RRT*FN算法同RRT*采樣策略相同，通過(guò)在狀態(tài)空間中均勻采樣獲得隨機(jī)節(jié)點(diǎn)的方式不斷擴(kuò)展樹(shù)的內(nèi)存：首先在可行區(qū)域隨機(jī)采樣到一個(gè)節(jié)點(diǎn)xrand，接著在當(dāng)前樹(shù)中找到與隨機(jī)點(diǎn)的最近鄰節(jié)點(diǎn)xnearest，然后連接這兩個(gè)節(jié)點(diǎn)，獲得單位向量，并從xnearest出發(fā)以一定的步長(zhǎng)ρ生成新節(jié)點(diǎn)：

(2)

如果最近鄰節(jié)點(diǎn)與新節(jié)點(diǎn)之間的路徑與障礙物穿過(guò)障礙物區(qū)域，即與障礙物發(fā)生碰撞，那么刪除新節(jié)點(diǎn)重新進(jìn)行采樣，否則將新節(jié)點(diǎn)加入樹(shù)中，完成一次樹(shù)的生長(zhǎng).

RRT*算法對(duì)父節(jié)點(diǎn)的選擇和鄰近區(qū)域的節(jié)點(diǎn)重連作出了改進(jìn)[8]：在確定隨機(jī)點(diǎn)位置后，對(duì)新加入搜索樹(shù)的新節(jié)點(diǎn)進(jìn)行重選父節(jié)點(diǎn)和重新布線操作，節(jié)點(diǎn)標(biāo)號(hào)表示樹(shù)中節(jié)點(diǎn)產(chǎn)生的順序，如圖1所示.節(jié)點(diǎn)1表示初始節(jié)點(diǎn)，節(jié)點(diǎn)8，即新節(jié)點(diǎn)表示新加入的節(jié)點(diǎn)，鄰近節(jié)點(diǎn)表示與隨機(jī)點(diǎn)距離最近的樹(shù)中已有節(jié)點(diǎn)，默認(rèn)為新節(jié)點(diǎn)的父節(jié)點(diǎn)，節(jié)點(diǎn)與節(jié)點(diǎn)之間連接線上的數(shù)字表示兩節(jié)點(diǎn)之間的路徑代價(jià).重置父節(jié)點(diǎn)就是找到一條從初始節(jié)點(diǎn)1到節(jié)點(diǎn)8的路徑代價(jià)值最小的可行路徑，在圖1a中，以新節(jié)點(diǎn)為中心，給定閾值范圍內(nèi)的節(jié)點(diǎn)為待選節(jié)點(diǎn)，找到各個(gè)待選節(jié)點(diǎn)與新節(jié)點(diǎn)相連接后節(jié)點(diǎn)代價(jià)值最低的待選節(jié)點(diǎn)作為父節(jié)點(diǎn)，重置新節(jié)點(diǎn)的父節(jié)點(diǎn)，待選節(jié)點(diǎn)分別為節(jié)點(diǎn)4和節(jié)點(diǎn)7，原來(lái)的路徑為1—5—8，路徑代價(jià)為10+2=12.備選節(jié)點(diǎn)與新節(jié)點(diǎn)組成的路徑為1—2—4—8，1—5—7—8，路徑代價(jià)分別為2+5+3=10，10+3+2=15，因此將節(jié)點(diǎn)8的父節(jié)點(diǎn)改為節(jié)點(diǎn)4，完成父節(jié)點(diǎn)重置.圖1b是重新生成的隨機(jī)樹(shù)；為進(jìn)一步使隨機(jī)樹(shù)節(jié)點(diǎn)間連接的代價(jià)盡量小，對(duì)隨機(jī)樹(shù)進(jìn)行重新布線.重布線的過(guò)程可以表述為：如果待選節(jié)點(diǎn)的父節(jié)點(diǎn)改為新節(jié)點(diǎn)可以減小路徑代價(jià)，則進(jìn)行更改.圖1c中的待選節(jié)點(diǎn)為節(jié)點(diǎn)5和節(jié)點(diǎn)7，節(jié)點(diǎn)5的父節(jié)點(diǎn)是節(jié)點(diǎn)1，路徑是1—5，路徑代價(jià)為10，如果將節(jié)點(diǎn)5的父節(jié)點(diǎn)重置為節(jié)點(diǎn)8，那么到達(dá)節(jié)點(diǎn)的路徑為1—2—4—8—5，代價(jià)為2+5+3+2=12，大于原始的路徑代價(jià)，不對(duì)節(jié)點(diǎn)5進(jìn)行重布線操作，同理，將節(jié)點(diǎn)7的父節(jié)點(diǎn)由節(jié)點(diǎn)5更改為節(jié)點(diǎn)8后，更改后的路徑為1—2—4—8—7，路徑代價(jià)為2+5+3+2=12，小于原始路徑1—5—7的路徑代價(jià)，即10+3=13，因此，將節(jié)點(diǎn)7的父節(jié)點(diǎn)重置為節(jié)點(diǎn)8，圖1d是重布線后的隨機(jī)樹(shù).

圖1 RRT*算法的搜索過(guò)程Fig.1 RRT * algorithm search process(a)—父節(jié)點(diǎn)重置； (b)—重置后更新隨機(jī)樹(shù)； (c)—再次父節(jié)點(diǎn)重置； (d)—重置后更新隨機(jī)樹(shù).

RRT*FN算法是在RRT*的基礎(chǔ)上，固定樹(shù)中節(jié)點(diǎn)數(shù)，并且當(dāng)樹(shù)中節(jié)點(diǎn)數(shù)達(dá)到給定的預(yù)設(shè)值時(shí)，刪除樹(shù)中的葉子節(jié)點(diǎn)，使算法在路徑搜索過(guò)程中只使用給定的固定內(nèi)存空間.RRT*FN不僅節(jié)省了內(nèi)存空間，還保留了RRT*原有算法的優(yōu)勢(shì)，概率完備性和漸進(jìn)最優(yōu)性[9]，即只要路徑探索迭代的次數(shù)足夠多，得到的最終路徑就可以是較為接近最短的可行路徑.

1.3 概率完備性分析

文獻(xiàn)[2,9]對(duì)RRT相關(guān)算法進(jìn)行了概率完備性和收斂性的證明：對(duì)于任何可行的路徑規(guī)劃問(wèn)題，隨迭代次數(shù)的不斷增加，節(jié)點(diǎn)擴(kuò)展數(shù)逐漸趨近于正無(wú)窮大時(shí)，算法找到可行路徑的概率為1.本文算法在進(jìn)行路徑搜索時(shí)，預(yù)設(shè)N為固定節(jié)點(diǎn)數(shù)，當(dāng)N為無(wú)窮大時(shí)，如果此時(shí)還未找到初始路徑，那么相當(dāng)于在進(jìn)行RRT*算法搜索，算法具備概率完備性.在實(shí)際搜索過(guò)程中，N通常是考慮機(jī)器人所擁有的內(nèi)存空間大小、計(jì)算能力及實(shí)際的工作環(huán)境共同進(jìn)行設(shè)定的.文獻(xiàn)[10]對(duì)RRT*FN算法進(jìn)行分析時(shí)指出，如果當(dāng)前節(jié)點(diǎn)數(shù)已到達(dá)最大節(jié)點(diǎn)預(yù)設(shè)值但還未找到初始路徑時(shí)，則需要重新開(kāi)始，因此算法的概率完備性和收斂性受到給定固定節(jié)點(diǎn)N的影響，具體的N值在仿真實(shí)驗(yàn)中進(jìn)行了分析.

2 改進(jìn)RRT*FN算法介紹

2.1 采樣策略

與RRT*算法搜索方式相似，本文算法搜索以初始起點(diǎn)作為搜索樹(shù)的根開(kāi)始，在每次迭代中，算法都會(huì)在配置空間中生成一個(gè)隨機(jī)點(diǎn)，搜索樹(shù)中最近的點(diǎn)，步長(zhǎng)為一個(gè)量級(jí).為了克服搜索盲目性，在未搜索到初始路徑時(shí)，以一定的概率偏向于朝著目標(biāo)點(diǎn)方向探索.當(dāng)搜索到一條可行路徑時(shí)，以可行路徑中的節(jié)點(diǎn)為最近節(jié)點(diǎn)進(jìn)行采樣計(jì)算，圍繞可行路徑產(chǎn)生隨機(jī)點(diǎn)，減少算法收斂到最優(yōu)解的計(jì)算時(shí)間.將產(chǎn)生的一個(gè)新的隨機(jī)點(diǎn)進(jìn)行碰撞檢測(cè)，并將與障礙物無(wú)碰撞的點(diǎn)擴(kuò)展到搜索隨機(jī)樹(shù)中，當(dāng)最新搜索節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)時(shí)，算法搜索到一條可行路徑.RRT*搜索與目標(biāo)偏置探索如圖2所示.

圖2 RRT*搜索與目標(biāo)偏置搜索Fig.2 RRT* search with target bias search(a)—傳統(tǒng)RRT*搜索； (b)—目標(biāo)偏置搜索.

左上角為起點(diǎn)，右下角為終點(diǎn)，黑色折線段為搜索到的一條可行路徑.可以看出本文算法的改進(jìn)策略減少了隨機(jī)采樣的次數(shù)，提高了算法效率.

2.2 節(jié)點(diǎn)刪除

當(dāng)樹(shù)中的節(jié)點(diǎn)到達(dá)給定預(yù)設(shè)值N時(shí)，基本的RRT*FN算法對(duì)樹(shù)中的節(jié)點(diǎn)采取隨機(jī)刪除節(jié)點(diǎn)的方法.考慮到不同路徑狀態(tài)下，節(jié)點(diǎn)的影響不同，將本文算法的刪除策略分為兩種情況：一是當(dāng)前采樣節(jié)點(diǎn)數(shù)已超過(guò)給定內(nèi)存范圍，但還未搜索到初始路徑；二是當(dāng)前采樣節(jié)點(diǎn)數(shù)超過(guò)給定內(nèi)存范圍并已搜索到一條可行路徑.

當(dāng)改進(jìn)算法還未找到初始路徑時(shí)，考慮到目標(biāo)節(jié)點(diǎn)周?chē)墓?jié)點(diǎn)對(duì)找到初始路徑貢獻(xiàn)可能大于其他節(jié)點(diǎn)，并盡量減少不必要的父節(jié)點(diǎn)重連與重布線過(guò)程，所以選擇隨機(jī)刪除樹(shù)中遠(yuǎn)離目標(biāo)點(diǎn)并且沒(méi)有子節(jié)點(diǎn)的節(jié)點(diǎn).當(dāng)改進(jìn)算法已經(jīng)找到一條可行路徑時(shí)，考慮到可行路徑周?chē)墓?jié)點(diǎn)可能對(duì)最優(yōu)路徑有更大影響，因此保留可行路徑一定范圍內(nèi)的節(jié)點(diǎn)，刪除樹(shù)中遠(yuǎn)離最優(yōu)路徑且沒(méi)有子節(jié)點(diǎn)的節(jié)點(diǎn)，保留高性能節(jié)點(diǎn).令T表示算法樹(shù)中的節(jié)點(diǎn)集合，goal為目標(biāo)節(jié)點(diǎn)，radius為預(yù)保留的節(jié)點(diǎn)區(qū)域范圍長(zhǎng)度，PathNodes為可行路徑集合，TemTree為臨時(shí)的待選節(jié)點(diǎn)集合，ChildlessNodes為T(mén)中無(wú)子節(jié)點(diǎn)的節(jié)點(diǎn)集合，節(jié)點(diǎn)刪除的偽代碼由表1給出.

表1 NodeRemove偽代碼Table 1 Pseudo code of NodeRemove

2.3 路徑平滑

經(jīng)過(guò)本文算法規(guī)劃出的原始路徑是由一些離散點(diǎn)組合成的折線段，不僅有不能滿足機(jī)器人轉(zhuǎn)彎曲率要求的可能，還會(huì)導(dǎo)致機(jī)器人在轉(zhuǎn)彎途中增加不必要的轉(zhuǎn)彎時(shí)間，不適合直接應(yīng)用于機(jī)器人路徑規(guī)劃中.為了讓最后的路徑變得光滑，需要對(duì)路徑進(jìn)行平滑處理[11]，使節(jié)點(diǎn)保持連續(xù)性的同時(shí)避免碰撞.常見(jiàn)的平滑處理技巧有B樣條曲線插值[12]、二次貝塞爾曲線插值[9]、五次多項(xiàng)式插值[13]和Cantmull-Rom曲線插值[14].貝塞爾曲線依據(jù)伯恩斯坦多項(xiàng)式發(fā)展而來(lái)，因其控制簡(jiǎn)便，圖像描述能力強(qiáng)的特點(diǎn)被廣泛應(yīng)用于工業(yè)和計(jì)算機(jī)圖形學(xué)領(lǐng)域.貝塞爾曲線通過(guò)選取控制點(diǎn)將折線段通過(guò)公式擬合成連續(xù)光滑的曲線，最終得到的曲線逼近折線段.改進(jìn)算法采用二次Bezier曲線進(jìn)行最終的路徑擬合，如圖3所示.

圖3 二次貝塞爾曲線擬合圖Fig.3 Quadratic Bezier curve fitting graph

設(shè)p1,p13,p3依次是一條拋物線上的三個(gè)不同點(diǎn)，其中經(jīng)過(guò)點(diǎn)p1和p3與拋物線相切的兩條切線相交于點(diǎn)p2，經(jīng)過(guò)點(diǎn)p13與拋物線相切的切線與p1p2相交于點(diǎn)p12，與p2p3相交于點(diǎn)p23.根據(jù)拋物線的三切線定理，有如下比例等式：

(3)

固定p1,p3引入?yún)?shù)t，令等式(3)的比值為t:(1-t)，即有

p12=(1-t)×p1+t×p2,

(4)

p23=(1-t)×p2+t×p3,

(5)

p13=(1-t)×p12+t×p23.

(6)

將式(4)和式(5)代入式(6)得

p13=(1-t)2×p1+2×t×(1-t)×p2+t2×p3.

(7)

當(dāng)t從0變到1時(shí)，式(7)表示的曲線是由三個(gè)固定的點(diǎn)p1,p2,p3定義的一條二次Bezier曲線.

不同的插值方法示意圖如圖4所示，其中黑色圓圈表示給定的控制節(jié)點(diǎn)，將各個(gè)節(jié)點(diǎn)用黑色直線連接形成初始路徑.從圖中可以看出，使用B樣條曲線平滑后的路徑曲率較大，且不再經(jīng)過(guò)原有節(jié)點(diǎn)，五次多項(xiàng)式插值帶來(lái)了較大的拐點(diǎn)浮動(dòng)，不適用狹窄環(huán)境下的路徑，相比于前兩種方法，使用二次貝塞爾曲線和Cantmull-Rom插值后的曲線更接近原始路徑.本文提出一種更優(yōu)的局部Bezier曲線進(jìn)行路徑平滑，圖4中的紅色虛、實(shí)曲線與原始路徑更為接近，局部貝塞爾曲線優(yōu)化策略是：①選擇待優(yōu)化拐點(diǎn)的左、右兩條線段，以兩條線段的中點(diǎn)作為待優(yōu)化的起點(diǎn)和終點(diǎn)，其余部分保持原狀態(tài)，不進(jìn)行插值優(yōu)化，縮短待優(yōu)化的線段長(zhǎng)度；②只對(duì)原始路徑中出現(xiàn)拐點(diǎn)的路徑進(jìn)行插值處理，其他非拐點(diǎn)處的路徑保持不變，處理結(jié)果如圖4中的紅色虛實(shí)曲線，可以看出，優(yōu)化后的曲線具有更小的曲率半徑，與原有的折線段也最為接近.

圖4 局部Bezier插值曲線示意圖Fig.4 Schematic diagram of local Bezier interpolation curves

3 MATLAB仿真實(shí)驗(yàn)

為了驗(yàn)證本文算法的性能，在MATLAB仿真平臺(tái)上分別設(shè)置了稠密障礙物環(huán)境、唯一可行路徑的狹窄通道環(huán)境及“U”型障礙物三種障礙環(huán)境地圖，對(duì)應(yīng)地圖1～3，并進(jìn)行了100次仿真模擬.在地圖1，2中將機(jī)器人視為質(zhì)點(diǎn)運(yùn)動(dòng)，同時(shí)地圖3選擇圓形機(jī)器人模型進(jìn)行仿真模擬，設(shè)置本文算法的最大固定節(jié)點(diǎn)數(shù)為5 000，最大迭代次數(shù)為10 000.采用的軌跡規(guī)劃方案包括：原始地圖的大小為40 mm×40 mm；固定初始點(diǎn)和目標(biāo)點(diǎn).將本文算法的仿真結(jié)果與文獻(xiàn)[7]中所提算法、RRT*及基本RRT*FN進(jìn)行對(duì)比分析，驗(yàn)證本文算法的優(yōu)越性和正確性.

地圖1為稠密障礙物的復(fù)雜環(huán)境，用于驗(yàn)證算法在包含多障礙物時(shí)的搜索能力，設(shè)置的初始點(diǎn)坐標(biāo)為[1,1]，目標(biāo)點(diǎn)坐標(biāo)為[38,38].地圖1的規(guī)劃結(jié)果如圖5所示.圖5a中綠色部分表示算法生成樹(shù)的節(jié)點(diǎn)分布情況，紅色線段連接成初始路徑，可以看出樹(shù)中隨機(jī)節(jié)點(diǎn)在初始路徑附近分布較為集中，這也反映出本文所提采樣策略和節(jié)點(diǎn)刪除策略的有效性.圖5b是對(duì)圖5a中生成的初始路徑進(jìn)行了局部貝塞爾曲線優(yōu)化，并只對(duì)原始路徑中出現(xiàn)拐點(diǎn)的路徑局部?jī)?yōu)化.將本文實(shí)驗(yàn)數(shù)據(jù)與文獻(xiàn)[7]的實(shí)驗(yàn)數(shù)據(jù)整理見(jiàn)表2.

圖5 地圖1規(guī)劃結(jié)果Fig.5 Map 1 planning results(a)—初始生成路徑及樹(shù)的結(jié)點(diǎn)分布；(b)—初始路徑局部?jī)?yōu)化.

表2 地圖1仿真環(huán)境下的實(shí)驗(yàn)數(shù)據(jù)Table 2 Experimental data in map 1 simulation environment

在地圖1環(huán)境下，4種算法路徑收斂情況對(duì)比如圖6所示.由表2和圖6可知，RRT*和基本的RRT*FN算法可以在較短時(shí)間內(nèi)獲得可行路徑，但路徑的長(zhǎng)度較長(zhǎng)，文獻(xiàn)[7]所提的改進(jìn)算法可以獲得較短的路徑長(zhǎng)度，但以犧牲運(yùn)行時(shí)間為代價(jià).

圖6 4種算法路徑收斂情況對(duì)比Fig.6 Comparison of path convergence of four algorithms

相比表2中的前3種算法，本文算法利用目標(biāo)偏置策略可以更快找到1條初始路徑，當(dāng)樹(shù)中結(jié)點(diǎn)達(dá)到固定節(jié)點(diǎn)預(yù)設(shè)值時(shí)，刪除無(wú)子節(jié)點(diǎn)且對(duì)最終路徑影響較低，即遠(yuǎn)離初始路徑的節(jié)點(diǎn)，使隨機(jī)采樣點(diǎn)圍繞初始路徑進(jìn)行迭代，迭代精度與文獻(xiàn)[7]所提算法接近.從表2可以得到本文算法比文獻(xiàn)[7]所提算法平均路徑縮短了0.18%，在平均收斂時(shí)間上少用時(shí)2.95%，因此本文算法可以在相對(duì)較短的時(shí)間內(nèi)加快收斂速度，提高收斂效率，獲得較短路徑.

地圖2為僅存在唯一可行路徑的狹窄通道環(huán)境，用于驗(yàn)證算法對(duì)狹窄通道的搜索能力，設(shè)置的初始點(diǎn)坐標(biāo)為[1,1]，目標(biāo)點(diǎn)坐標(biāo)為[38,38]，增加了算法采樣與路徑平滑的難度，實(shí)驗(yàn)結(jié)果如圖7所示.

圖7 地圖2規(guī)劃結(jié)果Fig.7 Map 2 planning results(a)—初始生成路徑及樹(shù)的結(jié)點(diǎn)分布；(b)—初始路徑局部?jī)?yōu)化.

圖7a綠色部分表示算法生成樹(shù)的節(jié)點(diǎn)分布情況，連接的紅色線段為初始路徑；圖7b對(duì)圖7a中生成的初始路徑中出現(xiàn)拐點(diǎn)的路徑進(jìn)行了局部貝塞爾優(yōu)化.

將本文實(shí)驗(yàn)數(shù)據(jù)與文獻(xiàn)[7]的實(shí)驗(yàn)數(shù)據(jù)整理見(jiàn)表3，地圖1環(huán)境下4種算法路徑收斂情況對(duì)比如圖8所示.

表3 地圖2仿真環(huán)境下的實(shí)驗(yàn)數(shù)據(jù)Table 3 Experimental data in map 2 simulation environment

圖8 4種算法路徑收斂情況對(duì)比Fig.8 Comparison of path convergence of four algorithms

地圖3為“U”型障礙物的地圖環(huán)境，用于驗(yàn)證算法對(duì)特定形狀障礙物的搜索能力，設(shè)置的初始點(diǎn)坐標(biāo)為[14，24]，目標(biāo)點(diǎn)坐標(biāo)為[26，16]，將機(jī)器人當(dāng)作質(zhì)點(diǎn)運(yùn)動(dòng)的仿真實(shí)驗(yàn)如圖9所示.

圖9 地圖3規(guī)劃結(jié)果Fig.9 Map 3 planning results(a)—初始生成路徑及生成樹(shù)節(jié)點(diǎn)分布； (b)—Bezier優(yōu)化后的路徑； (c)—機(jī)器人運(yùn)動(dòng)情況.

圖9a中綠色部分表示算法生成樹(shù)的節(jié)點(diǎn)分布情況，紅色線段連接成初始路徑，可以看出樹(shù)中隨機(jī)節(jié)點(diǎn)在初始路徑附近分布較為集中，這也反映出本文所提采樣策略和節(jié)點(diǎn)刪除策略的有效性.對(duì)圖9b中生成的初始路徑中出現(xiàn)拐點(diǎn)的路徑進(jìn)行了局部Bezier優(yōu)化，綠色部分為Bezier優(yōu)化后的隨機(jī)樹(shù)節(jié)點(diǎn)分布情況，紅色曲線表示對(duì)初始路徑最終優(yōu)化后的路徑效果.圖9c將機(jī)器人設(shè)置為占有一定空間的藍(lán)色圓圈，同時(shí)對(duì)障礙物進(jìn)行膨脹處理，保證機(jī)器人在行駛過(guò)程中不會(huì)與障礙物發(fā)生碰撞.本文設(shè)置的機(jī)器人直徑為2 mm，膨脹長(zhǎng)度則取機(jī)器人的半徑1 mm.

將機(jī)器人的運(yùn)動(dòng)視為通過(guò)旋轉(zhuǎn)和平移的方式從1點(diǎn)至另1不同點(diǎn)的運(yùn)動(dòng)，機(jī)器人從初始點(diǎn)到目標(biāo)點(diǎn)對(duì)規(guī)劃好的路徑進(jìn)行跟蹤的結(jié)果如圖9c所示，其中機(jī)器人圓圈中突出的藍(lán)色線段指向了機(jī)器人的旋轉(zhuǎn)方向.機(jī)器人在拐彎處行駛相對(duì)緩慢，藍(lán)色圓形顯示相對(duì)集中，在直線部分行駛相對(duì)順暢，藍(lán)色圓形顯示相對(duì)稀疏，機(jī)器人在不與障礙物發(fā)生碰撞的前提下，沿規(guī)劃好的路徑作曲線運(yùn)動(dòng).將本文實(shí)驗(yàn)數(shù)據(jù)和文獻(xiàn)[7]的實(shí)驗(yàn)數(shù)據(jù)整理匯總見(jiàn)表4，圖10是地圖3環(huán)境下4種算法路徑收斂情況對(duì)比.

表4 地圖3仿真環(huán)境下的實(shí)驗(yàn)數(shù)據(jù)Table 4 Experimental data in map 3 simulation environment

圖10 4種算法路徑收斂情況對(duì)比Fig.10 Comparison of path convergence among four algorithms

由表4分析可知，在地圖3環(huán)境中，相比于RRT*和基本的RRT*FN算法，文獻(xiàn)[7]所提的改進(jìn)RRT*FN算法和本文算法可以在較短的時(shí)間內(nèi)獲得較短的路徑長(zhǎng)度，平均路徑縮短了0.29%，在平均收斂的時(shí)間上少用時(shí)14.27%.從圖10可以看出，本文算法和文獻(xiàn)[7]所提算法收斂效果較為接近，但改進(jìn)RRT*FN算法可以在更短的時(shí)間內(nèi)獲得較短路徑，總體提高了收斂效率，獲得較短路徑.

4 實(shí)際場(chǎng)景測(cè)試

為了驗(yàn)證實(shí)際場(chǎng)景下本文算法的性能，計(jì)算機(jī)配置為Ubuntu16.04LTS，處理器為Intel I5-6500,主頻為3.2 Hz，運(yùn)行內(nèi)存為16 GB，選擇第3代TurtleBot3雙輪差分驅(qū)動(dòng)機(jī)器人為實(shí)驗(yàn)對(duì)象，實(shí)物如圖11所示.利用開(kāi)源機(jī)器人軟件系統(tǒng)ROS，室內(nèi)10 m×5 m的狹窄通道作為實(shí)驗(yàn)環(huán)境，如圖12所示.紙箱代表環(huán)境中的障礙物，規(guī)劃的路徑如圖13所示.圖中綠色箭頭表示目標(biāo)點(diǎn)的位置，圖13a表示機(jī)器人在初始點(diǎn)的狀態(tài)；圖13b～圖13d是機(jī)器人根據(jù)算法規(guī)劃的路徑躲避障礙物的過(guò)程.可以看出，機(jī)器人在不碰撞障礙物的前提下沿規(guī)劃路徑行駛，成功避障；圖13e是機(jī)器人成功到達(dá)目標(biāo)點(diǎn)的狀態(tài).移動(dòng)機(jī)器人從初始點(diǎn)出發(fā)，沿規(guī)劃好的路徑進(jìn)行導(dǎo)航規(guī)劃，成功到達(dá)目標(biāo)點(diǎn)，實(shí)驗(yàn)驗(yàn)證了所提算法的可行性和有效性.

圖11 Turtlebot burger移動(dòng)機(jī)器人實(shí)物Fig.11 Real object of Turtlebot burger mobile robot

圖12 實(shí)驗(yàn)整體環(huán)境Fig.12 Overall context of the experiment

圖13 實(shí)物路徑規(guī)劃過(guò)程Fig.13 Physical path planning process(a)—機(jī)器人在初始點(diǎn)； (b)—機(jī)器人在避障； (c)—機(jī)器人在避障； (d)—機(jī)器人在避障； (e)—機(jī)器人到達(dá)目標(biāo)點(diǎn).

5 結(jié) 論

1) 以RRT*算法為基礎(chǔ)，結(jié)合實(shí)際機(jī)器人避障模型的應(yīng)用場(chǎng)景，提出基于改進(jìn)的RRT*FN路徑規(guī)劃算法.利用狀態(tài)空間障礙物膨脹處理避免機(jī)器人在路徑上發(fā)生碰撞，在未找到初始路徑時(shí)利用目標(biāo)偏置采樣的方式提高搜索速度，找到初始路徑后使隨機(jī)采樣點(diǎn)圍繞初始路徑進(jìn)行選擇，提升算法效率.

2) 在路徑迭代過(guò)程中，如果樹(shù)中的節(jié)點(diǎn)已超過(guò)固定節(jié)點(diǎn)最大值，但還未找到初始路徑時(shí)，刪除樹(shù)中遠(yuǎn)離目標(biāo)點(diǎn)并且沒(méi)有子節(jié)點(diǎn)的節(jié)點(diǎn)，如果是已找到初始路徑，刪除樹(shù)中遠(yuǎn)離最優(yōu)路徑且沒(méi)有子節(jié)點(diǎn)的節(jié)點(diǎn)，保留高性能節(jié)點(diǎn)，提高算法收斂到最優(yōu)路徑的效率.

3) 采用局部貝塞爾(Bezier)插值對(duì)折線段路徑進(jìn)行平滑處理，通過(guò)改進(jìn)算法滿足初始位置和目標(biāo)位置的方向限制，在不同地圖環(huán)境中進(jìn)行仿真.結(jié)果表明改進(jìn)后的算法在路徑收斂效率、路徑長(zhǎng)度、運(yùn)行時(shí)間及平滑性上相較于其他算法具有明顯優(yōu)勢(shì).在實(shí)際環(huán)境下的實(shí)驗(yàn)驗(yàn)證了所提算法的可行性和有效性.