基于rrt的運(yùn)動(dòng)規(guī)劃算法綜述

1、基于 基于 RRT 的運(yùn)動(dòng)規(guī)劃 的運(yùn)動(dòng)規(guī)劃算法綜述 算法綜述 1.介紹 介紹 在過(guò)去的十多年中， 機(jī)器人的運(yùn)動(dòng)規(guī)劃問(wèn)題已經(jīng)收到了大量的關(guān)注， 因?yàn)闄C(jī)器人開(kāi)始成為現(xiàn)代工業(yè)和日常生活的重要組成部分。 最早的運(yùn)動(dòng)規(guī)劃的問(wèn)題只是考慮如何移動(dòng)一架鋼琴?gòu)囊粋€(gè)房間到另一個(gè)房間而沒(méi)有碰撞任何物體。 早期的算法則關(guān)注研究一個(gè)最完備的運(yùn)動(dòng)規(guī)劃算法（完備性指如果存在這么一條規(guī)劃的路徑，那么算法一定能夠在有限時(shí)間找到它） ，例如用一個(gè)多邊形表示機(jī)器人， 其他的

2、多邊形表示障礙物體， 然后轉(zhuǎn)化為一個(gè)代數(shù)問(wèn)題去求解。但是這些算法遇到了計(jì)算的復(fù)雜性問(wèn)題，他們有一個(gè)指數(shù)時(shí)間的復(fù)雜度。在 1979 年，Reif 則證明了鋼琴搬運(yùn)工問(wèn)題的運(yùn)動(dòng)規(guī)劃是一個(gè) PSPACE-hard 問(wèn)題[1]。所以這種完備的規(guī)劃算法無(wú)法應(yīng)用在實(shí)際中。 在實(shí)際應(yīng)用中的運(yùn)動(dòng)規(guī)劃算法有胞分法[2]，勢(shì)場(chǎng)法[3]，路徑圖法[4]等。這些算法在參數(shù)設(shè)置的比較好的時(shí)候， 可以保證規(guī)劃的完備性， 在復(fù)雜環(huán)境中也可以保證花費(fèi)的時(shí)間上限。然而，

3、這些算法在實(shí)際應(yīng)用中有許多缺點(diǎn)。例如在高維空間中這些算法就無(wú)法使用， 像胞分法會(huì)使得計(jì)算量過(guò)大。勢(shì)場(chǎng)法會(huì)陷入局部極小值，導(dǎo)致規(guī)劃失敗[5],[6]。 基于采樣的運(yùn)動(dòng)規(guī)劃算法是最近十幾年提出的一種算法，并且已經(jīng)吸引了極大的關(guān)注。概括的講， 基于采樣的運(yùn)動(dòng)規(guī)劃算法一般是連接一系列從無(wú)障礙的空間中隨機(jī)采樣的點(diǎn)， 試圖建立一條從初始狀態(tài)到目標(biāo)狀態(tài)的路徑。 與最完備的運(yùn)動(dòng)規(guī)劃算法相反， 基于采樣的方法通過(guò)避免在狀態(tài)空間中顯式地構(gòu)造障礙物來(lái)提供大量

4、的計(jì)算節(jié)省。 即使這些算法沒(méi)有實(shí)現(xiàn)完整性， 但是它們是概率完備， 這意味著規(guī)劃算法不能返回解的概率隨著樣本的數(shù)量趨近無(wú)窮而衰減到零[7]，并且這個(gè)下降速率是指數(shù)型的。 快速擴(kuò)展隨機(jī)樹(shù)(Rapidly-exploring Random Trees，RRT)算法，是近十幾年得到廣泛發(fā)展與應(yīng)用的基于采樣的運(yùn)動(dòng)規(guī)劃算法，它由美國(guó)愛(ài)荷華州立大學(xué)的 Steven M. LaValle 教授在1998 年提出，他一直從事 RRT 算法的改進(jìn)和應(yīng)用研究

5、，他的相關(guān)工作奠定了 RRT 算法的基礎(chǔ)。 RRT 算法是一種在多維空間中有效率的規(guī)劃方法。 原始的 RRT 算法是通過(guò)一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過(guò)隨機(jī)采樣，增加葉子節(jié)點(diǎn)的方式，生成一個(gè)隨機(jī)擴(kuò)展樹(shù)，當(dāng)隨機(jī)樹(shù)中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或進(jìn)入了目標(biāo)區(qū)域， 便可以在隨機(jī)樹(shù)中找到一條由樹(shù)節(jié)點(diǎn)組成的從初始點(diǎn)到目標(biāo)點(diǎn)的路徑。 快速擴(kuò)展隨機(jī)樹(shù)（RRT）也是一種數(shù)據(jù)結(jié)構(gòu)和算法，其設(shè)計(jì)用途是用來(lái)有效搜索高維非凸空間，可應(yīng)用于路徑規(guī)劃、虛擬現(xiàn)實(shí)等研究。RRT

6、是一種基于概率采樣的搜索方法，它采用一種特殊的增量方式進(jìn)行構(gòu)造， 這種方式能迅速縮短一個(gè)隨機(jī)狀態(tài)點(diǎn)與樹(shù)的期望距離。 該方法的特點(diǎn)是能夠快速有效的搜索高維空間， 通過(guò)狀態(tài)空間的隨機(jī)采樣點(diǎn)， 把搜索導(dǎo)向空白區(qū)域， 從而尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的規(guī)劃路徑。 它通過(guò)對(duì)狀態(tài)空間中的采樣點(diǎn)進(jìn)行碰撞檢測(cè)，避免了對(duì)空間的建模，能夠有效的解決高維空間和復(fù)雜約束的路徑規(guī)劃問(wèn)題。RRT算法適合解決多自由度機(jī)器人在復(fù)雜環(huán)境下和動(dòng)態(tài)環(huán)境中的路徑規(guī)劃問(wèn)題[8]。

7、與其他的隨機(jī)路徑規(guī)劃方法相比，RRT 算法更適用于非完整約束和多自由度的系統(tǒng)中[9]。 相比于最原始的 RRT 算法的一些缺點(diǎn)，又提出了許多改進(jìn)的 RRT 算法，例如： （1）基于概率 P 的 RRT 為了加快隨機(jī)樹(shù)到達(dá)目標(biāo)點(diǎn)的速度，簡(jiǎn)單的改進(jìn)方法是：在隨機(jī)樹(shù)每次的生長(zhǎng)過(guò)程中，根據(jù)隨機(jī)概率 （0.0 到 1.0 的隨機(jī)值 p） 來(lái)選擇生長(zhǎng)方向是目標(biāo)點(diǎn)還是隨機(jī)點(diǎn)。 2001 年， LaValle定義 定義 2.7 路徑長(zhǎng)度（ 路徑長(zhǎng)度（p

8、ath cost）定義𝑝𝑐: ∑𝐶𝑓𝑟𝑒𝑒 → 𝑅>0為一條路徑的非負(fù)長(zhǎng)度?！?#119862;𝑓𝑟𝑒𝑒表示所有自由空間中的路徑集合。 2.3 運(yùn)動(dòng)規(guī)劃的基本定義 運(yùn)動(dòng)規(guī)劃的基本定義 用 C-空間的思路，運(yùn)動(dòng)規(guī)劃問(wèn)題在概念上變得非常簡(jiǎn)單：任務(wù)

9、是在中尋找一條從起始位姿到目標(biāo)位姿的路徑。運(yùn)動(dòng)規(guī)劃問(wèn)題的示意圖如圖 2.1 所示,圖中整個(gè)水滴表示位姿空間𝐶 = 𝐶 𝑓𝑟𝑒𝑒 ? 𝐶𝑜𝑏𝑠. 圖 2.1 運(yùn)動(dòng)規(guī)劃示意圖 有了上述概念，C-空間中的運(yùn)動(dòng)規(guī)劃問(wèn)題可描述如下： （1）定義一個(gè)工作空間 W； （2）定義 W 中的障礙物

10、區(qū)域 O 和機(jī)器人 A； （3）所有可能的機(jī)器人位姿構(gòu)成 C-空間，并且劃分為 Cobs 和 Cfree； （4）指定機(jī)器人初始位姿 qinit 和目標(biāo)目標(biāo)位姿 qgoal； （5）可行規(guī)劃（Feasible planning）一個(gè)完整的算法必須計(jì)算一條連續(xù)的路徑τ: [0,1] →𝐶 𝑓𝑟𝑒𝑒，使得τ(0) = 𝑞𝑖

11、9899;𝑖𝑡，τ(1) = 𝑞𝑔𝑜𝑎𝑙或者正確報(bào)告這樣的路徑不存在。 （6）最優(yōu)規(guī)劃（Optimal planning）在所有的可行的規(guī)劃的路徑里面花費(fèi)代價(jià)最少的一條路徑𝑚𝑖𝑛𝜏∈∑𝐶𝑓𝑟𝑒

12、9890;𝑝𝑐(𝜏)，或者報(bào)告這樣的路徑不存在。 3.算法 算法 在介紹 RRT 算法之前，先說(shuō)明一下路徑的表示方法。我們用一個(gè)有向圖來(lái)表示路徑 G=（V,E） ，那么一條可行的路徑就是一個(gè)頂點(diǎn)的序列(𝑣1 , 𝑣2 , 𝑣3 , … , 𝑣𝑛),𝑣1 = 𝑞𝑖&

13、#119899;𝑖𝑡 , 𝑣𝑛 =𝑞𝑔𝑜𝑎𝑙.同時(shí)(𝑣𝑖 , 𝑣𝑖+1) ∈ 𝐸, 1 ≤ 𝑖 ≤ 𝑛 ? 1表示邊。 這樣子問(wèn)題就變成了使用采樣到的點(diǎn)來(lái)擴(kuò)展圖 G，使之能找到一條從初

14、始節(jié)點(diǎn)到達(dá)目標(biāo)節(jié)點(diǎn)的路徑。 3.1 原始的 原始的 RRT 算法 算法 算法 1：RRT 算法主體部分 1. 𝑉 ← {𝑞𝑖𝑛𝑖𝑡};𝐸 ← ?; 𝑖 ← 0; 2. 𝐰𝐡𝐢𝐥𝐞 𝑖 < 𝑁 &

15、#119837;𝐨 3. 𝐺 ← (𝑉, 𝐸); 4. 𝑞𝑟𝑎𝑛𝑑 ← Sample(𝑖); 𝑖 ← 𝑖 + 1; 5. (𝑉, 𝐸) ← Extend(⻒