9.1 原理

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

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

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

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

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

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

注意：

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

9.2 算法思路

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

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

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

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

9.3 偽碼

9.4 程序示例

下圖中的 圓圈代表提前采好的隨機(jī)點(diǎn)

9.5 參考

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