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

1. 算法概述

1.1 RRT快速拓展隨機數算法

RRT 的思想是快速擴張一群像樹一樣的路徑以探索（填充）空間的大部分區域，伺機找到可行的路徑。雖然不知道出路在哪里，但是通過隨機的反復試探還是能碰對的，而且碰對的概率隨著試探次數的增多越來越大，只要探索次數足夠，對于有解的問題最終必然能得到結果。

RRT算法通過對狀態空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規劃問題。該方法的特點是能夠快速有效地搜索高維空間，通過狀態空間的隨機采樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規劃路徑，適合解決多自由度機器人在復雜環境下和動態環境中的路徑規劃。該方法是概率完備但不最優的。

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

將起點$x_{init}$加入樹的節點集合$X_{free}$

在空間上隨機取點$x_{random}$，在集合$X_{free}$中找到距離隨機點最近的節點$x_{near}$

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

若$x_{near}$到$x_{new}$的路徑不碰到障礙，則將$x_{near}$作為$x_{near}$的子節點加入$X_{free}$，否則重復2、3步

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

需要注意的是，完全隨機的取點會導致拓展方向完全隨機，沒有方向性，有可能使得探索到終點的時間極大。因此可以考慮隨機點$x_{rand}$有一定的概率直接取終點$x_{goal}$，使得拓展有一定的方向性，同時又不失隨機性。

在實際解決問題時，有可能問題無解，則可以設置拓展節點的個數上限，到達上限仍未達到終點后則判定為路徑查找失敗。

在找到滿足要求的路徑后，還需要對路徑進行簡化。通過貪心算法，從起點開始作為$p1$，依次找下一個節點$p2$，直到當前$p1$和$p2$能無碰撞連接，但$p2$再往后一個就會發生碰撞位置。$p1$到$p2$就是簡化的子路徑。然后再將$p2$賦值給$p1$，$p2$繼續往后找路徑節點，直到$p2$到達終點并成功生成全部路徑。

1.2 雙向RRT算法

RRT算法是一種純粹的隨機搜索算 法，對環境類型不敏感。為了改進RRT搜索空間的盲目性、節點拓展環節缺乏記憶性的缺點，提高空間搜索速度，在RRT算法的基礎上，又有雙向RRT算法。雙向RRT算法有兩棵樹，具有雙向搜索的引導策略，并且在生長方式的 基礎上加上了貪婪策略加快了搜索速度，并且減少空白區域的無用搜索，節省搜索時間。

雙向RRT算法的其中一棵樹以另一棵樹最后生成的節點作為新的拓展方向。如果拓展成功則繼續往該方向拓展，直到不能拓展為止。下面的說明以從終點開始拓展的樹作為例子。

需要說明的是，由持續拓展直到不能拓展的算法，可能會得到兩棵樹的節點數不平衡的狀態。因此，當一棵樹拓展完時，到下一次拓展前進行判斷，哪棵樹的節點數較小就拓展哪棵樹，從而保證兩棵樹的節點數盡量相等。

雙向RRT的基本算法如下：

將起點$x_{init}$加入樹的節點集合$X_1$，將終點$x_{goal}$加入第二棵樹的節點集合$X_2$

判斷$X_1$和$X_2$的節點數，哪棵樹的節點數小則拓展哪棵樹。

拓展樹：

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

重復2、3步，直到$X_1$和$X_2$最近的兩個節點相距小于等于$\Delta q$，則連接這兩個節點，得到最終路徑

同樣，得到路徑后將路徑通過貪心算法進行簡化，得到最后的結果。

2. 代碼實現

2.1 場景地圖的構建

本次實驗我通過python3.7實現。

首先配置需要的參數：

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

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

場景地圖部分代碼的運行結果如下：

拓展前：

拓展后：

可以明顯看出，場景地圖被正確讀入并拓展了，場景地圖構建完成。

2.2 RRT算法的實現

創建points_sample為所有mapimg_status == 0的點的集合，也就是說，points_sample是所有通路的點。之后取隨機點可以在該集合中取。points_sampled為構造的樹的節點集合，初始化時只有起點。 graph為鄰接矩陣，橫縱坐標表示points_sampled的點，矩陣值為 0 表示兩個節點不連通，為 1 表示前一個節點是后一個節點的父節點，為 2 表示前一個節點是后一個節點的子節點。由此，不僅記錄了樹的連接關系，同時也提供了從葉子節點到根節點的回溯方法，便于之后得到路徑。

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

接下來就是拓展過程了。若拓展節點數沒有到最大節點數，進行以下的循環：

取隨機點進行拓展。random.randint(0,100)生成一定范圍內的隨機數，通過參數調整能控制取到的$x_{rand}$是終點還是隨機點。下面的代碼有 80% 的幾率取隨機點， 20% 取終點。

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_sampled = expand(p_rand, points_sampled) # 進行拓展

2.3 雙向RRT

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

雙向RRT樹要對兩棵樹進行拓展，在兩棵樹相交時得到通路。定義以下變量：

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

接著進入循環，若沒得到結果則進行拓展。拓展前先進行判斷：

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

哪棵樹的節點少則拓展哪棵樹，依次維持兩棵樹的節點數基本平衡。

如果是拓展從起點開始的RRT樹，則向單向的RRT算法一樣，隨機生成節點進行拓展：

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)

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

需要注意的是，在拓展完后就要進行判斷，是否出現了重復節點或節點間的距離小于deltaq，若是則找到解：

# 只有拓展成功才進行判斷 if p_new is not None:# 找到第二棵樹離新節點最近的節點points_sampled_list2_tmp = points_sampled2.tolist()points_sampled_list2_tmp.sort(key = lambda x:np.linalg.norm(x - p_new))# 若兩點之間的距離小于deltaq，則連接兩點，得到結果if np.linalg.norm(points_sampled_list2_tmp[0] - p_new) <= deltaq:# 進一步拓展，并更新鄰接矩陣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# 記錄相交的節點p_cross = np.array(points_sampled_list2_tmp[0])break # 找到解，退出循環

若是拓展從終點開始的RRT樹，則先要進行判斷：上一次第一棵樹的拓展是否成功。若成功，則將第一棵樹拓展的節點作為第二棵樹的拓展方向，否則也進行隨機選取，有 20% 的幾率選到起點。

flag = False # 記錄第一棵樹上次拓展是否成功 if p_new is not None:flag = Truep_rand = p_new # 若成功，隨機點直接取上次拓展節點 else: # 否則進行隨機取點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)

接著進行拓展。拓展完后無論是否拓展的方向是原先的q_new的方向，都將q_new改為None，從而表示已經拓展過。

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

3. 實驗結果

配置機器人半徑為5，單次移動距離deltaq為10，對第一張地圖的拓展前后的結果為：

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

使用RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

可以看出，在樹的拓展前期，因為被大片的障礙阻礙，進行了大量的重復拓展。而在有節點和終點之間的通路基本上無障礙時，能夠較為順利地拓展到終點。

使用雙向RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

雙向RRT的拓展節點數明顯比RRT少了很多。實際上，仔細觀察找到的路徑不難發現，路徑出現了幾條很長的直線。這其實是沿著一個方向拓展的一系列節點。終點向起點的新拓展節點方向進行拓展大幅縮短了兩棵樹之間的差距，在進行少量隨機拓展后，很容易就能繞過障礙，連接兩棵樹的節點。

下面再來看另一張地圖。配置機器人半徑為5，單次移動距離deltaq為10，對第二張地圖的拓展前后的結果為：

使用RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

在這張地圖中，起點到終點要進行多次的迂回。在RRT算法中，只能通過隨機生成節點，以碰運氣的方式進行迂回，可以看出在需要多次迂回的場景下需要進行大量拓展，效果不佳。

使用雙向RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

在起點和終點附近，雙向RRT算法也進行了大量的拓展。而且在這種反復迂回的場景下，終點樹的多次拓展幾乎只在最后起到作用。總的來說比單純的RRT算法要好，但也存在明顯的缺點。

最后是第三張地圖。配置機器人半徑為5，單次移動距離deltaq為10，對第三張地圖的拓展前后的結果為：

使用RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

本地圖到終點有個狹窄的通路。RRT沒有策略性，向無頭蒼蠅一樣四處拓展，進行了大量的無意義拓展。從樹圖可以看出，整張地圖幾乎被拓展的節點鋪滿，而終點卻仍然沒能拓展到。這很能暴露了RRT面對狹窄通道時的缺點。

使用雙向RRT算法，得到的樹圖、找到的路徑和簡化路徑如下圖所示：

考慮到終點發起的隨機拓展，在這種場景下能較快從狹窄通道中拓展出來。雙向RRT比RRT的優勢較好地體現了出來。

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。