內容無關：最近的課題內容和機器人運動規劃方法有關，我把學習的內容整理成為工具箱上傳到了我的github倉庫，稍後將會發一篇說明介紹使用方法。

XM522706601/robotics_tutorial_for_zhihu?

github.com圖標

上一篇文章 小明工坊：【基礎知識】機器人運動規劃原理與實現（一）——概率路線圖(PRM)方法 我們講到了機器人運動規劃中一個比較重要的方法——概率路線圖（PRM）方法，並通過編程進行了演示和分析，下面簡單回顧如下：

  • 機器人運動規劃的基本任務為從開始位置運動到目標位置
  • 主要難點有二：躲避障礙物（全局約束）和滿足自身運動性能（微分約束）
  • 抽樣規劃是解決全局約束問題的重要方法
  • 抽樣規劃演算法分為綜合查詢方法和單一查詢方法
  • 綜合查詢方法的代表為概率路線圖演算法(PRM)，單一查詢方法的代表為快速擴展隨機樹演算法(RRT)

本章我們同樣用一個例子來講解快速擴展隨機樹演算法(RRT)演算法。

其實RRT演算法與PRM演算法十分類似，都是通過抽樣來在已知的地圖上建立無向圖，進而通過搜索方法尋找相對最優的路徑。不同點在於，PRM演算法在一開始就通過抽樣在地圖上構建出完整的無向圖，再進行圖搜索；而RRT演算法則是從某個點出發一邊搜索，一邊抽樣並建圖。

與PRM演算法相同，RRT演算法也是概率完備的：只要路徑存在，且規劃的時間足夠長，就一定能確保找到一條路徑解。注意「且規劃的時間足夠長」這一前提條件，說明了如果規劃器的參數設置不合理（如搜索次數限制太少、採樣點過少等），就可能找不到解。

演算法說明

我們可以把RRT演算法比較形象地看做「樹型演算法」。它從一個起始構型（對於二維圖，就是一個點）出發，不斷延伸樹型數據，最終與目標點相連。先放一張規劃的結果可能更加便於理解：

圖 1 RRT 演算法，從左上角出發呈樹型向目標點延伸

演算法的步驟如下：

1.初始化

選擇或繪製一張bmp格式的圖像，作為規劃的構型空間，為了便於進行碰撞檢測，將其二值化。選擇左上角[0, 0]點作為起始點；右下角[499, 499]作為目標點。

原圖像
二值化圖像

2. 隨機採樣

我們已經確定了規劃的起始點，按道理它需要不斷地向著目標點進行生長。但需要注意的是，由於存在障礙物，如果我們讓樹型一味朝著目標點延伸，則可能會因為「撞牆」而失敗。因此，我們採取了一種隨機採樣方法：在每次選擇生長方向時，有一定的概率會向著目標點延伸，也有一定的概率會隨機在地圖內選擇一個方向延伸一段距離，關鍵代碼如下：

# 利用rand()函數在[0,1]區間內隨機生成一個數
if np.random.rand() < 0.5:
# 如果小於0.5，則在圖 img_binary 的範圍內隨機採樣一個點
sample = np.mat(np.random.randint(0,
img_binary.shape[0] - 1,
(1, 2)))
else:
# 否則用目標點作為採樣點
sample = self.point_goal

我們每一步讓RRT樹有0.5的概率直接採樣終點向目標點前進，有0.5的概率向地圖內任意方向前進。

圖 2 隨機採樣一個點，或直接採樣終點，概率各一半

3.生長點選擇與碰撞檢測

從圖 2 可以看到，由於每次生長都存在一定的隨機性，因此RRT樹會逐漸出現許多分支，那麼每一步中我們該如何選擇要延伸哪個分支呢？這裡我們直接選擇RRT樹中離採樣點最近的點，並向其延伸。

假設我們採樣了空間中隨機一個點，接下來從現有的RRT樹中選擇離採樣點最近的一個點，並向採樣點延伸一段距離。假如在這段延伸中沒有發生碰撞（碰撞檢測），而且新點與現有的所有點的距離大於某個判斷閾值（防止生長到RRT已經探索過的位置），則將這個新點也加入RRT樹。

4. 終止條件

由於我們每次延伸的距離是固定的，所以並不能保證最後一次延伸能夠剛好到達終點的位置，更可能的情況是在終點周圍來回跳動。因此我們設定一個閾值，假如本次延伸的新點與終點的距離小於這個閾值，我們就認為已經規劃成功。

下面是隨機採樣概率0.5，步長20，採樣上限20000次的結果

成功找到路徑

分析

前面提到，RRT演算法是概率完備的，預設參數可能對規劃結果造成影響。那麼有哪些參數會影響規劃效果呢？這裡我列舉幾個：

隨機採樣概率：

我們每一次採樣，都有一定概率朝著任意方向走，或朝著終點走。這個概率顯然會影響搜索效果。給人最直接的感覺是，隨機採樣的概率越大，RRT樹的分支也就越多，反之則難以發生新的分支。下面我們修改隨機採樣概率來看看效果。

設隨機採樣的概率為0.01，採樣上限20000次。可以看到，直到達到採樣上限也沒有成功找到解。這是因為RRT產生分支的概率太小，經歷了許多次碰撞才能憑藉分支繞過障礙物。

隨機採樣的概率為0.01，採樣上限20000次

設隨機採樣的概率為1.0，採樣上限20000次。可以看到，雖然規劃得以成功，但由於生長缺乏方向性，其實是一種「碰運氣」式的搜索。RRT樹的分支填充了所有空間直至找到目標點。這樣的搜索會消耗大量的時間。

隨機採樣的概率為1.0，採樣上限20000次

生長步長：

我們的RRT樹每一次延伸，都有一個固定的步長。這個步長的設置顯然也會影響樹的形狀。當步長太大時，可能由於太過笨拙而無法成功繞過障礙物；當步長過小時，生長的速度顯然會有所減慢（因為同樣的距離要生長更多次）。一般來說，空間越複雜，步長越小。這裡必須注意的是，生長步長一定要比判斷是否為同一個採樣點的閾值要大。

步長10，採樣上限20000次。可以看到，採樣點極其密集，消耗的時間更長。

步長10，採樣上限20000次

步長200，採樣上限20000次。沒有搜索到最終結果，可以看到，由於步長太大，生長點在障礙物與終點之間來回跳動，始終不能滿足碰撞檢測或終止條件的要求。

步長200，採樣上限20000次

更多演示

RRT演算法的適用性同樣很廣，舉例如下：

參考文獻：

[1] Siciliano B, Oussama K. Springer Handbook of Robotics[M]. 2007.

[2] RRT路徑規劃演算法 XXX已失聯

推薦閱讀：

相关文章