8.1 原理

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

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

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

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

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

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

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

①路徑優(yōu)化

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

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

具體操作如下：

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

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

②智能采樣

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

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

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

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

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

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

注意：

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

8.2 偽碼

8.3 程序示例

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

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

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

8.4 參考

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

2、Rapid convergence implementation of RRT* towards optimal solution