matlab-快速搜索随机树算法实现RRT

快速搜索随机树算法实现RRT以matlab语言实现 包含：My_RRT.m函数代码、maze.mat地图 可参考博客代码原理https://blog.csdn.net/qinze5857/article/details/80350317

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