RRT(matlab）

%主函数
clc;
clear all;
close all;
%% 参数初始化
x_s = 1; y_s = 1;
x_g = 750; y_g = 750;
Thr=30;
step=40;
%% Tree初始化
T.v(1).x = x_s;  %列坐标
T.v(1).y = y_s;  %行坐标
T.v(1).xPre = x_s;
T.v(1).yPre = y_s;
T.v(1).indPre = 0; %父节点在T中的索引
%% plot初始化
figure(1);
ImpRgb = imread('newmap.png');
Imp = rgb2gray(ImpRgb);
imshow(Imp);
xL = size(Imp,1); %地图row
yL = size(Imp,2); %地图col
hold on;
% 画起始点和目标点
plot(x_s, y_s, 'mp', 'MarkerSize', 10, 'MarkerFaceColor', 'm');
plot(x_g, y_g, 'gp', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
%% 开始循环生长节点，搜索路径
count=1;
findPath=false;
tic;
for iter=1:3000
    %step1.在地图上生成随机采样点
    x_rand = [unifrnd(0,800),unifrnd(0,800)];
    %step2.遍历树节点，找到最近点
    minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2);
    minIndex=1;
    for i = 2:size(T.v,2)
        distance = sqrt((x_rand(1) - T.v(i).x)^2 + (x_rand(2) - T.v(i).y)^2);
        if(distance < minDis)
            minDis = distance;
            minIndex = i;
        end
    end
    x_near(1) = T.v(minIndex).x;
    x_near(2) = T.v(minIndex).y;
    
    %step3.生成子节点
    theta = atan2(x_rand(2) - x_near(2),x_rand(1) - x_near(1));
    x_new(1) = x_near(1) + step * cos(theta);
    x_new(2) = x_near(2) + step * sin(theta);
    %step4.判断当前点和子节点连线有没有发生碰撞
    if ~noCollision(x_near,x_new,Imp)
        continue;
    end
    %step5.将子节点加入到Tree中
    count = count+1;
    T.v(count).x = x_new(1);
    T.v(count).y = x_new(2);
    T.v(count).xPre = x_near(1);
    T.v(count).yPre = x_near(2);
    T.v(count).indPre = minIndex;
    
    %step6.将新生成的树枝画出来，便于显示pause（0.02)
    plot([x_near(1),x_new(1)],[x_near(2),x_new(2)],'b','LineWidth',1);
    plot(x_new(1),x_new(2),'ko','MarkerSize',4,'MarkerFaceColor','k');
    pause(0.02); %暂停0.02秒
    
    %step7.判断有无到达目标点附近
    dis2goal = sqrt((x_new(1)-x_g)^2+(x_new(2)-y_g)^2);
    if dis2goal<Thr
        findPath = true;
        disp('fing path!');
        break;
    end
end
toc;

%% 找到路径，进行路径回溯
if findPath
    path.pose(1).x=x_g;
    path.pose(1).y=y_g;
    path.pose(2).x=T.v(end).x;
    path.pose(2).y=T.v(end).y;
    index=T.v(end).indPre;
    j=3;
    while true
        path.pose(j).x=T.v(index).x;
        path.pose(j).y=T.v(index).y;
        index=T.v(index).indPre;
        j=j+1;
        if index==1
            path.pose(j).x=T.v(index).x;
            path.pose(j).y=T.v(index).y;
            break;
        end
    end
else
    disp('no path!');
end
%% 显示路径
for i = 2:length(path.pose)
    plot([path.pose(i).x,path.pose(i-1).x],[path.pose(i).y,path.pose(i-1).y],'g','LineWidth',4);
end

%碰撞检测函数
function noCollised = noCollision(x_near,x_new,map)
noCollised=true;
theta = atan2(x_new(2) - x_near(2),x_new(1) - x_near(1));
%首先检查x_new是否是位于障碍物范围
if ~(checkPoint(ceil(x_new),map) && checkPoint(floor(x_new),map) ...
     && checkPoint([ceil(x_new(1)),floor(x_new(2))],map) ...
     && checkPoint([floor(x_new(1)),ceil(x_new(2))],map))
        noCollised = false;
        return;
end
%以0.5为步长，逐步增长检测有没有发生碰撞
for r=0:0.5:sqrt(sum((x_new - x_near).^2))
    point=x_near + r.*[cos(theta),sin(theta)];
    if ~(checkPoint(ceil(point),map) && checkPoint(floor(point),map) ...
     && checkPoint([ceil(point(1)),floor(point(2))],map) ...
     && checkPoint([floor(point(1)),ceil(point(2))],map))
        noCollised = false;
        break;
    end
end
        
end

%检查点是否碰撞、有效
function feasible = checkPoint(point,map)
feasible = true;
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) ...
        && map(point(2),point(1))==255)
    feasible = false;
end
end