資源下載地址：https://download.csdn.net/download/sheziqiong/85707617
資源下載地址：https://download.csdn.net/download/sheziqiong/85707617

1. 算法概述

1.1 RRT快速拓展隨機(jī)數(shù)算法

RRT 的思想是快速擴(kuò)張一群像樹(shù)一樣的路徑以探索（填充）空間的大部分區(qū)域，伺機(jī)找到可行的路徑。雖然不知道出路在哪里，但是通過(guò)隨機(jī)的反復(fù)試探還是能碰對(duì)的，而且碰對(duì)的概率隨著試探次數(shù)的增多越來(lái)越大，只要探索次數(shù)足夠，對(duì)于有解的問(wèn)題最終必然能得到結(jié)果。

RRT算法通過(guò)對(duì)狀態(tài)空間中的采樣點(diǎn)進(jìn)行碰撞檢測(cè)，避免了對(duì)空間的建模，能夠有效地解決高維空間和復(fù)雜約束的路徑規(guī)劃問(wèn)題。該方法的特點(diǎn)是能夠快速有效地搜索高維空間，通過(guò)狀態(tài)空間的隨機(jī)采樣點(diǎn)，把搜索導(dǎo)向空白區(qū)域，從而尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的規(guī)劃路徑，適合解決多自由度機(jī)器人在復(fù)雜環(huán)境下和動(dòng)態(tài)環(huán)境中的路徑規(guī)劃。該方法是概率完備但不最優(yōu)的。

在機(jī)器人探索迷宮的情景下，RRT算法的基本步驟是：

將起點(diǎn)$x_{init}$加入樹(shù)的節(jié)點(diǎn)集合$X_{free}$

在空間上隨機(jī)取點(diǎn)$x_{random}$，在集合$X_{free}$中找到距離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)$x_{near}$

將$x_{near}$向$x_{random}$的方向延伸$\Delta q$的距離，得到新的點(diǎn)$x_{new}$

若$x_{near}$到$x_{new}$的路徑不碰到障礙，則將$x_{near}$作為$x_{near}$的子節(jié)點(diǎn)加入$X_{free}$，否則重復(fù)2、3步

重復(fù)2~4步，直到第四步生成的$x_{new}$和終點(diǎn)$x_{goal}$差距小于$\Delta q$，此時(shí)直接將終點(diǎn)作為$x_{new}$的子節(jié)點(diǎn)，就得到了從起點(diǎn)到終點(diǎn)的路徑

需要注意的是，完全隨機(jī)的取點(diǎn)會(huì)導(dǎo)致拓展方向完全隨機(jī)，沒(méi)有方向性，有可能使得探索到終點(diǎn)的時(shí)間極大。因此可以考慮隨機(jī)點(diǎn)$x_{rand}$有一定的概率直接取終點(diǎn)$x_{goal}$，使得拓展有一定的方向性，同時(shí)又不失隨機(jī)性。

在實(shí)際解決問(wèn)題時(shí)，有可能問(wèn)題無(wú)解，則可以設(shè)置拓展節(jié)點(diǎn)的個(gè)數(shù)上限，到達(dá)上限仍未達(dá)到終點(diǎn)后則判定為路徑查找失敗。

在找到滿(mǎn)足要求的路徑后，還需要對(duì)路徑進(jìn)行簡(jiǎn)化。通過(guò)貪心算法，從起點(diǎn)開(kāi)始作為$p1$，依次找下一個(gè)節(jié)點(diǎn)$p2$，直到當(dāng)前$p1$和$p2$能無(wú)碰撞連接，但$p2$再往后一個(gè)就會(huì)發(fā)生碰撞位置。$p1$到$p2$就是簡(jiǎn)化的子路徑。然后再將$p2$賦值給$p1$，$p2$繼續(xù)往后找路徑節(jié)點(diǎn)，直到$p2$到達(dá)終點(diǎn)并成功生成全部路徑。

1.2 雙向RRT算法

RRT算法是一種純粹的隨機(jī)搜索算 法，對(duì)環(huán)境類(lèi)型不敏感。為了改進(jìn)RRT搜索空間的盲目性、節(jié)點(diǎn)拓展環(huán)節(jié)缺乏記憶性的缺點(diǎn)，提高空間搜索速度，在RRT算法的基礎(chǔ)上，又有雙向RRT算法。雙向RRT算法有兩棵樹(shù)，具有雙向搜索的引導(dǎo)策略，并且在生長(zhǎng)方式的 基礎(chǔ)上加上了貪婪策略加快了搜索速度，并且減少空白區(qū)域的無(wú)用搜索，節(jié)省搜索時(shí)間。

雙向RRT算法的其中一棵樹(shù)以另一棵樹(shù)最后生成的節(jié)點(diǎn)作為新的拓展方向。如果拓展成功則繼續(xù)往該方向拓展，直到不能拓展為止。下面的說(shuō)明以從終點(diǎn)開(kāi)始拓展的樹(shù)作為例子。

需要說(shuō)明的是，由持續(xù)拓展直到不能拓展的算法，可能會(huì)得到兩棵樹(shù)的節(jié)點(diǎn)數(shù)不平衡的狀態(tài)。因此，當(dāng)一棵樹(shù)拓展完時(shí)，到下一次拓展前進(jìn)行判斷，哪棵樹(shù)的節(jié)點(diǎn)數(shù)較小就拓展哪棵樹(shù)，從而保證兩棵樹(shù)的節(jié)點(diǎn)數(shù)盡量相等。

雙向RRT的基本算法如下：

將起點(diǎn)$x_{init}$加入樹(shù)的節(jié)點(diǎn)集合$X_1$，將終點(diǎn)$x_{goal}$加入第二棵樹(shù)的節(jié)點(diǎn)集合$X_2$

判斷$X_1$和$X_2$的節(jié)點(diǎn)數(shù)，哪棵樹(shù)的節(jié)點(diǎn)數(shù)小則拓展哪棵樹(shù)。

拓展樹(shù)：

• 若拓展$X_1$，則和RRT的拓展方法相同
• 若拓展$X_2$則進(jìn)行判斷：
  • 若$X_1$最后拓展節(jié)點(diǎn)的方向至少能讓$X_2$成功拓展一次，則向該方向拓展直到不能拓展
  • 若$X_1$最后拓展節(jié)點(diǎn)的方向一次都不能讓$X_2$拓展成功，則取隨機(jī)方向拓展

重復(fù)2、3步，直到$X_1$和$X_2$最近的兩個(gè)節(jié)點(diǎn)相距小于等于$\Delta q$，則連接這兩個(gè)節(jié)點(diǎn)，得到最終路徑

同樣，得到路徑后將路徑通過(guò)貪心算法進(jìn)行簡(jiǎn)化，得到最后的結(jié)果。

2. 代碼實(shí)現(xiàn)

2.1 場(chǎng)景地圖的構(gòu)建

本次實(shí)驗(yàn)我通過(guò)python3.7實(shí)現(xiàn)。

首先配置需要的參數(shù)：

mapimg = Image.open('map1.png') # 讀入地圖圖片 mapimg_array = np.array(mapimg) # 將地圖圖片轉(zhuǎn)換為矩陣 wid, hei = mapimg.size # 獲取地圖大小 robot_radius = 5 # 設(shè)置機(jī)器人半徑 deltaq = 20 # 節(jié)點(diǎn)拓展距離 n = 10000 # 最大迭代次數(shù) color_start = (236, 28, 36) # 起點(diǎn)顏色（由地圖圖片決定） color_end = (63, 72, 204) # 終點(diǎn)顏色（由地圖圖片決定） color_sample = (139, 129, 76) # 最終展示路徑的顏色

取一張圖片作為地圖，如下所示：

場(chǎng)景地圖部分代碼的運(yùn)行結(jié)果如下：

拓展前：

拓展后：

可以明顯看出，場(chǎng)景地圖被正確讀入并拓展了，場(chǎng)景地圖構(gòu)建完成。

2.2 RRT算法的實(shí)現(xiàn)

創(chuàng)建points_sample為所有mapimg_status == 0的點(diǎn)的集合，也就是說(shuō)，points_sample是所有通路的點(diǎn)。之后取隨機(jī)點(diǎn)可以在該集合中取。points_sampled為構(gòu)造的樹(shù)的節(jié)點(diǎn)集合，初始化時(shí)只有起點(diǎn)。 graph為鄰接矩陣，橫縱坐標(biāo)表示points_sampled的點(diǎn)，矩陣值為 0 表示兩個(gè)節(jié)點(diǎn)不連通，為 1 表示前一個(gè)節(jié)點(diǎn)是后一個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)，為 2 表示前一個(gè)節(jié)點(diǎn)是后一個(gè)節(jié)點(diǎn)的子節(jié)點(diǎn)。由此，不僅記錄了樹(shù)的連接關(guān)系，同時(shí)也提供了從葉子節(jié)點(diǎn)到根節(jié)點(diǎn)的回溯方法，便于之后得到路徑。

points_sample = np.argwhere(mapimg_status == 0) # 通路點(diǎn)的集合 points_sampled = np.array([point_start]) # RRT樹(shù)的所有節(jié)點(diǎn) graph = np.zeros([n, n]).astype(int) # 鄰接矩陣

接下來(lái)就是拓展過(guò)程了。若拓展節(jié)點(diǎn)數(shù)沒(méi)有到最大節(jié)點(diǎn)數(shù)，進(jìn)行以下的循環(huán)：

取隨機(jī)點(diǎn)進(jìn)行拓展。random.randint(0,100)生成一定范圍內(nèi)的隨機(jī)數(shù)，通過(guò)參數(shù)調(diào)整能控制取到的$x_{rand}$是終點(diǎn)還是隨機(jī)點(diǎn)。下面的代碼有 80% 的幾率取隨機(jī)點(diǎn)， 20% 取終點(diǎn)。

if random.randint(0,100) <= 80:idx = np.random.choice(np.arange(points_sample.shape[0]), 1)p_rand = points_sample[idx][0] # 在通路點(diǎn)中隨機(jī)取一個(gè)else:p_rand = np.array(point_end) # 取終點(diǎn)points_sampled = expand(p_rand, points_sampled) # 進(jìn)行拓展

2.3 雙向RRT

雙向RRT的基本算法與RRT有一定的重合之處，下面只說(shuō)明不同的地方，完整的代碼附在最后。

雙向RRT樹(shù)要對(duì)兩棵樹(shù)進(jìn)行拓展，在兩棵樹(shù)相交時(shí)得到通路。定義以下變量：

points_sampled1 = np.array([point_start]) # 從起點(diǎn)開(kāi)始的RRT樹(shù) points_sampled2 = np.array([point_end]) # 從終點(diǎn)開(kāi)始的RRT樹(shù) graph1 = np.zeros([n, n]).astype(int) # 起點(diǎn)RRT樹(shù)的鄰接矩陣 graph2 = np.zeros([n, n]).astype(int) # 終點(diǎn)RRT樹(shù)的鄰接矩陣 p_cross = np.array([]) # 記錄兩棵樹(shù)相交的點(diǎn)

接著進(jìn)入循環(huán)，若沒(méi)得到結(jié)果則進(jìn)行拓展。拓展前先進(jìn)行判斷：

if points_sampled1.shape[0] <= points_sampled2.shape[0]:

哪棵樹(shù)的節(jié)點(diǎn)少則拓展哪棵樹(shù)，依次維持兩棵樹(shù)的節(jié)點(diǎn)數(shù)基本平衡。

如果是拓展從起點(diǎn)開(kāi)始的RRT樹(shù)，則向單向的RRT算法一樣，隨機(jī)生成節(jié)點(diǎn)進(jìn)行拓展：

if random.randint(0,100) <= 80:idx = np.random.choice(np.arange(points_sample.shape[0]), 1)p_rand = points_sample[idx][0] else:p_rand = np.array(point_end) points_sampled1,p_new = expand1(p_rand, points_sampled1)

其中，隨機(jī)節(jié)點(diǎn)有 20% 的幾率直接取終點(diǎn)，使得節(jié)點(diǎn)拓展方向總體向終點(diǎn)延伸。進(jìn)行拓展的函數(shù)expand1和之前的RRT算法基本一致，只是還會(huì)返回拓展后的新節(jié)點(diǎn)p_new。若拓展失敗（隨機(jī)點(diǎn)、新點(diǎn)在點(diǎn)集中或到新節(jié)點(diǎn)的路徑出現(xiàn)障礙則拓展失敗），p_new為None。保存p_new的原因是，拓展從終點(diǎn)發(fā)出的RRT樹(shù)時(shí)，要以此為拓展方向。

需要注意的是，在拓展完后就要進(jìn)行判斷，是否出現(xiàn)了重復(fù)節(jié)點(diǎn)或節(jié)點(diǎn)間的距離小于deltaq，若是則找到解：

# 只有拓展成功才進(jìn)行判斷 if p_new is not None:# 找到第二棵樹(shù)離新節(jié)點(diǎn)最近的節(jié)點(diǎn)points_sampled_list2_tmp = points_sampled2.tolist()points_sampled_list2_tmp.sort(key = lambda x:np.linalg.norm(x - p_new))# 若兩點(diǎn)之間的距離小于deltaq，則連接兩點(diǎn)，得到結(jié)果if np.linalg.norm(points_sampled_list2_tmp[0] - p_new) <= deltaq:# 進(jìn)一步拓展，并更新鄰接矩陣points_sampled1 = np.append(points_sampled1, np.array([points_sampled_list2_tmp[0]]), axis=0)graph1[points_sampled1.shape[0]-2, points_sampled1.shape[0]-1] = 1graph1[points_sampled1.shape[0]-1, points_sampled1.shape[0]-2] = 2# 記錄相交的節(jié)點(diǎn)p_cross = np.array(points_sampled_list2_tmp[0])break # 找到解，退出循環(huán)

若是拓展從終點(diǎn)開(kāi)始的RRT樹(shù)，則先要進(jìn)行判斷：上一次第一棵樹(shù)的拓展是否成功。若成功，則將第一棵樹(shù)拓展的節(jié)點(diǎn)作為第二棵樹(shù)的拓展方向，否則也進(jìn)行隨機(jī)選取，有 20% 的幾率選到起點(diǎn)。

flag = False # 記錄第一棵樹(shù)上次拓展是否成功 if p_new is not None:flag = Truep_rand = p_new # 若成功，隨機(jī)點(diǎn)直接取上次拓展節(jié)點(diǎn) else: # 否則進(jìn)行隨機(jī)取點(diǎn)if random.randint(0,100) <= 80:idx = np.random.choice(np.arange(points_sample.shape[0]), 1)p_rand = points_sample[idx][0]else:p_rand = np.array(point_start)

接著進(jìn)行拓展。拓展完后無(wú)論是否拓展的方向是原先的q_new的方向，都將q_new改為None，從而表示已經(jīng)拓展過(guò)。

之后展示結(jié)果需要做一些簡(jiǎn)單的調(diào)整。顯示路徑和樹(shù)只需要將兩棵樹(shù)的結(jié)果都顯示出來(lái)即可，和單向RRT基本相同。路徑簡(jiǎn)化時(shí)，考慮到從起點(diǎn)發(fā)起的樹(shù)為正向，從終點(diǎn)發(fā)起的樹(shù)為逆向，需要先依次記錄從相交節(jié)點(diǎn)到起點(diǎn)的路徑，反向后加上從相交節(jié)點(diǎn)到終點(diǎn)的距離。這樣一來(lái)，簡(jiǎn)化函數(shù)就能和之前的單向RRT算法完全一致。展示結(jié)果部分的代碼都和第一部分基本相同，這里不再贅述。

3. 實(shí)驗(yàn)結(jié)果

配置機(jī)器人半徑為5，單次移動(dòng)距離deltaq為10，對(duì)第一張地圖的拓展前后的結(jié)果為：

可以看出，地圖成功地拓展了。

使用RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

可以看出，在樹(shù)的拓展前期，因?yàn)楸淮笃恼系K阻礙，進(jìn)行了大量的重復(fù)拓展。而在有節(jié)點(diǎn)和終點(diǎn)之間的通路基本上無(wú)障礙時(shí)，能夠較為順利地拓展到終點(diǎn)。

使用雙向RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

雙向RRT的拓展節(jié)點(diǎn)數(shù)明顯比RRT少了很多。實(shí)際上，仔細(xì)觀(guān)察找到的路徑不難發(fā)現(xiàn)，路徑出現(xiàn)了幾條很長(zhǎng)的直線(xiàn)。這其實(shí)是沿著一個(gè)方向拓展的一系列節(jié)點(diǎn)。終點(diǎn)向起點(diǎn)的新拓展節(jié)點(diǎn)方向進(jìn)行拓展大幅縮短了兩棵樹(shù)之間的差距，在進(jìn)行少量隨機(jī)拓展后，很容易就能繞過(guò)障礙，連接兩棵樹(shù)的節(jié)點(diǎn)。

下面再來(lái)看另一張地圖。配置機(jī)器人半徑為5，單次移動(dòng)距離deltaq為10，對(duì)第二張地圖的拓展前后的結(jié)果為：

使用RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

在這張地圖中，起點(diǎn)到終點(diǎn)要進(jìn)行多次的迂回。在RRT算法中，只能通過(guò)隨機(jī)生成節(jié)點(diǎn)，以碰運(yùn)氣的方式進(jìn)行迂回，可以看出在需要多次迂回的場(chǎng)景下需要進(jìn)行大量拓展，效果不佳。

使用雙向RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

在起點(diǎn)和終點(diǎn)附近，雙向RRT算法也進(jìn)行了大量的拓展。而且在這種反復(fù)迂回的場(chǎng)景下，終點(diǎn)樹(shù)的多次拓展幾乎只在最后起到作用。總的來(lái)說(shuō)比單純的RRT算法要好，但也存在明顯的缺點(diǎn)。

最后是第三張地圖。配置機(jī)器人半徑為5，單次移動(dòng)距離deltaq為10，對(duì)第三張地圖的拓展前后的結(jié)果為：

使用RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

本地圖到終點(diǎn)有個(gè)狹窄的通路。RRT沒(méi)有策略性，向無(wú)頭蒼蠅一樣四處拓展，進(jìn)行了大量的無(wú)意義拓展。從樹(shù)圖可以看出，整張地圖幾乎被拓展的節(jié)點(diǎn)鋪滿(mǎn)，而終點(diǎn)卻仍然沒(méi)能拓展到。這很能暴露了RRT面對(duì)狹窄通道時(shí)的缺點(diǎn)。

使用雙向RRT算法，得到的樹(shù)圖、找到的路徑和簡(jiǎn)化路徑如下圖所示：

考慮到終點(diǎn)發(fā)起的隨機(jī)拓展，在這種場(chǎng)景下能較快從狹窄通道中拓展出來(lái)。雙向RRT比RRT的優(yōu)勢(shì)較好地體現(xiàn)了出來(lái)。

資源下載地址：https://download.csdn.net/download/sheziqiong/85707617
資源下載地址：https://download.csdn.net/download/sheziqiong/85707617

總結(jié)

以上是生活随笔為你收集整理的基于Python实现RRT与双向RRT算法的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。