9.1 原理

        FMT*算法專門針對解決高維構(gòu)型空間中的復(fù)雜運動規(guī)劃問題，它是為高密度障礙物的環(huán)境構(gòu)建的算法。該算法被證明是漸近最優(yōu)的，并且比同類型算法（RRT*）更快收斂到最優(yōu)解。FMT*算法在預(yù)先確定的概率繪制的樣本數(shù)量上執(zhí)行“惰性”動態(tài)規(guī)劃遞歸，以生長路徑樹，該路徑樹在成本到達(dá)空間中穩(wěn)定地向外移動。

        FMT*的最終產(chǎn)物是一棵樹，它在連續(xù)空間中獲取一批樣本，然后它能在圖中使用惰性的動態(tài)編程搜索該樣本集合，并以此找到路徑，這也是一個漸進(jìn)最優(yōu)的解決方案，F(xiàn)MT*相比于RRT*的加速效果優(yōu)勢在高維和碰撞檢查很昂貴的情況下尤其突出。很棒的一點是，F(xiàn)MT*是從起始位置開始構(gòu)建，而不是像RRT*是在空間的任意位置采點，因為這可能會得到非常遠(yuǎn)的點或非常近的點，這有什么好處呢？這意味著你不必在樹中回溯以進(jìn)行重布線，因為這在計算上效率低下。FMT*比RRT*更好，因為它創(chuàng)建的連接接近最佳，沒有重布線。

        FMT*算法在預(yù)先確定的采樣點數(shù)量上執(zhí)行前向動態(tài)規(guī)劃遞歸，并相應(yīng)地通過在代價到達(dá)空間中穩(wěn)步向外移動生成路徑樹。FMT*執(zhí)行動態(tài)規(guī)劃遞歸，其特點有三個關(guān)鍵特征:

·它是為磁盤連接圖量身定制的，其中兩個樣本的距離低于給定的界限（稱為連接半徑）則這兩個樣本被認(rèn)為是鄰居，因此是可連接的。

·它同時執(zhí)行圖構(gòu)造和圖搜索。

·為了評估動態(tài)規(guī)劃遞歸中的即時成本，算法“懶惰地”忽略了障礙物的存在，每當(dāng)與新樣本的局部最優(yōu)(假設(shè)沒有障礙物)連接與障礙物相交時，該樣本就會簡單地跳過并留待以后，而不是在鄰域中尋找其他連接。

注意：

FMT*的樣本點是提前生成好的，然后把這些點固定，再利用這些固定好的點來生成行進(jìn)樹，注意區(qū)別于RRT*那一套，RRT*是生成隨機點的同時，生成行進(jìn)樹。

9.2 算法思路

上圖（a）是RRT*添加新節(jié)點的某一步，上圖（b）（c）是FMT*添加新節(jié)點的某一步。

        先看RRT*，節(jié)點9是新考慮的節(jié)點，它在第一次重布線時，需要從它的所有鄰居節(jié)點中找出一個父節(jié)點，使得節(jié)點9到達(dá)起始節(jié)點的cost最小，因此節(jié)點9需要計算它到4、5、6、8號節(jié)點的距離并同時進(jìn)行碰撞檢測，因此，第一次重布線過程就要求待加入的節(jié)點對它的所有鄰居節(jié)點進(jìn)行一次碰撞檢測，第二次重布線過程也需要計算距離和碰撞檢測，但這在第一次重布線過程中做過了，可以記錄先來直接利用，因此第二次重布線過程碰撞檢測這一步可以不用重復(fù)進(jìn)行，因此，總的來說，RRT*每新加入一個節(jié)點，該節(jié)點需要對它的所有鄰居節(jié)點進(jìn)行一次碰撞檢測。

        再看FMT*，上圖（b）（c）中的x就是新考慮的節(jié)點，在圖（b）中，x需計算它到集合V_open中所有的鄰居節(jié)點的cost，但不需要進(jìn)行碰撞檢測，從中選擇一個能使它到達(dá)初始節(jié)點總cost最小的節(jié)點作為它的父節(jié)點，然后，對它和這個父節(jié)點的連線進(jìn)行碰撞檢測，如果能通過碰撞檢測，則加入x，若不能，則下一個x，因此，總的來說，F(xiàn)MT*每新加入一個節(jié)點，永遠(yuǎn)只需要進(jìn)行一次碰撞檢測。

FMT*比RRT*每新加入一個節(jié)點需要進(jìn)行的碰撞檢測次數(shù)少得多，而且FMT*也是漸進(jìn)最優(yōu)的，這就是FMT*相比于RRT*的優(yōu)勢所在。

9.3 偽碼

9.4 程序示例

下圖中的 圓圈代表提前采好的隨機點

9.5 參考

Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions?