買智強(qiáng)，辛舟，張雪琪

(蘭州理工大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730050)

0 引言

近年來，機(jī)器人技術(shù)的快速發(fā)展使得許多與機(jī)器人相關(guān)的研究變得熱門起來，路徑規(guī)劃就是其中之一。為了使機(jī)器人安全無障礙地從起始點(diǎn)平穩(wěn)運(yùn)行到目標(biāo)點(diǎn)，給機(jī)器人規(guī)劃一條無障礙路徑就是路徑規(guī)劃的主要內(nèi)容。在此基礎(chǔ)上，隨著國內(nèi)外學(xué)者的不斷研究，逐漸對路徑規(guī)劃提出了新的要求，如最短、最優(yōu)、最快等。為了達(dá)到這些更高的要求，不同學(xué)者提出了不同的解決方法。

快速擴(kuò)展隨機(jī)樹RRT算法(rapidly-exploring random tree)是其中比較典型的一種算法，由LAVALLE S M于1998年提出[1]。這是一種典型的樹狀結(jié)構(gòu)搜索算法，以初始狀態(tài)作為根節(jié)點(diǎn)，通過在狀態(tài)空間中進(jìn)行隨機(jī)采樣，不斷擴(kuò)展，逐步覆蓋狀態(tài)空間的自由區(qū)域，并最終覆蓋整個狀態(tài)空間，從而獲得一條從初始狀態(tài)到目標(biāo)狀態(tài)的路徑。RRT算法結(jié)構(gòu)簡單，不需要對障礙物進(jìn)行精確的建模，不僅適用于二維空間，三維空間同樣可以[2]。其缺點(diǎn)在于得到的路徑并非最優(yōu)且由于是隨機(jī)規(guī)劃，效率也并不高。FRAZZOLI E等針對RRT算法規(guī)劃出的路徑不是最優(yōu)解，提出RRT*算法，通過對新節(jié)點(diǎn)至根節(jié)點(diǎn)的權(quán)值比較，找出最優(yōu)解[3]，也有學(xué)者提出過刪除低性能節(jié)點(diǎn)規(guī)劃高緯機(jī)器人路徑[4]，或者是將局部優(yōu)化引入，進(jìn)而提高效率[5]。雖然在一定程度上提高了路徑的平滑性，但卻使得效率大大降低。LAVALLE S M等則提出雙向隨機(jī)樹(bidirectional RRT，bi-RRT)用來規(guī)劃高緯度路徑[6]。bi-RRT采用起始點(diǎn)和目標(biāo)點(diǎn)分別生成一顆自己的隨機(jī)樹，直到二者相遇即得路徑。這種方法雖然縮短了時間，但卻未能得到最優(yōu)路徑。

近年來，隨著智能算法的興起，結(jié)合強(qiáng)化學(xué)習(xí)，CABALLERO F[7]等提出基于RRT*擴(kuò)展的快速學(xué)習(xí)隨機(jī)樹法(rapidly-exploring learning trees，RLT*)。對于RRT算法的隨機(jī)性，很多人選擇引入人工勢場法來進(jìn)行改善，且取得了不錯的效果[8-9]。但對于密集障礙物附近的路徑仍不能做到動態(tài)調(diào)整以達(dá)到最優(yōu)。本文則基于人工勢場引入障礙物對目標(biāo)點(diǎn)的斥力函數(shù)，動態(tài)調(diào)整RRT算法在經(jīng)過障礙物附近時的步長，以此來改善路徑。

1 算法描述

RRT算法基本原理如下：首先掃描全局地圖，獲取起始點(diǎn)Po、目標(biāo)點(diǎn)Pg及障礙物等相關(guān)信息，然后以Po為樹的根節(jié)點(diǎn)，在地圖內(nèi)隨機(jī)選取一個點(diǎn)Prand，比較隨機(jī)點(diǎn)Prand與每一個樹節(jié)點(diǎn)(包括根節(jié)點(diǎn))的距離，選擇距離最短的那個。此時最短距離所對應(yīng)的那個樹節(jié)點(diǎn)作為該隨機(jī)點(diǎn)Prand的父節(jié)點(diǎn)Pnear，沿Pnear到Prand方向以步長q為間隔選取一個新點(diǎn)Pnew，判斷Pnear到Pnew是否經(jīng)過障礙物。若是則重新選取隨機(jī)點(diǎn)，再比較再判斷；若未經(jīng)過障礙物，則將Pnew加入到樹節(jié)點(diǎn)，作為新的樹節(jié)點(diǎn)。不斷重復(fù)這個過程，直到找到目標(biāo)點(diǎn)Pg，或者達(dá)到最大迭代次數(shù)[10]。原理圖如圖1所示。

圖1 原理圖

從圖1中可以看到，傳統(tǒng)RRT算法在規(guī)劃路徑時擴(kuò)展步長是不變的，這樣在一定程度上大大簡化了算法，但其規(guī)劃出的路徑效果并不好。步長大小完全取決于使用者，長步長一般適用于那些路徑長且障礙物情況簡單的場合，短步長一般適用于路徑較短但路況復(fù)雜，需要躲避的障礙物多的情況，但實(shí)際應(yīng)用中碰到的情況往往是以上二者混合的。對于一段長且復(fù)雜的路徑，既希望能以長步長迅速接近目標(biāo)，又希望在經(jīng)過復(fù)雜障礙物的時候步長能變短一些，好讓得出的軌跡在障礙物附近相對平穩(wěn)。為解決這個問題，結(jié)合人工勢場中有關(guān)于障礙物斥力場及斥力函數(shù)的想法，本文提出在傳統(tǒng)RRT算法中引入斥力場及斥力函數(shù)。

人工勢場法是一種虛擬力法，多用于機(jī)器人路徑規(guī)劃，其基本原理是在障礙物和目標(biāo)點(diǎn)附近構(gòu)建勢力場，通過對機(jī)器人位置的勢場強(qiáng)度求負(fù)梯度后得到勢場力。其中，目標(biāo)點(diǎn)產(chǎn)生引力，障礙物產(chǎn)生斥力，二者的合力方向?yàn)闄C(jī)器人運(yùn)動方向[11]。本文不涉及引力勢及引力函數(shù)，對其不做介紹。目前常用的斥力函數(shù)如下：

(1)

式中：Ur(X)表示在X點(diǎn)處的斥力場；η是斥力度因子；ρ0是障礙物的影響范圍；X0表示障礙物位置；ρ(X,X0)表示位置點(diǎn)X與障礙物之間的最短距離。當(dāng)距離>ρ0時，機(jī)器人的運(yùn)動不受障礙物的影響。斥力場產(chǎn)生的斥力為斥力場的負(fù)梯度，其表達(dá)式為

Fr=-?[Ur(X)]=

(2)

式中：Fr表示斥力；?ρ(X,X0)表示障礙物位置指向X位置的單位向量。

當(dāng)機(jī)器人距離障礙物較遠(yuǎn)時，采用長步長迅速靠近以減少時間，一旦進(jìn)入障礙物影響范圍ρ0，斥力函數(shù)啟動，減小步長。具體變化如下：

(3)

式中：S為改進(jìn)后的步長；k為步長系數(shù)；|Fr|為斥力Fr取標(biāo)量。當(dāng)ρ≥ρ0，即未進(jìn)入障礙物影響范圍時，步長固定為q；當(dāng)ρ<ρ0機(jī)器人進(jìn)入障礙物影響范圍時，動態(tài)步長啟動，距離障礙物越近，斥力Fr越大，步長S越小。

2 仿真結(jié)果及分析

1)Matlab仿真平臺

圖2、圖3分別是是由Matlab搭建的兩張地圖。地圖規(guī)模均是[500，500]，方框即為地圖邊界，黑色區(qū)域代表障礙物，路徑規(guī)劃的起始點(diǎn)為[1，1]，目標(biāo)點(diǎn)為[490，490]，固定步長q=30，障礙物影響范圍ρ0取50。

圖2 仿真地圖1

圖3 仿真地圖2

2)傳統(tǒng)RRT算法與改進(jìn)算法

傳統(tǒng)RRT算法路徑規(guī)劃結(jié)果如圖4、圖5所示。圖中，與目標(biāo)點(diǎn)相連的黑色細(xì)線表示最終路徑。為了便于觀察，程序設(shè)定在每一次打點(diǎn)后都停頓1 s，最終圖4用時40.230 s，圖5用時37.560 s。觀察圖中傳統(tǒng)RRT算法所得路徑，可以明顯看出，在路徑經(jīng)過障礙物附近時平滑性大大降低，尤其是在地圖2這種障礙物空間復(fù)雜的情況下，路徑點(diǎn)轉(zhuǎn)折更大。

圖4 地圖1規(guī)劃結(jié)果

圖5 地圖2規(guī)劃結(jié)果

改進(jìn)后的RRT算法規(guī)劃路徑如圖6、圖7所示。為了獲取更加準(zhǔn)確的距離信息，包絡(luò)障礙物邊緣，加入動態(tài)步長后，路徑點(diǎn)在障礙物附近的路徑平滑程度大大提高。地圖1用時48.532 s，地圖2用時52.548 s。從時間上看，圖6相比于圖4，改進(jìn)后算法多用時25.58%，圖7相比于圖5多用時22.4%。

圖6 地圖1規(guī)劃結(jié)果

圖7 地圖2規(guī)劃結(jié)果

3)曲線擬合比較

由于RRT算法得出的軌跡是一條折線段，機(jī)器人在經(jīng)過拐角時會出現(xiàn)運(yùn)動不平穩(wěn)的現(xiàn)象。為了解決這個問題，選擇對路徑進(jìn)行光滑處理。貝塞爾曲線是于1962年由法國工程師皮埃爾·貝塞爾(Pierre Bézier)所發(fā)表，用貝塞爾曲線來為汽車的主體進(jìn)行設(shè)計(jì)。后經(jīng)人不斷改良[12]，廣泛應(yīng)用于二維平面。本文采用該方法進(jìn)行路徑曲線的擬合。圖8、圖9為經(jīng)過光滑處理后的最終路徑。顯然，改進(jìn)后的RRT算法得到的路徑在經(jīng)過障礙物附近時更為光滑。

圖8 RRT算法擬合

圖9 改進(jìn)RRT算法擬合

3 結(jié)語

本文完成了對傳統(tǒng)RRT算法的改進(jìn)，基于人工勢場法中的斥力場及斥力函數(shù)，加入了動態(tài)步長，對RRT算法經(jīng)過障礙物附近時的路徑進(jìn)行優(yōu)化，再對得到的路徑進(jìn)行貝塞爾曲線擬合，使得改進(jìn)后的算法在經(jīng)過障礙物附近時路徑的平滑程度大大提高，缺點(diǎn)是在一定程度上延長了規(guī)劃時間。后續(xù)的研究將會更加注重規(guī)劃時間的縮短。