function?My_RRT
clc
clear
close?all
%%??color?map
load?maze.mat?map
[map_heightmap_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?1
??????????0?0?0
??????????1?0?0?
??????????0?1?0
??????????0?0?1];
imshow（uint8（map）colormap）
hold?on
%%?rrt?tree?%行是y坐標，列是x坐標
%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:K
????arrived=is_goal_arrived（verticesq_goaldelta_q）;
????if?arrived
????????vertices=[vertices;q_goal];
????????edges?=?[edges;[size（vertices1）size（vertices1）-1]];
????????break;
????end
????if?rand?<=?p
????????q_rand?=?q_goal;%q（1）寬x，q（2）高y
????else
????????q_rand?=?[randi（map_height）randi（map_width）];
????end
????if?map（?q_rand（11）q_rand（12）?）?==?1?%map（1）heightmap（2）width
????????continue;
????end
????[q_newq_nearq_near_indvector_dir]?=?get_qnew_qnear（delta_qq_randvertices）;
????add_qnew?=?is_add_in_veritces（mapq_newq_nearvector_dir10）;
????if?add_qnew
????????vertices=[vertices;q_new];
????????r_v?=?size（vertices1）;
????????edges?=?[edges;[r_vq_near_ind]];
????else
????????continue;
????end
%?????plot（q_near（11）q_near（21）‘*b‘）;
???plot（[q_near（12）q_new（12）][q_near（11）q_new（11）]‘-b‘）
???drawnow
end
path?=find_path_node（edges）;
%plot?base?path
plot（vertices（path2）vertices（path1）‘-r‘）
%smooth
path_smooth?=?smooth（pathverticesmap）;
%plot?smooth?path
plot（vertices（path_smooth2）vertices（path_smooth1）‘-g‘）;
end
%%?sub?function
function?arrived=is_goal_arrived（verticesq_goaldelta_q）
%判斷是否到達終點
dist=pdist2（vertices（end:）q_goal）;
if?dist?<=?delta_q
????arrived=1;
else
????arrived=0;
end
end

function?[q_newq_nearq_near_indvector_dir]?=?get_qnew_qnear（delta_qq_randvertices）
%獲得節點中最近的和新節點
dist_rand?=?pdist2（verticesq_rand）;
[dist_minq_near_ind]=min（dist_rand）;
q_near=vertices（q_near_ind:）;
vector_dir?=q_rand-q_near;
vector_dir?=?vector_dir./dist_min;
if?dist_min?>?delta_q????%隨機點到最近點的距離不確定
????q_new?=?floor（?q_near+delta_q*vector_dir?）;
else
????q_new=q_rand;
end
end

function?add_qnew?=?is_add_in_veritces（mapq_newq_nearvector_dirinsert_p）
%判斷是否加入到列表中，q_new與edges_new
%輸出:add_qnew=1加入?0不加入
%注意：sub2ind[y高x寬]=size（map）q_goal=[x寬y高]
dist_new2near?=?norm（q_new?-?q_near）;%此處有問題
dist_gap?=?dist_new2near/insert_p;
ii?=1:insert_p;
insert_point?=?repmat（q_nearinsert_p1）+ii‘.*dist_gap*?vector_dir;
insert_point?=[floor（insert_point）;q_new];
insert_num?=?sub2ind（size（map）insert_point（:1）insert_point（:2））;
or?=find（?map（insert_num）==1?）;
if?~isempty（or）??
????add_qnew=0;
else
????add_qnew=1;
end

end

function?path?=find_path_node（edges）
%返回路徑?path代表在頂點中的行數，返回的是行向量
e=edges（end2）;
path?=?edges（end:）;
while?true
???ind=?find（edges（:1）==e）;
????tmp_e?=?edges（ind

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????4159??2018-05-17?14:33??My_RRT.m

?????文件???????6168??2015-10-30?17:25??maze.mat

-----------?---------??----------?-----??----

????????????????10327????????????????????2