10.1 原理

        在RRT中，當(dāng)初始路徑已經(jīng)生成之后，如果重點(diǎn)在初始路徑周圍進(jìn)行采樣的話，可以明顯提高路徑優(yōu)化效率。Informed RRT就是進(jìn)一步優(yōu)化了采樣函數(shù)，采樣的方式是以起點(diǎn)和終點(diǎn)為焦點(diǎn)構(gòu)建橢圓形采樣區(qū)域。

        回顧一下RRT*-smart，因?yàn)樵谀硡^(qū)域撒點(diǎn)越多，該區(qū)域的優(yōu)化效果越好，而單純的RRT*是在全域內(nèi)隨機(jī)撒點(diǎn)，優(yōu)化效果沒有得以集中，RRT*-smart認(rèn)為經(jīng)過路徑優(yōu)化后的路徑的拐點(diǎn)在障礙物的附近，它認(rèn)為這個(gè)拐點(diǎn)的附近需要著重優(yōu)化，所以RRT*-smart在進(jìn)一步撒點(diǎn)的過程中，將一些隨機(jī)點(diǎn)偏袒的撒在這個(gè)拐點(diǎn)的附近鄰域。

        這里的informed RRT*也是這樣認(rèn)為，它認(rèn)為單純的RRT*在整個(gè)區(qū)域內(nèi)隨機(jī)撒點(diǎn)，優(yōu)化效果太過分散，如果我能知道我最終優(yōu)化的路徑在哪一塊區(qū)域，那我就只在這一區(qū)域內(nèi)撒點(diǎn)不就好了嗎？informed RRT*就是這樣做的。

注意：

        informed RRT*是在RRT*算法給出一條初始路徑后，對(duì)這個(gè)初始路徑繼續(xù)優(yōu)化的步驟才起作用的，它對(duì)于這個(gè)初始路徑的生成沒有幫助。

10.2 思路

        根據(jù)高中數(shù)學(xué)知識(shí)可以知道，在橢圓上的點(diǎn)到橢圓兩焦點(diǎn)的距離之和相同，橢圓外的點(diǎn)的距離到兩焦點(diǎn)的距離之和大于橢圓上的點(diǎn)到兩焦點(diǎn)的距離之和，橢圓內(nèi)的點(diǎn)反之。

        回顧一下RRT*的搜索圖，根據(jù)上面這個(gè)知識(shí)點(diǎn)可以發(fā)現(xiàn)，其實(shí)RRT在已經(jīng)得到一條可行路徑之后，可以將采樣空間收縮到一個(gè)橢圓形區(qū)域中，區(qū)域之外的點(diǎn)對(duì)于縮短規(guī)劃出的路徑長(zhǎng)度并沒有實(shí)際價(jià)值。

        informed RRT就是的主要思想就是上面這種思想，在獲取可行路徑之后，將采樣空間限制在一個(gè)橢圓形區(qū)域中，并且隨著路徑長(zhǎng)度的不斷縮短，逐漸縮小該橢圓形區(qū)域。這個(gè)思想其實(shí)在以前就有，但是提出informed RRT的論文中提出了對(duì)這個(gè)橢圓形區(qū)域直接采樣的方法。

        可能有人會(huì)直接想，這里只不過是縮小了采樣空間，并不會(huì)明顯改進(jìn)算法。但是實(shí)際上，當(dāng)拓展到高維空間時(shí)，效率的提升是巨大的。

那么，如何表達(dá)這個(gè)橢圓呢？下面介紹橢圓采樣區(qū)域的表達(dá)方式

方法1：

先在標(biāo)準(zhǔn)橢圓的方程中采樣，再將采樣點(diǎn)旋轉(zhuǎn)平移到實(shí)際采樣區(qū)域，需要兩個(gè)矩陣：平移向量、旋轉(zhuǎn)矩陣。這兩個(gè)參數(shù)只需要在初始化時(shí)計(jì)算即可

轉(zhuǎn)換后的坐標(biāo)為：

方法2：

利用超橢圓體然后在二維平面映射

這里放一段.m文件取橢圓隨機(jī)點(diǎn)的代碼（思路如方法2）：

除了采樣過程外，Informed RRT*的流程和RRT*是一樣的。

10.3 偽碼

偽代碼中是在RRT的偽代碼基礎(chǔ)上改的，標(biāo)紅的地方是informed RRT 更改的地方?？梢钥闯?，其實(shí)主體框架上面并沒有太多更改，實(shí)際上也是，主要的更改都在第七行，也就是采樣這一步。

這是采樣這一步的偽代碼。informed RRT將目前已經(jīng)搜索到的最短路徑作為cbest，起點(diǎn)和終點(diǎn)之間的距離作為cmin，以此構(gòu)建橢圓。當(dāng)還沒有規(guī)劃結(jié)果時(shí)，cbest為inf，也就是和經(jīng)典RRT沒有區(qū)別。

10.4 程序示例

程序在尋找初始路徑的過程和普通RRT*一樣，在全局域中隨機(jī)撒點(diǎn)，迭代到1282次時(shí)首次找到初始路徑，然后我們以起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)，初始路徑的長(zhǎng)度為點(diǎn)到兩焦點(diǎn)的距離之和，畫出一個(gè)橢圓：

我們隨后的隨機(jī)點(diǎn)的選取范圍不再是全局域了，新采的樣本點(diǎn)被限制在這個(gè)橢圓中，下圖中的圓圈代表迭代1283-2509次的隨機(jī)點(diǎn)的分布，可見，新的隨機(jī)點(diǎn)全部被限制在橢圓中：

當(dāng)?shù)?510次時(shí)，新的總長(zhǎng)度更短的路徑被找到，，隨后，我們以起始點(diǎn)和目標(biāo)點(diǎn)為焦點(diǎn)，以這個(gè)新的路徑的長(zhǎng)度為到兩焦點(diǎn)的距離，畫出一個(gè)比之前更小的橢圓：

同樣的，迭代次數(shù)為2510-2865次的循環(huán)中的新的隨機(jī)點(diǎn)被限制在這個(gè)新的更小的橢圓中，使隨機(jī)點(diǎn)資源進(jìn)一步集中：

當(dāng)?shù)?866次時(shí)，找到一個(gè)路徑更短的路徑：

10.5 參考

Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic