基于雙向生長(zhǎng)改進(jìn)的RRT 機(jī)器人路徑規(guī)劃算法

李曉偉，于會(huì)山

（聊城大學(xué)物理科學(xué)與信息工程學(xué)院，聊城252059）

0 引言

在移動(dòng)機(jī)器人的研究領(lǐng)域中，路徑規(guī)劃算法是最為重要而且不可缺少的組成部分，是移動(dòng)機(jī)器人在障礙物環(huán)境下實(shí)現(xiàn)自主移動(dòng)導(dǎo)航的基礎(chǔ)。隨著路徑規(guī)劃算法研究的不斷深入，研究人員已經(jīng)改善并優(yōu)化了很多傳統(tǒng)的路徑規(guī)劃算法，并不斷提出了很多新的路徑規(guī)劃算法，使得路徑規(guī)劃算法的研究趨勢(shì)已經(jīng)逐步偏向復(fù)雜化和智能化[1]。常用的路徑規(guī)劃算法包括A*、人工勢(shì)場(chǎng)法、概率路線圖（PRM）算法、蟻群算法、遺傳算法以及諸多啟發(fā)式算法等[2]。這些算法的收斂速度過于緩慢，需要提前對(duì)未知的障礙物空間環(huán)境進(jìn)行建模，當(dāng)環(huán)境改變時(shí)無法在未知的障礙空間中使用。

快速擴(kuò)展隨機(jī)樹算法由Steven M.LaValle 于1998年首次提出來[3]。RRT（快速探索隨機(jī)樹）算法能有效解決這些傳統(tǒng)路徑規(guī)劃算法存在的問題。與其他傳統(tǒng)路徑規(guī)劃算法相比，RRT 算法不僅具有快速、高效的特點(diǎn)，而且不需要依賴于環(huán)境建模就能夠有效地解決未知復(fù)雜障礙物空間和高維動(dòng)態(tài)環(huán)境的路徑規(guī)劃問題。RRT 是基于空間中樹木分支的創(chuàng)建，它迭代地對(duì)新的狀態(tài)進(jìn)行采樣，然后將離每個(gè)樣本更近的現(xiàn)有節(jié)點(diǎn)指向一個(gè)新的樣本，從而形成一個(gè)具有分支的樹。即在空間里，從根節(jié)點(diǎn)開始，進(jìn)行任意采樣，當(dāng)采樣形成的新點(diǎn)為目標(biāo)點(diǎn)或新點(diǎn)與目標(biāo)點(diǎn)的距離小于一定步長(zhǎng)時(shí)，則形成一條由根節(jié)點(diǎn)至目標(biāo)點(diǎn)的路徑[4]。

1 RRT算法

在快速擴(kuò)展隨機(jī)樹中，qinit為樹的根節(jié)點(diǎn)（起始點(diǎn)），qgoal為目標(biāo)點(diǎn)，qrand為任意擴(kuò)展的隨機(jī)節(jié)點(diǎn)，qnear為每次擴(kuò)展時(shí)距離qrand最近的節(jié)點(diǎn)，qnew為新節(jié)點(diǎn)[5]。首先在搜索空間中采用隨機(jī)的方式生成隨機(jī)樹的隨機(jī)擴(kuò)展節(jié)點(diǎn)qrand，然后遍歷當(dāng)前已有的隨機(jī)樹，從樹中的節(jié)點(diǎn)里尋找距離qrand最近的節(jié)點(diǎn)qnear，在qnear向qrand延伸一定步長(zhǎng)之后可以得到新節(jié)點(diǎn)qnew，之后需要對(duì)新節(jié)點(diǎn)進(jìn)行碰撞檢測(cè)，若qnew碰到障礙物便把這個(gè)節(jié)點(diǎn)舍去；反之若qnew沒有碰到障礙物，且在規(guī)定空間內(nèi)，即把qnew添加到擴(kuò)展樹中，可知此時(shí)qnew的父節(jié)點(diǎn)為qnear，按照上述方式繼續(xù)擴(kuò)展，直到qnew和qgoal之間的距離小于一定步長(zhǎng)時(shí)，便規(guī)劃出了一條由根節(jié)點(diǎn)至目標(biāo)點(diǎn)的路徑[6]?；舅惴ㄈ缦滤荆?/p>

基本的RRT 算法是從根節(jié)點(diǎn)開始擴(kuò)展的RRT 樹搜索規(guī)定空間。若有兩棵隨機(jī)樹，分別從根節(jié)點(diǎn)和目標(biāo)點(diǎn)來搜索狀態(tài)空間，速度會(huì)相對(duì)較快。因此，提出一種基于雙向擴(kuò)展的算法，即RRT-connect 算法。

在搜索空間內(nèi)，定義了兩棵隨機(jī)樹。一棵從起始點(diǎn)開始擴(kuò)展，另一棵從目標(biāo)點(diǎn)擴(kuò)展，兩棵隨機(jī)樹分別是Ta和Tb，任意選取兩隨機(jī)樹Ta和Tb中離qrand最近節(jié)點(diǎn)qnear，再從qnear向qrand擴(kuò)展得到qnew，直到兩棵樹的qnew小于一定步長(zhǎng)，則可確定Ta和Tb連通，即路徑規(guī)劃成功[7]。其算法如下所示：

RRT 算法在采樣時(shí)，使用啟發(fā)式采樣，即當(dāng)隨機(jī)樹在擴(kuò)展時(shí)，通過隨機(jī)概率p1（0≤p1＜1）確定qrand為qgoal的可能性。當(dāng)目標(biāo)點(diǎn)就是采樣點(diǎn)P（qgoal=qrand）=p1，能夠提高RRT 樹對(duì)目標(biāo)點(diǎn)擴(kuò)展的能力，使隨機(jī)樹在生長(zhǎng)時(shí)更有指向性，樹的生長(zhǎng)過程會(huì)相對(duì)收斂。

隨機(jī)樹在擴(kuò)展新節(jié)點(diǎn)時(shí)，基本RRT 中qnew的計(jì)算如下：

qnew=qnear＋ρ（qrand-qnear）/||qrand-qnear||

ρ 為隨機(jī)樹生長(zhǎng)時(shí)的步長(zhǎng)，（qrand-qnear）表示兩向量單位化，||qrand-qnear|||qrand-qnear||為qrand和qnear的歐氏距離。

當(dāng)把目標(biāo)引力思想添加到基本RRT 后，qnew的計(jì)算如下：

qnew=qnear＋ρ1（qrand-qnear）/||qrand-qnear||+ρ2（qgoal-qnear）/||qgoal-qnear||

ρ1是隨機(jī)樹向任意點(diǎn)方向擴(kuò)展的步長(zhǎng)，ρ2是隨機(jī)樹向目標(biāo)點(diǎn)方向擴(kuò)展的步長(zhǎng)，為qgoal和qnear的歐氏距離。

基本RRT 算法中的新節(jié)點(diǎn)是通過最近節(jié)點(diǎn)向隨機(jī)點(diǎn)延伸一個(gè)步長(zhǎng)得到的，新節(jié)點(diǎn)只朝著隨機(jī)點(diǎn)生長(zhǎng)，可能會(huì)偏離目標(biāo)點(diǎn)，規(guī)劃出的路徑耗時(shí)長(zhǎng)，距離遠(yuǎn)。當(dāng)添加目標(biāo)引力函數(shù)后，qnew也會(huì)向qgoal方向生長(zhǎng)。當(dāng)ρ1＞ρ2時(shí)，新節(jié)點(diǎn)會(huì)偏向隨機(jī)點(diǎn)方向，這就比較接近基本RRT 算法；當(dāng)ρ1＜ρ2時(shí)，新節(jié)點(diǎn)會(huì)偏向于目標(biāo)點(diǎn)方向，加快隨機(jī)樹的收斂性。

2 基于雙向生長(zhǎng)改進(jìn)的RRT算法

雙向RRT 算法嘗試擴(kuò)展兩棵樹到彼此，相比于基本RRT 提高了算法的收斂速度。不過因基本RRT 在擴(kuò)展時(shí)有一定的隨機(jī)性，從而導(dǎo)致了雙向RRT 無指向性，缺乏穩(wěn)定性。針對(duì)于雙向生長(zhǎng)具有不穩(wěn)定的問題，提出一種雙向生長(zhǎng)改進(jìn)的路徑規(guī)劃算法[8]。該算法將雙向生長(zhǎng)和目標(biāo)引力思想相結(jié)合起來。在RRT 雙向生長(zhǎng)算法中引入目標(biāo)吸引函數(shù)，目標(biāo)吸引函數(shù)使隨機(jī)樹向目標(biāo)生長(zhǎng)。這不僅避免了隨機(jī)樹搜索全局空間，很大程度上降低算法的計(jì)算復(fù)雜度，提高了實(shí)時(shí)性，改善了路徑的平穩(wěn)性[9]。

其主要思想是兩棵隨機(jī)樹Ta和Tb分別從起始點(diǎn)和目標(biāo)點(diǎn)開始擴(kuò)展，若Ta從qinit開始生長(zhǎng)，通過隨機(jī)擴(kuò)展的方式得到采樣點(diǎn)，之后再結(jié)合目標(biāo)引力思想擴(kuò)展得到新節(jié)點(diǎn)。Tb從qgoal開始生長(zhǎng)，生長(zhǎng)時(shí)把Ta擴(kuò)展得到的新節(jié)點(diǎn)當(dāng)作目標(biāo)點(diǎn)，采用和Ta相同的方式來擴(kuò)展。Ta和Tb接下來在生長(zhǎng)時(shí)都把彼此上一步擴(kuò)展得到的新節(jié)點(diǎn)當(dāng)成目標(biāo)點(diǎn)，來進(jìn)行下一步生長(zhǎng)，直到兩棵樹的距離小于一定步長(zhǎng)，即路徑規(guī)劃成功。

圖1 改進(jìn)RRT構(gòu)建過程

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

通過用MATLAB 軟件來驗(yàn)證文中改進(jìn)的算法，設(shè)置根節(jié)點(diǎn)坐標(biāo)是（0,0），目標(biāo)點(diǎn)坐標(biāo)是（490,490）。規(guī)劃一條由根節(jié)點(diǎn)至目標(biāo)點(diǎn)的有效路徑。

圖2

表1

圖3

表2

圖4

表3

通過在不同環(huán)境下進(jìn)行仿真試驗(yàn)，得到如各表中的實(shí)驗(yàn)數(shù)據(jù)。從而可以得出，相對(duì)于基本RRT 算法來說，Bi-RRT 路徑規(guī)劃用時(shí)和路徑長(zhǎng)度更優(yōu)些。但本文改進(jìn)之后的算法相比其余兩種算法路徑更優(yōu)，運(yùn)行時(shí)間更短，接近最優(yōu)路徑。

4 結(jié)語

本文介紹了路徑規(guī)劃的原始算法、雙向擴(kuò)展的方式和加入目標(biāo)引力思想的雙向擴(kuò)展算法。本文改進(jìn)的路徑規(guī)劃算法在擴(kuò)展時(shí)減少了路徑的隨機(jī)性，讓隨機(jī)樹生長(zhǎng)更具有目標(biāo)性和方向性，但其仍然存在許多不足，例如規(guī)劃出的路徑不平滑，需要進(jìn)一步進(jìn)行完善。