基于RRT算法的路徑規(guī)劃實(shí)現(xiàn)（matlab）

快速隨機(jī)擴(kuò)展樹算法(Rapidly-exploring Random Tree, RRT)

RRT算法基于采樣的方式在配置空間中搜索，它的搜索過程類似于一棵樹不斷生長、向四周擴(kuò)散的過程，它以起點(diǎn)作為搜索樹T根節(jié)點(diǎn)。

偽代碼

詳細(xì)步驟

1.起點(diǎn)為X_init，目標(biāo)地點(diǎn)為X_goal;
2.在空間中隨機(jī)采樣，得到隨機(jī)點(diǎn)X_rand;
3.然后計(jì)算隨機(jī)點(diǎn)X_rand與隨機(jī)樹Ｔ中所有節(jié)點(diǎn)的距離，找出離隨機(jī)點(diǎn)X_rand最近的節(jié)點(diǎn)作為鄰近點(diǎn)X_near;
4.隨機(jī)樹Ｔ從鄰近點(diǎn)X_near向隨機(jī)點(diǎn)X_rand方向移動給定步長，擴(kuò)展生成一 個(gè)新的節(jié)點(diǎn)X_new;
5.如果在鄰近點(diǎn)X_near向新節(jié)點(diǎn)X_new方向擴(kuò)展的過程中不與障礙物發(fā)生碰撞，則將新節(jié)點(diǎn)X_new加入到隨機(jī)樹Ｔ中，若發(fā)生碰撞，重新采樣隨機(jī)點(diǎn)X_rand。
6.最后判斷新節(jié)點(diǎn)X_new是否在目標(biāo)點(diǎn)X_goal的區(qū)域內(nèi)，若不在目標(biāo)點(diǎn)區(qū)域內(nèi)，重復(fù)上過程；若在目標(biāo)點(diǎn)區(qū)域內(nèi)，將新節(jié)點(diǎn)X_new作為目標(biāo)點(diǎn)的父節(jié)點(diǎn)，并將目標(biāo)點(diǎn)X_goal加入到隨機(jī)樹Ｔ中.

新節(jié)點(diǎn)生長過程

部分代碼

function My_RRT clc clear close all %% color map load maze.mat map [map_height,map_width]=size(map); %行是高y，列是寬x q_start = [206, 198]; %q s t a r t ( 1 ) : x寬 , q s t a r t ( 2 ) : y高 q_goal = [416, 612]; colormap=[1 1 10 0 01 0 0 0 1 00 0 1]; imshow(uint8(map),colormap) hold on %% rrt tree %行是y坐標(biāo)，列是x坐標(biāo) %initial vertices=q_start; edges = []; K=10000; delta_q=50; p=0.3; q_rand=[]; q_near=[]; q_new=[]; %main loop plot(q_start(2),q_start(1),'*b') plot(q_goal(2),q_goal(1),'*y') for k = 1:Karrived=is_goal_arrived(vertices,q_goal,delta_q);if arrivedvertices=[vertices;q_goal];edges = [edges;[size(vertices,1),size(vertices,1)-1]];break;endif rand <= pq_rand = q_goal;%q(1)寬x，q(2)高yelseq_rand = [randi(map_height),randi(map_width)];endif map( q_rand(1,1),q_rand(1,2) ) == 1 %map(1)height,map(2)widthcontinue;end[q_new,q_near,q_near_ind,vector_dir] = get_qnew_qnear(delta_q,q_rand,vertices);add_qnew = is_add_in_veritces(map,q_new,q_near,vector_dir,10);if add_qnewvertices=[vertices;q_new];r_v = size(vertices,1);edges = [edges;[r_v,q_near_ind]];elsecontinue;end % plot(q_near(1,1),q_near(2,1),'*b');plot([q_near(1,2),q_new(1,2)],[q_near(1,1),q_new(1,1)],'-b')drawnow end path =find_path_node(edges); %plot base path plot(vertices(path,2),vertices(path,1),'-r') %smooth path_smooth = smooth(path,vertices,map); %plot smooth path plot(vertices(path_smooth,2),vertices(path_smooth,1),'-g'); end

RRT算法實(shí)現(xiàn)結(jié)果

下載鏈接

https://download.csdn.net/download/iii66yy/80675832

參考文獻(xiàn)
[1] 阮曉鋼,周靜,張晶晶,朱曉慶.基于子目標(biāo)搜索的機(jī)器人目標(biāo)導(dǎo)向RRT路徑規(guī)劃算法[J].控制與決策,2020,35(10):2543-2548.DOI:10.13195/j.kzyjc.2019.0043.

總結(jié)

以上是生活随笔為你收集整理的基于RRT算法的路径规划实现（matlab）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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