蟻群算法在實際中的應用，蟻群算法路徑規劃_環境感知與規劃專題（十）——基于采樣的路徑規劃算法（二）...

前言

蟻群算法在實際中的應用、 上一篇介紹了快速搜索隨機樹（RRT）算法的原理，這是一種基于采樣的路徑規劃算法，在地圖尺寸較大時，其效率將顯著的優于基于圖搜索的路徑規劃算法（如A*）。

然而，RRT也有其局限性，如：

• 狹窄通道情況下的搜索效率急劇下降
• 搜索得到的路徑不是全局最優的

本篇將針對RRT算法應用時出現的幾個問題展開討論，并闡述幾種RRT算法的改進型。

快速獲取搜索樹中的相鄰節點

RRT算法中有一個重要的環節——獲取搜索樹種距離采樣點最近的節點：

該算法的效率直接影響了RRT算法的搜索效率，因此，本篇單獨對其進行討論。

在工程中，我們將搜索樹構建為KD-Tree（K-dimensional Tree），通過KD-Tree來搜索相鄰節點，它改進的就是上圖中的

函數的效率，那么什么是

KD-Tree(K-dimension tree)是一種對

維空間中的實例點進行存儲以便對其進行快速檢索的樹形數據結構。

維空間的一個劃分，構造

維空間切分，構成一系列的

維超矩形區域。

維超矩形區域。利用

對于RRT算法，我們對搜索樹進行KD-Tree構建，以二維問題為例，我們取所有節點中

方向的中位數節點為切割點，將所有節點分為兩部分，再對所有節點中

方向的中位數節點為切割點分割，以此類推，最終將搜索樹中的所有節點劃分完畢，即得到

在得到KD-Tree后，我們可以根據需要，快速地搜索到距離

節點最近的節點，大大提高

函數的搜索效率。

狹窄通道問題（Narrow Passage)

由于RRT算法通常采用蒙特卡洛法采樣，其采樣概率是均勻分布的，因此，對于狹窄通道（Narrow Passage）的情況，RRT算法的搜索效率將大大降低（搜索樹的生長需要經過狹窄通道才能達到目標點，而采樣點剛好落在狹窄通道的概率相對于整張地圖而言較低），由此，會產生如下現象：

對于該類問題，我們往往采用RRT-Connect算法進行處理，如下圖所示，與RRT不同的是，RRT-Connect分別從起始點和目標點構建搜索樹，直到兩棵搜索樹相交，找到一條可通行的路徑。

該算法可以大大降低Narrow Passage中由于狹窄通道采樣概率較低的問題，具體的實現在此不做贅述，讀者可自行搜索Bidirectional RRT / RRT Connect。

RRT * 與 Informed-RRT*

傳統的RRT算法搜索得到的路徑往往不是全局最優的，于是，RRT算法的改進型——RRT*應運而生。

RRT*是在 RRT算法框架基礎上進行了一定的改進，算法流程如下圖所示：

與RRT相似的是，我們都需要通過

函數在地圖中進行采樣，然后在搜索樹

中尋找其最近的節點

同時，在

與

連線方向進行擴展得到

，然后，我們對

節點在地圖中進行碰撞檢測，若

則進行下一步；反之，則放棄該節點。

與RRT不同的是，RRT*中，我們在碰撞檢測成功后，搜索以

為半徑，以

節點為圓心范圍內的搜索樹上的相鄰節點集合

，即上圖中的

函數。

這里可以對上文提及的KD-Tree搜索方法進行一定的改進，即將

節點與搜索樹中歐氏距離小于

的所有滿足條件的葉子節點添加進相鄰節點集合

即可。

然后，在相鄰節點集合

中為

選取其父節點，即

函數，該函數通過計算集合

中所有葉子節點與

節點的歐氏距離的最小值來選擇

節點的父節點，從而將其加入搜索樹

。

最后，我們需要對重建的搜索樹進行剪枝（

），該步驟也是

集合中的所有葉子節點到起始節點的代價（歐氏距離）進行判斷，若存在更短路徑，則重新修改葉子節點的父節點，從而完成剪枝。

例如,

節點的父節點原本為

，由于

經過

到起始點的路徑代價更小，因此，這里修改

的父節點為

，同理，對其他相鄰節點進行類似操作，從而完成剪枝。

整個過程如下所示，可以看到，RRT* 在搜索到一條可通行路徑后，并未停止迭代，而是繼續剪枝。從而得到一條接近全局最優的可通行路徑，因此，RRT* 較RRT在實際工程中有更廣泛的應用。

從上圖可以很容易地發現一個問題，當RRT*找到一條可通行路徑時，其采樣函數依然在整個地圖空間中進行均勻采樣，然而進行剪枝等操作，那么這樣的采樣方式顯然會耗費大量不必要的時間。

Informed-RRT* 被用于優化該問題，如下圖所示，Informed-RRT* 較RRT*節省了大量的時間，究其原因在于優化了采樣方式。

Informed-RRT* 在得到一條可通行路徑的基礎上，以起始點與目標點之間的連線為橢圓的長軸構建橢圓形采樣區域，采樣函數的采樣范圍被重新限制在該區域范圍之中，隨著搜索樹的不斷剪枝，橢圓范圍也在不斷地縮小，采樣時間也隨之減少，由此，大幅度節省RRT*的時間開銷。

總結

本篇闡述了一種提高RRT算法搜索效率的數據結構——KD-Tree，并圍繞RRT算法應用中碰到的幾個問題展開，簡述了RRT算法的幾種改進型——RRT* 與Informed-RRT*，下一篇中將對滿足動力學約束的基于采樣的路徑規劃算法展開討論。

作者簡介： 一個被Coding耽誤的無人機算法工程師，控制、導航略懂一二，熱衷技術，喜歡乒乓、音樂、電影，歡迎交流。

知乎： @遙遠的烏托邦

GitHub: https://github.com/DistantUtopia

微信公眾號： @遙遠的烏托邦