8.1 原理

        最初，RRT*-Smart 像 RRT* 一樣隨機搜索狀態(tài)空間。類似地，找到第一條路徑就像 RRT* 會嘗試通過配置空間中的隨機采樣來找到路徑一樣。一旦找到第一條路徑，它就會通過互連直接可見的節(jié)點來優(yōu)化它。此優(yōu)化路徑產生用于智能采樣的偏置點。在這些偏置點，采樣以規(guī)則的間隔進行

        隨著算法的進展和路徑的不斷優(yōu)化，此過程將繼續(xù)進行。每當找到更短的路徑時，偏差就會轉向新路徑。

        RRT*是一邊生長一邊優(yōu)化的，RRT*的重心在于找到最優(yōu)路徑。RRT*樹生長到能連接起點和終點后，這就已經有一條初始路徑了。

        這顆RRT*樹可以繼續(xù)生長，越生長可以得到的路徑相比初始路徑會越優(yōu)。然而，這個繼續(xù)生長的過程對于RRT*而言效率非常低，由此衍生出RRT*-smart算法，專門解決這一問題。

        注意到，RRT*-smart是在RRT*算法已生成初始路徑后，在此基礎上，想對初始路徑繼續(xù)優(yōu)化的步驟才起作用，所以RRT*-smart對于生成初始路徑并沒有加速幫助。

        RRT*-smart的優(yōu)勢在于：它專注于提升路徑接近障礙物拐點處的優(yōu)化速度。RRT*-smart算法的思路是這樣的：

在原始RRT*算法的基礎上加了兩步：

①路徑優(yōu)化

路徑優(yōu)化的本質是利用三角形兩邊之和大于第三邊

假設RRT*生成的初始路徑長這樣

具體操作如下：

        一旦RRT*給出了一條初始路徑，將初始路徑中彼此可見的節(jié)點直接相連。迭代過程從xgoal開始，向xinit檢查與每個節(jié)點的連續(xù)父節(jié)點的直接連接，直到無沖突條件失敗。下圖給出一個示例。

信標（Z_beacons）：經過路徑優(yōu)化后的路徑中除了起點和終點之外的節(jié)點，標記為信標（Z_beacons），如上圖中的綠點。

②智能采樣

在RRT*算法中，在生成初始路徑后，在此RRT*樹的基礎上繼續(xù)采點，采點越多，路徑優(yōu)化效果越好，但此采點，是完全隨機的采點，因此效率低下，RRT*-smart則不是完全隨機的采點。

在RRT*-smart算法中，利用了這樣一種思想：智能采樣背后的想法是通過生成盡可能靠近障礙物頂點的節(jié)點來接近最優(yōu)性。

在基于采樣的RRT*-smart中，路徑僅沿著靠近障礙物拐點的外圍進行優(yōu)化，解決的辦法就是：在障礙物拐點的外圍多多采點。如下圖所示：

問：那么RRT*-smart如何實現(xiàn)在障礙物拐點的外圍多多采點呢？

答：利用①路徑優(yōu)化過程中給出的信標（Z_beacons）。

一旦找到初始路徑，智能采樣就會開始，在以Z beacons為中心的半徑為R beacons的球中直接生成一定數(shù)量的樣本。采樣偏向于這些信標，因為它們提供了有關障礙物頂點（或圓形障礙物的外圍）位置的有用線索。因此，這些信標需要被最大節(jié)點包圍，以優(yōu)化這些轉彎處的路徑。與 RRT* 相比，這一特征迫使所提出的算法以更少的迭代次數(shù)達到最優(yōu)解。

注意：

信標（Z_beacons）是需要隨著優(yōu)化路徑的更新而更新的。即當z_goal.cost變小時，說明路徑得到了優(yōu)化，那么就要啟用之前①路徑優(yōu)化算法來重新確定新的z_beacons。

8.2 偽碼

8.3 程序示例

下圖中的線段代表由RRT*生成的初始RRT樹，圓圈代表在初始RRT樹基礎上，繼續(xù)采點的分布，可見在幾個“拐點處”的圓形領域內我們有額外的采點以加強在這部分的采點

路徑優(yōu)化后確定出拐點

經過路徑優(yōu)化后的路徑：

8.4 參考

1、RRT*-SMART: A Rapid Convergence Implementation of RRT*

2、Rapid convergence implementation of RRT* towards optimal solution