7.1 原理

        RRT*是一種基于采樣的最優(yōu)化路徑規(guī)劃方式，與RRT的區(qū)別是，RRT盡量使新節(jié)點(diǎn)以及其周圍的節(jié)點(diǎn)到起點(diǎn)的cost(可以是路徑或者時(shí)間等目標(biāo)函數(shù))最短，而不是僅僅尋找離它近的節(jié)點(diǎn)，而且在找到路徑后不會(huì)停止，而是繼續(xù)進(jìn)行采樣來(lái)優(yōu)化得到的路徑。

        盡管RRT算法是一個(gè)相對(duì)高效率，同時(shí)可以較好的處理帶有非完整約束的路徑規(guī)劃問(wèn)題的算法，并且在很多方面有很大的優(yōu)勢(shì)，但是RRT算法并不能保證所得出的可行路徑是相對(duì)優(yōu)化的。因此許多關(guān)于RRT算法的改進(jìn)也致力于解決路徑優(yōu)化的問(wèn)題，RRT*算法就是其中一個(gè)。RRT*算法的主要特征是能快速的找出初始路徑，之后隨著采樣點(diǎn)的增加，不斷地進(jìn)行優(yōu)化直到找到目標(biāo)點(diǎn)或者達(dá)到設(shè)定的最大循環(huán)次數(shù)。RRT*算法是漸進(jìn)優(yōu)化的，也就是隨著迭代次數(shù)的增加，得出的路徑是越來(lái)越優(yōu)化的，而且永遠(yuǎn)不可能在有限的時(shí)間中得出最優(yōu)的路徑。因此換句話說(shuō)，要想得出相對(duì)滿意的優(yōu)化路徑，是需要一定的運(yùn)算時(shí)間的。所以RRT*算法的收斂時(shí)間是一個(gè)比較突出的研究問(wèn)題。但不可否認(rèn)的是，RRT*算法計(jì)算出的路徑的代價(jià)相比RRT來(lái)說(shuō)減小了不少。RRT*算法與RRT算法的區(qū)別主要在于兩個(gè)針對(duì)新節(jié)點(diǎn) xnew 的重計(jì)算過(guò)程，分別為：

·重新為 xnew 選擇父節(jié)點(diǎn)的過(guò)程

·重布線隨機(jī)樹的過(guò)程

7.1.1 重新選擇父節(jié)點(diǎn)過(guò)程

        在新產(chǎn)生的節(jié)點(diǎn) xnew 附近以定義的半徑范圍內(nèi)尋找“近鄰”，作為替換 xnew 父節(jié)點(diǎn)的備選。依次計(jì)算“近鄰”節(jié)點(diǎn)到起點(diǎn)的路徑代價(jià)加上 xnew 到每個(gè)“近鄰”的路徑代價(jià)，具體過(guò)程見上圖。

        圖(a)中表現(xiàn)的是隨機(jī)樹擴(kuò)展過(guò)程中的一個(gè)時(shí)刻，節(jié)點(diǎn)標(biāo)號(hào)表示產(chǎn)生該節(jié)點(diǎn)的順序，0節(jié)點(diǎn)是初始節(jié)點(diǎn)，9節(jié)點(diǎn)是新產(chǎn)生的節(jié)點(diǎn) xnew，6節(jié)點(diǎn)是產(chǎn)生9節(jié)點(diǎn)的 xnear，節(jié)點(diǎn)與節(jié)點(diǎn)之間連接的邊上數(shù)字代表兩個(gè)節(jié)點(diǎn)之間的歐氏距離(這里我們用歐氏距離來(lái)表示路徑代價(jià))。

        在重新找父節(jié)點(diǎn)的過(guò)程中，以9節(jié)點(diǎn) xnew 為圓心，以事先規(guī)定好的半徑，找到在這個(gè)圓的范圍內(nèi) xnew 的近鄰，也就是4，5，8節(jié)點(diǎn)。

        原來(lái)的路徑0 - 4 - 6 - 9代價(jià)為10 + 5 + 1 = 16，備選的三個(gè)節(jié)點(diǎn)與 xnew 組成的路徑0 - 1 - 5 - 9，0 - 4 - 9和0 - 1 - 5 - 8 - 9代價(jià)分別為3 + 5 + 3 = 11，10 + 4 = 14和3 + 5 + 1 + 3 = 12，因此如果5節(jié)點(diǎn)作為9節(jié)點(diǎn)的新父節(jié)點(diǎn)，則路徑代價(jià)相對(duì)是最小的，因此我們把9節(jié)點(diǎn)的父節(jié)點(diǎn)由原來(lái)的節(jié)點(diǎn)4變?yōu)楣?jié)點(diǎn)5，則重新生成的隨機(jī)樹如圖(b)所示。

7.1.2 重布線隨機(jī)樹過(guò)程

    在為xnew 重新選擇父節(jié)點(diǎn)之后，為進(jìn)一步使得隨機(jī)樹節(jié)點(diǎn)間連接的代價(jià)盡量小，為隨機(jī)樹進(jìn)行重新布線。過(guò)程示意如上圖重布線的過(guò)程也可以被表述成：如果近鄰節(jié)點(diǎn)的父節(jié)點(diǎn)改為 xnew 可以減小路徑代價(jià)，則進(jìn)行更改。

    如圖(c)，9節(jié)點(diǎn)為新生成的節(jié)點(diǎn) xnew ，近鄰節(jié)點(diǎn)分別為節(jié)點(diǎn)4 , 6 , 8 。它們父節(jié)點(diǎn)分別為節(jié)點(diǎn)0 , 4 , 5。路徑分別為0 - 4，0 - 4 - 6，0 - 1 - 5 - 8，代價(jià)分別為10，10 + 5 = 15 和3 + 5 + 1 = 9。

    如果將4節(jié)點(diǎn)的父節(jié)點(diǎn)改為9節(jié)點(diǎn) xnew ，則到達(dá)節(jié)點(diǎn)4的路徑變?yōu)? - 1 - 5 - 9 - 4，代價(jià)為3 + 5 + 3 + 4 = 15 大于原來(lái)的路徑代價(jià)10，因此不改變4節(jié)點(diǎn)的父節(jié)點(diǎn)。

    同理，改變了8節(jié)點(diǎn)的父節(jié)點(diǎn)，路徑代價(jià)將由原來(lái)的9變?yōu)?4，也不改變8節(jié)點(diǎn)的父節(jié)點(diǎn)。如果改變6節(jié)點(diǎn)的父節(jié)點(diǎn)為 xnew 則路徑變?yōu)? - 1 - 5 - 9 - 6，代價(jià)為3 + 5 + 3 + 1 = 12小于原來(lái)的路徑代價(jià)15，因此將6的父節(jié)點(diǎn)改為節(jié)點(diǎn)9，生成的新隨機(jī)樹如圖(d)。

    重布線過(guò)程的意義在于每當(dāng)生成了新的節(jié)點(diǎn)后，是否可以通過(guò)重新布線，使得某些節(jié)點(diǎn)的路徑代價(jià)減少。如果以整體的眼光看，并不是每一個(gè)重新布線的節(jié)點(diǎn)都會(huì)出現(xiàn)在最終生成的路徑中，但在生成隨機(jī)樹的過(guò)程中，每一次的重布線都盡可能的為最終路徑代價(jià)減小創(chuàng)造機(jī)會(huì)。

    RRT*算法的核心在于上述的兩個(gè)過(guò)程：重新選擇父節(jié)點(diǎn)和重布線。這兩個(gè)過(guò)程相輔相成，重新選擇父節(jié)點(diǎn)使新生成的節(jié)點(diǎn)路徑代價(jià)盡可能小，重布線使得生成新節(jié)點(diǎn)后的隨機(jī)樹減少冗余通路，減小路徑代價(jià)。

7.2 偽碼

7.3 程序示例

7.4 參考

1、Sampling-based Algorithms for Optimal Motion Planning

2、https://blog.csdn.net/weixin_43795921/article/details/88557317#t0

3、https://zhuanlan.zhihu.com/p/51087819