10.1 原理

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

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

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

注意：

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

10.2 思路

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

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

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

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

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

方法1：

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

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

方法2：

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

這里放一段.m文件取橢圓隨機點的代碼（思路如方法2）：

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

10.3 偽碼

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

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

10.4 程序示例

程序在尋找初始路徑的過程和普通RRT*一樣，在全局域中隨機撒點，迭代到1282次時首次找到初始路徑，然后我們以起始點和目標點為焦點，初始路徑的長度為點到兩焦點的距離之和，畫出一個橢圓：

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

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

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

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

10.5 參考

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