8.1 原理

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

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

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

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

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

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

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

①路徑優(yōu)化

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

假設(shè)RRT*生成的初始路徑長(zhǎng)這樣

具體操作如下：

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

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

②智能采樣

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

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

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

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

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

一旦找到初始路徑，智能采樣就會(huì)開始，在以Z beacons為中心的半徑為R beacons的球中直接生成一定數(shù)量的樣本。采樣偏向于這些信標(biāo)，因?yàn)樗鼈兲峁┝擞嘘P(guān)障礙物頂點(diǎn)（或圓形障礙物的外圍）位置的有用線索。因此，這些信標(biāo)需要被最大節(jié)點(diǎn)包圍，以優(yōu)化這些轉(zhuǎn)彎處的路徑。與 RRT* 相比，這一特征迫使所提出的算法以更少的迭代次數(shù)達(dá)到最優(yōu)解。

注意：

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

8.2 偽碼

8.3 程序示例

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

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

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

8.4 參考

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

2、Rapid convergence implementation of RRT* towards optimal solution