RRT算法介紹

RRT算法原理介紹：RRT搜索樹與樹的生長相類似，即不斷生長的同時又向四周擴(kuò)散。算法以路徑起點(diǎn)Xstart作為隨機(jī)樹T的根節(jié)點(diǎn)，樹中節(jié)點(diǎn)xj用集合V存儲，節(jié)點(diǎn)間的連接用連接邊集E存儲，所有節(jié)點(diǎn)xj滿足屬于集合Xfree的條件。路徑規(guī)劃中，從空閑區(qū)域選取隨機(jī)節(jié)點(diǎn)來引導(dǎo)樹的生長方向，隨機(jī)節(jié)點(diǎn)數(shù)依照具體情況而定，直到到達(dá)目標(biāo)點(diǎn)才完成整個搜索，標(biāo)出根節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的連接線。
RRT算法偽代碼如下：
Build_RRT(xstart, xfinish, K)

T.start(xstart);

for k = 1 to K do

Xrand<-RANDOM();

Xnear<-NEAREST(Xrand, T);

if OBS_NOT_FREE(Xnew, Xrand)

continue;

Xnew<-NEW_START(Xnew, Xrand, u);

V<-T.add_vertex(Xnew);

E<-T.add_edge(Xnear, Xnew);

if dis[Xnew, xfinish]<u

Return T;
圖1 RRT算法擴(kuò)展圖

算法在第1-2步處進(jìn)行初始化，設(shè)置最大迭代次數(shù)后進(jìn)入循環(huán)。
第3-7步產(chǎn)生新節(jié)點(diǎn)，在空間區(qū)域中，隨機(jī)節(jié)點(diǎn)Xrand屬于集合RANDOM；NEAREST函數(shù)利用歐氏距離選擇節(jié)點(diǎn)中距離Xrand最近的節(jié)點(diǎn)，產(chǎn)生Xnear節(jié)點(diǎn)。如果Xrand與Xnear連線上存在障礙物，那么判斷之后就會停止當(dāng)前循環(huán)，進(jìn)入下一次循環(huán)。NEW_START函數(shù)從Xnear與Xrand兩節(jié)點(diǎn)的連線方向上擴(kuò)展固定步長u，從而得到新節(jié)點(diǎn)Xnew。節(jié)點(diǎn)擴(kuò)展過程如圖1所示。
第8-9步將產(chǎn)生的新節(jié)點(diǎn)以及新產(chǎn)生的邊添加至原隨機(jī)樹。
第10-11步判斷條件，若新節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的歐式距離小于固定步長或者新節(jié)點(diǎn)已經(jīng)到達(dá)最終目標(biāo)節(jié)點(diǎn)，則返回隨機(jī)樹T；另一方面，若未達(dá)到目標(biāo)節(jié)點(diǎn)但達(dá)到了最大迭代次數(shù)K，此時算法會終止，連接到目標(biāo)節(jié)點(diǎn)。
隨機(jī)樹確定后，標(biāo)記目標(biāo)點(diǎn)返回，循環(huán)遍歷父節(jié)點(diǎn)，探索至樹的根節(jié)點(diǎn)。此時，算法從隨機(jī)樹中找到一條連接根節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)且無碰撞障礙物的可行路徑。

MATLAB仿真：

Time = 33.240944 s
Path Length = 1.8452e+03

總結(jié)

以上是生活随笔為你收集整理的RRT算法介绍的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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