復(fù)雜多障礙物環(huán)境下改進(jìn)的RRT*路徑規(guī)劃算法

余艷碧

（重慶師范大學(xué)數(shù)學(xué)科學(xué)學(xué)院，重慶 401331）

0 引言

近幾年來(lái)，路徑規(guī)劃算法在移動(dòng)機(jī)器人領(lǐng)域得到廣泛的研究，大量經(jīng)典的路徑規(guī)劃算法被國(guó)內(nèi)外學(xué)者提出。包括基于構(gòu)建虛擬勢(shì)場(chǎng)的人工勢(shì)場(chǎng)法［1］，基于計(jì)算智能的蟻群算法［2］、遺傳算法［3］、基于圖搜索的A*算法［4］，等。然而，上述算法在解決復(fù)雜多障礙環(huán)境下或高維狀態(tài)空間下的路徑規(guī)劃問(wèn)題時(shí)，搜索效率都明顯降低。

基于采樣的路徑規(guī)劃算法被提出，目的是提高算法在復(fù)雜多障礙物環(huán)境或高維狀態(tài)空間下搜索能力?？焖贁U(kuò)展隨機(jī)樹（RRT）［5］是當(dāng)前應(yīng)用最廣泛的基于采樣的算法，作為概率完備的快速搜索算法，RRT可以彌補(bǔ)上述算法搜索速度慢等缺陷。然而，由于傳統(tǒng)的RRT算法是均勻采樣的，引導(dǎo)信息不足，從而算法在復(fù)雜多障礙物環(huán)境下搜索的路徑復(fù)雜曲折，且耗費(fèi)時(shí)間較長(zhǎng)［6］。為了解決基本RRT算法的缺陷，國(guó)內(nèi)外學(xué)者提出了許多改進(jìn)算法。LaValle和Kuffner通過(guò)從起始點(diǎn)和目標(biāo)點(diǎn)兩個(gè)方向同時(shí)向?qū)Ψ綌U(kuò)展形成兩棵隨機(jī)樹，提出了Bi-RRT算法［5］，之后在此基礎(chǔ)上增加貪婪策略，提出了RRT-connect算法［7］，提高了節(jié)點(diǎn)擴(kuò)展效率。

由于RRT的最大缺陷是沒(méi)有考慮可行解的成本，沒(méi)辦法搜索出最優(yōu)路徑。Karaman和Frazoli在2011年提出了漸進(jìn)最優(yōu)的RRT*算法［8］，該算法在RRT擴(kuò)展樹節(jié)點(diǎn)的基礎(chǔ)上根據(jù)最優(yōu)標(biāo)準(zhǔn)來(lái)調(diào)整隨機(jī)樹中的節(jié)點(diǎn)。雖然確保了近似最優(yōu)性，但是該算法仍需對(duì)所有狀態(tài)空間進(jìn)行采樣來(lái)搜索路徑，增加了不必要的內(nèi)存和計(jì)算。為了提高RRT*的搜索效率，可以通過(guò)搜索一條從起始點(diǎn)到目標(biāo)點(diǎn)的初始引導(dǎo)路徑來(lái)確定采樣區(qū)域，比如從智能采樣和路徑優(yōu)化兩方面改進(jìn)RRT*的RRT*-smart算法［9］和借鑒RRT-Connect雙向擴(kuò)展樹優(yōu)勢(shì)的B-RRT*算法［10］，以及Gammell等提出的Informed-RRT*算法［11］，該算法利用RRT*找到初始引導(dǎo)路徑，然后生成由起始點(diǎn)、目標(biāo)點(diǎn)和路徑長(zhǎng)度決定的橢圓作為采樣區(qū)域，從而加速收斂以獲得最優(yōu)路徑。然而，在復(fù)雜的多障礙環(huán)境中，Informed-RRT*算法在搜索初始引導(dǎo)路徑時(shí)容易因?yàn)檫^(guò)多的節(jié)點(diǎn)導(dǎo)致占用大量?jī)?nèi)存，搜索效率較低。

綜合已有研究成果和存在的問(wèn)題，本文提出了一種針對(duì)復(fù)雜多障礙物環(huán)境下基于A*引導(dǎo)域的改進(jìn)RRT*路徑規(guī)劃算法。該算法將A*算法的最優(yōu)性和RRT*算法快速性結(jié)合起來(lái)，首先利用A*算法在柵格化后的低分辨率地圖中搜索出最優(yōu)路徑，并將該路徑作為引導(dǎo)路徑生成引導(dǎo)域，限制RRT*算法的采樣區(qū)域，從而提高其采樣效率，然后在引導(dǎo)域中不斷迭代搜索，得到一條漸進(jìn)最優(yōu)且無(wú)碰撞的路徑，最后基于B樣條曲線的路徑優(yōu)化，使移動(dòng)機(jī)器人能夠平穩(wěn)安全到達(dá)終點(diǎn)。通過(guò)對(duì)RRT*算法與Informed-RRT*算法的仿真比較實(shí)驗(yàn)，驗(yàn)證本文算法的有效性和優(yōu)越性。

1 RRT*算法基本原理

1998年，LaValle教授根據(jù)最優(yōu)控制、隨機(jī)采樣算法等理論提出了一種隨機(jī)采樣增量式運(yùn)動(dòng)規(guī)劃算法，即RRT算法［5］。該算法是從起始點(diǎn)開始在狀態(tài)空間中隨機(jī)采樣向目標(biāo)點(diǎn)擴(kuò)展的一個(gè)樹狀結(jié)構(gòu)，直至找到一條連接起點(diǎn)與終點(diǎn)的路徑。RRT算法的搜索方法避免了對(duì)整個(gè)環(huán)境地圖的模型構(gòu)建，加快了算法的收斂，在全局路徑規(guī)劃中有廣泛的應(yīng)用和改進(jìn)，其算法的有效性和完備性已經(jīng)得到了充分的驗(yàn)證，但算法仍然存在搜索效率低，路徑不優(yōu)，占用內(nèi)存過(guò)大等問(wèn)題。

Karaman和Frazzoli于2011年提出漸進(jìn)最優(yōu)性（最短距離）的RRT*算法［8］，該算法基本結(jié)構(gòu)和RRT算法相似，都將初始狀態(tài)作為根節(jié)點(diǎn)增量式地?cái)U(kuò)展隨機(jī)樹，在避開障礙物的同時(shí)，當(dāng)樹的節(jié)點(diǎn)到達(dá)預(yù)設(shè)目標(biāo)狀態(tài)附近區(qū)域則結(jié)束對(duì)狀態(tài)空間的搜索。隨機(jī)樹T最初只有一個(gè)根節(jié)點(diǎn)qinit［圖1（a）中節(jié)點(diǎn)0］，每次迭代都會(huì)在狀態(tài)空間中隨機(jī)采樣一個(gè)點(diǎn)的qrand，并計(jì)算隨機(jī)樹T中距離qrand最近的節(jié)點(diǎn)qnear［圖1（a）中節(jié)點(diǎn)5］，如果從最近鄰節(jié)點(diǎn)qnear向qrand擴(kuò)展一定預(yù)設(shè)步長(zhǎng)移動(dòng)可達(dá)即無(wú)碰撞，那么這個(gè)可達(dá)的點(diǎn)記為新節(jié)點(diǎn)qnew［圖1（a）中節(jié)點(diǎn)9］，并將qnew加入隨機(jī)樹T中，此時(shí)qnear成為qnew的父節(jié)點(diǎn)，RRT算法便是重復(fù)上述過(guò)程最終規(guī)劃出路徑。但RRT*算法在這里會(huì)啟動(dòng)重選父節(jié)點(diǎn)過(guò)程，即qnear不一定是最后的父節(jié)點(diǎn)。當(dāng)確定qnew后，根據(jù)近鄰準(zhǔn)則找出qnew周圍一組近鄰節(jié)點(diǎn)［圖1（b）中節(jié)點(diǎn)4、5、7、8］，并計(jì)算Qnew中每一個(gè)近鄰節(jié)點(diǎn)到根節(jié)點(diǎn)qinit的累積成本與該近鄰節(jié)點(diǎn)到qnew的距離成本的和，取這些和的最小值所對(duì)應(yīng)的近鄰節(jié)點(diǎn)作為qnew的新父節(jié)點(diǎn)q′near［例如圖1（a）中，qnew經(jīng)過(guò)節(jié)點(diǎn)5索引到qinit的路徑為9-5-2-0，累積成本為9；qnew經(jīng)過(guò)節(jié)點(diǎn)7索引至qinit的路徑為9-7-2-0，累積成本為7，代價(jià)最小，故節(jié)點(diǎn)7為qnew新的父節(jié)點(diǎn)q′near，如圖1（b）所示］，并以q′near作為父節(jié)點(diǎn)將qnew加入隨機(jī)樹T中。接下來(lái)優(yōu)化Qnew中每一個(gè)近鄰節(jié)點(diǎn)，具體是讓qnew代替每一個(gè)近鄰點(diǎn)的父節(jié)點(diǎn)，如果此時(shí)近鄰點(diǎn)到根節(jié)點(diǎn)qinit的累積成本比qnew代替前更小，則放棄原先的父節(jié)點(diǎn)而把qnew作為該點(diǎn)的父節(jié)點(diǎn)重新連接到近鄰節(jié)點(diǎn)上［如圖1（c）中，節(jié)點(diǎn)8以qnew為父節(jié)點(diǎn)時(shí)索引至qinit的路徑為8-9-7-2-0，累積成本為8，而不以qnew為父節(jié)點(diǎn)時(shí)索引至qinit的路徑為8-5-2-0，累積成本為10，故修剪隨機(jī)樹T，將節(jié)點(diǎn)8的父節(jié)點(diǎn)改為qnew，如圖1（d）所示］，從而對(duì)隨機(jī)樹T實(shí)現(xiàn)逐步優(yōu)化。

圖1 RRT*算法修剪節(jié)點(diǎn)示意圖

2 A*算法

2.1 A*算法基本原理

A*算法是一種基于深度優(yōu)先算法和廣度優(yōu)先算法的啟發(fā)式融合搜索算法，基于深度優(yōu)先算法能以最快速度找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，但不能保證最優(yōu)性，基于廣度優(yōu)先算法則必然能找到最短路徑，但需要遍歷所有結(jié)點(diǎn)，計(jì)算復(fù)雜。而A*算法融合了這兩種算法的優(yōu)點(diǎn)，基于啟發(fā)函數(shù)構(gòu)建了代價(jià)函數(shù)，考慮了新結(jié)點(diǎn)距離初始點(diǎn)和目標(biāo)點(diǎn)的代價(jià)［12］。

2.2 低分辨率環(huán)境地圖下的A*算法

A*算法是在柵格化后的環(huán)境地圖中搜索，搜索過(guò)程中將每一個(gè)柵格作為一個(gè)狀態(tài)，當(dāng)柵格尺寸越大，即地圖分辨率越低，則環(huán)境地圖分解成的狀態(tài)數(shù)就越少，使得當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)坐標(biāo)不變時(shí)，A*算法執(zhí)行的時(shí)間會(huì)變短，規(guī)劃出來(lái)的路徑也會(huì)發(fā)生相應(yīng)變化［6］。圖2是大小為128×128的環(huán)境地圖，起始點(diǎn)坐標(biāo)（11，11），目標(biāo)點(diǎn)坐標(biāo)（100，100）。對(duì)于圖中復(fù)雜的障礙物環(huán)境，本文分別進(jìn)行128×128，64×64和43×43的柵格化，A*算法在三種不同分辨率下的規(guī)劃結(jié)果如圖2（a）、（b）、（c）所示。

圖2 三種分辨率下A*算法規(guī)劃結(jié)果

在三種不同分辨率的環(huán)境地圖下，A*算法規(guī)劃時(shí)間分別為477 ms、64 ms和23 ms，由此可以看出柵格地圖分辨率越小，A*算法的規(guī)劃時(shí)間大幅減少，更重要的是，A*算法在不同分辨率下規(guī)劃出來(lái)的路徑趨勢(shì)是一致的。

3 基于A*引導(dǎo)域的RRT＊算法

3.1 A*引導(dǎo)域

記A*算法規(guī)劃路徑經(jīng)過(guò)的第i個(gè)柵格中心點(diǎn)坐標(biāo)為A i（x i,y i），則引導(dǎo)域上界點(diǎn)集定義為：

下界點(diǎn)集定義為：

k為單個(gè)引導(dǎo)路徑點(diǎn)橫縱坐標(biāo)移動(dòng)長(zhǎng)度，再將上界點(diǎn)集和下界點(diǎn)集分別按順序連接成一條線得到上界和下界，最后連接上下界形成封閉引導(dǎo)域。圖3為引導(dǎo)域示意圖，藍(lán)色線條為A*算法規(guī)劃出的引導(dǎo)路徑，紅色線條內(nèi)的區(qū)域?yàn)樯傻囊龑?dǎo)域。

圖3 A*算法生成的引導(dǎo)域

3.2 采樣策略

由于RRT*算法采樣范圍大且隨機(jī)性強(qiáng)，缺乏一定的引導(dǎo)性，因此本文在3.1節(jié)引導(dǎo)域的基礎(chǔ)上，采用目標(biāo)偏向策略，以一定的概率p選擇目標(biāo)點(diǎn)作為采樣點(diǎn)進(jìn)行隨機(jī)樹擴(kuò)展，提高隨機(jī)樹向目標(biāo)點(diǎn)擴(kuò)展的概率，以達(dá)到減少采樣點(diǎn)的個(gè)數(shù)，加速隨機(jī)樹的形成。

在采樣過(guò)程中，為了判斷采樣點(diǎn)是否在引導(dǎo)域外，即判斷采樣點(diǎn)qnew和其父節(jié)點(diǎn)qnear連線與引導(dǎo)域邊界是否發(fā)生碰撞，需進(jìn)行碰撞檢測(cè)。如果發(fā)生碰撞，則放棄此次采樣重新選擇采樣點(diǎn)。如圖4，檢測(cè)是否碰撞，即判斷線段qnearqnew和線段p1p2是否相交可以利用向量的叉乘。先固定線段qnearqnew，然后以qnear為軸，計(jì)算與是否異號(hào)；再固定線段p1p2，然后以p1為軸，計(jì)算是否異號(hào)。當(dāng)上述的叉乘都異號(hào)的時(shí)候，說(shuō)明兩條線段相交，發(fā)生碰撞，則重新選擇采樣點(diǎn)。

圖4 碰撞檢測(cè)

3.3 基于B樣條曲線的路徑優(yōu)化

由于RRT*算法隨機(jī)采樣的特性，導(dǎo)致算法最終搜索出來(lái)的路徑一般是曲折、不平滑的，不利于移動(dòng)機(jī)器人進(jìn)行路徑追蹤。而B樣條曲線具有連續(xù)性、局部可調(diào)整性等優(yōu)點(diǎn)，使得該曲線在路徑規(guī)劃中被廣泛的使用［13］，因此本文提出了基于三次B樣條曲線的路徑優(yōu)化處理，與算法相結(jié)合生成一條平穩(wěn)光滑的路徑，確保移動(dòng)機(jī)器人運(yùn)動(dòng)的連續(xù)性和平穩(wěn)性。

Riesenfeld等人在1972年構(gòu)造了B樣條曲線，將Bernstein基函數(shù)用B樣條基函數(shù)來(lái)代替。B樣條曲線是分段組成的，每一段的參數(shù)區(qū)間為[0,1]，因此克服了貝塞爾曲線改變?nèi)我饪刂泣c(diǎn)其曲線上所有點(diǎn)也改變的缺陷，使得樣條曲線具有局部修改的特性［14］，這種優(yōu)點(diǎn)使得B樣條曲線廣泛應(yīng)用于路徑規(guī)劃中。

設(shè)有n+1個(gè)控制點(diǎn)P0,P1,…,P n，則k階B樣條曲線的數(shù)學(xué)表達(dá)式為：

其中，B i,k（u）是第i個(gè)k階B樣條基函數(shù)，u是自變量。

基函數(shù)具有如下Cox-deBoor遞推式

式（4）和式（5）中u i是一組被稱為節(jié)點(diǎn)矢量的非遞減序列的連續(xù)變化值。

4 仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證本文提出的復(fù)雜多障礙物環(huán)境下基于A*引導(dǎo)域的RRT*算法搜索路徑的有效性和快速性，本文在復(fù)雜環(huán)境地圖下對(duì)該算法的規(guī)劃結(jié)果與RRT*、Informed-RRT*兩種算法的規(guī)劃結(jié)果進(jìn)行仿真對(duì)比，最后給出所有算法仿真實(shí)驗(yàn)的結(jié)果與分析。

本文將仿真地圖大小設(shè)置為128×128，起始點(diǎn)坐標(biāo)（11，11），目標(biāo)點(diǎn)坐標(biāo)（100，100），障礙物由多個(gè)矩形和若干線條組成，分別對(duì)本文算法，RRT*算法和Informed-RRT*算法進(jìn)行10次仿真，最大迭代次數(shù)為1000次，擴(kuò)展步長(zhǎng)為5，目標(biāo)采樣率為10%，并將三種算法找到初始路徑時(shí)的初始路徑平均長(zhǎng)度和初始平均搜索時(shí)間以及到達(dá)最大迭代次數(shù)后的最終路徑平均長(zhǎng)度進(jìn)行對(duì)比分析。圖5為三種算法最終規(guī)劃出的路徑示意圖，其中紅色曲線為搜索的最終路徑。

圖5 三種算法的最終規(guī)劃結(jié)果

表1為仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比。從表中可以看出，本文算法搜索出的初始路徑平均長(zhǎng)度和初始平均搜索時(shí)間比另外兩種算法的都要少，與Informed-RRT*算法的初始路徑長(zhǎng)度和初始搜索時(shí)間對(duì)比分別減少了5.4%和46.01%，搜索時(shí)間大幅下降，這說(shuō)明本文算法的收斂速度更快。而從達(dá)到最大迭代次數(shù)后后的最終路徑平均長(zhǎng)度上來(lái)看，本文算法也是優(yōu)于另外兩種算法的，并且對(duì)比本文算法的初始路徑長(zhǎng)度和最終路徑長(zhǎng)度，在大部分情況下，本文算法搜索出的初始路徑已經(jīng)趨于最優(yōu)。以此證明了本文算法的有效性和優(yōu)越性。

表1 仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比

5 結(jié)語(yǔ)

針對(duì)RRT*算法和其一些改進(jìn)算法存在的搜索效率低、占用內(nèi)存過(guò)多等不足，本文提出了一種復(fù)雜多障礙物環(huán)境下基于A*引導(dǎo)域的改進(jìn)RRT*路徑規(guī)劃算法。該算法有效地將A*算法的最優(yōu)性和RRT*算法快速性結(jié)合起來(lái)，第一步利用A*算法在柵格化后的低分辨率仿真地圖中搜索出最優(yōu)路徑，并將該路徑作為引導(dǎo)路徑生成引導(dǎo)域，限制RRT*算法的采樣區(qū)域，從而提高其采樣效率，然后在引導(dǎo)域中不斷迭代搜索，得到從起始點(diǎn)到目標(biāo)點(diǎn)的漸進(jìn)最優(yōu)且無(wú)碰撞的路徑，最后基于B樣條曲線的路徑優(yōu)化，生成一條平滑且曲率連續(xù)的優(yōu)化路徑，使移動(dòng)機(jī)器人能夠平穩(wěn)安全到達(dá)終點(diǎn)。最后，通過(guò)對(duì)RRT*算法與Informed-RRT*算法的仿真比較實(shí)驗(yàn)，驗(yàn)證本文算法的有效性和優(yōu)越性。