一、背景意義

? ? ? ?運(yùn)動(dòng)規(guī)劃問題是要找到一條動(dòng)態(tài)可行的軌跡，將機(jī)器人從初始狀態(tài)帶到目標(biāo)狀態(tài)，同時(shí)避免與障礙物發(fā)生碰撞。這對(duì)機(jī)器人實(shí)現(xiàn)自主運(yùn)動(dòng)、高效執(zhí)行任務(wù)、降低人為成本等具有非常重要的意義。一個(gè)運(yùn)動(dòng)規(guī)劃算法應(yīng)該表現(xiàn)出兩個(gè)特性：一個(gè)是形式上的完整性保證，另一個(gè)是漸進(jìn)的最優(yōu)性。RRT一種基于采樣的快速探索隨機(jī)樹算法，能夠相對(duì)快速地找到一個(gè)可行的運(yùn)動(dòng)軌跡，即使在高維狀態(tài)空間中，保證了運(yùn)動(dòng)規(guī)劃算法的完整性。但是其尋找的運(yùn)動(dòng)軌跡并非是當(dāng)前從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)軌跡。而RRT*會(huì)對(duì)樹節(jié)點(diǎn)有個(gè)修建和重新選擇的過程，即將新節(jié)點(diǎn)加入搜索樹時(shí)父節(jié)點(diǎn)會(huì)有一個(gè)重連過程，也就是在以新節(jié)點(diǎn)為圓心，半徑為r的鄰域內(nèi)，找到與新節(jié)點(diǎn)連接后路徑代價(jià)(從起點(diǎn)移動(dòng)到新節(jié)點(diǎn)的路徑長(zhǎng)度)最小的節(jié)點(diǎn)，并重新選擇最短距離節(jié)點(diǎn)作為新節(jié)點(diǎn)的父節(jié)點(diǎn)，而不是最近節(jié)點(diǎn)。這樣保證了尋找的路徑可以收斂到最優(yōu)解，同時(shí)還在概率上保證了軌跡的完整性。

二、RRT（快速探索隨機(jī)樹）

? ? ?1.?基本思想

? ? ??軌跡連通圖采用樹的形式，在空間中隨機(jī)采樣，將采樣的節(jié)點(diǎn)加入到樹中并于最近的樹節(jié)點(diǎn)相連，然后進(jìn)行一步步的擴(kuò)展達(dá)到目標(biāo)點(diǎn)后停止。需要考慮的是機(jī)器人的運(yùn)動(dòng)執(zhí)行能力，以及是否可以通過樹結(jié)構(gòu)直接回溯得到可行的路徑。

? ? ? 2.?基本流程

? ? ? ?以起點(diǎn)為根節(jié)點(diǎn)，建立搜索樹，對(duì)樹進(jìn)行擴(kuò)張（extend）：在狀態(tài)空間中，隨機(jī)采樣一個(gè)狀態(tài)，稱為Xsample；在現(xiàn)有的探索樹查找與Xsample距離最近的節(jié)點(diǎn)Xnearest；然后依據(jù)Xsample和Xnearest確定新的樹擴(kuò)張新節(jié)點(diǎn)。這一步需要注意的是：二維路徑規(guī)劃可根據(jù)Xsample和Xnearest的方向結(jié)合步長(zhǎng)確定新的節(jié)點(diǎn)Xnew，在高維路徑規(guī)劃可考慮機(jī)器人的系統(tǒng)狀態(tài)方程，以Xsample和Xnearest構(gòu)建新的輸入u，以Xnearest作為當(dāng)前狀態(tài)x，根據(jù)系統(tǒng)的狀態(tài)方程，得到下一個(gè)狀態(tài)即新的擴(kuò)張節(jié)點(diǎn)Xnew。接著要有一個(gè)判斷，判斷Xsample和Xnearest是否會(huì)碰到障礙物，而且不在搜索樹中，如果不符合則舍去；最后終止判斷，已經(jīng)到達(dá)終點(diǎn)，規(guī)劃成功，返回從初始狀態(tài)到終點(diǎn)的節(jié)點(diǎn)路徑。?

? ? ? 3.?不足與可改進(jìn)的點(diǎn)

? ? ? ? 當(dāng)空間中包含大量的障礙物，或者狹窄的通道約束時(shí)，算法的收斂速度很慢，有時(shí)甚至無法找到可行解，而且得到的路徑并非最優(yōu)路徑。針對(duì)改進(jìn)，可以從以下方面著手：①算法的代碼初始時(shí)，給定一定的采樣概率向著目標(biāo)點(diǎn)進(jìn)行采樣②采用何種方式進(jìn)行采樣，初始的步長(zhǎng)是否可以調(diào)整來適應(yīng)更細(xì)致的連接③如何定義和查找最近節(jié)點(diǎn)④樹的將如何擴(kuò)張來更快的找到路徑

? ? ? 4.?確定最近距離和查找最近點(diǎn)

? ? ? ①計(jì)算距離最簡(jiǎn)單的方法是歐氏距離，但在很多場(chǎng)景下此種距離并不可行

? ? ? ②另一種可行方法是對(duì)距離的不同分量進(jìn)行加權(quán)平均，不同分量的重要程度用權(quán)重大小表示

? ? ? ③尋找最近點(diǎn)有很多可行的算法，如K-D樹，Hash算法等

三、RRT*（快速探索隨機(jī)樹的擴(kuò)展）

? ?1. 原理

? ? 首先定義一個(gè)成本函數(shù)表示從初始狀態(tài)到任意狀態(tài)的代價(jià)，并且設(shè)置初始狀態(tài)成本為0，然后RRT* 找到最近的狀態(tài)和相鄰的狀態(tài)，將最近的狀態(tài)添加到樹中，并在存在最小代價(jià)連接的情況下添加最小代價(jià)連接，最后RRT*通過連接來消除開銷較大的連接。

? ?2.?基本流程

? ? 與RRT一樣，RRT*通過從空間中隨機(jī)采樣Xsample并求解一條軌跡Xnew，將樹中最近的節(jié)點(diǎn)Xnearest向樣本延伸來逐步建立樹。如果這個(gè)軌跡沒有與障礙物發(fā)生碰撞，標(biāo)準(zhǔn)的RRT將新的節(jié)點(diǎn)Xnew插入到以Xnearest為父節(jié)點(diǎn)的樹中，并繼續(xù)進(jìn)行下一次迭代。在這里，RRT*的操作是不同的。RRT*不是選擇最近的節(jié)點(diǎn)作為父節(jié)點(diǎn)，而是考慮Xnew附近的所有節(jié)點(diǎn)并評(píng)估選擇每個(gè)節(jié)點(diǎn)作為父節(jié)點(diǎn)的成本。最后進(jìn)行進(jìn)一步的回溯，如果存在一條路徑Xnew經(jīng)過到達(dá)Xnearest花費(fèi)的代價(jià)小于Xnearest的父節(jié)點(diǎn)到達(dá)Xnearest的代價(jià)，說明原來到達(dá)的代價(jià)不是最優(yōu)的，需要選擇Xnew到達(dá)Xnearest的路徑作為新路徑。對(duì)所有的鄰近點(diǎn)集合內(nèi)的點(diǎn)都做相同的操作，經(jīng)過N次迭代得到接近最優(yōu)的次優(yōu)路徑了。

? ?3.優(yōu)點(diǎn)與改進(jìn)的地方

? ? ①相對(duì)與RRT最大的改進(jìn)就是，會(huì)有一個(gè)選擇重連的過程，來確保與新節(jié)點(diǎn)與父節(jié)點(diǎn)連接的成本是最小的，以達(dá)到一條漸進(jìn)最優(yōu)的路徑

? ? ②RRT*的缺點(diǎn)也很明顯，因?yàn)樾枰x擇重連，計(jì)算量大，生成路徑的效率低，甚至在復(fù)雜的環(huán)境或者窄縫的情況下會(huì)出現(xiàn)找不到路徑。

? ? ③可以從擴(kuò)展的范圍進(jìn)行改進(jìn)，而不是全局進(jìn)行隨機(jī)采樣搜索，還可以分別從起點(diǎn)終點(diǎn)建立探索樹，以加快搜索效率。在步長(zhǎng)方面是否也可以進(jìn)行改進(jìn)，改進(jìn)為自適應(yīng)步長(zhǎng)，來更快通過無障礙區(qū)域。

四、參考文獻(xiàn)

[1] S. Karaman and E. Frazzoli, “Sampling-based Algorithms for Optimal Motion Planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, June 2011.

[2] Klemm S, Oberl?nder J, Hermann A, et,?“Rrt-connect: Faster, asymptotically optimal motion planning”015 IEEE international conference on robotics and biomimetics (ROBIO). IEEE, 2015: 1670-1677.

?