文章目錄

• 優(yōu)缺點(diǎn)
• 偽代碼
• 具體流程
• 效率問(wèn)題
• 代碼

優(yōu)缺點(diǎn)

優(yōu)缺點(diǎn)先明說(shuō)，優(yōu)點(diǎn)RRT Star適用于任何地圖，不像A Star，Dijkstra那樣受限于柵格地圖。
缺點(diǎn)：1.找到的路徑可能不是最優(yōu)的；2.路徑可能不符合機(jī)器人的運(yùn)動(dòng)學(xué)動(dòng)力學(xué)模型；3.效率問(wèn)題。

偽代碼

具體流程

給出起點(diǎn)和終點(diǎn)，以及設(shè)置好障礙物的地圖，如下所示，將起點(diǎn)記作是根節(jié)點(diǎn)。

---

進(jìn)行空間撒點(diǎn)采樣，在空間中隨機(jī)選擇一點(diǎn)Xrand。（這里對(duì)應(yīng)偽代碼當(dāng)中的Sample()函數(shù)）

---

接著尋找距離Xrand最近的一個(gè)已知節(jié)點(diǎn)Xnear（這一步對(duì)應(yīng)偽代碼當(dāng)中的near()函數(shù)）。因?yàn)楫?dāng)前只有一個(gè)根節(jié)點(diǎn)（起點(diǎn)），所以根節(jié)點(diǎn)即為Xnear。

---

“樹(shù)的生長(zhǎng)”（執(zhí)行Steer函數(shù)）
將Xnear與Xrand兩節(jié)點(diǎn)連接起來(lái)作為“樹(shù)”生長(zhǎng)的方向。

---

會(huì)設(shè)置一個(gè)步長(zhǎng)作為“樹(shù)枝”。
因?yàn)椴介L(zhǎng)有長(zhǎng)度，可能長(zhǎng)度不會(huì)恰好等于Xnear到Xrand的距離。
所以稱“樹(shù)枝”的末端節(jié)點(diǎn)為Xnew。

---

現(xiàn)在，“樹(shù)枝”的長(zhǎng)度就是從Xnear到Xnew的長(zhǎng)度。
將在“樹(shù)枝”上的點(diǎn)（Xnear到Xnew這之間無(wú)限的點(diǎn)）都?xì)w為節(jié)點(diǎn)。
得到Xnew之后，之前的Xrand就舍棄了，只保留了“樹(shù)枝”，之后重新進(jìn)行采樣。

重新采樣

---

重新采樣之后，發(fā)現(xiàn)連接的路徑穿過(guò)了障礙物。

---

碰到連線穿過(guò)障礙物的，就拋棄這一次采樣，重新采樣。

---

發(fā)現(xiàn)，隨著采樣的進(jìn)行，會(huì)越來(lái)越靠近終點(diǎn)。

---

但是要使算法停止，就必須要隨機(jī)采樣點(diǎn)剛好是終點(diǎn)，這樣的概率是非常小的。所以會(huì)設(shè)置一個(gè)提前停止的條件：

因?yàn)槊恳欢螛?shù)枝的末端都是Xnew，所以每產(chǎn)生一次Xnew節(jié)點(diǎn)，我們都判斷一下Xnew與終點(diǎn)之間的距離，看這個(gè)距離是否小于步長(zhǎng)，如果小于步長(zhǎng)且沒(méi)有經(jīng)過(guò)障礙物，則就直接把Xnew與終點(diǎn)進(jìn)行相連。

---

綜上，就能找到一條從起點(diǎn)到終點(diǎn)的路徑。

---

效率問(wèn)題

存在一個(gè)效率問(wèn)題，如下圖所示，按照每次Xnew之后進(jìn)行Xnew與終點(diǎn)連線判斷的情況，下圖是可以直接按照這條軌跡到終點(diǎn)的。（因?yàn)樵谶@沒(méi)有碰到障礙物且只是步長(zhǎng)不滿足所給的條件）

這時(shí)候，改進(jìn)一下采樣的范圍，就是直接沿著這個(gè)線的周圍進(jìn)行采樣，限定范圍，就可以加快算法導(dǎo)向終點(diǎn)的速度，這就是我的下篇Blog所要詳解的Informed RRT*算法。

代碼

代碼在我的github以及gitee當(dāng)中可下載。

總結(jié)

以上是生活随笔為你收集整理的RRT算法原理和代码详解（快速扩展随机树）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。