基于改進(jìn)傳統(tǒng)RRT算法的機(jī)器人路徑規(guī)劃研究

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

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

1 算法描述

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

圖1 原理圖

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

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

(1)

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

Fr=-?[Ur(X)]=

(2)

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

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

(3)

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

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

1)Matlab仿真平臺(tái)

圖2、圖3分別是是由Matlab搭建的兩張地圖。地圖規(guī)模均是[500，500]，方框即為地圖邊界，黑色區(qū)域代表障礙物，路徑規(guī)劃的起始點(diǎn)為[1，1]，目標(biāo)點(diǎn)為[490，490]，固定步長(zhǎng)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用時(shí)40.230 s，圖5用時(shí)37.560 s。觀察圖中傳統(tǒng)RRT算法所得路徑，可以明顯看出，在路徑經(jīng)過障礙物附近時(shí)平滑性大大降低，尤其是在地圖2這種障礙物空間復(fù)雜的情況下，路徑點(diǎn)轉(zhuǎn)折更大。

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

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

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

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

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

3)曲線擬合比較

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

圖8 RRT算法擬合

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

3 結(jié)語

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